From 85afd9e2455bb8dd8f1288b9d84f8f5b5ec85bb4 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Thu, 30 Jun 2016 07:48:37 -0700
Subject: [PATCH] Plane: remove  guided roll flag, handle it with timer=0

---
 ArduPlane/Attitude.cpp    |  8 ++++----
 ArduPlane/GCS_Mavlink.cpp | 25 +++++++++++--------------
 ArduPlane/Plane.h         |  6 ++----
 ArduPlane/system.cpp      |  2 +-
 4 files changed, 18 insertions(+), 23 deletions(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 52a8da544b..248b17a100 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -531,10 +531,10 @@ void Plane::calc_nav_roll()
 {
     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;        
+    // Received an external msg that guides roll in the last 3 seconds?
+    if (plane.guided_state.last_roll_ms > 0 &&
+       AP_HAL::millis() - plane.guided_state.last_roll_ms < 3000) {
+        commanded_roll = plane.guided_state.roll_cd;
     }
 
     nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 8a43f2fe7a..8aad24c95d 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -2105,20 +2105,20 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
 
     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).
+        // 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. 
+        // 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) {           
@@ -2127,13 +2127,10 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
                     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(); 
+            plane.guided_state.roll_cd = degrees(roll_rad) * 100.0f;
 
+            // Update timer for external roll to the nav control
+            plane.guided_state.last_roll_ms = AP_HAL::millis();
         }
         break;
     }
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 5c0bc68e74..145a032955 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -527,13 +527,11 @@ private:
     } 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;
+        int32_t roll_cd;
         
         // last time we heard from the external controller
-        uint32_t last_guided_ms;
+        uint32_t last_roll_ms;
     } guided_state;
 
     struct {
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 061db92eff..203ca13146 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -337,7 +337,7 @@ void Plane::set_mode(enum FlightMode mode)
     crash_state.impact_detected = false;
 
     // reset external attitude guidance
-    guided_state.guiding_roll = false;
+    guided_state.last_roll_ms = 0;
 
     // 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