Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tidy creation of simulated serial rangefinders #28997

Merged
merged 3 commits into from
Jan 5, 2025
Merged

Tidy creation of simulated serial rangefinders #28997

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
587aa17
AP_HAL_SITL: create structures to hold serial rangefinder init info
peterbarker Jan 3, 2025
cc9b35b
SITL: tidy creation of simulated rangefinders
peterbarker Jun 22, 2022
6c03bba
AP_HAL_SITL: tidy creation of simulated serial rangefinders
peterbarker Jan 3, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
249 changes: 66 additions & 183 deletions libraries/AP_HAL_SITL/SITL_State_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,20 +19,77 @@
#include <AP_Param/AP_Param.h>
#include <SITL/SIM_JSBSim.h>
#include <AP_HAL/utility/Socket_native.h>

extern const AP_HAL::HAL& hal;
#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))
SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const char *arg, const uint8_t portNumber)
{
if (streq(name, "benewake_tf02")) {
if (benewake_tf02 != nullptr) {
AP_HAL::panic("Only one benewake_tf02 at a time");
for (const auto &definition : serial_rangefinder_definitions) {
if (!streq(definition.name, name)) {
continue;
}
if (num_serial_rangefinders >= ARRAY_SIZE(serial_rangefinders)) {
AP_HAL::panic("Too many simulated serial rangefinders");
}
benewake_tf02 = NEW_NOTHROW SITL::RF_Benewake_TF02();
return benewake_tf02;
serial_rangefinders[num_serial_rangefinders] = definition.createfn();
return serial_rangefinders[num_serial_rangefinders++];
}

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 @@ -51,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 @@ -244,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 @@ -348,65 +288,8 @@ 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());
}
if (efi_ms != nullptr) {
efi_ms->update();
Expand Down
65 changes: 4 additions & 61 deletions libraries/AP_HAL_SITL/SITL_State_common.h
View file
Open in desktop
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 @@ -125,47 +107,8 @@ class HALSITL::SITL_State_Common {
SITL::Vicon *vicon;
#endif

// 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;
SITL::SerialRangeFinder *serial_rangefinders[16];
uint8_t num_serial_rangefinders;

// simulated Frsky devices
SITL::Frsky_D *frsky_d;
Expand Down
2 changes: 2 additions & 0 deletions libraries/SITL/SIM_RF_Ainstein_LR_D1.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ namespace SITL {
class RF_Ainstein_LR_D1 : public SerialRangeFinder {
public:

static SerialRangeFinder *create() { return NEW_NOTHROW RF_Ainstein_LR_D1(); }

uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;

uint16_t reading_interval_ms() const override { return 100; }
Expand Down
2 changes: 2 additions & 0 deletions libraries/SITL/SIM_RF_BLping.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ namespace SITL {
class RF_BLping : public SerialRangeFinder {
public:

static SerialRangeFinder *create() { return NEW_NOTHROW RF_BLping(); }

uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;

};
Expand Down
2 changes: 2 additions & 0 deletions libraries/SITL/SIM_RF_Benewake_TF02.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@ namespace SITL {
class RF_Benewake_TF02 : public RF_Benewake {
public:

static SerialRangeFinder *create() { return NEW_NOTHROW RF_Benewake_TF02(); }

// see AP_RangeFinder_Benewake.cpp for definitions
uint8_t byte4() const override { return 1; } // strength low-bits
uint8_t byte5() const override { return 1; } // strength high-bits
Expand Down
Loading
Loading