diff --git a/libraries/AP_Common/AP_Var.cpp b/libraries/AP_Common/AP_Var.cpp index 9750b23541..8faa2be7d8 100644 --- a/libraries/AP_Common/AP_Var.cpp +++ b/libraries/AP_Common/AP_Var.cpp @@ -19,6 +19,7 @@ extern "C" { extern void delay(unsigned long); } #include +#include #include // Global constants exported for general use. @@ -313,6 +314,14 @@ AP_Var::key(void) return var_header.key; } +// Default implementation of cast_to_float, which always fails. +// +float +AP_Var::cast_to_float(void) const +{ + return NAN; +} + // Return the next variable in the global list. // AP_Var * diff --git a/libraries/AP_Common/AP_Var.h b/libraries/AP_Common/AP_Var.h index eb5f15668b..27a4ad35a9 100644 --- a/libraries/AP_Common/AP_Var.h +++ b/libraries/AP_Common/AP_Var.h @@ -335,6 +335,17 @@ public: /// Key key(void); + /// Casts the value to float, if possible. + /// + /// This is a convenience method for code that would rather not try to + /// meta-cast to the various subtypes. It cannot guarantee that a useful value + /// will be returned; the caller must check the returned value against NAN + /// before using it. + /// + /// @return The value cast to float, or NAN if it cannot be cast. + /// + virtual float cast_to_float() const; + private: AP_Var_group *_group; ///< Group that the variable may be a member of AP_Var *_link; ///< linked list pointer to next variable @@ -547,10 +558,23 @@ public: return *this; } + /// AP_VarT types can implement AP_Var::cast_to_float + /// + virtual float cast_to_float() const; + protected: T _value; }; +// Implement AP_Var::cast_to_float. +// +template +float +AP_VarT::cast_to_float() const +{ + return (float)_value; +} + /// Template class for array variables. /// /// Objects created using this template behave like arrays of the type T, @@ -671,7 +695,6 @@ protected: T _value[N]; }; - /// Convenience macro for defining instances of the AP_VarT template. /// #define AP_VARDEF(_t, _n) typedef AP_VarT<_t> AP_##_n;