From 41661f815f119b092b67adfa0144f02fe53fd1df Mon Sep 17 00:00:00 2001 From: dgrat Date: Sat, 16 Apr 2016 11:58:46 +0200 Subject: [PATCH] AP_Math: Replace the pythagorous* functions with a variadic template The new function can deal with a variable number of function parameters. Additionally, I renamed the functions to norm(), because this is the standard name used in several other projects. --- APMrover2/APMrover2.cpp | 4 +-- AntennaTracker/tracking.cpp | 2 +- ArduCopter/Attitude.cpp | 4 +-- ArduCopter/control_acro.cpp | 2 +- ArduCopter/control_auto.cpp | 2 +- ArduCopter/control_guided.cpp | 2 +- ArduCopter/crash_check.cpp | 4 +-- ArduCopter/land_detector.cpp | 4 +-- ArduCopter/position_vector.cpp | 2 +- .../AC_AttitudeControl/AC_PosControl.cpp | 10 +++---- libraries/AC_Sprayer/AC_Sprayer.cpp | 2 +- libraries/AC_WPNav/AC_Circle.cpp | 2 +- libraries/AC_WPNav/AC_WPNav.cpp | 10 +++---- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 6 ++-- .../AP_Airspeed/Airspeed_Calibration.cpp | 2 +- libraries/AP_Common/Location.cpp | 2 +- libraries/AP_GPS/AP_GPS.cpp | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- libraries/AP_HAL_SITL/sitl_gps.cpp | 12 ++++---- .../AP_InertialNav/AP_InertialNav_NavEKF.cpp | 2 +- .../AP_InertialSensor/AP_InertialSensor.cpp | 4 +-- libraries/AP_Math/AP_Math.h | 29 +++++++++++++------ libraries/AP_Math/location.cpp | 2 +- libraries/AP_Math/tests/test_math.cpp | 16 +++++++--- libraries/AP_Math/vector2.cpp | 2 +- libraries/AP_Math/vector3.cpp | 2 +- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 3 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 2 +- libraries/AP_NavEKF/AP_NavEKF_core.cpp | 10 +++---- .../AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 2 +- .../AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 2 +- libraries/SITL/SIM_ADSB.cpp | 2 +- 34 files changed, 88 insertions(+), 70 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 4134a101057313..7d21c7a2f7cabf 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -159,7 +159,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 = pythagorous2(velocity.x, velocity.y); + ground_speed = norm(velocity.x, velocity.y); } if (should_log(MASK_LOG_ATTITUDE_FAST)) @@ -388,7 +388,7 @@ void Rover::update_GPS_10Hz(void) } Vector3f velocity; if (ahrs.get_velocity_NED(velocity)) { - ground_speed = pythagorous2(velocity.x, velocity.y); + ground_speed = norm(velocity.x, velocity.y); } else { ground_speed = gps.ground_speed(); } diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index 9342c0fb88138a..39c95acc5ff1d8 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -118,7 +118,7 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) vehicle.location.lng = msg.lon; vehicle.location.alt = msg.alt/10; vehicle.heading = msg.hdg * 0.01f; - vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f; + vehicle.ground_speed = norm(msg.vx, msg.vy) * 0.01f; vehicle.last_update_us = AP_HAL::micros(); vehicle.last_update_ms = AP_HAL::millis(); diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 7edf573e195213..bca814cc9e15e7 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -25,7 +25,7 @@ void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float pitch_in *= scaler; // do circular limit - float total_in = pythagorous2((float)pitch_in, (float)roll_in); + float total_in = norm(pitch_in, roll_in); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; @@ -82,7 +82,7 @@ float Copter::get_roi_yaw() float Copter::get_look_ahead_yaw() { const Vector3f& vel = inertial_nav.get_velocity(); - float speed = pythagorous2(vel.x,vel.y); + float speed = norm(vel.x,vel.y); // 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; diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index e749f9ecf5737b..2c67d8bf456f3c 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -57,7 +57,7 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in Vector3f rate_ef_level, rate_bf_level, rate_bf_request; // apply circular limit to pitch and roll inputs - float total_in = pythagorous2((float)pitch_in, (float)roll_in); + float total_in = norm(pitch_in, roll_in); if (total_in > ROLL_PITCH_INPUT_MAX) { float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 251b79c08a49e4..753b4b48b3b10d 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -488,7 +488,7 @@ void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, f // if we are outside the circle, point at the edge, otherwise hold yaw const Vector3f &curr_pos = inertial_nav.get_position(); - float dist_to_center = pythagorous2(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); + float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } else { diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index f988ab4df87944..1ed0cf0c9d26ea 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -526,7 +526,7 @@ void Copter::guided_angle_control_run() // constrain desired lean angles float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; - float total_in = pythagorous2(roll_in, pitch_in); + float total_in = norm(roll_in, pitch_in); float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 9db73c2c75950a..464cc5cceb5de4 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -34,7 +34,7 @@ void Copter::crash_check() // check for angle error over 30 degrees const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { + if (norm(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { crash_counter = 0; return; } @@ -100,7 +100,7 @@ void Copter::parachute_check() // check for angle error over 30 degrees const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { + if (norm(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { control_loss_count = 0; return; } diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index a478cead8da740..8f7dd2461f53f3 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -132,11 +132,11 @@ void Copter::update_throttle_thr_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 = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f); + bool large_angle_request = (norm(angle_target.x, angle_target.y) > 1500.0f); // check for large external disturbance - angle error over 30 degrees const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f); + bool large_angle_error = (norm(angle_error.x, angle_error.y) > 3000.0f); // check for large acceleration - falling or high turbulence Vector3f accel_ef = ahrs.get_accel_ef_blended(); diff --git a/ArduCopter/position_vector.cpp b/ArduCopter/position_vector.cpp index 4d81b8ea20caaa..c9bf08f4ba65c9 100644 --- a/ArduCopter/position_vector.cpp +++ b/ArduCopter/position_vector.cpp @@ -64,5 +64,5 @@ float Copter::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destinat // pv_get_horizontal_distance_cm - return distance between two positions in cm float Copter::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination) { - return pythagorous2(destination.x-origin.x,destination.y-origin.y); + return norm(destination.x-origin.x,destination.y-origin.y); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index fb4fdfba8be86c..9cfb395ab3b6e1 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -575,7 +575,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const } // calculate current velocity - float vel_total = pythagorous2(curr_vel.x, curr_vel.y); + float vel_total = norm(curr_vel.x, curr_vel.y); // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero if (kP <= 0.0f || _accel_cms <= 0.0f || is_zero(vel_total)) { @@ -794,7 +794,7 @@ void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainSc _pos_error.y = _pos_target.y - curr_pos.y; // constrain target position to within reasonable distance of current location - _distance_to_target = pythagorous2(_pos_error.x, _pos_error.y); + _distance_to_target = norm(_pos_error.x, _pos_error.y); if (_distance_to_target > _leash && _distance_to_target > 0.0f) { _pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target; _pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target; @@ -824,7 +824,7 @@ void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainSc // the event of a disturbance // scale velocity within limit - float vel_total = pythagorous2(_vel_target.x, _vel_target.y); + float vel_total = norm(_vel_target.x, _vel_target.y); if (vel_total > POSCONTROL_VEL_XY_MAX_FROM_POS_ERR) { _vel_target.x = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.x/vel_total; _vel_target.y = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.y/vel_total; @@ -841,7 +841,7 @@ void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainSc } // scale velocity within speed limit - float vel_total = pythagorous2(_vel_target.x, _vel_target.y); + float vel_total = norm(_vel_target.x, _vel_target.y); if (vel_total > _speed_cms) { _vel_target.x = _speed_cms * _vel_target.x/vel_total; _vel_target.y = _speed_cms * _vel_target.y/vel_total; @@ -919,7 +919,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bo } // scale desired acceleration if it's beyond acceptable limit - accel_total = pythagorous2(_accel_target.x, _accel_target.y); + accel_total = norm(_accel_target.x, _accel_target.y); if (accel_total > accel_max && accel_total > 0.0f) { _accel_target.x = accel_max * _accel_target.x/accel_total; _accel_target.y = accel_max * _accel_target.y/accel_total; diff --git a/libraries/AC_Sprayer/AC_Sprayer.cpp b/libraries/AC_Sprayer/AC_Sprayer.cpp index 02aac4b066d1fe..73aebaa0d950df 100644 --- a/libraries/AC_Sprayer/AC_Sprayer.cpp +++ b/libraries/AC_Sprayer/AC_Sprayer.cpp @@ -113,7 +113,7 @@ AC_Sprayer::update() // get horizontal velocity const Vector3f &velocity = _inav->get_velocity(); - ground_speed = pythagorous2(velocity.x,velocity.y); + ground_speed = norm(velocity.x,velocity.y); // get the current time now = AP_HAL::millis(); diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index fc4c7cca86d88c..5a34e6c42d41ae 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -186,7 +186,7 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result) Vector2f vec; // vector from circle center to current location vec.x = (curr_pos.x - _center.x); vec.y = (curr_pos.y - _center.y); - float dist = pythagorous2(vec.x, vec.y); + float dist = norm(vec.x, vec.y); // if current location is exactly at the center of the circle return edge directly behind vehicle if (is_zero(dist)) { diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 657553d50192aa..5050eaf3a1fc12 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -264,7 +264,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel); // constrain and scale the desired acceleration - float des_accel_change_total = pythagorous2(des_accel_diff.x, des_accel_diff.y); + float des_accel_change_total = norm(des_accel_diff.x, des_accel_diff.y); float accel_change_max = _loiter_jerk_max_cmsss * nav_dt; if (_loiter_jerk_max_cmsss > 0.0f && des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) { @@ -591,7 +591,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) track_error = curr_delta - track_covered_pos; // calculate the horizontal error - float track_error_xy = pythagorous2(track_error.x, track_error.y); + float track_error_xy = norm(track_error.x, track_error.y); // calculate the vertical error float track_error_z = fabsf(track_error.z); @@ -711,7 +711,7 @@ float AC_WPNav::get_wp_distance_to_destination() const { // get current location Vector3f curr = _inav.get_position(); - return pythagorous2(_destination.x-curr.x,_destination.y-curr.y); + return norm(_destination.x-curr.x,_destination.y-curr.y); } /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees @@ -777,7 +777,7 @@ void AC_WPNav::check_wp_leash_length() void AC_WPNav::calculate_wp_leash_length() { // length of the unit direction vector in the horizontal - float pos_delta_unit_xy = pythagorous2(_pos_delta_unit.x, _pos_delta_unit.y); + float pos_delta_unit_xy = norm(_pos_delta_unit.x, _pos_delta_unit.y); float pos_delta_unit_z = fabsf(_pos_delta_unit.z); float speed_z; @@ -1071,7 +1071,7 @@ bool AC_WPNav::advance_spline_target_along_track(float dt) track_error.z -= terr_offset; // calculate the horizontal error - float track_error_xy = pythagorous2(track_error.x, track_error.y); + float track_error_xy = norm(track_error.x, track_error.y); // calculate the vertical error float track_error_z = fabsf(track_error.z); diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 0c885f492ed81d..e4fb770adb05cc 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -165,7 +165,7 @@ AP_AHRS_DCM::reset(bool recover_eulers) // normalise the acceleration vector if (initAccVec.length() > 5.0f) { // calculate initial pitch angle - pitch = atan2f(initAccVec.x, pythagorous2(initAccVec.y, initAccVec.z)); + pitch = atan2f(initAccVec.x, norm(initAccVec.y, initAccVec.z)); // calculate initial roll angle roll = atan2f(-initAccVec.y, -initAccVec.z); } else { @@ -354,7 +354,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate) float AP_AHRS_DCM::_yaw_gain(void) const { - float VdotEFmag = pythagorous2(_accel_ef[_active_accel_instance].x, + float VdotEFmag = norm(_accel_ef[_active_accel_instance].x, _accel_ef[_active_accel_instance].y); if (VdotEFmag <= 4.0f) { return 0.2f*(4.5f - VdotEFmag); @@ -774,7 +774,7 @@ AP_AHRS_DCM::drift_correction(float deltat) float earth_error_Z = error.z; // equation 10 - float tilt = pythagorous2(GA_e.x, GA_e.y); + float tilt = norm(GA_e.x, GA_e.y); // equation 11 float theta = atan2f(GA_b[besti].y, GA_b[besti].x); diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 86d882de3d9b25..890ce8ae5e3798 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -53,7 +53,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) // No state prediction required because states are assumed to be time // invariant plus process noise // Ignore vertical wind component - float TAS_pred = state.z * pythagorous3(vg.x - state.x, vg.y - state.y, vg.z); + float TAS_pred = state.z * norm(vg.x - state.x, vg.y - state.y, vg.z); float TAS_mea = airspeed; // Calculate the observation Jacobian H_TAS diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 385a97f6ee02ab..c2c0ec24f60695 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -225,7 +225,7 @@ float Location_Class::get_distance(const struct Location &loc2) const { float dlat = (float)(loc2.lat - lat); float dlng = ((float)(loc2.lng - lng)) * longitude_scale(loc2); - return pythagorous2(dlat, dlng) * LOCATION_SCALING_FACTOR; + return norm(dlat, dlng) * LOCATION_SCALING_FACTOR; } // extrapolate latitude/longitude given distances (in meters) north and east diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index b6c635a1adadf8..5a610534609946 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -478,7 +478,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, istate.location = _location; istate.location.options = 0; istate.velocity = _velocity; - istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y); + istate.ground_speed = norm(istate.velocity.x, istate.velocity.y); istate.ground_course = wrap_360(degrees(atan2f(istate.velocity.y, istate.velocity.x))); istate.hdop = hdop; istate.num_sats = _num_sats; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index b73fe6bfaa0688..20315fa7f2ff89 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -929,7 +929,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 = pythagorous2(state.velocity.y, state.velocity.x); + state.ground_speed = norm(state.velocity.y, state.velocity.x); state.have_speed_accuracy = true; state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; #if UBLOX_FAKE_3DLOCK diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index c219621f8a688f..d728688cae9488 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -279,8 +279,8 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d) velned.ned_north = 100.0f * d->speedN; velned.ned_east = 100.0f * d->speedE; velned.ned_down = 100.0f * d->speedD; - velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100; - velned.speed_3d = pythagorous3(d->speedN, d->speedE, d->speedD) * 100; + velned.speed_2d = norm(d->speedN, d->speedE) * 100; + velned.speed_3d = norm(d->speedN, d->speedE, d->speedD) * 100; velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f; if (velned.heading_2d < 0.0f) { velned.heading_2d += 360.0f * 100000.0f; @@ -361,7 +361,7 @@ void SITL_State::_update_gps_mtk(const struct gps_data *d) p.latitude = d->latitude * 1.0e6; p.longitude = d->longitude * 1.0e6; p.altitude = d->altitude * 100; - p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; + p.ground_speed = norm(d->speedN, d->speedE) * 100; p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0f; if (p.ground_course < 0.0f) { p.ground_course += 360.0f * 1000000.0f; @@ -418,7 +418,7 @@ void SITL_State::_update_gps_mtk16(const struct gps_data *d) p.latitude = d->latitude * 1.0e6; p.longitude = d->longitude * 1.0e6; p.altitude = d->altitude * 100; - p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; + p.ground_speed = norm(d->speedN, d->speedE) * 100; p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; if (p.ground_course < 0.0f) { p.ground_course += 360.0f * 100.0f; @@ -476,7 +476,7 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d) p.latitude = d->latitude * 1.0e7; p.longitude = d->longitude * 1.0e7; p.altitude = d->altitude * 100; - p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100; + p.ground_speed = norm(d->speedN, d->speedE) * 100; p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; if (p.ground_course < 0.0f) { p.ground_course += 360.0f * 100.0f; @@ -584,7 +584,7 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d) d->have_lock?_sitl->gps_numsats:3, 2.0, d->altitude); - float speed_knots = pythagorous2(d->speedN, d->speedE)*1.94384449f; + float speed_knots = norm(d->speedN, d->speedE)*1.94384449f; float heading = ToDeg(atan2f(d->speedE, d->speedN)); if (heading < 0) { heading += 360.0f; diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index c91697c2e2cebb..a8f551bdbb2ef6 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -118,7 +118,7 @@ const Vector3f &AP_InertialNav_NavEKF::get_velocity() const */ float AP_InertialNav_NavEKF::get_velocity_xy() const { - return pythagorous2(_velocity_cm.x, _velocity_cm.y); + return norm(_velocity_cm.x, _velocity_cm.y); } /** diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 0011bd4b8298e2..92f92b5dd0ff29 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -590,7 +590,7 @@ AP_InertialSensor::detect_backends(void) */ bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch) { - trim_pitch = atan2f(accel_sample.x, pythagorous2(accel_sample.y, accel_sample.z)); + trim_pitch = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z)); trim_roll = atan2f(-accel_sample.y, -accel_sample.z); if (fabsf(trim_roll) > radians(10) || fabsf(trim_pitch) > radians(10)) { @@ -1381,7 +1381,7 @@ void AP_InertialSensor::_acal_save_calibrations() // The first level step of accel cal will be taken as gnd truth, // i.e. trim will be set as per the output of primary accel from the level step get_primary_accel_cal_sample_avg(0,aligned_sample); - _trim_pitch = atan2f(aligned_sample.x, pythagorous2(aligned_sample.y, aligned_sample.z)); + _trim_pitch = atan2f(aligned_sample.x, norm(aligned_sample.y, aligned_sample.z)); _trim_roll = atan2f(-aligned_sample.y, -aligned_sample.z); _new_trim = true; break; diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 96dce69340e881..bd9afb924f905f 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -127,19 +127,30 @@ static inline float degrees(float rad) { return rad * RAD_TO_DEG; } -// square -static inline float sq(float v) { - return v*v; +template +float sq(const T val) +{ + return powf(static_cast(val), 2); } -// 2D vector length -static inline float pythagorous2(float a, float b) { - return sqrtf(sq(a)+sq(b)); +/* + * Variadic template for calculating the square norm of a vector of any + * dimension. + */ +template +float sq(const T first, const Params... parameters) +{ + return sq(first) + sq(parameters...); } -// 3D vector length -static inline float pythagorous3(float a, float b, float c) { - return sqrtf(sq(a)+sq(b)+sq(c)); +/* + * Variadic template for calculating the norm (pythagoras) of a vector of any + * dimension. + */ +template +float norm(const T first, const Params... parameters) +{ + return sqrt(static_cast(sq(first, parameters...))); } template diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index e15e2c1f76319f..d73b2b00263e8c 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -60,7 +60,7 @@ float get_distance(const struct Location &loc1, const struct Location &loc2) { float dlat = (float)(loc2.lat - loc1.lat); float dlong = ((float)(loc2.lng - loc1.lng)) * longitude_scale(loc2); - return pythagorous2(dlat, dlong) * LOCATION_SCALING_FACTOR; + return norm(dlat, dlong) * LOCATION_SCALING_FACTOR; } // return distance in centimeters to between two locations diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index eac8c9dcfaa1d3..59f48206e21901 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/libraries/AP_Math/tests/test_math.cpp @@ -103,11 +103,19 @@ TEST(MathTest, Square) TEST(MathTest, Norm) { - float norm_5 = pythagorous2(3, 4); - float norm_6 = pythagorous3(4, 3, 12); + float norm_1 = norm(1); + float norm_2 = norm(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1); + float norm_3 = norm(0); + float norm_4 = norm(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0); + float norm_5 = norm(3,4); + float norm_6 = norm(4,3,12); - EXPECT_EQ(5.f, norm_5); - EXPECT_EQ(13.f, norm_6); + EXPECT_EQ(norm_1, 1.f); + EXPECT_EQ(norm_2, 4.f); + EXPECT_EQ(norm_3, 0.f); + EXPECT_EQ(norm_4, 0.f); + EXPECT_EQ(norm_5, 5.f); + EXPECT_EQ(norm_6, 13.f); } diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp index 456d666c411370..c2679b232bb4ad 100644 --- a/libraries/AP_Math/vector2.cpp +++ b/libraries/AP_Math/vector2.cpp @@ -24,7 +24,7 @@ template float Vector2::length(void) const { - return pythagorous2(x, y); + return norm(x, y); } diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 7255f1af05765d..b67cbf59adbb1f 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -273,7 +273,7 @@ T Vector3::operator *(const Vector3 &v) const template float Vector3::length(void) const { - return pythagorous3(x, y, z); + return norm(x, y, z); } template diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 00d9294688eef5..a76c057a098eec 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -368,8 +368,7 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float // across the input range instead of stopping when the input hits the constrain value // these calculations are based on an assumption of the user specified cyclic_max // coming into this equation at 4500 or less - - float total_out = pythagorous2(pitch_out, roll_out); + float total_out = norm(pitch_out, roll_out); if (total_out > (_cyclic_max/4500.0f)) { float ratio = (float)(_cyclic_max/4500.0f) / total_out; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 80b4215f9dcf9f..393ff62a617c32 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -139,7 +139,7 @@ void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f; float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f; float GPS_vector_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter). - float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. + float target_distance = 100.0f*norm(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. // initialise all angles to zero angles_to_target_rad.zero(); diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.cpp b/libraries/AP_NavEKF/AP_NavEKF_core.cpp index 24ba679d662b49..f33193e0578f0b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_core.cpp @@ -879,7 +879,7 @@ void NavEKF_core::UpdateStrapdownEquationsNED() // calculate a magnitude of the filtered nav acceleration (required for GPS // variance estimation) accNavMag = velDotNEDfilt.length(); - accNavMagHoriz = pythagorous2(velDotNEDfilt.x , velDotNEDfilt.y); + accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y); // save velocity for use in trapezoidal intergration for position calcuation Vector3f lastVelocity = state.velocity; @@ -2897,7 +2897,7 @@ void NavEKF_core::FuseAirspeed() vwe = statesAtVtasMeasTime.wind_vel.y; // calculate the predicted airspeed - VtasPred = pythagorous3((ve - vwe) , (vn - vwn) , vd); + VtasPred = norm((ve - vwe) , (vn - vwn) , vd); // perform fusion of True Airspeed measurement if (VtasPred > 1.0f) { @@ -4321,7 +4321,7 @@ void NavEKF_core::alignYawGPS() // representative of typical launch wind void NavEKF_core::setWindVelStates() { - float gndSpd = pythagorous2(state.velocity.x, state.velocity.y); + float gndSpd = norm(state.velocity.x, state.velocity.y); if (gndSpd > 4.0f) { // set the wind states to be the reciprocal of the velocity and scale float scaleFactor = STARTUP_WIND_SPEED / gndSpd; @@ -5058,7 +5058,7 @@ bool NavEKF_core::calcGpsGoodToAlign(void) // Check that the horizontal GPS vertical velocity is reasonable after noise filtering bool gpsHorizVelFail; if (!vehicleArmed) { - gpsHorizVelFilt = 0.1f * pythagorous2(velNED.x,velNED.y) + 0.9f * gpsHorizVelFilt; + gpsHorizVelFilt = 0.1f * norm(velNED.x,velNED.y) + 0.9f * gpsHorizVelFilt; gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_HORIZ_SPD); } else { @@ -5207,7 +5207,7 @@ void NavEKF_core::alignMagStateDeclination() // rotate the NE values so that the declination matches the published value Vector3f initMagNED = state.earth_magfield; - float magLengthNE = pythagorous2(initMagNED.x,initMagNED.y); + float magLengthNE = norm(initMagNED.x,initMagNED.y); state.earth_magfield.x = magLengthNE * cosf(magDecAng); state.earth_magfield.y = magLengthNE * sinf(magDecAng); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 7baf643b25005c..c03ca2a1f31ad6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -55,7 +55,7 @@ void NavEKF2_core::FuseAirspeed() vwe = stateStruct.wind_vel.y; // calculate the predicted airspeed - VtasPred = pythagorous3((ve - vwe) , (vn - vwn) , vd); + VtasPred = norm((ve - vwe) , (vn - vwn) , vd); // perform fusion of True Airspeed measurement if (VtasPred > 1.0f) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index a1e2e59a0d42f9..5a05f544759bc6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -949,7 +949,7 @@ void NavEKF2_core::alignMagStateDeclination() // rotate the NE values so that the declination matches the published value Vector3f initMagNED = stateStruct.earth_magfield; - float magLengthNE = pythagorous2(initMagNED.x,initMagNED.y); + float magLengthNE = norm(initMagNED.x,initMagNED.y); stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng); stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index f22e9a892c93fa..31c97a55292ef0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -104,7 +104,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) // This check can only be used if the vehicle is stationary bool gpsHorizVelFail; if (onGround) { - gpsHorizVelFilt = 0.1f * pythagorous2(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; + gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; gpsHorizVelFilt = constrain_float(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 ccb84eef591275..47ca6b2778f66b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -506,7 +506,7 @@ void NavEKF2_core::UpdateStrapdownEquationsNED() // calculate a magnitude of the filtered nav acceleration (required for GPS // variance estimation) accNavMag = velDotNEDfilt.length(); - accNavMagHoriz = pythagorous2(velDotNEDfilt.x , velDotNEDfilt.y); + accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y); // save velocity for use in trapezoidal intergration for position calcuation Vector3f lastVelocity = stateStruct.velocity; diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp index b0c07d19aaefb8..ce5d61a155506e 100644 --- a/libraries/SITL/SIM_ADSB.cpp +++ b/libraries/SITL/SIM_ADSB.cpp @@ -180,7 +180,7 @@ void ADSB::send_report(void) adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; adsb_vehicle.altitude = -vehicle.position.z * 1000; adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x))); - adsb_vehicle.hor_velocity = pythagorous2(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100; + adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100; adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100; memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign)); adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE;