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 is_equal with a type safe template function

Browse Source 
It makes sense to consider also other floating point types.
This commit is contained in:
dgrat 2016-04-16 11:59:20 +02:00 committed by Lucas De Marchi
parent 503867b7dc
commit 6d3b491c02
6 changed files with 22 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -98,7 +98,7 @@ void AC_Circle::init()
/// set_circle_rate - set circle rate in degrees per second
void AC_Circle::set_rate(float deg_per_sec)
{
if (!is_equal(deg_per_sec,_rate)) {
if (!is_equal(deg_per_sec, _rate.get())) {
_rate = deg_per_sec;
calc_velocities(false);
}

2
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -792,7 +792,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (AP_AHRS_DCM::use_compass()) {
if (have_gps() && is_equal(gps_gain,1.0f)) {
if (have_gps() && is_equal(gps_gain.get(), 1.0f)) {
error[besti].z *= sinf(fabsf(roll));
} else {
error[besti].z = 0;

2
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -227,7 +227,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
// compute integral error component to converge to a crosstrack of zero when traveling
// straight but reset it when disabled or if it changes. That allows for much easier
// tuning by having it re-converge each time it changes.
if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain, _L1_xtrack_i_gain_prev)) {
if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
_L1_xtrack_i = 0;
_L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
} else if (fabsf(Nu1) < radians(5)) {

13
libraries/AP_Math/AP_Math.cpp
View File

@ -1,6 +1,19 @@
#include "AP_Math.h"
#include <float.h>
template <class FloatOne, class FloatTwo>
bool is_equal(const FloatOne v_1, const FloatTwo v_2)
{
static_assert(std::is_arithmetic<FloatOne>::value, "template parameter not of type float or int");
static_assert(std::is_arithmetic<FloatTwo>::value, "template parameter not of type float or int");
return fabsf(v_1 - v_2) < std::numeric_limits<decltype(v_1 - v_2)>::epsilon();
}
template bool is_equal<int>(const int v_1, const int v_2);
template bool is_equal<short>(const short v_1, const short v_2);
template bool is_equal<float>(const float v_1, const float v_2);
template bool is_equal<double>(const double v_1, const double v_2);
template <class T>
float safe_asin(const T v)
{

7
libraries/AP_Math/AP_Math.h
View File

@ -24,8 +24,11 @@
// define AP_Param types AP_Vector3f and Ap_Matrix3f
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
// are two floats equal
static inline bool is_equal(const float fVal1, const float fVal2) { return fabsf(fVal1 - fVal2) < FLT_EPSILON ? true : false; }
/*
* Check whether two floats are equal
*/
template <class FloatOne, class FloatTwo>
bool is_equal(const FloatOne, const FloatTwo);
// is a float is zero
static inline bool is_zero(const float fVal1) { return fabsf(fVal1) < FLT_EPSILON ? true : false; }

2
libraries/AP_Mount/SoloGimbal_Parameters.cpp
View File

@ -190,7 +190,7 @@ void SoloGimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavli
break;
case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
if (i == GMB_PARAM_GMB_FLASH) {
if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && is_equal(packet.param_value,1)) {
if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && (int)packet.param_value == 1) {
_flashing_step = GMB_PARAM_NOT_FLASHING;
}
_params[i].value = 0;