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

AP_NavEKF3: readRngBcnData gets minor formatting fix

This commit is contained in:
Randy Mackay 2020-08-19 11:24:45 +09:00
parent 384029d278
commit b86e16e927
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -882,14 +882,14 @@ void NavEKF3_core::readRngBcnData()
}
// Check if the range beacon data can be used to align the vehicle position
if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) {
if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) && (beaconVehiclePosErr < 1.0f) && rngBcnAlignmentCompleted) {
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
const float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
const float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
if (posDiffSq < 9.0f * posDiffVar) {
rngBcnGoodToAlign = true;
// Set the EKF origin and magnetic field declination if not previously set
if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) {
if (!validOrigin && (PV_AidingMode != AID_ABSOLUTE)) {
// get origin from beacon system
Location origin_loc;
if (beacon->get_origin(origin_loc)) {