Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Replace wrap_* functions with template versions

Browse Source
This commit is contained in:
dgrat 2016-04-27 11:35:18 +02:00 committed by Lucas De Marchi
parent 49cfd6fd9b
commit 76362caee0
12 changed files with 161 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/test.cpp
View File

@ -402,7 +402,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
if (compass.healthy()) {
const Vector3f mag_ofs = compass.get_offsets();
const Vector3f mag = compass.get_field();
cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
(wrap_360_cd(ToDeg(heading) * 100)) /100,
(double)mag.x, (double)mag.y, (double)mag.z,
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);

3
AntennaTracker/Log.cpp
View File

@ -11,9 +11,8 @@ void Tracker::Log_Write_Attitude()
{
Vector3f targets;
targets.y = nav_status.pitch * 100.0f;
targets.z = wrap_360_cd_float(nav_status.bearing * 100.0f);
targets.z = wrap_360_cd(nav_status.bearing * 100.0f);
DataFlash.Log_Write_Attitude(ahrs, targets);
DataFlash.Log_Write_EKF(ahrs,false);
DataFlash.Log_Write_AHRS2(ahrs);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

2
AntennaTracker/servos.cpp
View File

@ -332,7 +332,7 @@ void Tracker::update_yaw_onoff_servo(float yaw)
void Tracker::update_yaw_cr_servo(float yaw)
{
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
float yaw_cd = wrap_180_cd_float(yaw*100.0f);
float yaw_cd = wrap_180_cd(yaw*100.0f);
float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);
channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid(err_cd));

2
ArduCopter/Log.cpp
View File

@ -372,7 +372,7 @@ void Copter::Log_Write_Performance()
void Copter::Log_Write_Attitude()
{
Vector3f targets = attitude_control.get_att_target_euler_cd();
targets.z = wrap_360_cd_float(targets.z);
targets.z = wrap_360_cd(targets.z);
DataFlash.Log_Write_Attitude(ahrs, targets);
#if OPTFLOW == ENABLED

2
ArduCopter/control_autotune.cpp
View File

@ -447,7 +447,7 @@ void Copter::autotune_attitude_control()
break;
case AUTOTUNE_AXIS_YAW:
// request pitch to 20deg
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd_float(direction_sign * autotune_target_angle + autotune_start_angle), false);
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false);
break;
}
} else {

4
ArduCopter/control_guided.cpp
View File

@ -247,7 +247,7 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd_float(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
@ -535,7 +535,7 @@ void Copter::guided_angle_control_run()
}
// wrap yaw request
float yaw_in = wrap_180_cd_float(guided_angle_state.yaw_cd);
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabs(wp_nav.get_speed_down()), wp_nav.get_speed_up());

2
ArduPlane/test.cpp
View File

@ -442,7 +442,7 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
if (compass.healthy()) {
const Vector3f &mag_ofs = compass.get_offsets();
const Vector3f &mag = compass.get_field();
cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
(wrap_360_cd(ToDeg(heading) * 100)) /100,
(double)mag.x, (double)mag.y, (double)mag.z,
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);

2
Tools/Replay/Replay.cpp
View File

@ -872,7 +872,7 @@ void Replay::log_check_solution(void)
float roll_error = degrees(fabsf(euler.x - check_state.euler.x));
float pitch_error = degrees(fabsf(euler.y - check_state.euler.y));
float yaw_error = wrap_180_cd_float(100*degrees(fabsf(euler.z - check_state.euler.z)))*0.01f;
float yaw_error = wrap_180_cd(100*degrees(fabsf(euler.z - check_state.euler.z)))*0.01f;
float vel_error = (velocity - check_state.velocity).length();
float pos_error = get_distance(check_state.pos, loc);

88
libraries/AP_Math/AP_Math.cpp
View File

