From cf2b65877e265274a6705e209c2af1244d767330 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 29 May 2023 18:42:08 -0600 Subject: [PATCH] AP_Math: Move conversion utilites next to AP_Math * This is next to the constraining functions Signed-off-by: Ryan Friedman --- libraries/AP_Math/AP_Math.cpp | 37 +++++++++++++++++++++++++++++++++++ libraries/AP_Math/AP_Math.h | 16 ++++++++++++++- 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index ec4b6912df..a333cac5cb 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -526,3 +526,40 @@ int32_t double_to_int32(const double v) { return int32_t(constrain_double(v, INT32_MIN, UINT32_MAX)); } + + +int32_t float_to_int32_le(const float& value) +{ + int32_t out; + static_assert(sizeof(value) == sizeof(out)); + + // Use memcpy because it's the most portable. + // It might not be the fastest way on all hardware. + // At least it's defined behavior in both c and c++. + memcpy(&out, &value, sizeof(out)); + return out; +} + +float int32_to_float_le(const uint32_t& value) +{ + float out; + static_assert(sizeof(value) == sizeof(out)); + + // Use memcpy because it's the most portable. + // It might not be the fastest way on all hardware. + // At least it's defined behavior in both c and c++. + memcpy(&out, &value, sizeof(out)); + return out; +} + +double uint64_to_double_le(const uint64_t& value) +{ + double out; + static_assert(sizeof(value) == sizeof(out)); + + // Use memcpy because it's the most portable. + // It might not be the fastest way on all hardware. + // At least it's defined behavior in both c and c++. + memcpy(&out, &value, sizeof(out)); + return out; +} diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index b3d550a290..f513565eed 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -366,7 +366,7 @@ float fixedwing_turn_rate(float bank_angle_deg, float airspeed); float degF_to_Kelvin(float temp_f); /* - conversion functions to prevent undefined behaviour + constraining conversion functions to prevent undefined behaviour */ int16_t float_to_int16(const float v); uint16_t float_to_uint16(const float v); @@ -375,3 +375,17 @@ uint32_t float_to_uint32(const float v); uint32_t double_to_uint32(const double v); int32_t double_to_int32(const double v); +/* + Convert from float to int32_t without breaking Wstrict-aliasing due to type punning +*/ +int32_t float_to_int32_le(const float& value) WARN_IF_UNUSED; + +/* + Convert from uint32_t to float without breaking Wstrict-aliasing due to type punning +*/ +float int32_to_float_le(const uint32_t& value) WARN_IF_UNUSED; + +/* + Convert from uint64_t to double without breaking Wstrict-aliasing due to type punning +*/ +double uint64_to_double_le(const uint64_t& value) WARN_IF_UNUSED;