aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: use enum class for takeoff/touchdown states

Browse Source 
Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
This commit is contained in:
Andrew Tridgell 2020-11-06 10:34:17 +11:00
parent 31fbb59384
commit 9b35bfba55
2 changed files with 60 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -428,6 +428,8 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const
return _accel_ef_ekf_blended;
}
#include <stdio.h>
void AP_AHRS_NavEKF::reset(bool recover_eulers)
{
// support locked access functions to AHRS data
@ -1995,6 +1997,22 @@ bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const
// this is not related to terrain following
void AP_AHRS_NavEKF::set_terrain_hgt_stable(bool stable)
{
switch (terrainHgtStableState) {
case TriState::UNKNOWN:
break;
case TriState::True:
if (stable) {
return;
}
break;
case TriState::False:
if (!stable) {
return;
}
break;
}
terrainHgtStableState = (TriState)stable;
#if HAL_NAVEKF2_AVAILABLE
EKF2.setTerrainHgtStable(stable);
#endif
@ -2113,6 +2131,22 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar,
void AP_AHRS_NavEKF::setTakeoffExpected(bool val)
{
switch (takeoffExpectedState) {
case TriState::UNKNOWN:
break;
case TriState::True:
if (val) {
return;
}
break;
case TriState::False:
if (!val) {
return;
}
break;
}
takeoffExpectedState = (TriState)val;
#if HAL_NAVEKF2_AVAILABLE
EKF2.setTakeoffExpected(val);
#endif
@ -2123,6 +2157,22 @@ void AP_AHRS_NavEKF::setTakeoffExpected(bool val)
void AP_AHRS_NavEKF::setTouchdownExpected(bool val)
{
switch (touchdownExpectedState) {
case TriState::UNKNOWN:
break;
case TriState::True:
if (val) {
return;
}
break;
case TriState::False:
if (!val) {
return;
}
break;
}
touchdownExpectedState = (TriState)val;
#if HAL_NAVEKF2_AVAILABLE
EKF2.setTouchdownExpected(val);
#endif

10
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -364,6 +364,16 @@ private:
// get the index of the current primary IMU
uint8_t get_primary_IMU_index(void) const;
// avoid setting current state repeatedly across all cores on all EKFs:
enum class TriState {
False = 0,
True = 1,
UNKNOWN = 3,
};
TriState touchdownExpectedState = TriState::UNKNOWN;
TriState takeoffExpectedState = TriState::UNKNOWN;
TriState terrainHgtStableState = TriState::UNKNOWN;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL *_sitl;
uint32_t _last_body_odm_update_ms;