Plane: fixed VTOL motor termination in AFS

ArduPlane/afs_plane.cpp

@@ -43,6 +43,11 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
     ch_yaw->output();
     ch_throttle->output();
     RC_Channel_aux::output_ch_all();
+
+    plane.quadplane.afs_terminate();
+    
+    // also disarm to ensure that ignition is cut
+    plane.disarm_motors();
 }
 
 void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)

ArduPlane/quadplane.cpp

@@ -2025,3 +2025,11 @@ void QuadPlane::guided_update(void)
     // run VTOL position controller
     vtol_position_controller();
 }
+
+void QuadPlane::afs_terminate(void)
+{
+    if (available()) {
+        motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+        motors->output();
+    }
+}

ArduPlane/quadplane.h

@@ -345,6 +345,8 @@ private:
     void tiltrotor_slew(float tilt);
     void tiltrotor_update(void);
     void tilt_compensate(float *thrust, uint8_t num_motors);
+
+    void afs_terminate(void);
     
 public:
     void motor_test_output();