diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index 1c24b947ed..98ac8c203a 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -487,3 +487,26 @@ float degF_to_Kelvin(float temp_f)
 {
     return (temp_f + 459.67) * 0.55556;
 }
+
+/*
+  conversion functions to prevent undefined behaviour
+ */
+int16_t float_to_int16(const float v)
+{
+    return int16_t(constrain_float(v, INT16_MIN, INT16_MAX));
+}
+
+int32_t float_to_int32(const float v)
+{
+    return int32_t(constrain_float(v, INT32_MIN, INT32_MAX));
+}
+
+uint16_t float_to_uint16(const float v)
+{
+    return uint16_t(constrain_float(v, 0, UINT16_MAX));
+}
+
+uint32_t float_to_uint32(const float v)
+{
+    return uint32_t(constrain_float(v, 0, UINT32_MAX));
+}
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 81b837ecd4..1f439f57c3 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -360,3 +360,11 @@ float fixedwing_turn_rate(float bank_angle_deg, float airspeed);
 // convert degrees farenheight to Kelvin
 float degF_to_Kelvin(float temp_f);
 
+/*
+  conversion functions to prevent undefined behaviour
+ */
+int16_t float_to_int16(const float v);
+uint16_t float_to_uint16(const float v);
+int32_t float_to_int32(const float v);
+uint32_t float_to_uint32(const float v);
+