From 8ac433f9913b1685a6e16b4b18581362c0773b00 Mon Sep 17 00:00:00 2001
From: Michael Day <mday299@yahoo.com>
Date: Mon, 9 May 2016 10:34:11 -0400
Subject: [PATCH] Plane: Can now command roll from a companion computer.

---
 ArduPlane/Attitude.cpp     | 10 +++++++++-
 ArduPlane/GCS_Mavlink.cpp  | 35 +++++++++++++++++++++++++++++++++++
 ArduPlane/Plane.h          | 12 +++++++++++-
 ArduPlane/capabilities.cpp |  1 +
 ArduPlane/system.cpp       |  3 +++
 5 files changed, 59 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index cf199ec9dc..52a8da544b 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -529,7 +529,15 @@ void Plane::calc_nav_pitch()
  */
 void Plane::calc_nav_roll()
 {
-    nav_roll_cd = constrain_int32(nav_controller->nav_roll_cd(), -roll_limit_cd, roll_limit_cd);
+    int32_t commanded_roll = nav_controller->nav_roll_cd();
+
+    //Received an external msg that guides roll in the last 3 seconds?
+    if(plane.guided_state.guiding_roll &&
+       AP_HAL::millis() - plane.guided_state.last_guided_ms < 3000) {
+        commanded_roll = plane.guided_state.guided_roll_cd;        
+    }
+
+    nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);
     update_load_factor();
 }
 
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index ef851980e0..8a43f2fe7a 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -2103,6 +2103,41 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         plane.DataFlash.remote_log_block_status_msg(chan, msg);
         break;
 
+    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+    {
+        //Only allow companion computer (or other external controller) to 
+        //control attitude in GUIDED mode.  We DON'T want external control
+        //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 != GUIDED) { //don't screw up failsafes
+            break; 
+        }
+
+        mavlink_set_attitude_target_t att_target;
+        mavlink_msg_set_attitude_target_decode(msg, &att_target);
+        //Unexepectedly, the mask is expecting "ones" for dimensions that should
+        //be IGNORNED rather than INCLUDED.  See mavlink documentation of the
+        //SET_ATTITUDE_TARGET message, type_mask field. 
+        const uint16_t roll_mask = 0b11111110; // (roll mask at bit 1)
+
+        if (att_target.type_mask & roll_mask) {           
+            // Extract the Euler roll angle from the Quaternion,
+            Quaternion q(att_target.q[0], att_target.q[1],
+                    att_target.q[2], att_target.q[3]);
+            float roll_rad = q.get_euler_roll();
+
+            plane.guided_state.guided_roll_cd = degrees(roll_rad) * 100.f;
+
+            // Set the flag for external roll to the nav control
+            plane.guided_state.guiding_roll = true;
+            // Update timer:
+            plane.guided_state.last_guided_ms = AP_HAL::millis(); 
+
+        }
+        break;
+    }
+
     case MAVLINK_MSG_ID_SET_HOME_POSITION:
     {
         mavlink_set_home_position_t packet;
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 4127ebfa72..5c0bc68e74 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -526,6 +526,16 @@ private:
         bool vtol_loiter:1;
     } auto_state;
 
+    struct {
+        // guiding roll from an external controller? (e.g., companion computer)
+        bool guiding_roll:1;
+        // roll commanded from external controller in centidegrees
+        int32_t guided_roll_cd;
+        
+        // last time we heard from the external controller
+        uint32_t last_guided_ms;
+    } guided_state;
+
     struct {
         // on hard landings, only check once after directly a landing so you
         // don't trigger a crash when picking up the aircraft
@@ -553,7 +563,7 @@ private:
 
     // this controls throttle suppression in auto modes
     bool throttle_suppressed;
-
+	
     // reduce throttle to eliminate battery over-current
     int8_t  throttle_watt_limit_max;
     int8_t  throttle_watt_limit_min; // for reverse thrust
diff --git a/ArduPlane/capabilities.cpp b/ArduPlane/capabilities.cpp
index 65f6ec1a5e..c389a45d9a 100644
--- a/ArduPlane/capabilities.cpp
+++ b/ArduPlane/capabilities.cpp
@@ -9,5 +9,6 @@ void Plane::init_capabilities(void)
     hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_COMMAND_INT);
     hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT);
     hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
+    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET);
 
 }
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index e810ee4c05..061db92eff 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -336,6 +336,9 @@ void Plane::set_mode(enum FlightMode mode)
     crash_state.is_crashed = false;
     crash_state.impact_detected = false;
 
+    // reset external attitude guidance
+    guided_state.guiding_roll = false;
+
     // always reset this because we don't know who called set_mode. In evasion
     // behavior you should set this flag after set_mode so you know the evasion
     // logic is controlling the mode. This allows manual override of the mode