From be32e5421bfbb7960bb23abf1c582cb3e254735e Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Mon, 25 Sep 2023 08:53:54 +0200 Subject: [PATCH] [module] ground_detect_sensor module (#3077) fix tests ground_detect_sensor cleanup unused final update --- conf/modules/ground_detect_sensor.xml | 26 +++++ .../stabilization/stabilization_indi.h | 2 + .../modules/nav/ground_detect_sensor.c | 109 ++++++++++++++++++ .../modules/nav/ground_detect_sensor.h | 36 ++++++ 4 files changed, 173 insertions(+) create mode 100644 conf/modules/ground_detect_sensor.xml create mode 100644 sw/airborne/modules/nav/ground_detect_sensor.c create mode 100644 sw/airborne/modules/nav/ground_detect_sensor.h diff --git a/conf/modules/ground_detect_sensor.xml b/conf/modules/ground_detect_sensor.xml new file mode 100644 index 0000000000..35e28fdadd --- /dev/null +++ b/conf/modules/ground_detect_sensor.xml @@ -0,0 +1,26 @@ + + + + Ground detection module to detect ground + + + + + + + + +
+ +
+ + + + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h index fca5507719..df252d643a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h @@ -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; diff --git a/sw/airborne/modules/nav/ground_detect_sensor.c b/sw/airborne/modules/nav/ground_detect_sensor.c new file mode 100644 index 0000000000..c3037bcd31 --- /dev/null +++ b/sw/airborne/modules/nav/ground_detect_sensor.c @@ -0,0 +1,109 @@ +/* + * 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) +{ + 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 + + +} \ No newline at end of file diff --git a/sw/airborne/modules/nav/ground_detect_sensor.h b/sw/airborne/modules/nav/ground_detect_sensor.h new file mode 100644 index 0000000000..799617dfa7 --- /dev/null +++ b/sw/airborne/modules/nav/ground_detect_sensor.h @@ -0,0 +1,36 @@ +/* + * 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.h" + * @author Dennis van Wijngaarden + * 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