@ -49,3 +49,91 @@ float linear_interpolate(float low_output, float high_output,
return low_output + p * (high_output - low_output);
}
template <class T>
float wrap_180(const T angle, float unit_mod)
{
const float ang_180 = 180.f * unit_mod;
const float ang_360 = 360.f * unit_mod;
float res = fmod(static_cast<float>(angle) + ang_180, ang_360);
if (res < 0 || is_zero(res)) {
res += ang_180;
} else {
res -= ang_180;
}
return res;
}
template float wrap_180<int>(const int angle, float unit_mod);
template float wrap_180<short>(const short angle, float unit_mod);
template float wrap_180<float>(const float angle, float unit_mod);
template float wrap_180<double>(const double angle, float unit_mod);
template <class T>
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
{
return wrap_180(angle, 100.f);
}
template auto wrap_180_cd<float>(const float angle) -> decltype(wrap_180(angle, 100.f));
template auto wrap_180_cd<int>(const int angle) -> decltype(wrap_180(angle, 100.f));
template auto wrap_180_cd<short>(const short angle) -> decltype(wrap_180(angle, 100.f));
template auto wrap_180_cd<double>(const double angle) -> decltype(wrap_360(angle, 100.f));
template <class T>
float wrap_360(const T angle, float unit_mod)
{
const float ang_360 = 360.f * unit_mod;
float res = fmodf(static_cast<float>(angle), ang_360);
if (res < 0) {
res += ang_360;
}
return res;
}
template float wrap_360<int>(const int angle, float unit_mod);
template float wrap_360<short>(const short angle, float unit_mod);
template float wrap_360<float>(const float angle, float unit_mod);
template float wrap_360<double>(const double angle, float unit_mod);
template <class T>
auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f))
{
return wrap_360(angle, 100.f);
}
template auto wrap_360_cd<float>(const float angle) -> decltype(wrap_360(angle, 100.f));
template auto wrap_360_cd<int>(const int angle) -> decltype(wrap_360(angle, 100.f));
template auto wrap_360_cd<short>(const short angle) -> decltype(wrap_360(angle, 100.f));
template auto wrap_360_cd<double>(const double angle) -> decltype(wrap_360(angle, 100.f));
template <class T>
float wrap_PI(const T radian)
{
float res = fmod(static_cast<float>(radian) + M_PI, M_2PI);
if (res < 0 || is_zero(res)) {
res += M_PI;
} else {
res -= M_PI;
}
return res;
}
template float wrap_PI<int>(const int radian);
template float wrap_PI<short>(const short radian);
template float wrap_PI<float>(const float radian);
template float wrap_PI<double>(const double radian);
template <class T>
float wrap_2PI(const T radian)
{
float res = fmodf(static_cast<float>(radian), M_2PI);
if (res < 0) {
res += M_2PI;
}
return res;
}
template float wrap_2PI<int>(const int radian);
template float wrap_2PI<short>(const short radian);
template float wrap_2PI<float>(const float radian);
template float wrap_2PI<double>(const double radian);

43
libraries/AP_Math/AP_Math.h
View File

@ -52,25 +52,45 @@ bool inverse4x4(float m[],float invOut[]);
float* mat_mul(float *A, float *B, uint8_t n);
/*
wrap an angle in centi-degrees
* Constrain an angle to be within the range: -180 to 180 degrees. The second
* parameter changes the units. Default: 1 == degrees, 10 == dezi,
* 100 == centi.
*/
int32_t wrap_360_cd(int32_t error);
int32_t wrap_180_cd(int32_t error);
float wrap_360_cd_float(float angle);
float wrap_180_cd_float(float angle);
float wrap_360(float angle);
template <class T>
float wrap_180(const T angle, float unit_mod = 1);
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
* Wrap an angle in centi-degrees. See wrap_180().
*/
float wrap_PI(float angle_in_radians);
template <class T>
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f));
/*
wrap an angle defined in radians to the interval [0,2*PI)
* Constrain an euler angle to be within the range: 0 to 360 degrees. The
* second parameter changes the units. Default: 1 == degrees, 10 == dezi,
* 100 == centi.
*/
float wrap_2PI(float angle);
template <class T>
float wrap_360(const T angle, float unit_mod = 1);
/*
* Wrap an angle in centi-degrees. See wrap_360().
*/
template <class T>
auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f));
/*
wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
template <class T>
float wrap_PI(const T radian);
/*
* wrap an angle in radians to 0..2PI
*/
template <class T>
float wrap_2PI(const T radian);
// constrain a value
// constrain a value
static inline float constrain_float(float amt, float low, float high)
{
@ -83,6 +103,7 @@ static inline float constrain_float(float amt, float low, float high)
}
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
// constrain a int16_t value
static inline int16_t constrain_int16(int16_t amt, int16_t low, int16_t high) {
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));

