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

AP_NavEKF2: use vector.xy().length() instead of norm(x,y)

Browse Source
This commit is contained in:
Josh Henderson 2021-09-11 06:33:42 -04:00 committed by Andrew Tridgell
parent f76f86c207
commit d5f5a3024a
6 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -169,7 +169,7 @@ void NavEKF2_core::Log_Write_NKF5(uint64_t time_us) const
normInnov : (uint8_t)(MIN(100*MAX(flowTestRatio[0],flowTestRatio[1]),255)), // normalised innovation variance ratio for optical flow observations fused by the main nav filter
FIX : (int16_t)(1000*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter
FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter
AFI : (int16_t)(1000*norm(auxFlowObsInnov.x,auxFlowObsInnov.y)), // optical flow LOS rate innovation from terrain offset estimator
AFI : (int16_t)(1000 * auxFlowObsInnov.length()), // optical flow LOS rate innovation from terrain offset estimator
HAGL : (int16_t)(100*(terrainState - stateStruct.position.z)), // height above ground level
offset : (int16_t)(100*terrainState), // // estimated vertical position of the terrain relative to the nav filter zero datum
RI : (int16_t)(100*innovRng), // range finder innovations

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -1124,7 +1124,7 @@ void NavEKF2_core::alignMagStateDeclination()
// rotate the NE values so that the declination matches the published value
Vector3F initMagNED = stateStruct.earth_magfield;
ftype magLengthNE = norm(initMagNED.x,initMagNED.y);
ftype magLengthNE = initMagNED.xy().length();
stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng);
stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng);

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -735,7 +735,7 @@ void NavEKF2_core::correctEkfOriginHeight()
} else if (activeHgtSource == HGT_SOURCE_RNG) {
// use the worse case expected terrain gradient and vehicle horizontal speed
const ftype maxTerrGrad = 0.25f;
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
ekfOriginHgtVar += sq(maxTerrGrad * stateStruct.velocity.xy().length() * deltaTime);
} else {
// by definition our height source is absolute so cannot run this filter
return;

2
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -983,7 +983,7 @@ void NavEKF2_core::selectHeightForFusion()
// If the terrain height is consistent and we are moving slowly, then it can be
// used as a height reference in combination with a range finder
// apply a hysteresis to the speed check to prevent rapid switching
ftype horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
ftype horizSpeed = stateStruct.velocity.xy().length();
bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
ftype trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;

2
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -105,7 +105,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
// This check can only be used if the vehicle is stationary
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = 0.1f * gpsDataDelayed.vel.xy().length() + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else {

2
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -676,7 +676,7 @@ void NavEKF2_core::UpdateStrapdownEquationsNED()
// calculate a magnitude of the filtered nav acceleration (required for GPS
// variance estimation)
accNavMag = velDotNEDfilt.length();
accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y);
accNavMagHoriz = velDotNEDfilt.xy().length();
// if we are not aiding, then limit the horizontal magnitude of acceleration
// to prevent large manoeuvre transients disturbing the attitude