move ground_detection to an overridable function

paparazzi · Sep 25, 2023 · e54ab9b · e54ab9b
1 parent aa68157
commit e54ab9b
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 20 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
@@ -72,10 +72,24 @@ static uint32_t autopilot_in_flight_counter;
 #define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
 #endif
 
-/** Z-acceleration threshold to detect ground in m/s^2 */
-#ifndef THRESHOLD_GROUND_DETECT
-#define THRESHOLD_GROUND_DETECT 25.0
-#endif
+
+/** Default ground detection estimation based on thrust and speed */
+__attribute__((weak)) bool autopilot_ground_detection(void) {
+  if (autopilot_in_flight_counter > 0) {
+    /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
+    if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
+        (fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
+        (fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
+      autopilot_in_flight_counter--;
+      if (autopilot_in_flight_counter == 0) {
+        return true;
+      }
+    } else { /* thrust, speed or accel not above min threshold, reset counter */
+      autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
+    }
+  }
+  return false;
+}
 
 
 #if USE_MOTOR_MIXING
@@ -234,9 +248,7 @@ void autopilot_event(void)
       || autopilot.mode == AP_MODE_FAILSAFE
 #endif
      ) {
-    struct NedCoor_f *accel = stateGetAccelNed_f();
-    if (accel->z < -THRESHOLD_GROUND_DETECT ||
-        accel->z > THRESHOLD_GROUND_DETECT) {
+    if (autopilot_ground_detection()) {
       autopilot.ground_detected = true;
       autopilot.detect_ground_once = false;
     }
@@ -255,19 +267,9 @@ void autopilot_reset_in_flight_counter(void)
 void autopilot_check_in_flight(bool motors_on)
 {
   if (autopilot.in_flight) {
-    if (autopilot_in_flight_counter > 0) {
-      /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
-      if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
-          (fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
-          (fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL) &&
-          !motors_on) {
-        autopilot_in_flight_counter--;
-        if (autopilot_in_flight_counter == 0) {
-          autopilot.in_flight = false;
-        }
-      } else { /* thrust, speed or accel not above min threshold, reset counter */
-        autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
-      }
+    if (autopilot_ground_detection()) {
+      autopilot.in_flight = false;
+      autopilot_in_flight_counter == 0;
     }
   } else { /* currently not in flight */
     if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.h b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.h
@@ -33,6 +33,8 @@
 
 extern uint8_t autopilot_mode_auto2;
 
+__attribute__((weak)) extern bool autopilot_ground_detection(void);
+
 extern void autopilot_firmware_init(void);
 
 #endif /* AUTOPILOT_FIRMWARE_H */