Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Nav_EKF: getFilterStatus returns nav_filter_status struct

Browse Source
This commit is contained in:
Randy Mackay 2015-01-02 17:07:42 +09:00 committed by Andrew Tridgell
parent 8a914af4a8
commit f4d8bc586c
2 changed files with 16 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4483,23 +4483,26 @@ return filter function status as a bitmasked integer
6 = terrain height estimate valid
7 = constant position mode
*/
void NavEKF::getFilterStatus(uint8_t &status) const
void NavEKF::getFilterStatus(nav_filter_status &status) const
{
// add code to set bits using private filter data here
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool notDeadReckoning = !constVelMode && !constPosMode;
bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
status = (!state.quat.is_nan()<<0 | // attitude valid (we need a better check)
(someHorizRefData && notDeadReckoning)<<1 | // horizontal velocity estimate valid
someVertRefData<<2 | // vertical velocity estimate valid
((doingFlowNav || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning)<<3 | // relative horizontal position estimate valid
(doingNormalGpsNav && notDeadReckoning)<<4 | // absolute horizontal position estimate valid
!hgtTimeout<<5 | // vertical position estimate valid
gndOffsetValid<<6 | // terrain height estimate valid
constPosMode<<7); // constant position mode
bool optFlowNavPossible = flowDataValid && (_fusionModeGPS == 3);
bool gpsNavPossible = !gpsNotAvailable && (_fusionModeGPS <= 2);
// add code to set bits using private filter data here
status.flags.attitude = !state.quat.is_nan(); // attitude valid (we need a better check)
status.flags.horiz_vel = someHorizRefData && notDeadReckoning; // horizontal velocity estimate valid
status.flags.vert_vel = someVertRefData; // vertical velocity estimate valid
status.flags.horiz_pos_rel = (doingFlowNav || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning; // relative horizontal position estimate valid
status.flags.horiz_pos_abs = doingNormalGpsNav && notDeadReckoning; // absolute horizontal position estimate valid
status.flags.vert_pos = !hgtTimeout; // vertical position estimate valid
status.flags.terrain_alt = gndOffsetValid; // terrain height estimate valid
status.flags.const_pos_mode = constPosMode; // constant position mode
}
// Check arm status and perform required checks and mode changes

13
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -27,6 +27,7 @@
#include <AP_Airspeed.h>
#include <AP_Compass.h>
#include <AP_Param.h>
#include <AP_Nav_Common.h>
// #define MATH_CHECK_INDEXES 1
@ -200,17 +201,9 @@ public:
void getFilterTimeouts(uint8_t &timeouts) const;
/*
return filter function status as a bitmasked integer
0 = attitude estimate valid
1 = horizontal velocity estimate valid
2 = vertical velocity estimate valid
3 = relative horizontal position estimate valid
4 = absolute horizontal position estimate valid
5 = vertical position estimate valid
6 = terrain height estimate valid
7 = unassigned
return filter status flags
*/
void getFilterStatus(uint8_t &status) const;
void getFilterStatus(nav_filter_status &status) const;
static const struct AP_Param::GroupInfo var_info[];