Plane: added AFS support for quadplanes

ArduPlane/GCS_Mavlink.cpp

@@ -1557,6 +1557,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             result = MAV_RESULT_ACCEPTED;
             
             if (!plane.geofence_present()) {
+                plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Fence not configured");
                 result = MAV_RESULT_FAILED;
             } else {
                 switch((uint16_t)packet.param1) {

ArduPlane/afs_plane.cpp

@@ -37,6 +37,12 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
     RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
     RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
     RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+
+    ch_roll->output();
+    ch_pitch->output();
+    ch_yaw->output();
+    ch_throttle->output();
+    RC_Channel_aux::output_ch_all();
 }
 
 void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
@@ -61,6 +67,12 @@ void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
     RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
     RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
     RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+
+    if (plane.quadplane.available()) {
+        // setup AP_Motors outputs for failsafe
+        uint16_t mask = plane.quadplane.motors->get_motor_mask();
+        hal.rcout->set_failsafe_pwm(mask, plane.quadplane.thr_min_pwm);
+    }
 }
 
 /*

ArduPlane/quadplane.cpp

@@ -980,6 +980,12 @@ void QuadPlane::update(void)
         return;
     }
 
+    if (plane.afs.should_crash_vehicle()) {
+        motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+        motors->output();
+        return;
+    }
+    
     if (motor_test.running) {
         motor_test_output();
         return;
@@ -1072,6 +1078,11 @@ void QuadPlane::check_throttle_suppression(void)
  */
 void QuadPlane::motors_output(void)
 {
+    if (plane.afs.should_crash_vehicle()) {
+        motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+        motors->output();
+        return;
+    }
     if (esc_calibration && AP_Notify::flags.esc_calibration && plane.control_mode == QSTABILIZE) {
         // output is direct from run_esc_calibration()
         return;

ArduPlane/quadplane.h

@@ -36,6 +36,7 @@ public:
     friend class Plane;
     friend class AP_Tuning_Plane;
     friend class GCS_MAVLINK_Plane;
+    friend class AP_AdvancedFailsafe_Plane;
     
     QuadPlane(AP_AHRS_NavEKF &_ahrs);