Use vel.xy().length() instead of norm(vel.x,vel.y)

ArduCopter/autoyaw.cpp

@@ -11,7 +11,7 @@ float Mode::AutoYaw::roi_yaw() const
 float Mode::AutoYaw::look_ahead_yaw()
 {
     const Vector3f& vel = copter.inertial_nav.get_velocity();
-    float speed = norm(vel.x,vel.y);
+    float speed = vel.xy().length();
     // Commanded Yaw to automatically look ahead.
     if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
         _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;

ArduCopter/land_detector.cpp

@@ -177,7 +177,7 @@ void Copter::update_throttle_mix()
 
         // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
         const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
-        bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);
+        bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
 
         // check for large external disturbance - angle error over 30 degrees
         const float angle_error = attitude_control->get_att_error_angle_deg();

ArduCopter/mode.cpp

@@ -674,10 +674,10 @@ void Mode::land_run_horizontal_control()
         // there is any position estimate drift after touchdown. We
         // limit attitude to 7 degrees below this limit and linearly
         // interpolate for 1m above that
-        float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(),
+        const float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(),
                                                      g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);
-        float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f;
-        float thrust_vector_mag = norm(thrust_vector.x, thrust_vector.y);
+        const float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f;
+        const float thrust_vector_mag = thrust_vector.xy().length();
         if (thrust_vector_mag > thrust_vector_max) {
             float ratio = thrust_vector_max / thrust_vector_mag;
             thrust_vector.x *= ratio;

ArduPlane/quadplane.cpp

@@ -3521,7 +3521,7 @@ void QuadPlane::update_throttle_mix(void)
 
         // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
         const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
-        bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);
+        bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
 
         // check for large external disturbance - angle error over 30 degrees
         const float angle_error = attitude_control->get_att_error_angle_deg();

ArduSub/Attitude.cpp

@@ -73,7 +73,7 @@ float Sub::get_roi_yaw()
 float Sub::get_look_ahead_yaw()
 {
     const Vector3f& vel = inertial_nav.get_velocity();
-    float speed = norm(vel.x,vel.y);
+    const float speed = vel.xy().length();
     // Commanded Yaw to automatically look ahead.
     if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
         yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;

Rover/Rover.cpp

@@ -256,7 +256,7 @@ void Rover::ahrs_update()
     // if using the EKF get a speed update now (from accelerometers)
     Vector3f velocity;
     if (ahrs.get_velocity_NED(velocity)) {
-        ground_speed = norm(velocity.x, velocity.y);
+        ground_speed = velocity.xy().length();
     } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         ground_speed = ahrs.groundspeed();
     }

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -312,7 +312,7 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
 
     // add desired target to yaw
     _ang_vel_body.z += _ang_vel_target.z;
-    _thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);
+    _thrust_error_angle = _att_error_rot_vec_rad.xy().length();
 }
 
 void AC_AttitudeControl_Heli::integrate_bf_rate_error_to_angle_errors()

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -1174,7 +1174,7 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
             if (breach_dist > deadzone) {
                 // this vector will help us decide how much we have to back away horizontally and vertically
                 const Vector3f margin_vector = vector_to_obstacle.normalized() * breach_dist;
-                const float xy_back_dist = norm(margin_vector.x, margin_vector.y);
+                const float xy_back_dist = margin_vector.xy().length();
                 const float z_back_dist = margin_vector.z;
                 calc_backup_velocity_3D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, xy_back_dist, vector_to_obstacle, kP_z, accel_cmss_z, z_back_dist, min_back_vel_z, max_back_vel_z, dt);
             }

libraries/AC_Sprayer/AC_Sprayer.cpp

@@ -132,7 +132,8 @@ void AC_Sprayer::update()
         // velocity will already be zero but this avoids a coverity warning
         velocity.zero();
     }
-    float ground_speed = norm(velocity.x * 100.0f, velocity.y * 100.0f);
+
+    float ground_speed = velocity.xy().length() * 100.0;
 
     // get the current time
     const uint32_t now = AP_HAL::millis();

libraries/AP_ADSB/AP_ADSB_Sagetech.cpp

@@ -500,7 +500,7 @@ void AP_ADSB_Sagetech::send_msg_GPS()
 
     // ground speed
     const Vector2f speed = AP::ahrs().groundspeed_vector();
-    float speed_knots = norm(speed.x, speed.y) * M_PER_SEC_TO_KNOTS;
+    float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
     snprintf((char*)&pkt.payload[21], 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
 
     // heading

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -383,8 +383,8 @@ AP_AHRS_DCM::_P_gain(float spin_rate)
 float
 AP_AHRS_DCM::_yaw_gain(void) const
 {
-    const float VdotEFmag = norm(_accel_ef[_active_accel_instance].x,
-                                   _accel_ef[_active_accel_instance].y);
+    const float VdotEFmag = _accel_ef[_active_accel_instance].xy().length();
+
     if (VdotEFmag <= 4.0f) {
         return 0.2f*(4.5f - VdotEFmag);
     }
@@ -850,7 +850,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     const float earth_error_Z = error.z;
 
     // equation 10
-    const float tilt = norm(GA_e.x, GA_e.y);
+    const float tilt = GA_e.xy().length();
 
     // equation 11
     const float theta = atan2f(GA_b[besti].y, GA_b[besti].x);

libraries/AP_GPS/AP_GPS.cpp

@@ -1839,7 +1839,7 @@ void AP_GPS::calc_blended_state(void)
     state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm;
 
     // Calculate ground speed and course from blended velocity vector
-    state[GPS_BLENDED_INSTANCE].ground_speed = norm(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.y);
+    state[GPS_BLENDED_INSTANCE].ground_speed = state[GPS_BLENDED_INSTANCE].velocity.xy().length();
     state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x)));
 
     // If the GPS week is the same then use a blended time_week_ms

libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp

@@ -68,7 +68,7 @@ void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_messag
     state.velocity.z = pkt.ned_vel_down;
 
     state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
-    state.ground_speed = norm(state.velocity.y, state.velocity.x);
+    state.ground_speed = state.velocity.xy().length();
 
     state.have_speed_accuracy = true;
     state.have_horizontal_accuracy = true;

libraries/AP_GPS/AP_GPS_MAV.cpp

@@ -86,7 +86,7 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
 
                 state.velocity = vel;
                 state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
-                state.ground_speed = norm(vel.x, vel.y);
+                state.ground_speed = vel.xy().length();
             }
 
             if (have_sa) {

libraries/AP_GPS/AP_GPS_MSP.cpp

@@ -66,7 +66,7 @@ void AP_GPS_MSP::handle_msp(const MSP::msp_gps_data_message_t &pkt)
     state.velocity = vel;
 
     state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
-    state.ground_speed = norm(state.velocity.y, state.velocity.x);
+    state.ground_speed = state.velocity.xy().length();
 
     state.have_speed_accuracy = true;
     state.have_horizontal_accuracy = true;

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -394,7 +394,7 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
         if (!uavcan::isNaN(cb.msg->ned_velocity[0])) {
             Vector3f vel(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]);
             interim_state.velocity = vel;
-            interim_state.ground_speed = norm(vel.x, vel.y);
+            interim_state.ground_speed = vel.xy().length();
             interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
             interim_state.have_vertical_velocity = true;
         } else {
@@ -518,7 +518,7 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
         if (!uavcan::isNaN(cb.msg->ned_velocity[0])) {
             Vector3f vel(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]);
             interim_state.velocity = vel;
-            interim_state.ground_speed = norm(vel.x, vel.y);
+            interim_state.ground_speed = vel.xy().length();
             interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
             interim_state.have_vertical_velocity = true;
         } else {

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1499,7 +1499,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.velocity.y = _buffer.velned.ned_east * 0.01f;
         state.velocity.z = _buffer.velned.ned_down * 0.01f;
         state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
-        state.ground_speed = norm(state.velocity.y, state.velocity.x);
+        state.ground_speed = state.velocity.xy().length();
         state.have_speed_accuracy = true;
         state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
 #if UBLOX_FAKE_3DLOCK

libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp

@@ -81,7 +81,7 @@ const Vector3f &AP_InertialNav_NavEKF::get_velocity() const
  */
 float AP_InertialNav_NavEKF::get_speed_xy() const
 {
-    return norm(_velocity_cm.x, _velocity_cm.y);
+    return _velocity_cm.xy().length();
 }
 
 /**

libraries/AP_NMEA_Output/AP_NMEA_Output.cpp

@@ -138,7 +138,7 @@ void AP_NMEA_Output::update()
 
     // get speed
     Vector2f speed = ahrs.groundspeed_vector();
-    float speed_knots = norm(speed.x, speed.y) * M_PER_SEC_TO_KNOTS;
+    float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
     float heading = wrap_360(degrees(atan2f(speed.x, speed.y)));
 
     // format RMC message

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

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

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;

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;

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 {

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

libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

@@ -189,7 +189,7 @@ void NavEKF3_core::Log_Write_XKF5(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*flowInnov[0]),  // optical flow LOS rate vector innovations from the main nav filter
         FIY : (int16_t)(1000*flowInnov[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),           // filter ground offset state error
         RI : (int16_t)(100*innovRng),                   // range finder innovations

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -1427,7 +1427,7 @@ void NavEKF3_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);
 

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -789,7 +789,7 @@ void NavEKF3_core::correctEkfOriginHeight()
     } else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
         // use the worse case expected terrain gradient and vehicle horizontal speed
         const ftype maxTerrGrad = 0.25;
-        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;

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -1081,7 +1081,7 @@ void NavEKF3_core::selectHeightForFusion()
         bool dontTrustTerrain, trustTerrain;
         if (filterStatus.flags.horiz_vel) {
             // We can use the velocity estimate
-            ftype horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
+            ftype horizSpeed = stateStruct.velocity.xy().length();
             dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable;
             ftype trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
             trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -101,7 +101,7 @@ void NavEKF3_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 {

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -786,7 +786,7 @@ void NavEKF3_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

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -259,7 +259,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
 
     // extract yaw and pitch from Obstacle Vector
     const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
-    const float pitch = wrap_180(degrees(M_PI_2 - atan2f(norm(obstacle.x, obstacle.y), obstacle.z))); 
+    const float pitch = wrap_180(degrees(M_PI_2 - atan2f(obstacle.xy().length(), obstacle.z))); 
 
     // allot to correct layer and sector based on calculated pitch and yaw
     const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw);