From d16872cca582715e24f5e61d9d8640b277315b68 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Tue, 22 Sep 2020 18:00:39 +0100
Subject: [PATCH] Plane: compile out ADSB mode if required

---
 ArduPlane/Attitude.cpp       |  8 ++++----
 ArduPlane/GCS_Mavlink.cpp    |  5 ++---
 ArduPlane/Plane.h            |  4 +++-
 ArduPlane/avoidance_adsb.cpp |  2 +-
 ArduPlane/control_modes.cpp  |  3 +++
 ArduPlane/geofence.cpp       |  7 ++++---
 ArduPlane/mode.h             | 10 ++++++++++
 ArduPlane/mode_avoidADSB.cpp |  4 ++++
 ArduPlane/navigation.cpp     |  2 +-
 ArduPlane/quadplane.cpp      |  9 +++++----
 ArduPlane/servos.cpp         |  2 +-
 11 files changed, 38 insertions(+), 18 deletions(-)

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 <stdint.h>
 #include <AP_Common/Location.h>
 #include <AP_Soaring/AP_Soaring.h>
+#include <AP_ADSB/AP_ADSB.h>
 
 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));