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

AP_NavEKF3: improve variable name

Browse Source
This commit is contained in:
priseborough 2016-12-13 11:25:09 +11:00 committed by Randy Mackay
parent 32b60432b4
commit c7e1828238
4 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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;

16
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -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;
}

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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;

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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)