Copter: complete addition of SafeRTL flight mode

ArduCopter/GCS_Mavlink.cpp

@@ -52,6 +52,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
     case CIRCLE:
     case POSHOLD:
     case BRAKE:
+    case SAFE_RTL:
         base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
         // APM does in any mode, as that is defined as "system finds its own goal

ArduCopter/flight_mode.cpp

@@ -369,6 +369,7 @@ void Copter::notify_flight_mode(control_mode_t mode)
         case AVOID_ADSB:
         case GUIDED_NOGPS:
         case LAND:
+        case SAFE_RTL:
             // autopilot modes
             AP_Notify::flags.autopilot_mode = true;
             break;

ArduCopter/heli.cpp

@@ -108,6 +108,7 @@ void Copter::heli_update_landing_swash()
             break;
 
         case RTL:
+        case SAFE_RTL:
             if (rtl_state == RTL_Land) {
                 motors->set_collective_for_landing(true);
             }else{

ArduCopter/navigation.cpp

@@ -32,6 +32,7 @@ void Copter::calc_wp_distance()
 
     case AUTO:
     case RTL:
+    case SAFE_RTL:
         wp_distance = wp_nav->get_wp_distance_to_destination();
         break;
 
@@ -59,6 +60,7 @@ void Copter::calc_wp_bearing()
 
     case AUTO:
     case RTL:
+    case SAFE_RTL:
         wp_bearing = wp_nav->get_wp_bearing_to_destination();
         break;
 

ArduCopter/sensors.cpp

@@ -360,6 +360,7 @@ void Copter::update_sensor_status_flags(void)
     case POSHOLD:
     case BRAKE:
     case THROW:
+    case SAFE_RTL:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
         break;