Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Log line numbers on constrain_nan's for constrain float

Browse Source
This commit is contained in:
Michael du Breuil 2020-10-09 11:54:45 -07:00 committed by Andrew Tridgell
parent 2681d3afe2
commit 051640e2b2
2 changed files with 32 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Math/AP_Math.cpp
View File

@ -259,6 +259,30 @@ template float wrap_2PI<short>(const short radian);
template float wrap_2PI<float>(const float radian);
template float wrap_2PI<double>(const double radian);
template <typename T>
T constrain_value_line(const T amt, const T low, const T high, uint32_t line)
{
// the check for NaN as a float prevents propagation of floating point
// errors through any function that uses constrain_value(). The normal
// float semantics already handle -Inf and +Inf
if (isnan(amt)) {
AP::internalerror().error(AP_InternalError::error_t::constraining_nan, line);
return (low + high) / 2;
}
if (amt < low) {
return low;
}
if (amt > high) {
return high;
}
return amt;
}
template float constrain_value_line<float>(const float amt, const float low, const float high, uint32_t line);
template <typename T>
T constrain_value(const T amt, const T low, const T high)
{

12
libraries/AP_Math/AP_Math.h
View File

@ -144,10 +144,14 @@ float wrap_2PI(const T radian);
template <typename T>
T constrain_value(const T amt, const T low, const T high);
inline float constrain_float(const float amt, const float low, const float high)
{
return constrain_value(amt, low, high);
}
template <typename T>
T constrain_value_line(const T amt, const T low, const T high, uint32_t line);
#if BOARD_FLASH_SIZE > 1024
#define constrain_float(amt, low, high) constrain_value_line(float(amt), float(low), float(high), uint32_t(__LINE__))
#else
#define constrain_float(amt, low, high) constrain_value(float(amt), float(low), float(high))
#endif
inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
{