Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge pull request #469 from flixr/in_flight_heuristic

Browse Source 
[rotorcraft] improve in_flight detection heuristic and fix takeoff-land-takeoff in nav
This commit is contained in:
Felix Ruess
2013-09-04 07:06:25 -07:00
parent 6e6dd7ae82 5e2647eb81
commit 6fb80641d6
8 changed files with 53 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+2
View File
@@ -38,6 +38,8 @@
<define name="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC" value="TRUE"/>
</subsystem>
<subsystem name="ins"/>
<!--define name="KILL_ON_GROUND_DETECT" value="TRUE"/-->
</firmware>
<servos driver="Pwm">
conf/flight_plans/rotorcraft_basic.xml
+4
View File
@@ -71,8 +71,12 @@
</block>
<block name="flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call fun="NavStartDetectGround()"/>
<stay climb="-0.8" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
</blocks>
</flight_plan>
sw/airborne/firmwares/rotorcraft/autopilot.c
+31 -44
View File
@@ -52,7 +52,22 @@ bool_t autopilot_power_switch;
bool_t autopilot_detect_ground;
bool_t autopilot_detect_ground_once;
#define AUTOPILOT_IN_FLIGHT_TIME 40
#define AUTOPILOT_IN_FLIGHT_TIME 20
/** minimum vertical speed for in_flight condition in m/s */
#ifndef AUTOPILOT_IN_FLIGHT_MIN_SPEED
#define AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2
#endif
/** minimum vertical acceleration for in_flight condition in m/s^2 */
#ifndef AUTOPILOT_IN_FLIGHT_MIN_ACCEL
#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0
#endif
/** minimum thrust for in_flight condition in pprz_t units */
#ifndef AUTOPILOT_IN_FLIGHT_MIN_THRUST
#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
#endif
#ifndef AUTOPILOT_DISABLE_AHRS_KILL
#include "subsystems/ahrs.h"
@@ -111,36 +126,6 @@ void autopilot_init(void) {
}
static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
if (stabilization_cmd[COMMAND_THRUST] == 0) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
else { /* !THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
else { /* not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
motors_on) {
if (stabilization_cmd[COMMAND_THRUST] > 0) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
else { /* THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = 0;
}
}
}
}
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
@@ -167,10 +152,6 @@ INFO("Using FAILSAFE_GROUND_DETECT")
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
// when we dont have RC, check in flight by looking at throttle
if (radio_control.status != RC_OK) {
autopilot_check_in_flight_no_rc(autopilot_motors_on);
}
}
@@ -269,29 +250,37 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
}
static inline void autopilot_check_in_flight( bool_t motors_on ) {
void autopilot_check_in_flight(bool_t motors_on) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
if (THROTTLE_STICK_DOWN()) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(abs(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(abs(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL))
{
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
else { /* !THROTTLE_STICK_DOWN */
else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
else { /* not in flight */
else { /* currently not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
motors_on) {
if (!THROTTLE_STICK_DOWN()) {
motors_on)
{
/* if thrust above min threshold, assume in_flight.
* Don't check for velocity and acceleration above threshold here...
*/
if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
else { /* THROTTLE_STICK_DOWN */
else { /* currently not in_flight and thrust below threshold, reset counter */
autopilot_in_flight_counter = 0;
}
}
@@ -344,8 +333,6 @@ void autopilot_on_rc_frame(void) {
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
autopilot_check_in_flight(autopilot_motors_on);
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
sw/airborne/firmwares/rotorcraft/autopilot.h
+1
View File
@@ -67,6 +67,7 @@ extern void autopilot_periodic(void);
extern void autopilot_on_rc_frame(void);
extern void autopilot_set_mode(uint8_t new_autopilot_mode);
extern void autopilot_set_motors_on(bool_t motors_on);
extern void autopilot_check_in_flight(bool_t motors_on);
extern bool_t autopilot_detect_ground;
extern bool_t autopilot_detect_ground_once;
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+5 -2
View File
@@ -237,13 +237,16 @@ void guidance_v_run(bool_t in_flight) {
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_CLIMB) {
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = -nav_climb;
gv_update_ref_from_zd_sp(guidance_v_zd_sp);
nav_flight_altitude = -guidance_v_z_sp;
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_MANUAL) {
guidance_v_z_sp = -nav_flight_altitude; // For display only
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = stateGetSpeedNed_i()->z;
GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
guidance_v_z_sum_err = 0;
guidance_v_delta_t = nav_throttle;
}
#if NO_RC_THRUST_LIMIT
sw/airborne/firmwares/rotorcraft/main.c
+2
View File
@@ -241,6 +241,8 @@ STATIC_INLINE void failsafe_check( void ) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
#endif
autopilot_check_in_flight(autopilot_motors_on);
}
STATIC_INLINE void main_event( void ) {
sw/airborne/firmwares/rotorcraft/navigation.c
+7
View File
@@ -341,4 +341,11 @@ bool_t nav_detect_ground(void) {
return TRUE;
}
bool_t nav_is_in_flight(void) {
if (autopilot_in_flight)
return TRUE;
else
return FALSE;
}
void nav_home(void) {}
sw/airborne/firmwares/rotorcraft/navigation.h
+1
View File
@@ -81,6 +81,7 @@ unit_t nav_reset_alt( void ) __attribute__ ((unused));
void nav_periodic_task(void);
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
bool_t nav_detect_ground(void);
bool_t nav_is_in_flight(void);
void nav_home(void);