ArduPlane: add terrain follow disable switch for CRUISE/FBWB

mohanbabi · Jul 8, 2020 · 4115603 · 4115603
1 parent b84b6b5
commit 4115603
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 3 deletions.
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -785,6 +785,10 @@ class Plane : public AP_Vehicle {
     // soaring mode-change timer
     uint32_t soaring_mode_timer;
 
+    // terrain disable for non AUTO modes, set with an RC Option switch
+    bool non_auto_terrain_disable;
+    bool terrain_disabled();
+
     // Attitude.cpp
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);

diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp
@@ -110,6 +110,10 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
         // want to startup with reverse thrust
         break;
 
+    case AUX_FUNC::TER_DISABLE:
+        do_aux_function(ch_option, ch_flag);
+        break;
+
     default:
         // handle in parent class
         RC_Channel::init_aux_function(ch_option, ch_flag);
@@ -168,6 +172,27 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
     case AUX_FUNC::FWD_THR:
         break; // VTOL forward throttle input label, nothing to do
 
+    case AUX_FUNC::TER_DISABLE:
+            switch (ch_flag) {
+            case AuxSwitchPos::HIGH:
+                plane.non_auto_terrain_disable = true;
+                if (plane.control_mode->allows_terrain_disable()) {
+                    plane.set_target_altitude_current();
+                }
+                break;
+            case AuxSwitchPos::MIDDLE:
+                break;
+            case AuxSwitchPos::LOW:
+                plane.non_auto_terrain_disable = false;
+                if (plane.control_mode->allows_terrain_disable()) {
+                    plane.set_target_altitude_current();
+                }
+                break;
+            }
+            gcs().send_text(MAV_SEVERITY_INFO, "NON AUTO TERRN: %s", plane.non_auto_terrain_disable?"OFF":"ON");
+        break;
+
+
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;

diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
@@ -200,7 +200,7 @@ void Plane::set_target_altitude_current(void)
 #if AP_TERRAIN_AVAILABLE
     // also record the terrain altitude if possible
     float terrain_altitude;
-    if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true)) {
+    if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true) && !terrain_disabled()) {
         target_altitude.terrain_following = true;
         target_altitude.terrain_alt_cm = terrain_altitude*100;
     } else {
@@ -289,12 +289,11 @@ void Plane::change_target_altitude(int32_t change_cm)
 {
     target_altitude.amsl_cm += change_cm;
 #if AP_TERRAIN_AVAILABLE
-    if (target_altitude.terrain_following) {
+    if (target_altitude.terrain_following && !terrain_disabled()) {
         target_altitude.terrain_alt_cm += change_cm;
     }
 #endif
 }
-
 /*
   change target altitude by a proportion of the target altitude offset
   (difference in height to next WP from previous WP). proportion
@@ -719,3 +718,13 @@ void Plane::rangefinder_height_update(void)
 
     }
 }
+
+/*
+  determine if Non Auto Terrain Disable is active and allowed in present control mode
+ */
+bool Plane::terrain_disabled()
+{
+    return control_mode->allows_terrain_disable() && non_auto_terrain_disable;
+}
+
+
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -69,6 +69,9 @@ class Mode
     // true for all q modes
     virtual bool is_vtol_mode() const { return false; }
 
+    // true if mode can have terrain following disabled by switch
+    virtual bool allows_terrain_disable() const { return false; }
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -288,6 +291,8 @@ class ModeFBWB : public Mode
     const char *name() const override { return "FLY_BY_WIRE_B"; }
     const char *name4() const override { return "FBWB"; }
 
+    bool allows_terrain_disable() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
@@ -304,6 +309,8 @@ class ModeCruise : public Mode
     const char *name() const override { return "CRUISE"; }
     const char *name4() const override { return "CRUS"; }
 
+    bool allows_terrain_disable() const override { return true; }
+
     // methods that affect movement of the vehicle in this mode
     void update() override;