From b29b609bccf1441b76c558d44bd42189a69c2cdc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 22 Jul 2016 18:36:28 +1000
Subject: [PATCH] Plane: added AFS support for quadplanes

---
 ArduPlane/GCS_Mavlink.cpp |  1 +
 ArduPlane/afs_plane.cpp   | 12 ++++++++++++
 ArduPlane/quadplane.cpp   | 11 +++++++++++
 ArduPlane/quadplane.h     |  1 +
 4 files changed, 25 insertions(+)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 223a2c749e..cb1d364097 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/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) {
diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp
index aee94667db..94ebf7558d 100644
--- a/ArduPlane/afs_plane.cpp
+++ b/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);
+    }
 }
 
 /*
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 374023e0d6..1762d8ef24 100644
--- a/ArduPlane/quadplane.cpp
+++ b/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;
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index d97549f180..265c918280 100644
--- a/ArduPlane/quadplane.h
+++ b/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);