Copter: barometer ground effect compensation

ArduCopter/APM_Config.h

@@ -39,6 +39,7 @@
 // features below are disabled by default on all boards
 //#define SPRAYER               ENABLED             // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
 //#define PRECISION_LANDING     ENABLED             // enable precision landing using companion computer or IRLock sensor
+//#define GNDEFFECT_COMPENSATION ENABLED            // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground)
 
 // other settings
 //#define THROTTLE_IN_DEADBAND   100                // redefine size of throttle deadband in pwm (0 ~ 1000)

ArduCopter/ArduCopter.cpp

@@ -319,6 +319,10 @@ void Copter::throttle_loop()
     // update trad heli swash plate movement
     heli_update_landing_swash();
 #endif
+
+#if GNDEFFECT_COMPENSATION == ENABLED
+    update_ground_effect_detector();
+#endif // GNDEFFECT_COMPENSATION == ENABLED
 }
 
 // update_mount - update camera mount position

ArduCopter/Copter.h

@@ -540,6 +540,16 @@ private:
     } heli_flags;
 #endif
 
+#if GNDEFFECT_COMPENSATION == ENABLED
+    // ground effect detector
+    struct {
+        bool takeoff_expected;
+        bool touchdown_expected;
+        uint32_t takeoff_time_ms;
+        float takeoff_alt_cm;
+    } gndeffect_state;
+#endif // GNDEFFECT_COMPENSATION == ENABLED
+
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
     static const struct LogStructure log_structure[];
@@ -842,6 +852,9 @@ private:
     void update_land_and_crash_detectors();
     void update_land_detector();
     void update_throttle_thr_mix();
+#if GNDEFFECT_COMPENSATION == ENABLED
+    void update_ground_effect_detector(void);
+#endif // GNDEFFECT_COMPENSATION == ENABLED
     void landinggear_update();
     void update_notify();
     void motor_test_output();

ArduCopter/baro_ground_effect.cpp

@@ -0,0 +1,72 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Copter.h"
+#if GNDEFFECT_COMPENSATION == ENABLED
+void Copter::update_ground_effect_detector(void)
+{
+    if(!motors.armed()) {
+        // disarmed - disable ground effect and return
+        gndeffect_state.takeoff_expected = false;
+        gndeffect_state.touchdown_expected = false;
+        ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
+        ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
+        return;
+    }
+
+    // variable initialization
+    uint32_t tnow_ms = millis();
+    float xy_des_speed_cms = 0.0f;
+    float xy_speed_cms = 0.0f;
+    float des_climb_rate_cms = pos_control.get_desired_velocity().z;
+
+    if(pos_control.is_active_xy()) {
+        Vector3f vel_target = pos_control.get_vel_target();
+        vel_target.z = 0.0f;
+        xy_des_speed_cms = vel_target.length();
+    }
+
+    if(position_ok() || optflow_position_ok()) {
+        Vector3f vel = inertial_nav.get_velocity();
+        vel.z = 0.0f;
+        xy_speed_cms = vel.length();
+    }
+
+    // takeoff logic
+
+    // if we are armed and haven't yet taken off
+    if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
+        gndeffect_state.takeoff_expected = true;
+    }
+
+    // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
+    bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.control_in > 0;
+    if (!throttle_up && ap.land_complete) {
+        gndeffect_state.takeoff_time_ms = tnow_ms;
+        gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
+    }
+
+    // if we are in takeoff_expected and we meet the conditions for having taken off
+    // end the takeoff_expected state
+    if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
+        gndeffect_state.takeoff_expected = false;
+    }
+
+    // landing logic
+    Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f);
+    bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
+    bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
+    bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f;
+    bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);
+
+    bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f;
+    bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
+    bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f;
+    bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));
+
+    gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;
+
+    // Prepare the EKF for ground effect if either takeoff or touchdown is expected.
+    ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
+    ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
+}
+#endif // GNDEFFECT_COMPENSATION == ENABLED

ArduCopter/config.h

@@ -213,6 +213,10 @@
  # define FS_GCS_TIMEOUT_MS             5000    // gcs failsafe triggers after 5 seconds with no GCS heartbeat
 #endif
 
+#ifndef GNDEFFECT_COMPENSATION
+ # define GNDEFFECT_COMPENSATION          DISABLED
+#endif
+
 // possible values for FS_GCS parameter
 #define FS_GCS_DISABLED                     0
 #define FS_GCS_ENABLED_ALWAYS_RTL           1