Plane: remove mode guided pointer comparisons.

ArduPlane/ArduPlane.cpp

@@ -866,7 +866,7 @@ bool Plane::set_target_location(const Location &target_loc)
 {
     Location loc{target_loc};
 
-    if (plane.control_mode != &plane.mode_guided) {
+    if (!plane.control_mode->is_guided_mode()) {
         // only accept position updates when in GUIDED mode
         return false;
     }

ArduPlane/Attitude.cpp

@@ -484,7 +484,7 @@ int16_t Plane::calc_nav_yaw_coordinated()
     bool using_rate_controller = false;
 
     // Received an external msg that guides yaw in the last 3 seconds?
-    if (control_mode->is_guided_mode() &&
+    if (control_mode->is_guided_or_adsb_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;

ArduPlane/GCS_Mavlink.cpp

@@ -868,7 +868,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
 
     // location is valid load and set
     if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
-        (plane.control_mode == &plane.mode_guided)) {
+        plane.control_mode->is_guided_mode()) {
         plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
 #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
         plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
@@ -899,7 +899,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
   switch(packet.command) {
     case MAV_CMD_GUIDED_CHANGE_SPEED: {
         // command is only valid in guided mode
-        if (plane.control_mode != &plane.mode_guided) {
+        if (!plane.control_mode->is_guided_mode()) {
             return MAV_RESULT_FAILED;
         }
 
@@ -938,7 +938,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
 
      case MAV_CMD_GUIDED_CHANGE_ALTITUDE: {
         // command is only valid in guided
-        if (plane.control_mode != &plane.mode_guided) {
+        if (!plane.control_mode->is_guided_mode()) {
             return MAV_RESULT_FAILED;
         }
 
@@ -972,7 +972,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
      case MAV_CMD_GUIDED_CHANGE_HEADING: {
 
         // command is only valid in guided mode
-        if (plane.control_mode != &plane.mode_guided) {
+        if (!plane.control_mode->is_guided_mode()) {
             return MAV_RESULT_FAILED;
         }
 
@@ -1129,7 +1129,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_comma
         // 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->is_guided_mode()) &&
+        if (!plane.control_mode->is_guided_or_adsb_mode() &&
             (plane.control_mode != &plane.mode_auto)) {
             // failed
             return MAV_RESULT_FAILED;
@@ -1306,7 +1306,7 @@ void GCS_MAVLINK_Plane::handle_set_attitude_target(const 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 != &plane.mode_guided) { // don't screw up failsafes
+        if (!plane.control_mode->is_guided_mode()) { // don't screw up failsafes
             return;
         }
 
@@ -1374,7 +1374,7 @@ void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_messa
         mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
 
         // exit if vehicle is not in Guided mode
-        if (plane.control_mode != &plane.mode_guided) {
+        if (!plane.control_mode->is_guided_mode()) {
             return;
         }
 
@@ -1397,7 +1397,7 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess
         // 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 != &plane.mode_guided) {
+        if (!plane.control_mode->is_guided_mode()) {
             //don't screw up failsafes
             return;
         }

ArduPlane/Log.cpp

@@ -268,7 +268,7 @@ void Plane::Log_Write_RC(void)
 void Plane::Log_Write_Guided(void)
 {
 #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
-    if (control_mode != &mode_guided) {
+    if (!control_mode->is_guided_mode()) {
         return;
     }
 

ArduPlane/fence.cpp

@@ -43,7 +43,7 @@ void Plane::fence_check()
             case AC_FENCE_ACTION_GUIDED_THROTTLE_PASS:
             case AC_FENCE_ACTION_RTL_AND_LAND:
                 if (plane.control_mode_reason == ModeReason::FENCE_BREACHED &&
-                    control_mode->is_guided_mode()) {
+                    control_mode->is_guided_or_adsb_mode()) {
                     set_mode(*previous_mode, ModeReason::FENCE_RETURN_PREVIOUS_MODE);
                 }
                 break;

ArduPlane/mode.cpp

@@ -306,7 +306,7 @@ bool Mode::use_throttle_limits() const
         return !plane.g.throttle_passthru_stabilize;
     }
 
-    if (is_guided_mode() && plane.guided_throttle_passthru) {
+    if (is_guided_or_adsb_mode() && plane.guided_throttle_passthru) {
         // manual pass through of throttle while in GUIDED
         return false;
     }
@@ -338,7 +338,7 @@ bool Mode::use_battery_compensation() const
         return false;
     }
 
-    if (is_guided_mode() && plane.guided_throttle_passthru) {
+    if (is_guided_or_adsb_mode() && plane.guided_throttle_passthru) {
         // manual pass through of throttle while in GUIDED
         return false;
     }

ArduPlane/mode.h

@@ -103,6 +103,9 @@ class Mode
     virtual bool is_vtol_man_mode() const { return false; }
 
     // guided or adsb mode
+    virtual bool is_guided_or_adsb_mode() const { return is_guided_mode(); }
+
+    // guided mode
     virtual bool is_guided_mode() const { return false; }
 
     // true if mode can have terrain following disabled by switch
@@ -617,7 +620,8 @@ class ModeAvoidADSB : public Mode
 
     void navigate() override;
 
-    virtual bool is_guided_mode() const override { return true; }
+    // guided or adsb mode
+    virtual bool is_guided_or_adsb_mode() const override { return true; }
 
     bool does_auto_throttle() const override { return true; }
 

ArduPlane/mode_auto.cpp

@@ -6,7 +6,7 @@ bool ModeAuto::_enter()
 #if HAL_QUADPLANE_ENABLED
     // check if we should refuse auto mode due to a missing takeoff in
     // guided_wait_takeoff state
-    if (plane.previous_mode == &plane.mode_guided &&
+    if (plane.previous_mode->is_guided_mode() &&
         quadplane.guided_wait_takeoff_on_mode_enter) {
         if (!plane.mission.starts_with_takeoff_cmd()) {
             gcs().send_text(MAV_SEVERITY_ERROR,"Takeoff waypoint required");

ArduPlane/mode_guided.cpp

@@ -45,7 +45,7 @@ void ModeGuided::update()
 #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 )
     // This function is used in Guided and AvoidADSB, check for guided
-    } else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
+    } else if (plane.control_mode->is_guided_mode() && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
         uint32_t tnow = AP_HAL::millis();
         float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f;
         plane.guided_state.target_heading_time_ms = tnow;

ArduPlane/mode_qloiter.cpp

@@ -151,7 +151,7 @@ void ModeQLoiter::run()
 
         pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);
         quadplane.check_land_complete();
-    } else if (plane.control_mode == &plane.mode_guided && quadplane.guided_takeoff) {
+    } else if (plane.control_mode->is_guided_mode() && quadplane.guided_takeoff) {
         quadplane.set_climb_rate_cms(0);
     } else {
         // update altitude target and call position controller

ArduPlane/navigation.cpp

@@ -188,7 +188,7 @@ void Plane::calc_airspeed_errors()
                                   get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
         }
 #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
-    } else if (control_mode == &mode_guided && guided_state.target_airspeed_cm >  0.0) { // if offboard guided speed change cmd not set, then this section is skipped
+    } else if (control_mode->is_guided_mode() && guided_state.target_airspeed_cm >  0.0) { // if offboard guided speed change cmd not set, then this section is skipped
         // offboard airspeed demanded
         uint32_t now = AP_HAL::millis();
         float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms);
@@ -229,7 +229,7 @@ void Plane::calc_airspeed_errors()
     } else if (flight_stage == AP_FixedWing::FlightStage::LAND) {
         // Landing airspeed target
         target_airspeed_cm = landing.get_target_airspeed_cm();
-    } else if (control_mode == &mode_guided && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
+    } else if (control_mode->is_guided_mode() && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
                        target_airspeed_cm = new_airspeed_cm;
     } else if (control_mode == &mode_auto) {
         target_airspeed_cm = mode_auto_target_airspeed_cm();
@@ -257,7 +257,7 @@ void Plane::calc_airspeed_errors()
 
     // when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
 #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
-    if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
+    if (control_mode->is_guided_mode() && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
         airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
     }
 #endif
@@ -319,7 +319,7 @@ void Plane::update_loiter_update_nav(uint16_t radius)
 #endif
 
     if ((loiter.start_time_ms == 0 &&
-         (control_mode == &mode_auto || control_mode == &mode_guided) &&
+         (control_mode == &mode_auto || control_mode->is_guided_mode()) &&
          auto_state.crosstrack &&
          current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) ||
         quadplane_qrtl_switch) {
@@ -359,7 +359,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->is_guided_mode()) {
+            if (control_mode->is_guided_or_adsb_mode()) {
                 // starting a loiter in GUIDED means we just reached the target point
                 gcs().send_mission_item_reached_message(0);
             }

ArduPlane/quadplane.cpp

@@ -1166,7 +1166,7 @@ bool QuadPlane::is_flying(void)
     if (!available()) {
         return false;
     }
-    if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
+    if (plane.control_mode->is_guided_mode() && guided_takeoff) {
         return true;
     }
     if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) {
@@ -1217,7 +1217,7 @@ bool QuadPlane::is_flying_vtol(void) const
         // in manual throttle modes with airmode on, don't consider aircraft landed
         return true;
     }
-    if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
+    if (plane.control_mode->is_guided_mode() && guided_takeoff) {
         return true;
     }
     if (plane.control_mode->is_vtol_man_mode()) {
@@ -1303,7 +1303,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const
 
     if ((plane.g.stick_mixing == StickMixing::NONE) &&
         (plane.control_mode == &plane.mode_qrtl ||
-         plane.control_mode->is_guided_mode() ||
+         plane.control_mode->is_guided_or_adsb_mode() ||
          in_vtol_auto())) {
         return 0;
     }
@@ -1385,7 +1385,7 @@ void QuadPlane::set_armed(bool armed)
     }
     motors->armed(armed);
 
-    if (plane.control_mode == &plane.mode_guided) {
+    if (plane.control_mode->is_guided_mode()) {
         guided_wait_takeoff = armed;
     }
 
@@ -2077,12 +2077,12 @@ bool QuadPlane::in_vtol_mode(void) const
     if (plane.control_mode->is_vtol_mode()) {
         return true;
     }
-    if (plane.control_mode->is_guided_mode()
+    if (plane.control_mode->is_guided_or_adsb_mode()
         && plane.auto_state.vtol_loiter &&
         poscontrol.get_state() > QPOS_APPROACH) {
         return true;
     }
-    if (plane.control_mode == &plane.mode_guided &&
+    if (plane.control_mode->is_guided_mode() &&
         guided_takeoff) {
         return true;
     }
@@ -2108,7 +2108,7 @@ bool QuadPlane::in_vtol_posvel_mode(void) const
 #if QAUTOTUNE_ENABLED
             plane.control_mode == &plane.mode_qautotune ||
 #endif
-            (plane.control_mode->is_guided_mode() &&
+            (plane.control_mode->is_guided_or_adsb_mode() &&
             plane.auto_state.vtol_loiter &&
              poscontrol.get_state() > QPOS_APPROACH) ||
             in_vtol_auto());
@@ -2783,7 +2783,7 @@ void QuadPlane::vtol_position_controller(void)
                 break;
             }
         }
-        if (plane.control_mode == &plane.mode_guided || vtol_loiter_auto) {
+        if (plane.control_mode->is_guided_mode() || vtol_loiter_auto) {
             plane.ahrs.get_location(plane.current_loc);
             int32_t target_altitude_cm;
             if (!plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN,target_altitude_cm)) {
@@ -3066,7 +3066,7 @@ void QuadPlane::takeoff_controller(void)
 
     uint32_t now = AP_HAL::millis();
     const auto spool_state = motors->get_desired_spool_state();
-    if (plane.control_mode == &plane.mode_guided && guided_takeoff
+    if (plane.control_mode->is_guided_mode() && guided_takeoff
         && tiltrotor.enabled() && !tiltrotor.fully_up() &&
         spool_state != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
         // waiting for motors to tilt up
@@ -3140,7 +3140,7 @@ void QuadPlane::takeoff_controller(void)
                                                                   get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
 
     float vel_z = wp_nav->get_default_speed_up();
-    if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
+    if (plane.control_mode->is_guided_mode() && guided_takeoff) {
         // for guided takeoff we aim for a specific height with zero
         // velocity at that height
         Location origin;
@@ -3856,7 +3856,7 @@ void QuadPlane::guided_start(void)
  */
 void QuadPlane::guided_update(void)
 {
-    if (plane.control_mode == &plane.mode_guided && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) {
+    if (plane.control_mode->is_guided_mode() && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) {
         throttle_wait = false;
         set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
         takeoff_controller();
@@ -3887,7 +3887,7 @@ bool QuadPlane::guided_mode_enabled(void)
         return false;
     }
     // only use quadplane guided when in AUTO or GUIDED mode
-    if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) {
+    if (!plane.control_mode->is_guided_mode() && plane.control_mode != &plane.mode_auto) {
         return false;
     }
     if (plane.control_mode == &plane.mode_auto &&
@@ -3909,7 +3909,7 @@ void QuadPlane::set_alt_target_current(void)
 // user initiated takeoff for guided mode
 bool QuadPlane::do_user_takeoff(float takeoff_altitude)
 {
-    if (plane.control_mode != &plane.mode_guided) {
+    if (!plane.control_mode->is_guided_mode()) {
         gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode");
         return false;
     }
@@ -4745,7 +4745,7 @@ bool QuadPlane::should_disable_TECS() const
     if (in_vtol_land_descent()) {
         return true;
     }
-    if (plane.control_mode == &plane.mode_guided &&
+    if (plane.control_mode->is_guided_mode() &&
         plane.auto_state.vtol_loiter) {
         return true;
     }