diff --git a/conf/airframes/tudelft/rotwing5.xml b/conf/airframes/tudelft/rotwing5.xml
index 73cd6908aa..e3fbf3fc4d 100644
--- a/conf/airframes/tudelft/rotwing5.xml
+++ b/conf/airframes/tudelft/rotwing5.xml
@@ -141,7 +141,8 @@
-
+
+
@@ -323,7 +324,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing6.xml b/conf/airframes/tudelft/rotwing6.xml
index 1a84394424..3c22a3e626 100644
--- a/conf/airframes/tudelft/rotwing6.xml
+++ b/conf/airframes/tudelft/rotwing6.xml
@@ -134,7 +134,7 @@
-
+
@@ -315,7 +315,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3b.xml b/conf/airframes/tudelft/rotwing_v3b.xml
index d8260d95b8..f1a8fce855 100644
--- a/conf/airframes/tudelft/rotwing_v3b.xml
+++ b/conf/airframes/tudelft/rotwing_v3b.xml
@@ -135,7 +135,7 @@
-
+
@@ -451,7 +451,7 @@
-
+
@@ -464,7 +464,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
index ace6216d23..d85317f991 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
@@ -135,7 +135,7 @@
-
+
@@ -251,7 +251,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
index 5f1e17478f..dcccf58df7 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
@@ -136,7 +136,7 @@
-
+
@@ -253,7 +253,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
index 8dd9265ca1..1753f9b3ab 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
@@ -128,7 +128,7 @@
-
+
@@ -244,7 +244,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
index 06364818d7..038927d614 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
@@ -96,7 +96,7 @@
-
+
@@ -213,7 +213,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3d.xml b/conf/airframes/tudelft/rotwing_v3d.xml
index 0e00331ba5..d14796ae4e 100644
--- a/conf/airframes/tudelft/rotwing_v3d.xml
+++ b/conf/airframes/tudelft/rotwing_v3d.xml
@@ -122,7 +122,7 @@
-
+
@@ -456,7 +456,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3e.xml b/conf/airframes/tudelft/rotwing_v3e.xml
index de4343fd96..9c31d6a42d 100644
--- a/conf/airframes/tudelft/rotwing_v3e.xml
+++ b/conf/airframes/tudelft/rotwing_v3e.xml
@@ -122,7 +122,7 @@
-
+
@@ -456,7 +456,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3f.xml b/conf/airframes/tudelft/rotwing_v3f.xml
index bba5d08540..31b8bfd117 100644
--- a/conf/airframes/tudelft/rotwing_v3f.xml
+++ b/conf/airframes/tudelft/rotwing_v3f.xml
@@ -122,7 +122,7 @@
-
+
@@ -456,7 +456,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3g.xml b/conf/airframes/tudelft/rotwing_v3g.xml
index c33876acee..ac76f9f699 100644
--- a/conf/airframes/tudelft/rotwing_v3g.xml
+++ b/conf/airframes/tudelft/rotwing_v3g.xml
@@ -127,7 +127,7 @@
-
+
@@ -460,7 +460,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_v3h.xml b/conf/airframes/tudelft/rotwing_v3h.xml
index 7a4a20e211..47e9eb564d 100644
--- a/conf/airframes/tudelft/rotwing_v3h.xml
+++ b/conf/airframes/tudelft/rotwing_v3h.xml
@@ -127,7 +127,7 @@
-
+
@@ -460,7 +460,7 @@
-
+
diff --git a/conf/modules/ground_detect.xml b/conf/modules/ground_detect.xml
new file mode 100644
index 0000000000..57d63793d4
--- /dev/null
+++ b/conf/modules/ground_detect.xml
@@ -0,0 +1,31 @@
+
+
+
+ Ground detection module to detect ground
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ground_detect_sensor.xml b/conf/modules/ground_detect_sensor.xml
deleted file mode 100644
index 35e28fdadd..0000000000
--- a/conf/modules/ground_detect_sensor.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
- Ground detection module to detect ground
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/sw/airborne/modules/nav/ground_detect.c b/sw/airborne/modules/nav/ground_detect.c
new file mode 100644
index 0000000000..337cc1b142
--- /dev/null
+++ b/sw/airborne/modules/nav/ground_detect.c
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2024 Ewoud Smeur
+ * Copyright (C) 2023 Dennis van Wijngaarden
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/** @file "modules/nav/ground_detect.c"
+ * Ground detection module
+ */
+
+#include "ground_detect.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
+#include "filters/low_pass_filter.h"
+#include "firmwares/rotorcraft/autopilot_firmware.h"
+
+#include "state.h"
+
+#if USE_GROUND_DETECT_INDI_THRUST
+#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
+#endif
+
+#if USE_GROUND_DETECT_AGL_DIST
+#include "modules/sonar/agl_dist.h"
+#ifndef GROUND_DETECT_AGL_MIN_VALUE
+#define GROUND_DETECT_AGL_MIN_VALUE 0.1
+#endif
+#endif
+
+#include "pprzlink/messages.h"
+#include "modules/datalink/downlink.h"
+
+#define GROUND_DETECT_COUNTER_TRIGGER 5
+
+// Cutoff frequency of accelerometer filter in Hz
+#define GROUND_DETECT_FILT_FREQ 5.0
+
+Butterworth2LowPass accel_filter;
+
+bool disarm_on_not_in_flight = false;
+
+int32_t counter = 0;
+bool ground_detected = false;
+
+#define DEBUG_GROUND_DETECT TRUE
+
+void ground_detect_init()
+{
+ float tau = 1.0 / (2.0 * M_PI * GROUND_DETECT_FILT_FREQ);
+ float sample_time = 1.0 / PERIODIC_FREQUENCY;
+ init_butterworth_2_low_pass(&accel_filter, tau, sample_time, 0.0);
+}
+
+bool ground_detect(void)
+{
+ return ground_detected;
+}
+
+// TODO: use standard pprz ground detection interface
+// bool autopilot_ground_detection(void) {
+// RunOnceEvery(10, printf("Running!\n"););
+// return ground_detected;
+// }
+
+void ground_detect_periodic()
+{
+
+ // Evaluate thrust given (less than hover thrust)
+ // Use the control effectiveness in thrust in order to estimate the thrust delivered (only works for multicopters)
+ float specific_thrust = 0.0; // m/s2
+ uint8_t i;
+ for (i = 0; i < INDI_NUM_ACT; i++) {
+ specific_thrust += actuator_state_filt_vect[i] * g1g2[3][i] * -((int32_t) act_is_servo[i] - 1);
+ }
+
+ // vertical component
+ float spec_thrust_down;
+ struct FloatRMat *ned_to_body_rmat = stateGetNedToBodyRMat_f();
+ spec_thrust_down = ned_to_body_rmat->m[8] * specific_thrust;
+
+ // Evaluate vertical speed (close to zero, not at terminal velocity)
+ float vspeed_ned = stateGetSpeedNed_f()->z;
+
+ // Detect free fall (to be done, rearm?)
+
+ // Detect noise level (to be done)
+
+ // Detect ground based on AND of all triggers
+ if ((fabsf(vspeed_ned) < 5.0)
+ && (spec_thrust_down > -5.0)
+ && (fabsf(accel_filter.o[0]) < 2.0)
+#if USE_GROUND_DETECT_AGL_DIST
+ && (agl_dist_valid && (agl_dist_value_filtered < GROUND_DETECT_AGL_MIN_VALUE))
+#endif
+ ) {
+
+ counter += 1;
+ if (counter > GROUND_DETECT_COUNTER_TRIGGER) {
+ ground_detected = true;
+
+ if (disarm_on_not_in_flight) {
+ autopilot_set_motors_on(false);
+ disarm_on_not_in_flight = false;
+ }
+ }
+ } else {
+ ground_detected = false;
+ counter = 0;
+ }
+
+#ifdef DEBUG_GROUND_DETECT
+ float payload[7];
+ payload[0] = vspeed_ned;
+ payload[1] = spec_thrust_down;
+ payload[2] = accel_filter.o[0];
+ payload[3] = stateGetAccelNed_f()->z;
+#if USE_GROUND_DETECT_AGL_DIST
+ payload[4] = agl_dist_valid;
+ payload[5] = agl_dist_value_filtered;
+#else
+ payload[4] = 0;
+ payload[5] = 0;
+#endif
+ payload[6] = 1.f * ground_detected;
+
+ RunOnceEvery(10, {DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 7, payload);});
+#endif
+}
+
+/**
+ * Filter the vertical acceleration with a low cutoff frequency.
+ *
+ */
+void ground_detect_filter_accel(void)
+{
+ struct NedCoor_f *accel = stateGetAccelNed_f();
+ update_butterworth_2_low_pass(&accel_filter, accel->z);
+}
diff --git a/sw/airborne/modules/nav/ground_detect_sensor.h b/sw/airborne/modules/nav/ground_detect.h
similarity index 76%
rename from sw/airborne/modules/nav/ground_detect_sensor.h
rename to sw/airborne/modules/nav/ground_detect.h
index 799617dfa7..9dcea4f985 100644
--- a/sw/airborne/modules/nav/ground_detect_sensor.h
+++ b/sw/airborne/modules/nav/ground_detect.h
@@ -18,19 +18,23 @@
* .
*/
-/** @file "modules/nav/ground_detect_sensor.h"
+/** @file "modules/nav/ground_detect.h"
* @author Dennis van Wijngaarden
* Ground detection module
*/
-#ifndef GROUND_DETECT_SENSOR_H
-#define GROUND_DETECT_SENSOR_H
+#ifndef GROUND_DETECT_H
+#define GROUND_DETECT_H
#include "std.h"
-extern void ground_detect_sensor_init(void);
-extern void ground_detect_sensor_periodic(void);
+extern void ground_detect_init(void);
+extern void ground_detect_periodic(void);
extern bool ground_detect(void);
-#endif // GROUND_DETECT_SENSOR_H
+extern void ground_detect_filter_accel(void);
+
+extern bool disarm_on_not_in_flight;
+
+#endif // GROUND_DETECT_H
diff --git a/sw/airborne/modules/nav/ground_detect_sensor.c b/sw/airborne/modules/nav/ground_detect_sensor.c
deleted file mode 100644
index fe3d421708..0000000000
--- a/sw/airborne/modules/nav/ground_detect_sensor.c
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Copyright (C) 2023 Dennis van Wijngaarden
- *
- * This file is part of paparazzi
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/** @file "modules/nav/ground_detect_sensor.c"
- * @author Dennis van Wijngaarden
- * Ground detection module relying on lidar to detect ground
- */
-
-#include "modules/nav/ground_detect_sensor.h"
-#include "state.h"
-
-#if USE_GROUND_DETECT_INDI_THRUST
-#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
-#endif
-
-#if USE_GROUND_DETECT_AGL_DIST
-#include "modules/sonar/agl_dist.h"
-#define GROUND_DETECT_SENSOR_AGL_MIN_VALUE 0.1
-#endif
-
-#include "pprzlink/messages.h"
-#include "modules/datalink/downlink.h"
-
-bool ground_detected = false;
-
-
-#ifndef GROUND_DETECT_SENSOR_COUNTER_TRIGGER
-#define GROUND_DETECT_SENSOR_COUNTER_TRIGGER 10
-#endif
-
-#ifndef GROUND_DETECT_SENSOR_SPECIFIC_THRUST_THRESHOLD
-#define GROUND_DETECT_SENSOR_SPECIFIC_THRUST_THRESHOLD -5.0
-#endif
-
-
-void ground_detect_sensor_init(void)
-{
- ground_detected = false;
-}
-
-bool ground_detect(void) {
- return ground_detected;
-}
-
-void ground_detect_sensor_periodic(void)
-{
- bool ground_detect_method = false;
-#if USE_GROUND_DETECT_INDI_THRUST
- ground_detect_method = true;
- static int32_t counter_thrust = 0;
- // Evaluate thrust given (less than hover thrust)
- // Use the control effectiveness in thrust in order to estimate the thrust delivered (only works for multicopters)
- float specific_thrust = 0.0; // m/s2
- uint8_t i;
- for (i = 0; i < INDI_NUM_ACT; i++) {
- specific_thrust += actuator_state_filt_vect[i] * g1g2[3][i] * -((int32_t) act_is_servo[i] - 1);
- }
-
- ground_detected = false;
- if (specific_thrust > GROUND_DETECT_SENSOR_SPECIFIC_THRUST_THRESHOLD ) {
- counter_thrust += 1;
- if (counter_thrust > GROUND_DETECT_SENSOR_COUNTER_TRIGGER) {
- ground_detected = true;
- }
- } else {
- counter_thrust = 0;
- }
-
- #ifdef DEBUG_GROUND_DETECT
- uint8_t test_gd = ground_detected;
- float payload[2];
- payload[0] = specific_thrust;
- payload[1] = stateGetAccelNed_f()->z;
-
- RunOnceEvery(10, {DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, &test_gd); DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 4, payload);} );
- #endif
-#endif
-
-#if USE_GROUND_DETECT_AGL_DIST
- ground_detect_method = true;
- static int32_t counter_agl_dist = 0;
- if (agl_dist_valid && (agl_dist_value_filtered < GROUND_DETECT_SENSOR_AGL_MIN_VALUE)) {
- counter_agl_dist += 1;
- } else {
- counter_agl_dist = 0;
- }
- if (counter_agl_dist > GROUND_DETECT_SENSOR_COUNTER_TRIGGER) {
- ground_detected = true;
- } else {
- ground_detected = false;
- }
-#endif
- if (!ground_detect_method) {
- ground_detected = false;
- }
-}
\ No newline at end of file