98
libraries/AP_Math/location.cpp
View File

@ -149,104 +149,6 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
(loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1));
}
/*
wrap an angle in centi-degrees to 0..35999
*/
int32_t wrap_360_cd(int32_t error)
{
if (error > 360000 || error < -360000) {
// for very large numbers use modulus
error = error % 36000;
}
while (error >= 36000) error -= 36000;
while (error < 0) error += 36000;
return error;
}
/*
wrap an angle in centi-degrees to -18000..18000
*/
int32_t wrap_180_cd(int32_t error)
{
if (error > 360000 || error < -360000) {
// for very large numbers use modulus
error = error % 36000;
}
while (error > 18000) { error -= 36000; }
while (error < -18000) { error += 36000; }
return error;
}
/*
wrap an angle in centi-degrees to 0..35999
*/
float wrap_360_cd_float(float angle)
{
if (angle >= 72000.0f || angle < -36000.0f) {
// for larger number use fmodulus
angle = fmod(angle, 36000.0f);
}
if (angle >= 36000.0f) angle -= 36000.0f;
if (angle < 0.0f) angle += 36000.0f;
return angle;
}
/*
wrap an angle in centi-degrees to -18000..18000
*/
float wrap_180_cd_float(float angle)
{
if (angle > 54000.0f || angle < -54000.0f) {
// for large numbers use modulus
angle = fmod(angle,36000.0f);
}
if (angle > 18000.0f) { angle -= 36000.0f; }
if (angle < -18000.0f) { angle += 36000.0f; }
return angle;
}
/*
wrap an angle in degrees to 0..360
*/
float wrap_360(float angle)
{
if (angle >= 720.0f || angle < -360.0f) {
// for larger number use fmodulus
angle = fmod(angle, 360.0f);
}
if (angle >= 360.0f) angle -= 360.0f;
if (angle < 0.0f) angle += 360.0f;
return angle;
}
/*
wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
*/
float wrap_PI(float angle_in_radians)
{
if (angle_in_radians > 10*M_PI || angle_in_radians < -10*M_PI) {
// for very large numbers use modulus
angle_in_radians = fmodf(angle_in_radians, 2*M_PI);
}
while (angle_in_radians > M_PI) angle_in_radians -= 2*M_PI;
while (angle_in_radians < -M_PI) angle_in_radians += 2*M_PI;
return angle_in_radians;
}
/*
* wrap an angle in radians to 0..2PI
*/
float wrap_2PI(float angle)
{
if (angle > 10*M_PI || angle < -10*M_PI) {
// for very large numbers use modulus
angle = fmodf(angle, 2*M_PI);
}
while (angle > 2*M_PI) angle -= 2*M_PI;
while (angle < 0) angle += 2*M_PI;
return angle;
}
/*
return true if lat and lng match. Ignores altitude and options
*/

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

