Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: publish 3-state range beacon estimator states

This commit is contained in:
priseborough 2016-12-17 21:00:33 +11:00 committed by Randy Mackay
parent 7c53573a0c
commit 92b8c33b19
4 changed files with 24 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1085,11 +1085,12 @@ void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
}
// return data for debugging range beacon fusion
bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED)
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow);
return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow, posNED);
} else {
return false;
}

10
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -201,17 +201,21 @@ public:
void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr);
/*
Returns the following data for debugging range beacon fusion from the specified instance
An out of range instance (eg -1) returns data for the the primary instance
Returns the following data for debugging range beacon fusion
ID : beacon identifier
rng : measured range to beacon (m)
innov : range innovation (m)
innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m)
offsetHigh : high hypothesis for range beacons system vertical offset (m)
offsetLow : low hypothesis for range beacons system vertical offset (m)
posNED : North,East,Down position estimate of receiver from 3-state filter
returns true if data could be found, false if it could not
*/
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect

11
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -70,7 +70,8 @@ void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
}
// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow)
bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED)
{
// if the states have not been initialised or we have not received any beacon updates then return zeros
if (!statesInitialised || N_beacons == 0) {
@ -88,10 +89,12 @@ bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov; // range innovation (m)
innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2)
testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio
beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon NED position
offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate
offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate
beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon receiver NED position (m)
offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate (m)
offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate (m)
posNED = receiverPos; // beacon system NED offset (m)
rngBcnFuseDataReportIndex++;
printf("%6.2f\n",(double)receiverPos.x);
return true;
}

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -210,8 +210,14 @@ public:
innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m)
offsetHigh : high hypothesis for range beacons system vertical offset (m)
offsetLow : low hypothesis for range beacons system vertical offset (m)
posNED : North,East,Down position estimate of receiver from 3-state filter
returns true if data could be found, false if it could not
*/
bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow);
bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
float &offsetHigh, float &offsetLow, Vector3f &posNED);
// called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect