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

@@ -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();
         }

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

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;

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;

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 {

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;

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

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

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

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;

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

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

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

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

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

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

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;

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

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;

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);
 }
 
 /**

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;

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<class T>
+float sq(const T val)
+{
+    return powf(static_cast<float>(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<class T, class... Params>
+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<class T, class... Params>
+float norm(const T first, const Params... parameters)
+{
+    return sqrt(static_cast<float>(sq(first, parameters...)));
 }
 
 template<typename A, typename B>