@ -183,24 +183,24 @@ TEST(MathWrapTest, Angle180)
}
}
EXPECT_EQ(4500.f, wrap_180_cd_float(4500.f));
EXPECT_EQ(9000.f, wrap_180_cd_float(9000.f));
EXPECT_EQ(18000.f, wrap_180_cd_float(18000.f));
EXPECT_EQ(-17990.f, wrap_180_cd_float(18010.f));
EXPECT_EQ(-9000.f, wrap_180_cd_float(27000.f));
EXPECT_EQ(0.f, wrap_180_cd_float(36000.f));
EXPECT_EQ(0.f, wrap_180_cd_float(72000.f));
EXPECT_EQ(0.f, wrap_180_cd_float(360000.f));
EXPECT_EQ(0.f, wrap_180_cd_float(720000.f));
EXPECT_EQ(0.f, wrap_180_cd_float(-3600000000.f));
EXPECT_EQ(4500.f, wrap_180_cd(4500.f));
EXPECT_EQ(9000.f, wrap_180_cd(9000.f));
EXPECT_EQ(18000.f, wrap_180_cd(18000.f));
EXPECT_EQ(-17990.f, wrap_180_cd(18010.f));
EXPECT_EQ(-9000.f, wrap_180_cd(27000.f));
EXPECT_EQ(0.f, wrap_180_cd(36000.f));
EXPECT_EQ(0.f, wrap_180_cd(72000.f));
EXPECT_EQ(0.f, wrap_180_cd(360000.f));
EXPECT_EQ(0.f, wrap_180_cd(720000.f));
EXPECT_EQ(0.f, wrap_180_cd(-3600000000.f));
EXPECT_EQ(-4500.f, wrap_180_cd_float(-4500.f));
EXPECT_EQ(-9000.f, wrap_180_cd_float(-9000.f));
EXPECT_EQ(18000.f, wrap_180_cd_float(-18000.f));
EXPECT_EQ(17990.f, wrap_180_cd_float(-18010.f));
EXPECT_EQ(9000.f, wrap_180_cd_float(-27000.f));
EXPECT_EQ(0.f, wrap_180_cd_float(-36000.f));
EXPECT_EQ(0.f, wrap_180_cd_float(-72000.f));
EXPECT_EQ(-4500.f, wrap_180_cd(-4500.f));
EXPECT_EQ(-9000.f, wrap_180_cd(-9000.f));
EXPECT_EQ(18000.f, wrap_180_cd(-18000.f));
EXPECT_EQ(17990.f, wrap_180_cd(-18010.f));
EXPECT_EQ(9000.f, wrap_180_cd(-27000.f));
EXPECT_EQ(0.f, wrap_180_cd(-36000.f));
EXPECT_EQ(0.f, wrap_180_cd(-72000.f));
}
TEST(MathWrapTest, Angle360)
@ -222,22 +222,22 @@ TEST(MathWrapTest, Angle360)
}
}
EXPECT_EQ(4500.f, wrap_360_cd_float(4500.f));
EXPECT_EQ(9000.f, wrap_360_cd_float(9000.f));
EXPECT_EQ(18000.f, wrap_360_cd_float(18000.f));
EXPECT_EQ(27000.f, wrap_360_cd_float(27000.f));
EXPECT_EQ(0.f, wrap_360_cd_float(36000.f));
EXPECT_EQ(0.f, wrap_360_cd_float(72000.f));
EXPECT_EQ(0.f, wrap_360_cd_float(360000.f));
EXPECT_EQ(0.f, wrap_360_cd_float(720000.f));
EXPECT_EQ( 0.f, wrap_360_cd_float(-3600000000.f));
EXPECT_EQ(4500.f, wrap_360_cd(4500.f));
EXPECT_EQ(9000.f, wrap_360_cd(9000.f));
EXPECT_EQ(18000.f, wrap_360_cd(18000.f));
EXPECT_EQ(27000.f, wrap_360_cd(27000.f));
EXPECT_EQ(0.f, wrap_360_cd(36000.f));
EXPECT_EQ(0.f, wrap_360_cd(72000.f));
EXPECT_EQ(0.f, wrap_360_cd(360000.f));
EXPECT_EQ(0.f, wrap_360_cd(720000.f));
EXPECT_EQ( 0.f, wrap_360_cd(-3600000000.f));
EXPECT_EQ(31500.f, wrap_360_cd_float(-4500.f));
EXPECT_EQ(27000.f, wrap_360_cd_float(-9000.f));
EXPECT_EQ(18000.f, wrap_360_cd_float(-18000.f));
EXPECT_EQ(9000.f, wrap_360_cd_float(-27000.f));
EXPECT_EQ(0.f, wrap_360_cd_float(-36000.f));
EXPECT_EQ(0.f, wrap_360_cd_float(-72000.f));
EXPECT_EQ(31500.f, wrap_360_cd(-4500.f));
EXPECT_EQ(27000.f, wrap_360_cd(-9000.f));
EXPECT_EQ(18000.f, wrap_360_cd(-18000.f));
EXPECT_EQ(9000.f, wrap_360_cd(-27000.f));
EXPECT_EQ(0.f, wrap_360_cd(-36000.f));
EXPECT_EQ(0.f, wrap_360_cd(-72000.f));
}
AP_GTEST_MAIN()