|
|
|
@ -327,6 +327,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -327,6 +327,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
switch(control_mode) |
|
|
|
|
{ |
|
|
|
|
case INITIALISING: |
|
|
|
|
throttle_allows_nudging = true; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = false; |
|
|
|
|
break; |
|
|
|
@ -335,17 +336,20 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -335,17 +336,20 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
case STABILIZE: |
|
|
|
|
case TRAINING: |
|
|
|
|
case FLY_BY_WIRE_A: |
|
|
|
|
throttle_allows_nudging = false; |
|
|
|
|
auto_throttle_mode = false; |
|
|
|
|
auto_navigation_mode = false; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUTOTUNE: |
|
|
|
|
throttle_allows_nudging = false; |
|
|
|
|
auto_throttle_mode = false; |
|
|
|
|
auto_navigation_mode = false; |
|
|
|
|
autotune_start(); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case ACRO: |
|
|
|
|
throttle_allows_nudging = false; |
|
|
|
|
auto_throttle_mode = false; |
|
|
|
|
auto_navigation_mode = false; |
|
|
|
|
acro_state.locked_roll = false; |
|
|
|
@ -353,6 +357,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -353,6 +357,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case CRUISE: |
|
|
|
|
throttle_allows_nudging = false; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = false; |
|
|
|
|
cruise_state.locked_heading = false; |
|
|
|
@ -365,6 +370,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -365,6 +370,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case FLY_BY_WIRE_B: |
|
|
|
|
throttle_allows_nudging = false; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = false; |
|
|
|
|
|
|
|
|
@ -376,12 +382,14 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -376,12 +382,14 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
|
|
|
|
|
case CIRCLE: |
|
|
|
|
// the altitude to circle at is taken from the current altitude
|
|
|
|
|
throttle_allows_nudging = false; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = true; |
|
|
|
|
next_WP_loc.alt = current_loc.alt; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUTO: |
|
|
|
|
throttle_allows_nudging = true; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = true; |
|
|
|
|
if (quadplane.available() && quadplane.enable == 2) { |
|
|
|
@ -397,6 +405,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -397,6 +405,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case RTL: |
|
|
|
|
throttle_allows_nudging = true; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = true; |
|
|
|
|
prev_WP_loc = current_loc; |
|
|
|
@ -404,6 +413,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -404,6 +413,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case LOITER: |
|
|
|
|
throttle_allows_nudging = true; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = true; |
|
|
|
|
do_loiter_at_location(); |
|
|
|
@ -418,6 +428,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -418,6 +428,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
|
|
|
|
|
case AVOID_ADSB: |
|
|
|
|
case GUIDED: |
|
|
|
|
throttle_allows_nudging = true; |
|
|
|
|
auto_throttle_mode = true; |
|
|
|
|
auto_navigation_mode = true; |
|
|
|
|
guided_throttle_passthru = false; |
|
|
|
@ -434,6 +445,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
@@ -434,6 +445,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
|
|
|
|
|
case QLOITER: |
|
|
|
|
case QLAND: |
|
|
|
|
case QRTL: |
|
|
|
|
throttle_allows_nudging = true; |
|
|
|
|
auto_navigation_mode = false; |
|
|
|
|
if (!quadplane.init_mode()) { |
|
|
|
|
control_mode = previous_mode; |
|
|
|
|