From 80ce6feb19ddf321e4d3ae98270dce5e644b295d Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:25:53 -0400 Subject: [PATCH 01/15] Copter: use vector.xy().length() instead of norm(x,y) --- ArduCopter/autoyaw.cpp | 2 +- ArduCopter/land_detector.cpp | 2 +- ArduCopter/mode.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index ae34ebc46be27..6646abb76cd78 100644 --- a/ArduCopter/autoyaw.cpp +++ b/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; diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 644ee52932f2f..65069b69b0ba3 100644 --- a/ArduCopter/land_detector.cpp +++ b/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(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 1e18b0ab8d503..7e86ae028b119 100644 --- a/ArduCopter/mode.cpp +++ b/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; From 1c6ddb3a69fc13699151b2c92c932f99093f4b14 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:26:49 -0400 Subject: [PATCH 02/15] Quadplane: use vector.xy().length() instead of norm(x,y) --- ArduPlane/quadplane.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 51c52302a6b6b..608d2516f6ffb 100644 --- a/ArduPlane/quadplane.cpp +++ b/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(); From f87faa6a3efabdb8ac1fa4ae841e75153545d315 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:27:19 -0400 Subject: [PATCH 03/15] Sub: use vector.xy().length() instead of norm(x,y) --- ArduSub/Attitude.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 311ecd5662e20..a93c194bd95b7 100644 --- a/ArduSub/Attitude.cpp +++ b/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; From 787c0d84a6aa0fb5f53550e59eb3de8c4eb6ad73 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:28:01 -0400 Subject: [PATCH 04/15] AC_AttitudeControl: use vector.xy().length() instead of norm(x,y) --- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index c4b6adb74db21..be1a01351f971 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/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() From 76ae03964add5bf0edf6b6af792436a24ff40432 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:28:25 -0400 Subject: [PATCH 05/15] AC_Avoid: use vector.xy().length() instead of norm(x,y) --- libraries/AC_Avoidance/AC_Avoid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 399594f9630d8..efb2b87087dbe 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/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); } From e0da06265702e2444f84f43ea2ae42734c56dad0 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:28:53 -0400 Subject: [PATCH 06/15] AC_Sprayer: use vector.xy().length() instead of norm(x,y) --- libraries/AC_Sprayer/AC_Sprayer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AC_Sprayer/AC_Sprayer.cpp b/libraries/AC_Sprayer/AC_Sprayer.cpp index 9b678135b5c86..6c9a9ca2ff7cd 100644 --- a/libraries/AC_Sprayer/AC_Sprayer.cpp +++ b/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(); From a029e192c7b92c0f3e1da39dded6a9753c54546b Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:29:48 -0400 Subject: [PATCH 07/15] AP_AHRS: use vector.xy().length() instead of norm(x,y) --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index b3cf473e52651..ae44096c4bfdb 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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); From 430f206f611b028687d5b874691e4f7b14a34dab Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:31:17 -0400 Subject: [PATCH 08/15] AP_GPS: use vector.xy().length() instead of norm(x,y) --- libraries/AP_GPS/AP_GPS.cpp | 2 +- libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp | 2 +- libraries/AP_GPS/AP_GPS_MAV.cpp | 2 +- libraries/AP_GPS/AP_GPS_MSP.cpp | 2 +- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 4 ++-- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- 6 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 26a3538efa3b4..62487f9a69075 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp index 928eb1942ca7a..bd9c910c79a95 100644 --- a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index 2e49e81be625d..62cc13da760e6 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/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) { diff --git a/libraries/AP_GPS/AP_GPS_MSP.cpp b/libraries/AP_GPS/AP_GPS_MSP.cpp index 16ce23a5de054..c3a807fc0c55f 100644 --- a/libraries/AP_GPS/AP_GPS_MSP.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index e7c4fcdd95f4c..a04771cf5d1ae 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/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 { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 420c65fdcbf4c..6845696c093a0 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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 From 9f02c731b07cfdce16190a31f92cc820993d92f5 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:32:32 -0400 Subject: [PATCH 09/15] AP_InertialNav: use vector.xy().length() instead of norm(x,y) --- libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index 93a066aa107ee..99ea36a9cbb6f 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/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(); } /** From 07bf9a46df7b9382856fd9ac08f3a85aa66f1921 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:33:42 -0400 Subject: [PATCH 10/15] AP_NavEKF2: use vector.xy().length() instead of norm(x,y) --- libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp index 684d22ca4c3c4..76bc29cbfaf3a 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 2552a20230398..9db5aaa2c828b 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 24d51709a07ff..0dd728669ed44 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 9190c65574ac5..5e44dfa4a5848 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 23121106d593b..d8162503fc109 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 d04b0f531364c..94765ea2ddea4 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 From 0ec5c13c3c1d746d851bb65c8e222392c5b21cd6 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:34:45 -0400 Subject: [PATCH 11/15] AP_NavEKF3: use vector.xy().length() instead of norm(x,y) --- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 513c51ae58ec9..000e076a5feea 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 1ad1708180749..118424d866537 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 5ab6d5a7940f4..26baff311573b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index c60afbc072cac..081e40e21fe19 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index d927486a53d21..f00f558f566c8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/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 { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index dd68ff3cc10ee..0cb1cfe0b9c16 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/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 From 2c869b44d4a5ce4adafc8e75a6250b895483c1e9 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:35:11 -0400 Subject: [PATCH 12/15] AP_NMEA_Output: use vector.xy().length() instead of norm(x,y) --- libraries/AP_NMEA_Output/AP_NMEA_Output.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp index 7a491f56e6e06..972f3c25d960f 100644 --- a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp +++ b/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 From 08d52b26219401e31a55b0433819ee4f42baa44f Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:35:38 -0400 Subject: [PATCH 13/15] AP_Proximity: use vector.xy().length() instead of norm(x,y) --- libraries/AP_Proximity/AP_Proximity_MAV.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index 2936a315aee69..babe855f0625f 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/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); From d4e57673af886cfdd1efa2e8cb3b0c3952275c6e Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:35:52 -0400 Subject: [PATCH 14/15] Rover: use vector.xy().length() instead of norm(x,y) --- Rover/Rover.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 48e2142bcdb4c..bb4366e140024 100644 --- a/Rover/Rover.cpp +++ b/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(); } From 6e75841226f50d6dc40bfc99b2e1ef239d16668e Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 22:46:18 -0400 Subject: [PATCH 15/15] AP_ADSB: sagetech use vector.xy().length() instead of norm(x,y) --- libraries/AP_ADSB/AP_ADSB_Sagetech.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp index b196a5474f5b4..b11a36abbbf7e 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp +++ b/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