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

AP_InertialNav: use ekf::get_vert_pos_rate during high vibration

Browse Source
This commit is contained in:
Randy Mackay 2019-04-01 15:55:58 +09:00
parent 5baed38266
commit adf4d8fd47
3 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -25,10 +25,10 @@ public:
AP_InertialNav() {} AP_InertialNav() {}
/** /**
* update - updates velocity and position estimates using latest info from accelerometers * updates velocity and position estimates pulling data from EKF
* augmented with gps and baro readings * high_vibes should be set to true if high vibration have been detected
*/ */
virtual void update(void) = 0; virtual void update(bool high_vibes = false) = 0;
/** /**
* get_filter_status : returns filter status as a series of flags * get_filter_status : returns filter status as a series of flags

10
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

@ -1,4 +1,5 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Baro/AP_Baro.h>
#include "AP_InertialNav.h" #include "AP_InertialNav.h"
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
@ -12,7 +13,7 @@
/** /**
update internal state update internal state
*/ */
void AP_InertialNav_NavEKF::update() void AP_InertialNav_NavEKF::update(bool high_vibes)
{ {
// get the NE position relative to the local earth frame origin // get the NE position relative to the local earth frame origin
Vector2f posNE; Vector2f posNE;
@ -30,6 +31,13 @@ void AP_InertialNav_NavEKF::update()
// get the velocity relative to the local earth frame // get the velocity relative to the local earth frame
Vector3f velNED; Vector3f velNED;
if (_ahrs_ekf.get_velocity_NED(velNED)) { if (_ahrs_ekf.get_velocity_NED(velNED)) {
// during high vibration events use vertical position change
if (high_vibes) {
float rate_z;
if (_ahrs_ekf.get_vert_pos_rate(rate_z)) {
velNED.z = rate_z;
}
}
_velocity_cm = velNED * 100; // convert to cm/s _velocity_cm = velNED * 100; // convert to cm/s
_velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
} }

2
libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
View File

@ -19,7 +19,7 @@ public:
/** /**
update internal state update internal state
*/ */
void update() override; void update(bool high_vibes = false) override;
/** /**
* get_filter_status - returns filter status as a series of flags * get_filter_status - returns filter status as a series of flags