Browse Source

Plane: separate out auto and guided VTOL states

this prevents a switch to AUTO from using VTOL mode incorrectly
mission-4.1.18
Andrew Tridgell 9 years ago
parent
commit
3fc43b94f9
  1. 2
      ArduPlane/ArduPlane.cpp
  2. 5
      ArduPlane/Plane.h
  3. 2
      ArduPlane/commands.cpp
  4. 2
      ArduPlane/navigation.cpp
  5. 2
      ArduPlane/quadplane.cpp
  6. 3
      ArduPlane/system.cpp

2
ArduPlane/ArduPlane.cpp

@ -563,7 +563,7 @@ void Plane::update_flight_mode(void) @@ -563,7 +563,7 @@ void Plane::update_flight_mode(void)
break;
case GUIDED:
if (auto_state.vtol_mode && quadplane.available()) {
if (auto_state.vtol_guided && quadplane.available()) {
quadplane.guided_update();
break;
}

5
ArduPlane/Plane.h

@ -502,8 +502,11 @@ private: @@ -502,8 +502,11 @@ private:
// barometric altitude at start of takeoff
float baro_takeoff_alt;
// are we in VTOL mode?
// are we in VTOL mode in AUTO?
bool vtol_mode:1;
// are we doing GUIDED mode as a VTOL?
bool vtol_guided:1;
} auto_state;
struct {

2
ArduPlane/commands.cpp

@ -97,7 +97,7 @@ void Plane::set_guided_WP(void) @@ -97,7 +97,7 @@ void Plane::set_guided_WP(void)
loiter.start_time_ms = 0;
// start in non-VTOL mode
auto_state.vtol_mode = false;
auto_state.vtol_guided = false;
loiter_angle_reset();
}

2
ArduPlane/navigation.cpp

@ -175,7 +175,7 @@ void Plane::update_loiter(uint16_t radius) @@ -175,7 +175,7 @@ void Plane::update_loiter(uint16_t radius)
if (loiter.start_time_ms != 0 &&
quadplane.available() &&
quadplane.guided_mode != 0) {
auto_state.vtol_mode = true;
auto_state.vtol_guided = true;
} else if (loiter.start_time_ms == 0 &&
control_mode == AUTO &&
!auto_state.no_crosstrack &&

2
ArduPlane/quadplane.cpp

@ -1100,7 +1100,7 @@ bool QuadPlane::in_vtol_mode(void) @@ -1100,7 +1100,7 @@ bool QuadPlane::in_vtol_mode(void)
plane.control_mode == QLOITER ||
plane.control_mode == QLAND ||
plane.control_mode == QRTL ||
(plane.control_mode == GUIDED && plane.auto_state.vtol_mode) ||
(plane.control_mode == GUIDED && plane.auto_state.vtol_guided) ||
in_vtol_auto());
}

3
ArduPlane/system.cpp

@ -382,6 +382,7 @@ void Plane::set_mode(enum FlightMode mode) @@ -382,6 +382,7 @@ void Plane::set_mode(enum FlightMode mode)
// assume non-VTOL mode
auto_state.vtol_mode = false;
auto_state.vtol_guided = false;
switch(control_mode)
{
@ -784,7 +785,7 @@ void Plane::frsky_telemetry_send(void) @@ -784,7 +785,7 @@ void Plane::frsky_telemetry_send(void)
*/
int8_t Plane::throttle_percentage(void)
{
if (auto_state.vtol_mode) {
if (quadplane.in_vtol_mode()) {
return quadplane.throttle_percentage();
}
// to get the real throttle we need to use norm_output() which

Loading…
Cancel
Save