From 219363e102d559bb73dd126e2fc4f7f188cbbd3d Mon Sep 17 00:00:00 2001
From: NoahWe <74419424+NoahWe@users.noreply.github.com>
Date: Fri, 3 Apr 2026 10:08:58 +0200
Subject: [PATCH] [Rotwing] Changes to ground detect and add hx711 based strain
gauges (#3616)
* changes to ground detect and hx711
* keep reverse thrust even if ground_detect cuts out e.g. during tip over
* force sensor struct
* update pprzlink
---
conf/abi.xml | 6 +
...wing_v3c_oneloop_optitrack_ext_pose_AG.xml | 4 +-
.../airframes/tudelft/rotwing_25kg_common.xml | 2 +-
conf/airframes/tudelft/rotwing_7kg_common.xml | 9 +-
.../airframes/tudelft/rotwing_v3c_oneloop.xml | 4 +-
.../tudelft/rotwing_v3c_oneloop_optitrack.xml | 4 +-
...rotwing_v3c_oneloop_optitrack_ext_pose.xml | 4 +-
.../rotwing_v3c_oneloop_simulation.xml | 4 +-
.../airframes/tudelft/rotwing_v3g_oneloop.xml | 4 +-
.../tudelft/rotwing_v3g_oneloop_optitrack.xml | 4 +-
...rotwing_v3g_oneloop_optitrack_ext_pose.xml | 4 +-
conf/airframes/tudelft/rotwing_v3h.xml | 11 +
conf/flight_plans/tudelft/rotwing_EHVB.xml | 2 +-
conf/modules/ground_detect.xml | 24 +-
conf/modules/hx711.xml | 32 +++
conf/telemetry/highspeed_rotorcraft.xml | 2 +
.../arch/chibios/modules/sensors/hx711.c | 242 ++++++++++++++++
.../arch/chibios/modules/sensors/hx711.h | 36 +++
sw/airborne/modules/nav/ground_detect.c | 267 +++++++++++++++---
sw/airborne/modules/nav/ground_detect.h | 27 ++
sw/ext/pprzlink | 2 +-
21 files changed, 627 insertions(+), 67 deletions(-)
create mode 100644 conf/modules/hx711.xml
create mode 100644 sw/airborne/arch/chibios/modules/sensors/hx711.c
create mode 100644 sw/airborne/arch/chibios/modules/sensors/hx711.h
diff --git a/conf/abi.xml b/conf/abi.xml
index 57522280bf..b32fb57601 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -275,6 +275,12 @@
Pointer pointing at the actuators_t4_extra_data_in array
+
+ Timestamp in micro-seconds
+ Number of force sensor values in the array
+ Pointer to force sensor values (driver raw units)
+
+
diff --git a/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml
index 86f3f1699a..63c5735863 100644
--- a/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml
+++ b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml
@@ -263,8 +263,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rotwing_25kg_common.xml b/conf/airframes/tudelft/rotwing_25kg_common.xml
index 056f1d43b5..99b58faf36 100644
--- a/conf/airframes/tudelft/rotwing_25kg_common.xml
+++ b/conf/airframes/tudelft/rotwing_25kg_common.xml
@@ -51,7 +51,7 @@
-
+
diff --git a/conf/airframes/tudelft/rotwing_7kg_common.xml b/conf/airframes/tudelft/rotwing_7kg_common.xml
index 712661f4a9..067a308ec0 100644
--- a/conf/airframes/tudelft/rotwing_7kg_common.xml
+++ b/conf/airframes/tudelft/rotwing_7kg_common.xml
@@ -53,10 +53,11 @@
-
-
-
-
+
+
+
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
index 5004266791..1ca5ce8e48 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
@@ -265,8 +265,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
index 4a77196bb0..bd90b27177 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
@@ -267,8 +267,8 @@
-
-
+
+
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 47d8628160..2b4f050f0f 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
@@ -256,8 +256,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
index 70bbd141c3..ec799add69 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
@@ -224,8 +224,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3g_oneloop.xml b/conf/airframes/tudelft/rotwing_v3g_oneloop.xml
index 1ba5906812..1aee84be62 100644
--- a/conf/airframes/tudelft/rotwing_v3g_oneloop.xml
+++ b/conf/airframes/tudelft/rotwing_v3g_oneloop.xml
@@ -261,8 +261,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack.xml
index 4cc3f48ccd..178c36b216 100644
--- a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack.xml
+++ b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack.xml
@@ -261,8 +261,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml
index e75070c353..ea1ba07867 100644
--- a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml
+++ b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml
@@ -255,8 +255,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3h.xml b/conf/airframes/tudelft/rotwing_v3h.xml
index 9dd7d26bd5..7bff0605fd 100644
--- a/conf/airframes/tudelft/rotwing_v3h.xml
+++ b/conf/airframes/tudelft/rotwing_v3h.xml
@@ -42,6 +42,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/tudelft/rotwing_EHVB.xml b/conf/flight_plans/tudelft/rotwing_EHVB.xml
index d075f6d24b..e1a18bcb25 100644
--- a/conf/flight_plans/tudelft/rotwing_EHVB.xml
+++ b/conf/flight_plans/tudelft/rotwing_EHVB.xml
@@ -218,7 +218,7 @@
-
+
diff --git a/conf/modules/ground_detect.xml b/conf/modules/ground_detect.xml
index 57d63793d4..510efe9bbc 100644
--- a/conf/modules/ground_detect.xml
+++ b/conf/modules/ground_detect.xml
@@ -1,15 +1,31 @@
- Ground detection module to detect ground
-
+
+ Ground detection module to detect ground.
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -20,7 +36,7 @@
-
+
diff --git a/conf/modules/hx711.xml b/conf/modules/hx711.xml
new file mode 100644
index 0000000000..8174a9f6c3
--- /dev/null
+++ b/conf/modules/hx711.xml
@@ -0,0 +1,32 @@
+
+
+
+
+
+ Reading of multiple HX711 strain gauge sensors.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
index f7cf0d21f8..3f32332fec 100644
--- a/conf/telemetry/highspeed_rotorcraft.xml
+++ b/conf/telemetry/highspeed_rotorcraft.xml
@@ -66,6 +66,7 @@
+
@@ -387,6 +388,7 @@
+
diff --git a/sw/airborne/arch/chibios/modules/sensors/hx711.c b/sw/airborne/arch/chibios/modules/sensors/hx711.c
new file mode 100644
index 0000000000..399ca35a69
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/sensors/hx711.c
@@ -0,0 +1,242 @@
+/*
+ * Copyright (C) Freek van Tienen
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file arch/chibios/modules/sensors/hx711.c
+ * Interface for the HX711 sensor
+ *
+ */
+
+#include "hx711.h"
+#include "mcu_periph/gpio.h"
+#include "mcu_periph/sys_time.h"
+#include "modules/core/abi.h"
+#include "modules/datalink/downlink.h"
+#include "modules/datalink/telemetry.h"
+#include BOARD_CONFIG
+
+#ifndef HX711_DEVICES_NB
+#define HX711_DEVICES_NB 4
+#endif
+
+#ifndef HX711_GAIN
+#define HX711_GAIN 1
+#endif
+
+#ifndef HX711_PWM_FREQUENCY
+#define HX711_PWM_FREQUENCY 6000000
+#endif
+
+#ifndef HX711_DEVICES
+#define HX711_DEVICES {}
+#endif
+
+#ifndef HX711_FORCE_SENSOR_ID
+#define HX711_FORCE_SENSOR_ID ABI_BROADCAST
+#endif
+
+#ifndef HX711_DEBUG
+#define HX711_DEBUG false
+#endif
+
+#if HX711_DEBUG
+#include
+#endif
+
+// Running at 50kHz for a full clock pulse (10us high, 10us low)
+#define HX711_PERIOD (HX711_PWM_FREQUENCY / 50000)
+
+float hx711_meas_time = 0;
+struct hx711_dev_t {
+ ioportid_t data_port;
+ uint16_t data_pin;
+ int32_t measurement;
+};
+
+struct hx711_t {
+ volatile bool busy;
+ volatile bool measurement_ready;
+ struct hx711_dev_t devices[HX711_DEVICES_NB];
+
+ volatile uint8_t read_bit_idx;
+};
+
+
+static void pwmpcb(PWMDriver *pwmp __attribute__((unused)));
+
+static volatile struct hx711_t hx711 = {
+ .busy = false,
+ .measurement_ready = false,
+ .devices = {
+ HX711_DEVICES
+ },
+ .read_bit_idx = 0
+};
+
+static PWMConfig pwmcfg = {
+ .frequency = HX711_PWM_FREQUENCY,
+ .period = HX711_PERIOD,
+ .callback = NULL,
+ .channels = {
+ { PWM_OUTPUT_DISABLED, NULL },
+ { PWM_OUTPUT_DISABLED, NULL },
+ { PWM_OUTPUT_DISABLED, NULL },
+ { PWM_OUTPUT_DISABLED, NULL },
+ },
+ .cr2 = 0,
+ .bdtr = 0,
+ .dier = 0
+};
+
+
+void hx711_init(void)
+{
+ /*----------------
+ * Configure GPIO
+ *----------------*/
+ gpio_setup_pin_af(HX711_PWM_PORT, HX711_PWM_PIN, HX711_PWM_AF, true);
+ pwmcfg.channels[HX711_PWM_CHANNEL].mode = PWM_OUTPUT_ACTIVE_HIGH;
+ pwmcfg.channels[HX711_PWM_CHANNEL].callback = pwmpcb;
+
+ for(uint8_t i = 0; i < HX711_DEVICES_NB; i++) {
+ gpio_setup_input(hx711.devices[i].data_port, hx711.devices[i].data_pin);
+ }
+
+ /*---------------
+ * Configure PWM
+ *---------------*/
+ pwmStart(&HX711_PWM_DRIVER, &pwmcfg);
+ hx711.busy = false;
+ hx711.measurement_ready = false;
+ hx711.read_bit_idx = 0;
+}
+
+/**
+ * Start a measurement and publish the results when possible
+ */
+void hx711_event(void)
+{
+ // If busy reading data, return
+ if(hx711.busy)
+ return;
+
+ // Check if we have a measurement to read
+ if(hx711.measurement_ready) {
+ int32_t values[HX711_DEVICES_NB];
+ for(uint8_t i = 0; i < HX711_DEVICES_NB; i++) {
+ values[i] = hx711.devices[i].measurement;
+ }
+
+ AbiSendMsgFORCE_SENSOR(HX711_FORCE_SENSOR_ID, get_sys_time_usec(), HX711_DEVICES_NB, values);
+
+ // Send down for debug
+ float raw_val[HX711_DEVICES_NB];
+ for(uint8_t i = 0; i < HX711_DEVICES_NB; i++) {
+ raw_val[i] = values[i];
+ }
+
+#if HX711_DEBUG
+ float current_time = get_sys_time_float();
+ float freq = 1 / (current_time - hx711_meas_time);
+ hx711_meas_time = current_time;
+
+ char hx711_str[10];
+ char hx711_freq_str[15];
+ int hx711_len = snprintf(hx711_str, sizeof(hx711_str), "HX711");
+ int hx711_freq_len = snprintf(hx711_freq_str, sizeof(hx711_freq_str), "HX711 freq");
+ RunOnceEvery(10, {
+ DOWNLINK_SEND_DEBUG_VECT(DefaultChannel, DefaultDevice, hx711_len, hx711_str, HX711_DEVICES_NB, raw_val);
+ });
+ RunOnceEvery(15, {
+ DOWNLINK_SEND_DEBUG_VECT(DefaultChannel, DefaultDevice, hx711_freq_len, hx711_freq_str, 1, &freq);
+ });
+ pprz_msg_send_DEBUG_VECT(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, hx711_len, hx711_str, HX711_DEVICES_NB, raw_val);
+ pprz_msg_send_DEBUG_VECT(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, hx711_freq_len, hx711_freq_str, 1, &freq);
+#endif
+
+ hx711.measurement_ready = false;
+ }
+
+ // Check if all data pins are low, ready to read data
+ for(uint8_t i = 0; i < HX711_DEVICES_NB; i++) {
+ if(gpio_get(hx711.devices[i].data_port, hx711.devices[i].data_pin) != 0)
+ return;
+ }
+
+ // Start reading data
+ chSysLock();
+ hx711.busy = true;
+ hx711.measurement_ready = false;
+ hx711.read_bit_idx = 0;
+ pwmEnableChannel(&HX711_PWM_DRIVER, HX711_PWM_CHANNEL, HX711_PERIOD/2); // 50% duty cycle
+ pwmEnableChannelNotification(&HX711_PWM_DRIVER, HX711_PWM_CHANNEL);
+ chSysUnlock();
+}
+
+/**
+ * Callback on the falling edge of the clock signal
+ */
+static void pwmpcb(PWMDriver *pwmp __attribute__((unused))) {
+ chSysLockFromISR();
+
+ /* In case we where not reading just stop */
+ if(!hx711.busy) {
+ pwmDisableChannelNotificationI(&HX711_PWM_DRIVER, HX711_PWM_CHANNEL);
+ pwmDisableChannelI(&HX711_PWM_DRIVER, HX711_PWM_CHANNEL);
+ chSysUnlockFromISR();
+ return;
+ }
+
+ /* Parse the measurements for all devices */
+ if(hx711.read_bit_idx < 24) {
+ for(uint8_t i = 0; i < HX711_DEVICES_NB; i++) {
+ volatile struct hx711_dev_t *dev = &hx711.devices[i];
+
+ // Reset or bitshift
+ if(hx711.read_bit_idx == 0)
+ dev->measurement = 0;
+ else
+ dev->measurement <<= 1;
+
+ // Read the bit
+ if(gpio_get(dev->data_port, dev->data_pin) != 0)
+ dev->measurement |= 1;
+
+ // Fix 2s complement for 32 bit signed integer from 24 bit
+ if(dev->measurement & (1 << 23)) {
+ dev->measurement |= 0xFF000000;
+ }
+ }
+ }
+ hx711.read_bit_idx++;
+
+ /* Send gain and finish reading data */
+ if(hx711.read_bit_idx >= (24 + HX711_GAIN)) {
+ pwmDisableChannelNotificationI(&HX711_PWM_DRIVER, HX711_PWM_CHANNEL);
+ pwmDisableChannelI(&HX711_PWM_DRIVER, HX711_PWM_CHANNEL);
+ hx711.busy = false;
+ hx711.measurement_ready = true;
+ chSysUnlockFromISR();
+ return;
+ }
+
+ chSysUnlockFromISR();
+}
diff --git a/sw/airborne/arch/chibios/modules/sensors/hx711.h b/sw/airborne/arch/chibios/modules/sensors/hx711.h
new file mode 100644
index 0000000000..9898d60654
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/sensors/hx711.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) Freek van Tienen
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file arch/chibios/modules/sensors/hx711.h
+ * Interface for the HX711 sensor
+ *
+ */
+
+#include "std.h"
+
+#ifndef HX711_H
+#define HX711_H
+
+void hx711_init(void);
+void hx711_event(void);
+
+#endif /* HX711_H */
diff --git a/sw/airborne/modules/nav/ground_detect.c b/sw/airborne/modules/nav/ground_detect.c
index a3d4ad7431..97980a98d8 100644
--- a/sw/airborne/modules/nav/ground_detect.c
+++ b/sw/airborne/modules/nav/ground_detect.c
@@ -24,48 +24,207 @@
*/
#include "ground_detect.h"
-#include "filters/low_pass_filter.h"
+#include "filters/median_filter.h"
#include "firmwares/rotorcraft/autopilot_firmware.h"
+#include "modules/core/abi.h"
#include "state.h"
-#if USE_GROUND_DETECT_INDI_THRUST
-#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
+#include "pprzlink/messages.h"
+#include "modules/datalink/downlink.h"
+
+/* Number of triggers which need to be active to assume ground has been detected */
+#ifndef GROUND_DETECT_NUM_TRIGGERS
+#define GROUND_DETECT_NUM_TRIGGERS 3
#endif
-#if USE_GROUND_DETECT_AGL_DIST
+#if GROUND_DETECT_USE_INDI_THRUST
+#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
+#endif
+PRINT_CONFIG_VAR(GROUND_DETECT_USE_INDI_THRUST)
+
+#if GROUND_DETECT_USE_AGL_DIST
#include "modules/sonar/agl_dist.h"
#ifndef GROUND_DETECT_AGL_MIN_VALUE
#define GROUND_DETECT_AGL_MIN_VALUE 0.1
#endif
#endif
+PRINT_CONFIG_VAR(GROUND_DETECT_USE_AGL_DIST)
+
+#ifndef GROUND_DETECT_USE_FORCE_SENSOR
+#define GROUND_DETECT_USE_FORCE_SENSOR 0
+#endif
+
+#ifndef GROUND_DETECT_FORCE_SENSOR_THRESHOLD
+#define GROUND_DETECT_FORCE_SENSOR_THRESHOLD 100000
+#endif
+
+#ifndef GROUND_DETECT_FORCE_SENSOR_MEDIAN_FILT_SIZE
+#define GROUND_DETECT_FORCE_SENSOR_MEDIAN_FILT_SIZE 3
+#endif
+
+#ifndef FORCE_SENSOR_MAX_NB
+#define FORCE_SENSOR_MAX_NB 16
+#endif
+
+#if GROUND_DETECT_USE_FORCE_SENSOR
+#ifndef GROUND_DETECT_FORCE_SENSOR_ID
+#define GROUND_DETECT_FORCE_SENSOR_ID ABI_BROADCAST
+#endif
+#endif
+PRINT_CONFIG_VAR(GROUND_DETECT_USE_FORCE_SENSOR)
+
+#ifndef GROUND_DETECT_REVERSE_THRUST_ON_GROUND_DETECTED
+#define GROUND_DETECT_REVERSE_THRUST_ON_GROUND_DETECTED false
+#endif
+PRINT_CONFIG_VAR(GROUND_DETECT_REVERSE_THRUST_ON_GROUND_DETECTED)
+
+#if GROUND_DETECT_REVERSE_THRUST_ON_GROUND_DETECTED
+#ifndef GROUND_DETECT_REVERSE_THRUST_LEVEL
+#error "GROUND_DETECT_REVERSE_THRUST_LEVEL needs to be defined if GROUND_DETECT_REVERSE_THRUST_ON_GROUND_DETECTED is true"
+#endif
+uint16_t reverse_th_level = GROUND_DETECT_REVERSE_THRUST_LEVEL;
+#else
+uint16_t reverse_th_level = 0;
+#endif
#ifndef GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD
#define GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD -5.0
#endif
+PRINT_CONFIG_VAR(GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD)
-#include "pprzlink/messages.h"
-#include "modules/datalink/downlink.h"
+#ifndef GROUND_DETECT_VERTICAL_SPEED_THRESHOLD
+#define GROUND_DETECT_VERTICAL_SPEED_THRESHOLD 0.1
+#endif
+PRINT_CONFIG_VAR(GROUND_DETECT_VERTICAL_SPEED_THRESHOLD)
+#ifndef GROUND_DETECT_VERTICAL_ACCEL_THRESHOLD
+#define GROUND_DETECT_VERTICAL_ACCEL_THRESHOLD -3.0
+#endif
+PRINT_CONFIG_VAR(GROUND_DETECT_VERTICAL_ACCEL_THRESHOLD)
+
+#ifndef GROUND_DETECT_COUNTER_TRIGGER
#define GROUND_DETECT_COUNTER_TRIGGER 5
+#endif
// Cutoff frequency of accelerometer filter in Hz
+#ifndef GROUND_DETECT_FILT_FREQ
#define GROUND_DETECT_FILT_FREQ 5.0
-
-Butterworth2LowPass accel_filter;
+#endif
bool disarm_on_not_in_flight = false;
int32_t counter = 0;
bool ground_detected = false;
-#define DEBUG_GROUND_DETECT TRUE
+bool reverse_thrust = false;
+
+union ground_detect_bitmask_t ground_detect_status;
+struct ground_detect_values_t ground_detect_values;
+
+float force_sensor_ground_threshold = GROUND_DETECT_FORCE_SENSOR_THRESHOLD;
+
+struct force_sensor_data_t {
+ uint8_t count;
+ int32_t offsets[FORCE_SENSOR_MAX_NB];
+ int32_t values_filt[FORCE_SENSOR_MAX_NB];
+};
+
+static struct force_sensor_data_t force_sensor = {0};
+
+#if GROUND_DETECT_USE_FORCE_SENSOR
+static struct MedianFilterInt force_sensor_filt[FORCE_SENSOR_MAX_NB];
+static bool force_sensor_valid = false;
+static abi_event force_sensor_ev;
+
+static void force_sensor_autoset_offset(void)
+{
+ if (!force_sensor_valid) {
+ return;
+ }
+
+ for (uint8_t i = 0; i < force_sensor.count; i++) {
+ force_sensor.offsets[i] += force_sensor.values_filt[i];
+ }
+}
+
+static void force_sensor_cb(uint8_t sender_id UNUSED, uint32_t stamp UNUSED, uint8_t count, int32_t *values)
+{
+ uint8_t n = count;
+ if (n > FORCE_SENSOR_MAX_NB) {
+ n = FORCE_SENSOR_MAX_NB;
+ }
+
+ force_sensor.count = n;
+ force_sensor_valid = true;
+ for (uint8_t i = 0; i < n; i++) {
+ force_sensor.values_filt[i] = update_median_filter_i(&force_sensor_filt[i], values[i] - force_sensor.offsets[i]);
+ }
+}
+
+static bool force_sensor_detect_ground(void)
+{
+ if (!force_sensor_valid) {
+ return false;
+ }
+
+ for (uint8_t i = 0; i < force_sensor.count; i++) {
+ if (fabsf((float)force_sensor.values_filt[i]) > force_sensor_ground_threshold) {
+ return true;
+ }
+ }
+ return false;
+}
+#endif
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+static void send_ground_detect(struct transport_tx *trans, struct link_device *dev)
+{
+ uint8_t _ground_detected = ground_detected;
+
+ pprz_msg_send_GROUND_DETECT(trans, dev, AC_ID,
+ &_ground_detected,
+ &ground_detect_values.speed_down,
+ &ground_detect_values.spec_thrust_down,
+ &ground_detect_values.accel_down_filt.o[0],
+ &ground_detect_values.agl_dist_value_filtered,
+ force_sensor.count, force_sensor.values_filt,
+ &ground_detect_status.value
+ );
+}
+#endif
+
void ground_detect_init()
{
+ // Initialize the ground detection status
+ ground_detect_status.value = 0;
+ ground_detect_values.speed_down = 0.0;
+ ground_detect_values.spec_thrust_down = 0.0;
+ ground_detect_values.agl_dist_value_filtered = 0.0;
+ force_sensor.count = 1; // Needed for force sensor field in ground detect message
+ for (uint8_t i = 0; i < FORCE_SENSOR_MAX_NB; i++) {
+ force_sensor.offsets[i] = 0;
+ force_sensor.values_filt[i] = 0;
+ }
+
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);
+ init_butterworth_2_low_pass(&ground_detect_values.accel_down_filt, tau, sample_time, 0.0);
+
+#if GROUND_DETECT_USE_FORCE_SENSOR
+ for (uint8_t i = 0; i < FORCE_SENSOR_MAX_NB; i++) {
+ init_median_filter_i(&force_sensor_filt[i], GROUND_DETECT_FORCE_SENSOR_MEDIAN_FILT_SIZE);
+ }
+ force_sensor.count = 0;
+ force_sensor_valid = false;
+ AbiBindMsgFORCE_SENSOR(GROUND_DETECT_FORCE_SENSOR_ID, &force_sensor_ev, force_sensor_cb);
+#endif
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GROUND_DETECT, send_ground_detect);
+#endif
}
bool ground_detect(void)
@@ -86,7 +245,7 @@ void ground_detect_periodic()
// Use the control effectiveness in thrust in order to estimate the thrust delivered (only works for multicopters)
float specific_thrust = 0.0; // m/s2
-#if USE_GROUND_DETECT_INDI_THRUST
+#if GROUND_DETECT_USE_INDI_THRUST
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);
@@ -94,26 +253,44 @@ void ground_detect_periodic()
#endif
// 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;
+ ground_detect_values.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;
+ ground_detect_values.speed_down = 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 > GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD)
- && (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))
+ // Detect ground based on AND of some triggers
+#if GROUND_DETECT_USE_FORCE_SENSOR
+ ground_detect_status.force_sensor_trigger = force_sensor_detect_ground();
+#else
+ ground_detect_status.force_sensor_trigger = false;
#endif
- ) {
+ ground_detect_status.vspeed_trigger = (fabsf(ground_detect_values.speed_down) < GROUND_DETECT_VERTICAL_SPEED_THRESHOLD)? 1:0;
+
+#if GROUND_DETECT_USE_INDI_THRUST
+ ground_detect_status.spec_thrust_trigger = (ground_detect_values.spec_thrust_down > GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD)? 1:0;
+#else
+ ground_detect_status.spec_thrust_trigger = false;
+#endif
+
+ ground_detect_status.accel_filt_trigger = (fabsf(ground_detect_values.accel_down_filt.o[0]) < GROUND_DETECT_VERTICAL_ACCEL_THRESHOLD)? 1:0;
+
+#if GROUND_DETECT_USE_AGL_DIST
+ ground_detect_status.agl_trigger = (agl_dist_valid && (agl_dist_value_filtered < GROUND_DETECT_AGL_MIN_VALUE))? 1:0;
+ ground_detect_values.agl_dist_value_filtered = agl_dist_value_filtered;
+#else
+ ground_detect_status.agl_trigger = false;
+#endif
+
+ int trigger_sum = 0;
+ for(uint8_t i = 0; i < 16; i++) trigger_sum += (ground_detect_status.value >> i) & 0x1;
+
+ if (trigger_sum >= GROUND_DETECT_NUM_TRIGGERS) {
counter += 1;
if (counter > GROUND_DETECT_COUNTER_TRIGGER) {
ground_detected = true;
@@ -127,24 +304,6 @@ void ground_detect_periodic()
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
}
/**
@@ -154,5 +313,33 @@ void ground_detect_periodic()
void ground_detect_filter_accel(void)
{
struct NedCoor_f *accel = stateGetAccelNed_f();
- update_butterworth_2_low_pass(&accel_filter, accel->z);
+ update_butterworth_2_low_pass(&ground_detect_values.accel_down_filt, accel->z);
+}
+
+bool ground_detect_reverse_thrust(void)
+{
+ // Reverse thrust needs to be enabled with GROUND_DETECT_REVERSE_THRUST_ON_GROUND_DETECTED, and started through for example a flight plan block
+ if (GROUND_DETECT_REVERSE_THRUST_ON_GROUND_DETECTED && reverse_thrust) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+void ground_detect_stop_reverse_thrust(void)
+{
+ reverse_thrust = false;
+}
+
+void ground_detect_start_reverse_thrust(void)
+{
+ reverse_thrust = true;
+}
+
+void ground_detect_set_offset_sensors(bool set_offset)
+{
+ (void) set_offset;
+#if GROUND_DETECT_USE_FORCE_SENSOR
+ force_sensor_autoset_offset();
+#endif
}
diff --git a/sw/airborne/modules/nav/ground_detect.h b/sw/airborne/modules/nav/ground_detect.h
index 9dcea4f985..f58608b07c 100644
--- a/sw/airborne/modules/nav/ground_detect.h
+++ b/sw/airborne/modules/nav/ground_detect.h
@@ -27,6 +27,25 @@
#define GROUND_DETECT_H
#include "std.h"
+#include "filters/low_pass_filter.h"
+
+union ground_detect_bitmask_t {
+ uint16_t value;
+ struct {
+ bool vspeed_trigger : 1;
+ bool spec_thrust_trigger : 1;
+ bool accel_filt_trigger: 1;
+ bool agl_trigger : 1;
+ bool force_sensor_trigger : 1;
+ };
+};
+
+struct ground_detect_values_t {
+ float speed_down;
+ float spec_thrust_down;
+ Butterworth2LowPass accel_down_filt;
+ float agl_dist_value_filtered;
+};
extern void ground_detect_init(void);
extern void ground_detect_periodic(void);
@@ -36,5 +55,13 @@ extern bool ground_detect(void);
extern void ground_detect_filter_accel(void);
extern bool disarm_on_not_in_flight;
+extern bool ground_detect_reverse_thrust(void);
+extern void ground_detect_stop_reverse_thrust(void);
+extern void ground_detect_start_reverse_thrust(void);
+extern void ground_detect_set_offset_sensors(bool set_offset);
+
+extern float force_sensor_ground_threshold;
+
+extern uint16_t reverse_th_level; // Reverse thrust level in pprz units
#endif // GROUND_DETECT_H
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index a6225a2c99..820a010f14 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit a6225a2c999fda27c2e6f5f71563abdaa36aa740
+Subproject commit 820a010f14c073c7e6d189181fde5dfb777bd630