[module] ground_detect_sensor module (#3077)

fix tests ground_detect_sensor cleanup unused final update
2026-05-09 22:49:53 +08:00 · 2023-09-25 08:53:54 +02:00
parent 57756b0493
commit be32e5421b
4 changed files with 173 additions and 0 deletions
@@ -0,0 +1,26 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="ground_detect_sensor" dir="nav">
+  <doc>
+    <description>Ground detection module to detect ground</description>
+    <define name="USE_GROUND_DETECT_INDI_THRUST" value="1" description="Use INDI thrust less than 50 percent as detection"/>
+    <define name="GROUND_DETECT_SENSOR_SPECIFIC_THRUST_THRESHOLD" value="-5.0" description="[m/s2] positive down"/>
+
+    <define name="USE_GROUND_DETECT_AGL_DIST" value="0" description="Use agl_dist_filtered"/>
+    <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.1" description="AGL value low enough to be used as ground detection [m]"/>
+    
+    <define name="GROUND_DETECT_SENSOR_COUNTER_TRIGGER" value="10" description="Number of times a trigger must be valid to accept ground detection."/>
+  </doc>
+  <header>
+    <file name="ground_detect_sensor.h"/>
+  </header>
+  <init fun="ground_detect_sensor_init()"/>
+  <periodic fun="ground_detect_sensor_periodic()" freq="50"/>
+  <makefile>
+    <file name="ground_detect_sensor.c"/>
+    <test>
+      <define name="USE_GROUND_DETECT_INDI_THRUST" value="1"/>
+      <define name="INDI_OUTPUTS" value="1"/>
+      <define name="INDI_NUM_ACT" value="1"/>
+    </test>
+  </makefile>
+</module>
@@ -43,6 +43,8 @@ extern float act_pref[INDI_NUM_ACT];

 extern float indi_Wu[INDI_NUM_ACT];

+extern bool act_is_servo[INDI_NUM_ACT];
+
 struct Indi_gains {
  struct FloatRates att;
  struct FloatRates rate;
@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/nav/ground_detect_sensor.c"
+ * @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ * 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)
+{
+  static int32_t counter = 0;
+
+#if USE_GROUND_DETECT_INDI_THRUST
+  // 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 ) {
+    if (counter > GROUND_DETECT_SENSOR_COUNTER_TRIGGER) {
+      ground_detected = true;
+    }
+  } else {
+    counter = 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
+
+#elif USE_GROUND_DETECT_AGL_DIST
+  if (agl_value && (agl_dist_value_filtered < GROUND_DETECT_SENSOR_AGL_MIN_VALUE)) {
+    counter += 1;
+  } else {
+    counter = 0;
+  }
+  if (counter > GROUND_DETECT_SENSOR_COUNTER_TRIGGER) {
+    ground_detected = true;
+  } else {
+    ground_detected = false;
+  }
+#else
+  ground_detected = false;
+#endif
+
+
+}
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/nav/ground_detect_sensor.h"
+ * @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ * Ground detection module
+ */
+
+#ifndef GROUND_DETECT_SENSOR_H
+#define GROUND_DETECT_SENSOR_H
+
+#include "std.h"
+
+extern void ground_detect_sensor_init(void);
+extern void ground_detect_sensor_periodic(void);
+
+extern bool ground_detect(void);
+
+#endif  // GROUND_DETECT_SENSOR_H