diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 44c78c54ca..1f5e4eaf8e 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -403,6 +403,14 @@ Vector3f SITL::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro) return Vector3f(phiDot, thetaDot, psiDot); } +// get the rangefinder reading for the desired rotation, returns -1 for no data +float SITL::get_rangefinder(uint8_t instance) { + if (instance < RANGEFINDER_MAX_INSTANCES) { + return state.rangefinder_m[instance]; + } + return -1; +}; + } // namespace SITL diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index e7d4702b1c..30d3c02633 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -20,6 +20,7 @@ #include "SIM_EFI_MegaSquirt.h" #include "SIM_RichenPower.h" #include "SIM_Ship.h" +#include namespace SITL { @@ -67,6 +68,8 @@ struct sitl_fdm { struct vector3f_array points; struct float_array ranges; } scanner; + + float rangefinder_m[RANGEFINDER_MAX_INSTANCES]; }; // number of rc output channels @@ -390,6 +393,10 @@ public: AP_Int16 vicon_yaw_error; // vicon yaw error in degrees (added to reported yaw sent to vehicle) AP_Int8 vicon_type_mask; // vicon message type mask (bit0:vision position estimate, bit1:vision speed estimate, bit2:vicon position estimate) AP_Vector3f vicon_vel_glitch; // velocity glitch in m/s in vicon's local frame + + // get the rangefinder reading for the desired instance, returns -1 for no data + float get_rangefinder(uint8_t instance); + }; } // namespace SITL