Skip to content

Commit

Permalink
AP_HAL_SITL: tidy creation of simulated serial rangefinders
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Jan 5, 2025
1 parent cc9b35b commit 6c03bba
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 245 deletions.
230 changes: 47 additions & 183 deletions libraries/AP_HAL_SITL/SITL_State_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,57 @@
#include <AP_HAL/utility/Socket_native.h>
#include "SITL_State_common.h"

#include <SITL/SIM_RF_Ainstein_LR_D1.h>
#include <SITL/SIM_RF_Benewake_TF02.h>
#include <SITL/SIM_RF_Benewake_TF03.h>
#include <SITL/SIM_RF_Benewake_TFmini.h>
#include <SITL/SIM_RF_BLping.h>
#include <SITL/SIM_RF_GYUS42v2.h>
#include <SITL/SIM_RF_JRE.h>
#include <SITL/SIM_RF_Lanbao.h>
#include <SITL/SIM_RF_LeddarOne.h>
#include <SITL/SIM_RF_LightWareSerialBinary.h>
#include <SITL/SIM_RF_LightWareSerial.h>
#include <SITL/SIM_RF_MAVLink.h>
#include <SITL/SIM_RF_MaxsonarSerialLV.h>
#include <SITL/SIM_RF_NMEA.h>
#include <SITL/SIM_RF_NoopLoop.h>
#include <SITL/SIM_RF_RDS02UF.h>
#include <SITL/SIM_RF_TeraRanger_Serial.h>
#include <SITL/SIM_RF_USD1_v0.h>
#include <SITL/SIM_RF_USD1_v1.h>
#include <SITL/SIM_RF_Wasp.h>

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))
Expand All @@ -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) {
Expand All @@ -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) {
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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; i<num_serial_rangefinders; i++) {
serial_rangefinders[i]->update(sitl_model->rangefinder_range());
}
Expand Down
64 changes: 2 additions & 62 deletions libraries/AP_HAL_SITL/SITL_State_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,32 +14,14 @@
#include <SITL/SIM_ADSB_Sagetech_MXS.h>
#include <SITL/SIM_EFI_Hirth.h>
#include <SITL/SIM_Vicon.h>
#include <SITL/SIM_RF_Ainstein_LR_D1.h>
#include <SITL/SIM_RF_Benewake_TF02.h>
#include <SITL/SIM_RF_Benewake_TF03.h>
#include <SITL/SIM_RF_Benewake_TFmini.h>
#include <SITL/SIM_RF_NoopLoop.h>
#include <SITL/SIM_RF_TeraRanger_Serial.h>
#include <SITL/SIM_RF_JRE.h>
#include <SITL/SIM_RF_LightWareSerial.h>
#include <SITL/SIM_RF_LightWareSerialBinary.h>
#include <SITL/SIM_RF_Lanbao.h>
#include <SITL/SIM_RF_BLping.h>
#include <SITL/SIM_RF_LeddarOne.h>
#include <SITL/SIM_RF_RDS02UF.h>
#include <SITL/SIM_RF_USD1_v0.h>
#include <SITL/SIM_RF_USD1_v1.h>
#include <SITL/SIM_RF_MaxsonarSerialLV.h>
#include <SITL/SIM_RF_Wasp.h>
#include <SITL/SIM_RF_NMEA.h>
#include <SITL/SIM_RF_MAVLink.h>
#include <SITL/SIM_RF_GYUS42v2.h>
#include <SITL/SIM_VectorNav.h>
#include <SITL/SIM_MicroStrain.h>
#include <SITL/SIM_InertialLabs.h>
#include <SITL/SIM_AIS.h>
#include <SITL/SIM_GPS.h>

#include <SITL/SIM_SerialRangeFinder.h>

#include <SITL/SIM_Frsky_D.h>
#include <SITL/SIM_CRSF.h>
// #include <SITL/SIM_Frsky_SPort.h>
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 6c03bba

Please sign in to comment.