Copter: exit land into LOITER or ALT_HOLD when the throttle is raised

3drobotics · Jun 11, 2015 · 4d60b0c · 4d60b0c
1 parent e2b0e54
commit 4d60b0c
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 1 deletion.
diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
@@ -559,6 +559,7 @@ static float target_sonar_alt;      // desired altitude in cm above the ground
 static int32_t baro_alt;            // barometer altitude in cm above home
 static float baro_climbrate;        // barometer climbrate in cm/s
 static LowPassFilterVector3f land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF); // accelerations for land detector test
+static LowPassFilterFloat rc_throttle_control_in_filter(1.0f);
 
 ////////////////////////////////////////////////////////////////////////////////
 // 3D Location vectors

diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde
@@ -76,6 +76,15 @@ static void land_gps_run()
 
     // process pilot inputs
     if (!failsafe.radio) {
+        if(rc_throttle_control_in_filter.get() > 700){
+            // exit land
+            if (position_ok()) {
+                set_mode(LOITER);
+            } else {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
@@ -142,6 +151,15 @@ static void land_nogps_run()
 
     // process pilot inputs
     if (!failsafe.radio) {
+        if(rc_throttle_control_in_filter.get() > 700){
+            // exit land
+            if (position_ok()) {
+                set_mode(LOITER);
+            } else {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();

diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde
@@ -272,6 +272,15 @@ static void rtl_descent_run()
 
     // process pilot's input
     if (!failsafe.radio) {
+        if(rc_throttle_control_in_filter.get() > 700){
+            // exit land
+            if (position_ok()) {
+                set_mode(LOITER);
+            } else {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
@@ -354,6 +363,15 @@ static void rtl_land_run()
 
     // process pilot's input
     if (!failsafe.radio) {
+        if(rc_throttle_control_in_filter.get() > 700){
+            // exit land
+            if (position_ok()) {
+                set_mode(LOITER);
+            } else {
+                set_mode(ALT_HOLD);
+            }
+        }
+
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();

diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde
@@ -88,7 +88,6 @@ static void read_radio()
     uint32_t tnow_ms = millis();
 
     if (hal.rcin->new_input()) {
-        last_update_ms = tnow_ms;
         ap.new_radio_frame = true;
         uint16_t periods[8];
         hal.rcin->read(periods,8);
@@ -118,6 +117,10 @@ static void read_radio()
 
         // update output on any aux channels, for manual passthru
         RC_Channel_aux::output_ch_all();
+
+        float dt = (tnow_ms - last_update_ms)*1.0e-3f;
+        rc_throttle_control_in_filter.apply(g.rc_3.control_in,dt);
+        last_update_ms = tnow_ms;
     }else{
         uint32_t elapsed = tnow_ms - last_update_ms;
         // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE