aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: style change in BCN mesurements

Browse Source
This commit is contained in:
Pierre Kancir 2018-02-09 18:55:18 +01:00 committed by Randy Mackay
parent 4d2f2a2aad
commit 48c5a9b9c5
1 changed files with 4 additions and 4 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

@ -780,8 +780,8 @@ 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) {
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
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