|
|
|
@ -215,7 +215,7 @@ static void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
@@ -215,7 +215,7 @@ static void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// smooth throttle transition when switching from manual to automatic flight modes |
|
|
|
|
if (manual_flight_mode(old_control_mode) && !manual_flight_mode(new_control_mode) && motors.armed() && !ap.land_complete) { |
|
|
|
|
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) { |
|
|
|
|
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle |
|
|
|
|
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in)); |
|
|
|
|
} |
|
|
|
@ -246,8 +246,8 @@ static bool mode_requires_GPS(uint8_t mode) {
@@ -246,8 +246,8 @@ static bool mode_requires_GPS(uint8_t mode) {
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// manual_flight_mode - returns true if flight mode is completely manual (i.e. roll, pitch, yaw and throttle are controlled by pilot) |
|
|
|
|
static bool manual_flight_mode(uint8_t mode) { |
|
|
|
|
// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle) |
|
|
|
|
static bool mode_has_manual_throttle(uint8_t mode) { |
|
|
|
|
switch(mode) { |
|
|
|
|
case ACRO: |
|
|
|
|
case STABILIZE: |
|
|
|
@ -262,7 +262,7 @@ static bool manual_flight_mode(uint8_t mode) {
@@ -262,7 +262,7 @@ static bool manual_flight_mode(uint8_t mode) {
|
|
|
|
|
// mode_allows_arming - returns true if vehicle can be armed in the specified mode |
|
|
|
|
// arming_from_gcs should be set to true if the arming request comes from the ground station |
|
|
|
|
static bool mode_allows_arming(uint8_t mode, bool arming_from_gcs) { |
|
|
|
|
if (manual_flight_mode(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) { |
|
|
|
|
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) { |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
return false; |
|
|
|
|