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 26 Packages Projects Releases Wiki Activity

move ground detection and in_flight detection to overridable functions (#3081)

Browse Source
This commit is contained in:
Christophe De Wagter
2023-09-25 15:55:11 +02:00
committed by GitHub
parent 0e50371dad
commit 3e27292244
2 changed files with 39 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+33 -15
View File
@@ -77,6 +77,35 @@ static uint32_t autopilot_in_flight_counter;
#define THRESHOLD_GROUND_DETECT 25.0
#endif
/** Default ground-detection estimation based on accelerometer shock */
bool WEAK autopilot_ground_detection(void) {
struct NedCoor_f *accel = stateGetAccelNed_f();
if (accel->z < -THRESHOLD_GROUND_DETECT ||
accel->z > THRESHOLD_GROUND_DETECT) {
return true;
}
return false;
}
/** Default end-of-in-flight detection estimation based on thrust and speed */
bool WEAK autopilot_in_flight_end_detection(void) {
if (autopilot_in_flight_counter > 0) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
return true;
}
} else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
return false;
}
#if USE_MOTOR_MIXING
#include "modules/actuators/motor_mixing.h"
@@ -234,9 +263,7 @@ void autopilot_event(void)
|| autopilot.mode == AP_MODE_FAILSAFE
#endif
) {
struct NedCoor_f *accel = stateGetAccelNed_f();
if (accel->z < -THRESHOLD_GROUND_DETECT ||
accel->z > THRESHOLD_GROUND_DETECT) {
if (autopilot_ground_detection()) {
autopilot.ground_detected = true;
autopilot.detect_ground_once = false;
}
@@ -255,18 +282,9 @@ void autopilot_reset_in_flight_counter(void)
void autopilot_check_in_flight(bool motors_on)
{
if (autopilot.in_flight) {
if (autopilot_in_flight_counter > 0) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot.in_flight = false;
}
} else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
if (autopilot_in_flight_end_detection()) {
autopilot.in_flight = false;
autopilot_in_flight_counter = 0;
}
} else { /* currently not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
sw/airborne/firmwares/rotorcraft/autopilot_firmware.h
+6
View File
@@ -33,6 +33,12 @@
extern uint8_t autopilot_mode_auto2;
// Detect the ground and set NavGroundDetect() to true
extern bool autopilot_ground_detection(void);
// Detect the end of in_flight and stop integrators in control loops
extern bool autopilot_in_flight_end_detection(void);
extern void autopilot_firmware_init(void);
#endif /* AUTOPILOT_FIRMWARE_H */