Copter: set ground effect state during takeoff and landing

ArduCopter/ArduCopter.pde

@@ -380,6 +380,7 @@ static union {
         uint8_t system_time_set     : 1; // 20  // true if the system time has been set from the GPS
         uint8_t gps_base_pos_set    : 1; // 21  // true when the gps base position has been set (used for RTK gps only)
         enum HomeState home_state   : 2; // 22,23 - home status (unset, set, locked)
+        uint8_t in_ground_effect    : 1; // 24
     };
     uint32_t value;
 } ap;
@@ -1008,6 +1009,8 @@ static void throttle_loop()
     // check if we've landed
     update_land_detector();
 
+    update_ground_effect_detector();
+
     // check auto_armed status
     update_auto_armed();
 

ArduCopter/land_detector.pde

@@ -41,3 +41,67 @@ static void update_land_detector()
     // set land maybe flag
     set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
 }
+
+static void update_ground_effect_detector(void)
+{
+    if(!motors.armed()) {
+        // disarmed - disable ground effect and return
+        ap.in_ground_effect = false;
+        ahrs.setGndEffectMode(ap.in_ground_effect);
+        pos_control.setGndEffectMode(ap.in_ground_effect);
+        return;
+    }
+
+    // variable initialization
+    uint32_t tnow_ms = hal.scheduler->millis();
+    static uint32_t takeoff_time_ms;
+    static float takeoff_alt_cm;
+    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
+    bool throttle_up = motors.armed() && g.rc_3.servo_out >= g.throttle_min;
+    bool takeoffExpected;
+    if (!throttle_up || ap.land_complete) {
+        takeoff_time_ms = tnow_ms;
+        takeoff_alt_cm = current_loc.alt;
+    }
+
+    if (throttle_up && tnow_ms-takeoff_time_ms < 10000 && current_loc.alt-takeoff_alt_cm < 150.0f) {
+        takeoffExpected = true;
+    } else {
+        takeoffExpected = false;
+    }
+
+    // landing logic
+    bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 100.0f;
+    bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 50.0f;
+    bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy());
+
+    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(climb_rate) <= LAND_DETECTOR_CLIMBRATE_MAX*2.0f;
+    bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));
+
+    bool height_low = current_loc.alt < 1000;
+
+    bool touchdownExpected = slow_horizontal && slow_descent && height_low;
+
+    // Prepare the EKF and height controller for ground effect if either takeoff or touchdown is expected.
+    ap.in_ground_effect = takeoffExpected || touchdownExpected;
+    ahrs.setGndEffectMode(ap.in_ground_effect);
+    pos_control.setGndEffectMode(ap.in_ground_effect);
+}