Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Update ground detect (#3365)

Browse Source
This commit is contained in:
Christophe De Wagter
2024-10-01 12:56:08 +02:00
committed by GitHub
parent 02d93d9bc9
commit 5a45a6158d
17 changed files with 219 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rotwing5.xml
+3 -2
View File
@@ -141,7 +141,8 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="rotwing_automation"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks">
<define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
@@ -323,7 +324,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<!-- <define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/> -->
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing6.xml
+2 -2
View File
@@ -134,7 +134,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks">
<define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
@@ -315,7 +315,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<!-- <define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/> -->
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3b.xml
+3 -3
View File
@@ -135,7 +135,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/>
<module name="pfc_actuators"/>
@@ -451,7 +451,7 @@
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- prevent in-flight in start engine block-->
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -464,7 +464,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3c_oneloop.xml
+2 -2
View File
@@ -135,7 +135,7 @@
<define name="WLS_N_V_MAX" value = "6"/>
</module>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state_V2"/>
<module name="agl_dist"/>
<module name="rotwing_automation">
@@ -251,7 +251,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
+2 -2
View File
@@ -136,7 +136,7 @@
<define name="WLS_N_V_MAX" value = "6"/>
</module>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state_V2"/>
<module name="agl_dist"/>
<module name="rotwing_automation">
@@ -253,7 +253,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
+2 -2
View File
@@ -128,7 +128,7 @@
<define name="WLS_N_V_MAX" value = "6"/>
</module>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state_V2"/>
<module name="agl_dist"/>
<module name="rotwing_automation">
@@ -244,7 +244,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
+2 -2
View File
@@ -96,7 +96,7 @@
<define name="WLS_N_V_MAX" value = "6"/>
</module>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state_V2">
<define name = "USE_ROTMECH_VIRTUAL" value = "TRUE"/>
<define name = "ROTMECH_DYN" value = "3.0"/>
@@ -213,7 +213,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3d.xml
+2 -2
View File
@@ -122,7 +122,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/>
<module name="pfc_actuators"/>
@@ -456,7 +456,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3e.xml
+2 -2
View File
@@ -122,7 +122,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/>
<module name="pfc_actuators"/>
@@ -456,7 +456,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3f.xml
+2 -2
View File
@@ -122,7 +122,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/>
<module name="pfc_actuators"/>
@@ -456,7 +456,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3g.xml
+2 -2
View File
@@ -127,7 +127,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/>
<module name="pfc_actuators"/>
@@ -460,7 +460,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/airframes/tudelft/rotwing_v3h.xml
+2 -2
View File
@@ -127,7 +127,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="ground_detect_sensor"/>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/>
<module name="pfc_actuators"/>
@@ -460,7 +460,7 @@
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="GROUND_DETECT_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
conf/modules/ground_detect.xml
+31
View File
@@ -0,0 +1,31 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ground_detect" 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_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_AGL_MIN_VALUE" value="0.1" description="AGL value low enough to be used as ground detection [m]"/>
<define name="GROUND_DETECT_COUNTER_TRIGGER" value="10" description="Number of times a trigger must be valid to accept ground detection."/>
</doc>
<header>
<file name="ground_detect.h"/>
</header>
<init fun="ground_detect_init()"/>
<periodic fun="ground_detect_periodic()" freq="50"/>
<periodic fun="ground_detect_filter_accel()"/>
<makefile>
<file name="ground_detect.c"/>
<test firmware="rotorcraft">
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="1"/>
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="DOWNLINK_DEVICE" value="uart0"/>
<define name="USE_UART0"/>
<define name="INDI_OUTPUTS" value="1"/>
<define name="INDI_NUM_ACT" value="1"/>
</test>
</makefile>
</module>
conf/modules/ground_detect_sensor.xml
-26
View File
@@ -1,26 +0,0 @@
<!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>
sw/airborne/modules/nav/ground_detect.c
+152
View File
@@ -0,0 +1,152 @@
/*
* Copyright (C) 2024 Ewoud Smeur <e.j.j.smeur@tudelft.nl>
* 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.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);
}
sw/airborne/modules/nav/ground_detect_sensor.h → sw/airborne/modules/nav/ground_detect.h
+10 -6
View File
@@ -18,19 +18,23 @@
* <http://www.gnu.org/licenses/>.
*/
/** @file "modules/nav/ground_detect_sensor.h"
/** @file "modules/nav/ground_detect.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* 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
sw/airborne/modules/nav/ground_detect_sensor.c
-113
View File
@@ -1,113 +0,0 @@
/*
* 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)
{
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;
}
}