diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp index 684d22ca4c..76bc29cbfa 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 2552a20230..9db5aaa2c8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -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); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 24d51709a0..0dd728669e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 9190c65574..5e44dfa4a5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 23121106d5..d8162503fc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -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 { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index d04b0f5313..94765ea2dd 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -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