Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 09:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: create structures to hold serial rangefinder init info

Browse Source
This commit is contained in:
Peter Barker 2025-01-03 12:14:39 +11:00 committed by Peter Barker
parent f630357dc7
commit 98a06c566b
2 changed files with 24 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_HAL_SITL/SITL_State_common.cpp
View File

@ -19,14 +19,30 @@
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <SITL/SIM_JSBSim.h> #include <SITL/SIM_JSBSim.h>
#include <AP_HAL/utility/Socket_native.h> #include <AP_HAL/utility/Socket_native.h>
#include "SITL_State_common.h"
extern const AP_HAL::HAL& hal;
using namespace HALSITL; using namespace HALSITL;
static const struct {
const char *name;
SITL::SerialRangeFinder *(*createfn)();
} serial_rangefinder_definitions[] {
};
#define streq(a, b) (!strcmp(a, b)) #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) SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const char *arg, const uint8_t portNumber)
{ {
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");
}
serial_rangefinders[num_serial_rangefinders] = definition.createfn();
return serial_rangefinders[num_serial_rangefinders++];
}
if (streq(name, "benewake_tf02")) { if (streq(name, "benewake_tf02")) {
if (benewake_tf02 != nullptr) { if (benewake_tf02 != nullptr) {
AP_HAL::panic("Only one benewake_tf02 at a time"); AP_HAL::panic("Only one benewake_tf02 at a time");
@ -408,6 +424,9 @@ void SITL_State_Common::sim_update(void)
if (gyus42v2 != nullptr) { if (gyus42v2 != nullptr) {
gyus42v2->update(sitl_model->rangefinder_range()); 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) { if (efi_ms != nullptr) {
efi_ms->update(); efi_ms->update();
} }

3
libraries/AP_HAL_SITL/SITL_State_common.h
View File

@ -125,6 +125,9 @@ public:
SITL::Vicon *vicon; SITL::Vicon *vicon;
#endif #endif
SITL::SerialRangeFinder *serial_rangefinders[16];
uint8_t num_serial_rangefinders;
// simulated Ainstein LR-D1 rangefinder: // simulated Ainstein LR-D1 rangefinder:
SITL::RF_Ainstein_LR_D1 *ainsteinlrd1; SITL::RF_Ainstein_LR_D1 *ainsteinlrd1;
// simulated Benewake tf02 rangefinder: // simulated Benewake tf02 rangefinder: