diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 889e7dcff6..4c1ef6c021 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -334,11 +334,7 @@ #endif #ifndef HIL_SUPPORT -#if HAL_MINIMIZE_FEATURES -# define HIL_SUPPORT DISABLED -#else -# define HIL_SUPPORT ENABLED -#endif +# define HIL_SUPPORT !HAL_MINIMIZE_FEATURES #endif ////////////////////////////////////////////////////////////////////////////// @@ -350,11 +346,7 @@ ////////////////////////////////////////////////////////////////////////////// // Payload Gripper #ifndef GRIPPER_ENABLED - #if HAL_MINIMIZE_FEATURES - # define GRIPPER_ENABLED DISABLED - #else - # define GRIPPER_ENABLED ENABLED - #endif + #define GRIPPER_ENABLED !HAL_MINIMIZE_FEATURES #endif #ifndef STATS_ENABLED @@ -362,11 +354,7 @@ #endif #ifndef DEVO_TELEM_ENABLED -#if HAL_MINIMIZE_FEATURES - #define DEVO_TELEM_ENABLED DISABLED -#else - #define DEVO_TELEM_ENABLED ENABLED -#endif + #define DEVO_TELEM_ENABLED !HAL_MINIMIZE_FEATURES #endif #ifndef OSD_ENABLED @@ -374,11 +362,7 @@ #endif #ifndef SOARING_ENABLED -#if HAL_MINIMIZE_FEATURES - #define SOARING_ENABLED DISABLED -#else - #define SOARING_ENABLED ENABLED -#endif + #define SOARING_ENABLED !HAL_MINIMIZE_FEATURES #endif #ifndef LANDING_GEAR_ENABLED diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 7ed183f7be..6e2e748f24 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -63,6 +63,7 @@ enum mode_reason_t { MODE_REASON_SOARING_IN_THERMAL, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED, MODE_REASON_VTOL_FAILED_TRANSITION, + MODE_REASON_UNAVAILABLE, }; // type of stick mixing enabled diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 340ab0c103..2da3b7cf87 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -15,7 +15,9 @@ bool ModeAuto::_enter() // start or resume the mission, based on MIS_AUTORESET plane.mission.start_or_resume(); +#if SOARING_ENABLED == ENABLED plane.g2.soaring_controller.init_cruising(); +#endif return true; } diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 6c6dc4fd72..1ea3cdfa70 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -9,8 +9,10 @@ bool ModeCruise::_enter() plane.cruise_state.locked_heading = false; plane.cruise_state.lock_timer_ms = 0; +#if SOARING_ENABLED == ENABLED // for ArduSoar soaring_controller plane.g2.soaring_controller.init_cruising(); +#endif plane.set_target_altitude_current(); diff --git a/ArduPlane/mode_fbwb.cpp b/ArduPlane/mode_fbwb.cpp index e7f5843877..fedd412711 100644 --- a/ArduPlane/mode_fbwb.cpp +++ b/ArduPlane/mode_fbwb.cpp @@ -7,8 +7,10 @@ bool ModeFBWB::_enter() plane.auto_throttle_mode = true; plane.auto_navigation_mode = false; +#if SOARING_ENABLED == ENABLED // for ArduSoar soaring_controller plane.g2.soaring_controller.init_cruising(); +#endif plane.set_target_altitude_current(); diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index a107b53a88..45327a2be4 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -8,10 +8,12 @@ bool ModeLoiter::_enter() plane.auto_navigation_mode = true; plane.do_loiter_at_location(); +#if SOARING_ENABLED == ENABLED if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) { plane.g2.soaring_controller.init_thermalling(); plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path } +#endif return true; } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 36c6837421..374d28f1a9 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -244,10 +244,10 @@ bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason) } #if !QAUTOTUNE_ENABLED - if (&new_mode == plane.mode_qautotune) { + if (&new_mode == &plane.mode_qautotune) { gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled"); - set_mode(plane.mode_qhover); - return; + set_mode(plane.mode_qhover, MODE_REASON_UNAVAILABLE); + return false; } #endif