Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues 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 // 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 // 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); const float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; const float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
if (posDiffSq < 9.0f * posDiffVar) { if (posDiffSq < 9.0f * posDiffVar) {
rngBcnGoodToAlign = true; rngBcnGoodToAlign = true;
// Set the EKF origin and magnetic field declination if not previously set // Set the EKF origin and magnetic field declination if not previously set