Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: update ekf terrain height stable setting

Browse Source 
setting is true only when taking off or landing and horizontal speed is no more than 0.5m/s
This commit is contained in:
Randy Mackay 2020-06-22 13:00:44 +09:00
parent d0a2198e00
commit 980c1f5020
3 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.cpp
View File

@ -354,6 +354,7 @@ void Copter::throttle_loop()
// compensate for ground effect (if enabled) // compensate for ground effect (if enabled)
update_ground_effect_detector(); update_ground_effect_detector();
update_ekf_terrain_height_stable();
} }
// update_batt_compass - read battery and compass // update_batt_compass - read battery and compass

1
ArduCopter/Copter.h
View File

@ -677,6 +677,7 @@ private:
// baro_ground_effect.cpp // baro_ground_effect.cpp
void update_ground_effect_detector(void); void update_ground_effect_detector(void);
void update_ekf_terrain_height_stable();
// commands.cpp // commands.cpp
void update_home_from_EKF(); void update_home_from_EKF();

19
ArduCopter/baro_ground_effect.cpp
View File

@ -1,5 +1,9 @@
#include "Copter.h" #include "Copter.h"
#ifndef EKF_TERRAIN_HGT_STABLE_SPEED_MAX
# define EKF_TERRAIN_HGT_STABLE_SPEED_MAX 0.5f // terrain height is only considered stable under 0.75m/s. Note this is not related to terrain following
#endif
void Copter::update_ground_effect_detector(void) void Copter::update_ground_effect_detector(void)
{ {
if(!g2.gndeffect_comp_enabled || !motors->armed()) { if(!g2.gndeffect_comp_enabled || !motors->armed()) {
@ -67,3 +71,18 @@ void Copter::update_ground_effect_detector(void)
ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected); ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected); ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
} }
// update ekf terrain height stable setting
// when set to true, this allows the EKF to stabilize the normally barometer based altitude using a rangefinder
// this is not related to terrain following
void Copter::update_ekf_terrain_height_stable()
{
// set to false if no position estimate
if (!position_ok() && !optflow_position_ok()) {
ahrs.set_terrain_hgt_stable(false);
}
// consider terrain height stable if vehicle is taking off or landing and moving less than 0.5 m/s
const bool is_stable = (flightmode->is_taking_off() || flightmode->is_landing()) && (ahrs.groundspeed() <= EKF_TERRAIN_HGT_STABLE_SPEED_MAX);
ahrs.set_terrain_hgt_stable(is_stable);
}