diff --git a/libraries/AP_Common/AP_Param.cpp b/libraries/AP_Common/AP_Param.cpp index a6bdf8c707..440349b718 100644 --- a/libraries/AP_Common/AP_Param.cpp +++ b/libraries/AP_Common/AP_Param.cpp @@ -650,3 +650,21 @@ AP_Param *AP_Param::next_scalar(uint32_t *token, enum ap_var_type *ptype) } return ap; } + + +/// cast a variable to a float given its type +float AP_Param::cast_to_float(enum ap_var_type type) +{ + switch (type) { + case AP_PARAM_INT8: + return ((AP_Int8 *)this)->cast_to_float(); + case AP_PARAM_INT16: + return ((AP_Int16 *)this)->cast_to_float(); + case AP_PARAM_INT32: + return ((AP_Int32 *)this)->cast_to_float(); + case AP_PARAM_FLOAT: + return ((AP_Float *)this)->cast_to_float(); + default: + return NAN; + } +} diff --git a/libraries/AP_Common/AP_Param.h b/libraries/AP_Common/AP_Param.h index 5495db6a8a..fcccd44482 100644 --- a/libraries/AP_Common/AP_Param.h +++ b/libraries/AP_Common/AP_Param.h @@ -143,6 +143,9 @@ public: /// as needed static AP_Param *next_scalar(uint32_t *token, enum ap_var_type *ptype); + /// cast a variable to a float given its type + float cast_to_float(enum ap_var_type type); + private: /// EEPROM header ///