Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Replace the pythagorous* functions with a variadic template

Browse Source 
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.
This commit is contained in:
dgrat 2016-04-16 11:58:46 +02:00 committed by Lucas De Marchi
parent 880f130670
commit 41661f815f
34 changed files with 88 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/APMrover2.cpp
View File

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

2
AntennaTracker/tracking.cpp
View File

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

4
ArduCopter/Attitude.cpp
View File

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

2
ArduCopter/control_acro.cpp
View File

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

2
ArduCopter/control_auto.cpp
View File

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

2
ArduCopter/control_guided.cpp
View File

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

4
ArduCopter/crash_check.cpp
View File

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

4
ArduCopter/land_detector.cpp
View File

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

2
ArduCopter/position_vector.cpp
View File

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

10
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

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

2
libraries/AC_Sprayer/AC_Sprayer.cpp
View File

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

2
libraries/AC_WPNav/AC_Circle.cpp
View File

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

10
libraries/AC_WPNav/AC_WPNav.cpp
View File

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

6
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

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

2
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -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

2
libraries/AP_Common/Location.cpp
View File

@ -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

2
libraries/AP_GPS/AP_GPS.cpp
View File

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

2
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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

12
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

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

2
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

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

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

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

29
libraries/AP_Math/AP_Math.h
View File

@ -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>

2
libraries/AP_Math/location.cpp
View File

@ -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

16
libraries/AP_Math/tests/test_math.cpp
View File

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

2
libraries/AP_Math/vector2.cpp
View File

@ -24,7 +24,7 @@
template <typename T>
float Vector2<T>::length(void) const
{
return pythagorous2(x, y);
return norm(x, y);
}

2
libraries/AP_Math/vector3.cpp
View File

@ -273,7 +273,7 @@ T Vector3<T>::operator *(const Vector3<T> &v) const
template <typename T>
float Vector3<T>::length(void) const
{
return pythagorous3(x, y, z);
return norm(x, y, z);
}
template <typename T>

3
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

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

2
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

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

10
libraries/AP_NavEKF/AP_NavEKF_core.cpp
View File

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

2
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

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

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

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

2
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

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

2
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

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

2
libraries/SITL/SIM_ADSB.cpp
View File

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