diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0c0dd458e9..2833488db9 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -454,7 +454,7 @@ void Plane::calc_throttle() int32_t commanded_throttle = SpdHgt_Controller->get_throttle_demand(); // Received an external msg that guides throttle in the last 3 seconds? - if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && + if (control_mode->is_guided_mode() && plane.guided_state.last_forced_throttle_ms > 0 && millis() - plane.guided_state.last_forced_throttle_ms < 3000) { commanded_throttle = plane.guided_state.forced_throttle; @@ -478,7 +478,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler) int16_t commanded_rudder; // Received an external msg that guides yaw in the last 3 seconds? - if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && + if (control_mode->is_guided_mode() && plane.guided_state.last_forced_rpy_ms.z > 0 && millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) { commanded_rudder = plane.guided_state.forced_rpy_cd.z; @@ -566,7 +566,7 @@ void Plane::calc_nav_pitch() int32_t commanded_pitch = SpdHgt_Controller->get_pitch_demand(); // Received an external msg that guides roll in the last 3 seconds? - if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && + if (control_mode->is_guided_mode() && plane.guided_state.last_forced_rpy_ms.y > 0 && millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { commanded_pitch = plane.guided_state.forced_rpy_cd.y; @@ -584,7 +584,7 @@ void Plane::calc_nav_roll() int32_t commanded_roll = nav_controller->nav_roll_cd(); // Received an external msg that guides roll in the last 3 seconds? - if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && + if (control_mode->is_guided_mode() && plane.guided_state.last_forced_rpy_ms.x > 0 && millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { commanded_roll = plane.guided_state.forced_rpy_cd.x; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 2b5ac83980..a4ba4f6cbf 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -967,9 +967,8 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l // controlled modes (e.g., MANUAL, TRAINING) // this command should be ignored since it comes in from GCS // or a companion computer: - if ((plane.control_mode != &plane.mode_guided) && - (plane.control_mode != &plane.mode_auto) && - (plane.control_mode != &plane.mode_avoidADSB)) { + if ((!plane.control_mode->is_guided_mode()) && + (plane.control_mode != &plane.mode_auto)) { // failed return MAV_RESULT_FAILED; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 6aa9e5d2e0..3587799722 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -268,7 +268,9 @@ private: ModeAuto mode_auto; ModeRTL mode_rtl; ModeLoiter mode_loiter; +#if HAL_ADSB_ENABLED ModeAvoidADSB mode_avoidADSB; +#endif ModeGuided mode_guided; ModeInitializing mode_initializing; ModeManual mode_manual; @@ -641,7 +643,7 @@ private: #if HAL_ADSB_ENABLED AP_ADSB adsb; - // avoidance of adsb enabled vehicles (normally manned vheicles) + // avoidance of adsb enabled vehicles (normally manned vehicles) AP_Avoidance_Plane avoidance_adsb{adsb}; #endif diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index a7ca1530d8..ae102afbd5 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -203,5 +203,5 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl return false; } -#endif +#endif // HAL_ADSB_ENABLED diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index f75db68f2a..9a21bd0935 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -41,8 +41,11 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) ret = &mode_loiter; break; case Mode::Number::AVOID_ADSB: +#if HAL_ADSB_ENABLED ret = &mode_avoidADSB; break; +#endif + // if ADSB is not compiled in then fallthrough to guided case Mode::Number::GUIDED: ret = &mode_guided; break; diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index 4aa4f16014..e08239e0b2 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -334,7 +334,7 @@ void Plane::geofence_check(bool altitude_check_only) // GUIDED to the return point if (geofence_state != nullptr && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) && - (control_mode == &mode_guided || control_mode == &mode_avoidADSB) && + control_mode->is_guided_mode() && geofence_present() && geofence_state->boundary_uptodate && geofence_state->old_switch_position == oldSwitchPosition && @@ -397,7 +397,8 @@ void Plane::geofence_check(bool altitude_check_only) // we are outside the fence if (geofence_state->fence_triggered && - (control_mode == &mode_guided || control_mode == &mode_avoidADSB || control_mode == &mode_rtl || g.fence_action == FENCE_ACTION_REPORT)) { + (control_mode->is_guided_mode() + || control_mode == &mode_rtl || g.fence_action == FENCE_ACTION_REPORT)) { // we have already triggered, don't trigger again until the // user disables/re-enables using the fence channel switch return; @@ -481,7 +482,7 @@ bool Plane::geofence_stickmixing(void) { if (geofence_enabled() && geofence_state != nullptr && geofence_state->fence_triggered && - (control_mode == &mode_guided || control_mode == &mode_avoidADSB)) { + control_mode->is_guided_mode()) { // don't mix in user input return false; } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 7680c0080e..1474d4d6d0 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -5,6 +5,7 @@ #include #include #include +#include class Mode { @@ -72,6 +73,8 @@ public: virtual bool is_vtol_mode() const { return false; } virtual bool is_vtol_man_throttle() const { return false; } virtual bool is_vtol_man_mode() const { return false; } + // guided or adsb mode + virtual bool is_guided_mode() const { return false; } // true if mode can have terrain following disabled by switch virtual bool allows_terrain_disable() const { return false; } @@ -155,6 +158,8 @@ public: void navigate() override; + virtual bool is_guided_mode() const override { return true; } + protected: bool _enter() override; @@ -342,6 +347,7 @@ protected: uint32_t lock_timer_ms; }; +#if HAL_ADSB_ENABLED class ModeAvoidADSB : public Mode { public: @@ -354,10 +360,14 @@ public: void update() override; void navigate() override; + + virtual bool is_guided_mode() const override { return true; } + protected: bool _enter() override; }; +#endif class ModeQStabilize : public Mode { diff --git a/ArduPlane/mode_avoidADSB.cpp b/ArduPlane/mode_avoidADSB.cpp index 897084bf60..a8f41cbbae 100644 --- a/ArduPlane/mode_avoidADSB.cpp +++ b/ArduPlane/mode_avoidADSB.cpp @@ -1,6 +1,8 @@ #include "mode.h" #include "Plane.h" +#if HAL_ADSB_ENABLED + bool ModeAvoidADSB::_enter() { return plane.mode_guided.enter(); @@ -17,3 +19,5 @@ void ModeAvoidADSB::navigate() plane.update_loiter(0); } +#endif // HAL_ADSB_ENABLED + diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 346cf1a9f0..6e32a5f1e9 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -296,7 +296,7 @@ void Plane::update_loiter(uint16_t radius) auto_state.wp_proportion > 1) { // we've reached the target, start the timer loiter.start_time_ms = millis(); - if (control_mode == &mode_guided || control_mode == &mode_avoidADSB) { + if (control_mode->is_guided_mode()) { // starting a loiter in GUIDED means we just reached the target point gcs().send_mission_item_reached_message(0); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 6df05e8d74..a363393ee8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1350,8 +1350,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const if ((plane.g.stick_mixing == STICK_MIXING_DISABLED) && (plane.control_mode == &plane.mode_qrtl || - plane.control_mode == &plane.mode_guided || - plane.control_mode == &plane.mode_avoidADSB || + plane.control_mode->is_guided_mode() || in_vtol_auto())) { return 0; } @@ -2246,7 +2245,8 @@ bool QuadPlane::in_vtol_mode(void) const return false; } return (plane.control_mode->is_vtol_mode() || - ((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) || + (plane.control_mode->is_guided_mode() + && plane.auto_state.vtol_loiter) || in_vtol_auto()); } @@ -2262,7 +2262,8 @@ bool QuadPlane::in_vtol_posvel_mode(void) const plane.control_mode == &plane.mode_qland || plane.control_mode == &plane.mode_qrtl || plane.control_mode == &plane.mode_qautotune || - ((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) || + (plane.control_mode->is_guided_mode() && + plane.auto_state.vtol_loiter) || in_vtol_auto()); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 79ae5fcdeb..c2b6862f12 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -548,7 +548,7 @@ void Plane::set_servos_controlled(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(get_throttle_input(true), min_throttle, max_throttle)); } - } else if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && + } else if (control_mode->is_guided_mode() && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));