From 044b4bbcfeacd9d1b4a3c96208ebbee62f3bf406 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Jan 2025 14:42:41 +1100 Subject: [PATCH] AP_HAL_SITL: tidy creation of simulated serial rangefinders --- libraries/AP_HAL_SITL/SITL_State_common.cpp | 230 ++++---------------- libraries/AP_HAL_SITL/SITL_State_common.h | 64 +----- 2 files changed, 49 insertions(+), 245 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 0e765c556f392..ad63e21fcfb0f 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -21,12 +21,57 @@ #include #include "SITL_State_common.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + using namespace HALSITL; static const struct { const char *name; SITL::SerialRangeFinder *(*createfn)(); } serial_rangefinder_definitions[] { + { "ainsteinlrd1", SITL::RF_Ainstein_LR_D1::create }, + { "benewake_tf02", SITL::RF_Benewake_TF02::create }, + { "benewake_tf03", SITL::RF_Benewake_TF03::create }, + { "benewake_tfmini", SITL::RF_Benewake_TFmini::create }, + { "blping", SITL::RF_BLping::create }, + { "gyus42v2", SITL::RF_GYUS42v2::create }, + { "jre", SITL::RF_JRE::create }, + { "lanbao", SITL::RF_Lanbao::create }, + { "leddarone", SITL::RF_LeddarOne::create }, + { "leddarone", SITL::RF_LeddarOne::create }, + { "lightwareserial-binary", SITL::RF_LightWareSerialBinary::create }, + { "lightwareserial", SITL::RF_LightWareSerial::create }, + { "maxsonarseriallv", SITL::RF_MaxsonarSerialLV::create }, + { "nmea", SITL::RF_NMEA::create }, + { "nmea", SITL::RF_NMEA::create }, + { "nooploop_tofsense", SITL::RF_Nooploop::create }, + { "rds02uf", SITL::RF_RDS02UF::create }, +#if !defined(HAL_BUILD_AP_PERIPH) + { "rf_mavlink", SITL::RF_MAVLink::create }, +#endif + { "teraranger_serial", SITL::RF_TeraRanger_Serial::create }, + { "USD1_v0", SITL::RF_USD1_v0::create }, + { "USD1_v1", SITL::RF_USD1_v1::create }, + { "wasp", SITL::RF_Wasp::create }, }; #define streq(a, b) (!strcmp(a, b)) @@ -43,12 +88,8 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const return serial_rangefinders[num_serial_rangefinders++]; } - if (streq(name, "benewake_tf02")) { - if (benewake_tf02 != nullptr) { - AP_HAL::panic("Only one benewake_tf02 at a time"); - } - benewake_tf02 = NEW_NOTHROW SITL::RF_Benewake_TF02(); - return benewake_tf02; + if (false) { + // this is an empty clause to ease else-if syntax #if !defined(HAL_BUILD_AP_PERIPH) } else if (streq(name, "vicon")) { if (vicon != nullptr) { @@ -67,111 +108,6 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const } sitl_model->set_adsb(adsb); return adsb; -#endif - } else if (streq(name, "ainsteinlrd1")) { - if (ainsteinlrd1 != nullptr) { - AP_HAL::panic("Only one ainsteinlrd1 at a time"); - } - ainsteinlrd1 = NEW_NOTHROW SITL::RF_Ainstein_LR_D1(); - return ainsteinlrd1; - } else if (streq(name, "benewake_tf03")) { - if (benewake_tf03 != nullptr) { - AP_HAL::panic("Only one benewake_tf03 at a time"); - } - benewake_tf03 = NEW_NOTHROW SITL::RF_Benewake_TF03(); - return benewake_tf03; - } else if (streq(name, "benewake_tfmini")) { - if (benewake_tfmini != nullptr) { - AP_HAL::panic("Only one benewake_tfmini at a time"); - } - benewake_tfmini = NEW_NOTHROW SITL::RF_Benewake_TFmini(); - return benewake_tfmini; - } else if (streq(name, "nooploop_tofsense")) { - if (nooploop != nullptr) { - AP_HAL::panic("Only one nooploop_tofsense at a time"); - } - nooploop = NEW_NOTHROW SITL::RF_Nooploop(); - return nooploop; - } else if (streq(name, "teraranger_serial")) { - if (teraranger_serial != nullptr) { - AP_HAL::panic("Only one teraranger_serial at a time"); - } - teraranger_serial = NEW_NOTHROW SITL::RF_TeraRanger_Serial(); - return teraranger_serial; - } else if (streq(name, "lightwareserial")) { - if (lightwareserial != nullptr) { - AP_HAL::panic("Only one lightwareserial at a time"); - } - lightwareserial = NEW_NOTHROW SITL::RF_LightWareSerial(); - return lightwareserial; - } else if (streq(name, "lightwareserial-binary")) { - if (lightwareserial_binary != nullptr) { - AP_HAL::panic("Only one lightwareserial-binary at a time"); - } - lightwareserial_binary = NEW_NOTHROW SITL::RF_LightWareSerialBinary(); - return lightwareserial_binary; - } else if (streq(name, "lanbao")) { - if (lanbao != nullptr) { - AP_HAL::panic("Only one lanbao at a time"); - } - lanbao = NEW_NOTHROW SITL::RF_Lanbao(); - return lanbao; - } else if (streq(name, "blping")) { - if (blping != nullptr) { - AP_HAL::panic("Only one blping at a time"); - } - blping = NEW_NOTHROW SITL::RF_BLping(); - return blping; - } else if (streq(name, "leddarone")) { - if (leddarone != nullptr) { - AP_HAL::panic("Only one leddarone at a time"); - } - leddarone = NEW_NOTHROW SITL::RF_LeddarOne(); - return leddarone; - } else if (streq(name, "rds02uf")) { - if (rds02uf != nullptr) { - AP_HAL::panic("Only one rds02uf at a time"); - } - rds02uf = NEW_NOTHROW SITL::RF_RDS02UF(); - return rds02uf; - } else if (streq(name, "USD1_v0")) { - if (USD1_v0 != nullptr) { - AP_HAL::panic("Only one USD1_v0 at a time"); - } - USD1_v0 = NEW_NOTHROW SITL::RF_USD1_v0(); - return USD1_v0; - } else if (streq(name, "USD1_v1")) { - if (USD1_v1 != nullptr) { - AP_HAL::panic("Only one USD1_v1 at a time"); - } - USD1_v1 = NEW_NOTHROW SITL::RF_USD1_v1(); - return USD1_v1; - } else if (streq(name, "maxsonarseriallv")) { - if (maxsonarseriallv != nullptr) { - AP_HAL::panic("Only one maxsonarseriallv at a time"); - } - maxsonarseriallv = NEW_NOTHROW SITL::RF_MaxsonarSerialLV(); - return maxsonarseriallv; - } else if (streq(name, "wasp")) { - if (wasp != nullptr) { - AP_HAL::panic("Only one wasp at a time"); - } - wasp = NEW_NOTHROW SITL::RF_Wasp(); - return wasp; - } else if (streq(name, "nmea")) { - if (nmea != nullptr) { - AP_HAL::panic("Only one nmea at a time"); - } - nmea = NEW_NOTHROW SITL::RF_NMEA(); - return nmea; - -#if !defined(HAL_BUILD_AP_PERIPH) - } else if (streq(name, "rf_mavlink")) { - if (rf_mavlink != nullptr) { - AP_HAL::panic("Only one rf_mavlink at a time"); - } - rf_mavlink = NEW_NOTHROW SITL::RF_MAVLink(); - return rf_mavlink; #endif } else if (streq(name, "frsky-d")) { if (frsky_d != nullptr) { @@ -260,18 +196,6 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const sitl_model->set_ie24(&_sitl->ie24_sim); return &_sitl->ie24_sim; #endif // HAL_BUILD_AP_PERIPH - } else if (streq(name, "jre")) { - if (jre != nullptr) { - AP_HAL::panic("Only one jre at a time"); - } - jre = NEW_NOTHROW SITL::RF_JRE(); - return jre; - } else if (streq(name, "gyus42v2")) { - if (gyus42v2 != nullptr) { - AP_HAL::panic("Only one gyus42v2 at a time"); - } - gyus42v2 = NEW_NOTHROW SITL::RF_GYUS42v2(); - return gyus42v2; } else if (streq(name, "megasquirt")) { if (efi_ms != nullptr) { AP_HAL::panic("Only one megasquirt at a time"); @@ -364,66 +288,6 @@ void SITL_State_Common::sim_update(void) attitude); } #endif - if (ainsteinlrd1 != nullptr) { - ainsteinlrd1->update(sitl_model->rangefinder_range()); - } - if (benewake_tf02 != nullptr) { - benewake_tf02->update(sitl_model->rangefinder_range()); - } - if (benewake_tf03 != nullptr) { - benewake_tf03->update(sitl_model->rangefinder_range()); - } - if (benewake_tfmini != nullptr) { - benewake_tfmini->update(sitl_model->rangefinder_range()); - } - if (jre != nullptr) { - jre->update(sitl_model->rangefinder_range()); - } - if (nooploop != nullptr) { - nooploop->update(sitl_model->rangefinder_range()); - } - if (teraranger_serial != nullptr) { - teraranger_serial->update(sitl_model->rangefinder_range()); - } - if (lightwareserial != nullptr) { - lightwareserial->update(sitl_model->rangefinder_range()); - } - if (lightwareserial_binary != nullptr) { - lightwareserial_binary->update(sitl_model->rangefinder_range()); - } - if (lanbao != nullptr) { - lanbao->update(sitl_model->rangefinder_range()); - } - if (blping != nullptr) { - blping->update(sitl_model->rangefinder_range()); - } - if (leddarone != nullptr) { - leddarone->update(sitl_model->rangefinder_range()); - } - if (rds02uf != nullptr) { - rds02uf->update(sitl_model->rangefinder_range()); - } - if (USD1_v0 != nullptr) { - USD1_v0->update(sitl_model->rangefinder_range()); - } - if (USD1_v1 != nullptr) { - USD1_v1->update(sitl_model->rangefinder_range()); - } - if (maxsonarseriallv != nullptr) { - maxsonarseriallv->update(sitl_model->rangefinder_range()); - } - if (wasp != nullptr) { - wasp->update(sitl_model->rangefinder_range()); - } - if (nmea != nullptr) { - nmea->update(sitl_model->rangefinder_range()); - } - if (rf_mavlink != nullptr) { - rf_mavlink->update(sitl_model->rangefinder_range()); - } - if (gyus42v2 != nullptr) { - gyus42v2->update(sitl_model->rangefinder_range()); - } for (uint8_t i=0; iupdate(sitl_model->rangefinder_range()); } diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 3155036a60753..eb9099ad17cf9 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -14,32 +14,14 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include #include #include +#include + #include #include // #include @@ -128,48 +110,6 @@ class HALSITL::SITL_State_Common { SITL::SerialRangeFinder *serial_rangefinders[16]; uint8_t num_serial_rangefinders; - // simulated Ainstein LR-D1 rangefinder: - SITL::RF_Ainstein_LR_D1 *ainsteinlrd1; - // simulated Benewake tf02 rangefinder: - SITL::RF_Benewake_TF02 *benewake_tf02; - // simulated Benewake tf03 rangefinder: - SITL::RF_Benewake_TF03 *benewake_tf03; - //simulated JAE JRE rangefinder: - SITL::RF_JRE *jre; - // simulated Benewake tfmini rangefinder: - SITL::RF_Benewake_TFmini *benewake_tfmini; - //simulated NoopLoop TOFSense rangefinder: - SITL::RF_Nooploop *nooploop; - // simulated TeraRanger Serial: - SITL::RF_TeraRanger_Serial *teraranger_serial; - - // simulated LightWareSerial rangefinder - legacy protocol:: - SITL::RF_LightWareSerial *lightwareserial; - // simulated LightWareSerial rangefinder - binary protocol: - SITL::RF_LightWareSerialBinary *lightwareserial_binary; - // simulated Lanbao rangefinder: - SITL::RF_Lanbao *lanbao; - // simulated BLping rangefinder: - SITL::RF_BLping *blping; - // simulated LeddarOne rangefinder: - SITL::RF_LeddarOne *leddarone; - // simulated RDS02UF rangefinder: - SITL::RF_RDS02UF *rds02uf; - // simulated USD1 v0 rangefinder: - SITL::RF_USD1_v0 *USD1_v0; - // simulated USD1 v1 rangefinder: - SITL::RF_USD1_v1 *USD1_v1; - // simulated MaxsonarSerialLV rangefinder: - SITL::RF_MaxsonarSerialLV *maxsonarseriallv; - // simulated Wasp rangefinder: - SITL::RF_Wasp *wasp; - // simulated NMEA rangefinder: - SITL::RF_NMEA *nmea; - // simulated MAVLink rangefinder: - SITL::RF_MAVLink *rf_mavlink; - // simulated GYUS42v2 rangefinder: - SITL::RF_GYUS42v2 *gyus42v2; - // simulated Frsky devices SITL::Frsky_D *frsky_d; // SITL::Frsky_SPort *frsky_sport;