diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index e485047e43..70d148b8a7 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -336,6 +336,12 @@ int SITL_State::sim_fd(const char *name, const char *arg) // } // frsky_sport = new SITL::Frsky_SPortPassthrough(); // return frsky_sportpassthrough->fd(); + } else if (streq(name, "rplidara2")) { + if (rplidara2 != nullptr) { + AP_HAL::panic("Only one rplidara2 at a time"); + } + rplidara2 = new SITL::PS_RPLidarA2(); + return rplidara2->fd(); } AP_HAL::panic("unknown simulated device: %s", name); @@ -417,6 +423,11 @@ int SITL_State::sim_fd_write(const char *name) AP_HAL::panic("No frsky-d created"); } return frsky_d->write_fd(); + } else if (streq(name, "rplidara2")) { + if (rplidara2 == nullptr) { + AP_HAL::panic("No rplidara2 created"); + } + return rplidara2->write_fd(); } AP_HAL::panic("unknown simulated device: %s", name); } @@ -603,6 +614,9 @@ void SITL_State::_fdm_input_local(void) // if (frsky_sportpassthrough != nullptr) { // frsky_sportpassthrough->update(); // } + if (rplidara2 != nullptr) { + rplidara2->update(sitl_model->get_location()); + } if (_sitl) { _sitl->efi_ms.update(); diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index bccae174f1..3ef33b2861 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -41,6 +41,7 @@ #include // #include // #include +#include #include @@ -273,6 +274,8 @@ private: SITL::Frsky_D *frsky_d; // SITL::Frsky_SPort *frsky_sport; // SITL::Frsky_SPortPassthrough *frsky_sportpassthrough; + // simulated NMEA rangefinder: + SITL::PS_RPLidarA2 *rplidara2; // output socket for flightgear viewing SocketAPM fg_socket{true};