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

AP_NavEKF3: publish 3-state range beacon estimator states

Browse Source
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 // 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 (instance < 0 || instance >= num_cores) instance = primary;
if (core) { 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 { } else {
return false; 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); 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 Returns the following data for debugging range beacon fusion
An out of range instance (eg -1) returns data for the the primary instance
ID : beacon identifier ID : beacon identifier
rng : measured range to beacon (m) rng : measured range to beacon (m)
innov : range innovation (m) innov : range innovation (m)
innovVar : innovation variance (m^2) innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m) 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 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 // called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect // 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 // 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 the states have not been initialised or we have not received any beacon updates then return zeros
if (!statesInitialised || N_beacons == 0) { 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) innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov; // range innovation (m)
innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2) innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2)
testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio
beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon NED position beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon receiver NED position (m)
offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate (m)
offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate (m)
posNED = receiverPos; // beacon system NED offset (m)
rngBcnFuseDataReportIndex++; rngBcnFuseDataReportIndex++;
printf("%6.2f\n",(double)receiverPos.x);
return true; return true;
} }

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -210,8 +210,14 @@ public:
innovVar : innovation variance (m^2) innovVar : innovation variance (m^2)
testRatio : innovation consistency test ratio testRatio : innovation consistency test ratio
beaconPosNED : beacon NED position (m) 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 // called by vehicle code to specify that a takeoff is happening
// causes the EKF to compensate for expected barometer errors due to ground effect // causes the EKF to compensate for expected barometer errors due to ground effect