From d839d5aa59c1f23664ba57eec137a4d0e4cdf840 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 9 May 2014 23:44:07 -0700
Subject: [PATCH] Copter: reject bad RC roll-pitch inputs during stabilize
 flight

---
 ArduCopter/ArduCopter.pde | 26 ++++++++++++++++++--------
 1 file changed, 18 insertions(+), 8 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index f105e03db..91559e372 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1638,8 +1638,13 @@ void update_roll_pitch_mode(void)
         // apply SIMPLE mode transform
         update_simple_mode();
 
-        // convert pilot input to lean angles
-        get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
+        if(failsafe.radio) {
+            // don't allow copter to fly away during failsafe
+            get_pilot_desired_lean_angles(0.0f, 0.0f, control_roll, control_pitch);
+        } else {
+            // convert pilot input to lean angles
+            get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
+        }
 
         // pass desired roll, pitch to stabilize attitude controllers
         get_stabilize_roll(control_roll);
@@ -1659,8 +1664,13 @@ void update_roll_pitch_mode(void)
         // apply SIMPLE mode transform
         update_simple_mode();
 
-        // convert pilot input to lean angles
-        get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
+        if(failsafe.radio) {
+            // don't allow copter to fly away during failsafe
+            get_pilot_desired_lean_angles(0.0f, 0.0f, control_roll, control_pitch);
+        } else {
+            // convert pilot input to lean angles
+            get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
+        }
 
         // mix in user control with optical flow
         control_roll = get_of_roll(control_roll);
@@ -1679,6 +1689,10 @@ void update_roll_pitch_mode(void)
         // apply SIMPLE mode transform
         update_simple_mode();
 
+        // copy user input for reporting purposes
+        control_roll            = g.rc_1.control_in;
+        control_pitch           = g.rc_2.control_in;
+
         if(failsafe.radio) {
             // don't allow loiter target to move during failsafe
             wp_nav.move_loiter_target(0.0f, 0.0f, 0.01f);
@@ -1687,10 +1701,6 @@ void update_roll_pitch_mode(void)
             wp_nav.move_loiter_target(g.rc_1.control_in, g.rc_2.control_in, 0.01f);
         }
 
-        // copy latest output from nav controller to stabilize controller
-        control_roll = wp_nav.get_desired_roll();
-        control_pitch = wp_nav.get_desired_pitch();
-
         get_stabilize_roll(control_roll);
         get_stabilize_pitch(control_pitch);
         break;
-- 
GitLab