Plane: add new flight mode AVOID_ADSB to mimic GUIDED

ArduPlane/ArduPlane.cpp

@@ -581,6 +581,7 @@ void Plane::update_flight_mode(void)
         handle_auto_mode();
         break;
 
+    case AVOID_ADSB:
     case GUIDED:
         if (auto_state.vtol_loiter && quadplane.available()) {
             quadplane.guided_update();
@@ -806,6 +807,7 @@ void Plane::update_navigation()
         // fall through to LOITER
 
     case LOITER:
+    case AVOID_ADSB:
     case GUIDED:
         update_loiter(radius);
         break;

ArduPlane/Attitude.cpp

@@ -432,7 +432,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 == GUIDED &&
+    if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
             plane.guided_state.last_forced_throttle_ms > 0 &&
             millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
         commanded_throttle = plane.guided_state.forced_throttle;
@@ -458,7 +458,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 == GUIDED &&
+    if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
             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;
@@ -542,7 +542,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 == GUIDED &&
+    if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
             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;
@@ -560,7 +560,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 == GUIDED &&
+    if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
             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;
@@ -1064,7 +1064,7 @@ void Plane::set_servos(void)
             // manual pass through of throttle while in FBWA or
             // STABILIZE mode with THR_PASS_STAB set
             channel_throttle->set_radio_out(channel_throttle->get_radio_in());
-        } else if (control_mode == GUIDED && 
+        } else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
                    guided_throttle_passthru) {
             // manual pass through of throttle while in GUIDED
             channel_throttle->set_radio_out(channel_throttle->get_radio_in());
@@ -1342,6 +1342,7 @@ bool Plane::allow_reverse_thrust(void)
     case FLY_BY_WIRE_B:
         allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB);
         break;
+    case AVOID_ADSB:
     case GUIDED:
         allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED);
         break;

ArduPlane/GCS_Mavlink.cpp

@@ -52,6 +52,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
     case AUTO:
     case RTL:
     case LOITER:
+    case AVOID_ADSB:
     case GUIDED:
     case CIRCLE:
     case QRTL:
@@ -209,6 +210,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
     case AUTO:
     case RTL:
     case LOITER:
+    case AVOID_ADSB:
     case GUIDED:
     case CIRCLE:
     case QRTL:
@@ -1242,7 +1244,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             // this command should be ignored since it comes in from GCS
             // or a companion computer:
             result = MAV_RESULT_FAILED;
-            if (plane.control_mode != GUIDED && plane.control_mode != AUTO) {
+            if (plane.control_mode != GUIDED && plane.control_mode != AUTO && plane.control_mode != AVOID_ADSB) {
                 // failed
                 break;
             }
@@ -2122,7 +2124,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         // in e.g., RTL, CICLE. Specifying a single mode for companion
         // computer control is more safe (even more so when using
         // FENCE_ACTION = 4 for geofence failures).
-        if (plane.control_mode != GUIDED) { // don't screw up failsafes
+        if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) { // don't screw up failsafes
             break; 
         }
 
@@ -2220,7 +2222,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         // in modes such as RTL, CIRCLE, etc.  Specifying ONLY one mode
         // for companion computer control is more safe (provided
         // one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
-        if (plane.control_mode != GUIDED) {
+        if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) {
             //don't screw up failsafes
             break;
         }

ArduPlane/altitude.cpp

@@ -75,6 +75,7 @@ void Plane::setup_glide_slope(void)
      */
     switch (control_mode) {
     case RTL:
+    case AVOID_ADSB:
     case GUIDED:
         /* glide down slowly if above target altitude, but ascend more
            rapidly if below it. See

ArduPlane/defines.h

@@ -58,6 +58,7 @@ enum FlightMode {
     AUTO          = 10,
     RTL           = 11,
     LOITER        = 12,
+    AVOID_ADSB    = 14,
     GUIDED        = 15,
     INITIALISING  = 16,
     QSTABILIZE    = 17,

ArduPlane/events.cpp

@@ -36,6 +36,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
         break;
         
     case AUTO:
+    case AVOID_ADSB:
     case GUIDED:
     case LOITER:
         if(g.short_fs_action != 0) {
@@ -95,6 +96,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
         break;
         
     case AUTO:
+    case AVOID_ADSB:
     case GUIDED:
     case LOITER:
         if(g.long_fs_action == 3) {

ArduPlane/geofence.cpp

@@ -285,7 +285,7 @@ void Plane::geofence_check(bool altitude_check_only)
         // GUIDED to the return point
         if (geofence_state != NULL &&
             (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) &&
-            control_mode == GUIDED &&
+            (control_mode == GUIDED || control_mode == AVOID_ADSB) &&
             geofence_present() &&
             geofence_state->boundary_uptodate &&
             geofence_state->old_switch_position == oldSwitchPosition &&
@@ -348,7 +348,7 @@ void Plane::geofence_check(bool altitude_check_only)
 
     // we are outside the fence
     if (geofence_state->fence_triggered &&
-        (control_mode == GUIDED || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) {
+        (control_mode == GUIDED || control_mode == AVOID_ADSB || control_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;
@@ -432,7 +432,7 @@ bool Plane::geofence_stickmixing(void) {
     if (geofence_enabled() &&
         geofence_state != NULL &&
         geofence_state->fence_triggered &&
-        control_mode == GUIDED) {
+        (control_mode == GUIDED || control_mode == AVOID_ADSB)) {
         // don't mix in user input
         return false;
     }

ArduPlane/navigation.cpp

@@ -198,7 +198,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 == GUIDED) {
+            if (control_mode == GUIDED || control_mode == AVOID_ADSB) {
                 // starting a loiter in GUIDED means we just reached the target point
                 gcs_send_mission_item_reached_message(0);
             }

ArduPlane/quadplane.cpp

@@ -1253,7 +1253,7 @@ bool QuadPlane::in_vtol_mode(void)
             plane.control_mode == QLOITER ||
             plane.control_mode == QLAND ||
             plane.control_mode == QRTL ||
-            (plane.control_mode == GUIDED && plane.auto_state.vtol_loiter) ||
+            ((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) ||
             in_vtol_auto());
 }
 

ArduPlane/system.cpp

@@ -450,6 +450,7 @@ void Plane::set_mode(enum FlightMode mode)
         do_loiter_at_location();
         break;
 
+    case AVOID_ADSB:
     case GUIDED:
         auto_throttle_mode = true;
         auto_navigation_mode = true;
@@ -507,6 +508,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
     case AUTOTUNE:
     case FLY_BY_WIRE_B:
     case CRUISE:
+    case AVOID_ADSB:
     case GUIDED:
     case AUTO:
     case RTL:
@@ -706,6 +708,9 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case LOITER:
         port->print("Loiter");
         break;
+    case AVOID_ADSB:
+        port->print("AVOID_ADSB");
+        break;
     case GUIDED:
         port->print("Guided");
         break;