From 1ab8a3e3aad9b03ce3b6b12137b0cce8b5ab6830 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 16 Oct 2021 14:10:40 +1100 Subject: [PATCH] AP_HAL_SITL: make simulated GPS work as a SerialDevice --- libraries/AP_HAL_SITL/HAL_SITL_Class.cpp | 5 +++ libraries/AP_HAL_SITL/HAL_SITL_Class.h | 2 ++ libraries/AP_HAL_SITL/SITL_Periph_State.cpp | 5 +-- libraries/AP_HAL_SITL/SITL_Periph_State.h | 1 - libraries/AP_HAL_SITL/SITL_State.cpp | 32 +++++++++++++---- libraries/AP_HAL_SITL/SITL_State.h | 40 +++------------------ libraries/AP_HAL_SITL/UARTDriver.cpp | 6 ++-- 7 files changed, 42 insertions(+), 49 deletions(-) diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index a7a35cab46..695479805d 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -180,6 +180,11 @@ static void fill_stack_nan(void) fill_nanf(stk, ARRAY_SIZE(stk)); } +uint8_t HAL_SITL::get_instance() const +{ + return _sitl_state->get_instance(); +} + void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const { assert(callbacks); diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index cc0504064b..eaa18f58ac 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -35,6 +35,8 @@ public: } bool get_wipe_storage() const { return wipe_storage; } + uint8_t get_instance() const; + private: HALSITL::SITL_State *_sitl_state; diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index 711c26e182..67c9bc3bfa 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -49,6 +49,7 @@ void SITL_State::init(int argc, char * const argv[]) { exit(1); } } + printf("Running Instance: %d\n", _instance); } @@ -59,10 +60,6 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) { } -int SITL_State::gps_pipe(uint8_t index) { - return 0; -} - int SITL_State::sim_fd(const char *name, const char *arg) { return 0; } diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h index 08462d7d1c..1549f79ac0 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.h +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -29,7 +29,6 @@ class HALSITL::SITL_State { friend class HALSITL::GPIO; public: void init(int argc, char * const argv[]); - int gps_pipe(uint8_t index); // create a file descriptor attached to a virtual device; type of // device is given by name parameter diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 934d532f77..75e0e60840 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -79,7 +79,6 @@ void SITL_State::_sitl_setup(const char *home_str) if (_sitl != nullptr) { // setup some initial values _update_airspeed(0); - _update_gps(0, 0, 0, 0, 0, 0, 0, false); _update_rangefinder(0); if (enable_gimbal) { gimbal = new SITL::Gimbal(_sitl->state); @@ -164,17 +163,12 @@ void SITL_State::_fdm_input_step(void) _scheduler->sitl_begin_atomic(); if (_update_count == 0 && _sitl != nullptr) { - _update_gps(0, 0, 0, 0, 0, 0, 0, false); HALSITL::Scheduler::timer_event(); _scheduler->sitl_end_atomic(); return; } if (_sitl != nullptr) { - _update_gps(_sitl->state.latitude, _sitl->state.longitude, - _sitl->state.altitude, - _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD, - _sitl->state.yawDeg, true); _update_airspeed(_sitl->state.airspeed); _update_rangefinder(_sitl->state.range); @@ -397,6 +391,17 @@ int SITL_State::sim_fd(const char *name, const char *arg) } ais = new SITL::AIS(); return ais->fd(); + } else if (strncmp(name, "gps", 3) == 0) { + const char *p = strchr(name, ':'); + if (p == nullptr) { + AP_HAL::panic("Need a GPS number (e.g. sim:gps:1)"); + } + uint8_t x = atoi(p+1); + if (x <= 0 || x > ARRAY_SIZE(gps)) { + AP_HAL::panic("Bad GPS number %u", x); + } + gps[x-1] = new SITL::GPS(x-1); + return gps[x-1]->fd(); } AP_HAL::panic("unknown simulated device: %s", name); @@ -534,6 +539,16 @@ int SITL_State::sim_fd_write(const char *name) AP_HAL::panic("No AIS created"); } return ais->write_fd(); + } else if (strncmp(name, "gps", 3) == 0) { + const char *p = strchr(name, ':'); + if (p == nullptr) { + AP_HAL::panic("Need a GPS number (e.g. sim:gps:1)"); + } + const uint8_t x = atoi(p+1); + if (x <= 0 || x > ARRAY_SIZE(gps)) { + AP_HAL::panic("Bad GPS number %u", x); + } + return gps[x-1]->write_fd(); } AP_HAL::panic("unknown simulated device: %s", name); } @@ -763,6 +778,11 @@ void SITL_State::_fdm_input_local(void) if (ais != nullptr) { ais->update(); } + for (uint8_t i=0; iupdate(); + } + } if (_sitl && _use_fg_view) { _output_to_flightgear(); diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 039285b109..f6ff4663a2 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -76,7 +77,6 @@ public: Blimp }; - int gps_pipe(uint8_t index); ssize_t gps_read(int fd, void *buf, size_t count); uint16_t pwm_output[SITL_NUM_CHANNELS]; uint16_t pwm_input[SITL_RC_INPUT_CHANNELS]; @@ -145,42 +145,7 @@ private: void _update_rangefinder(float range_value); void _set_signal_handlers(void) const; - struct gps_data { - double latitude; - double longitude; - float altitude; - double speedN; - double speedE; - double speedD; - double yaw; - bool have_lock; - }; - -#define MAX_GPS_DELAY 100 - gps_data _gps_data[2][MAX_GPS_DELAY]; - - bool _gps_has_basestation_position; - gps_data _gps_basestation_data; - void _gps_write(const uint8_t *p, uint16_t size, uint8_t instance); - void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size, uint8_t instance); - void _update_gps_ubx(const struct gps_data *d, uint8_t instance); - uint8_t _gps_nmea_checksum(const char *s); - void _gps_nmea_printf(uint8_t instance, const char *fmt, ...); - void _update_gps_nmea(const struct gps_data *d, uint8_t instance); - void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload, uint8_t instance); - void _update_gps_sbp(const struct gps_data *d, uint8_t instance); - void _update_gps_sbp2(const struct gps_data *d, uint8_t instance); - void _update_gps_file(uint8_t instance); - void _update_gps_nova(const struct gps_data *d, uint8_t instance); - void _nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen, uint8_t instance); - uint32_t CRC32Value(uint32_t icrc); - uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); - - void _update_gps(double latitude, double longitude, float altitude, - double speedN, double speedE, double speedD, - double yaw, bool have_lock); void _update_airspeed(float airspeed); - void _update_gps_instance(SITL::SIM::GPSType gps_type, const struct gps_data *d, uint8_t instance); void _check_rc_input(void); bool _read_rc_sitl_input(); void _fdm_input_local(void); @@ -330,6 +295,9 @@ private: const char *_home_str; char *_gps_fifo[2]; + + // simulated GPS devices + SITL::GPS *gps[2]; // constrained by # of parameter sets }; #endif // defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index efaec830ee..30f14ee5b9 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -66,11 +66,13 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) if (strcmp(path, "GPS1") == 0) { /* gps */ _connected = true; - _fd = _sitlState->gps_pipe(0); + _fd = _sitlState->sim_fd("gps:1", ""); + _fd_write = _sitlState->sim_fd_write("gps:1"); } else if (strcmp(path, "GPS2") == 0) { /* 2nd gps */ _connected = true; - _fd = _sitlState->gps_pipe(1); + _fd = _sitlState->sim_fd("gps:2", ""); + _fd_write = _sitlState->sim_fd_write("gps:2"); } else { /* parse type:args:flags string for path. For example: