diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 84727c6727..3ad4bc88e8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -318,7 +318,7 @@ void NavEKF3_core::setAidingMode() if (canUseRangeBeacon) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y); - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosDownOffset); } // reset the last fusion accepted times to prevent unwanted activation of timeout logic lastPosPassTime_ms = imuSampleTime_ms; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index e696d4b9d8..fbc6b6483d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -53,7 +53,7 @@ void NavEKF3_core::FuseRngBcn() // calculate the vertical offset from EKF datum to beacon datum CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false); } else { - bcnPosOffset = 0.0f; + bcnPosDownOffset = 0.0f; } // copy required states to local variable names @@ -62,7 +62,7 @@ void NavEKF3_core::FuseRngBcn() pd = stateStruct.position.z; bcn_pn = rngBcnDataDelayed.beacon_posNED.x; bcn_pe = rngBcnDataDelayed.beacon_posNED.y; - bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset; + bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosDownOffset; // predicted range Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; @@ -313,7 +313,7 @@ void NavEKF3_core::FuseRngBcnStatic() } else { // we are using the beacons as the primary height source, so don't modify their vertical position - bcnPosOffset = 0.0f; + bcnPosDownOffset = 0.0f; } } else { @@ -344,7 +344,7 @@ void NavEKF3_core::FuseRngBcnStatic() bcnPosDownOffsetMin = stateStruct.position.z - receverPosDownMax; } else { // We are using the beacons as the primary height reference, so don't modify their vertical position - bcnPosOffset = 0.0f; + bcnPosDownOffset = 0.0f; } } @@ -354,7 +354,7 @@ void NavEKF3_core::FuseRngBcnStatic() } // calculate the observation jacobian - float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset; + float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosDownOffset; float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y; float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x; float t5 = t2*t2; @@ -405,7 +405,7 @@ void NavEKF3_core::FuseRngBcnStatic() // calculate range measurement innovation Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED; - deltaPosNED.z -= bcnPosOffset; + deltaPosNED.z -= bcnPosDownOffset; innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng; // update the state @@ -571,10 +571,10 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP // calculate the innovation for the main filter using the offset with the smallest innovation history // apply hysteresis to prevent rapid switching if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) { - bcnPosOffset = bcnPosDownOffsetMin; + bcnPosDownOffset = bcnPosDownOffsetMin; usingMinHypothesis = true; } else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) { - bcnPosOffset = bcnPosDownOffsetMax; + bcnPosDownOffset = bcnPosDownOffsetMax; usingMinHypothesis = false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8471d286d6..0136e58d83 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -294,7 +294,7 @@ void NavEKF3_core::InitialiseVariables() N_beacons = 0; maxBcnPosD = 0.0f; minBcnPosD = 0.0f; - bcnPosOffset = 0.0f; + bcnPosDownOffset = 0.0f; bcnPosDownOffsetMax = 0.0f; bcnPosOffsetMaxVar = 0.0f; OffsetMaxInnovFilt = 0.0f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index c993b1a98b..4c28e0ae63 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1011,7 +1011,7 @@ private: uint8_t N_beacons; // Number of range beacons in use float maxBcnPosD; // maximum position of all beacons in the down direction (m) float minBcnPosD; // minimum position of all beacons in the down direction (m) - float bcnPosOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + float bcnPosDownOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) bool usingMinHypothesis; // true when the min beacob constellatio offset hyopthesis is being used float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)