/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* simulate GPS sensors Usage example: param set SERIAL5_PROTOCOL 5 sim_vehicle.py -D --console --map -A --uartB=sim:gps:2 */ #pragma once #include #ifndef HAL_SIM_GPS_ENABLED #define HAL_SIM_GPS_ENABLED (AP_SIM_ENABLED && !defined(HAL_BUILD_AP_PERIPH)) #endif #if HAL_SIM_GPS_ENABLED #ifndef HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED #define HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && !defined(CYGWIN_BUILD) #endif #ifndef AP_SIM_GPS_FILE_ENABLED // really need to use AP_FileSystem for this. #define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) #endif #include "SIM_SerialDevice.h" namespace SITL { class GPS : public SerialDevice { public: CLASS_NO_COPY(GPS); enum Type { NONE = 0, UBLOX = 1, NMEA = 5, SBP = 6, #if AP_SIM_GPS_FILE_ENABLED FILE = 7, #endif NOVA = 8, SBP2 = 9, GSOF = 11, // matches GPS_TYPE MSP = 19, }; GPS(uint8_t _instance); // update state void update(); ssize_t write_to_autopilot(const char *p, size_t size) const override; uint32_t device_baud() const override; // 0 meaning unset private: uint8_t instance; int ext_fifo_fd; uint32_t last_update; // milliseconds // for delay simulation: struct gps_data { uint32_t timestamp_ms; double latitude; double longitude; float altitude; double speedN; double speedE; double speedD; double yaw_deg; double roll_deg; double pitch_deg; bool have_lock; // Get heading [rad], where 0 = North in WGS-84 coordinate system float heading() const WARN_IF_UNUSED; // Get 2D speed [m/s] in WGS-84 coordinate system float speed_2d() const WARN_IF_UNUSED; }; // last 20 samples, allowing for up to 20 samples of delay gps_data _gps_history[20]; #if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED // this will be allocated if needed: char *_gps_fifo; #endif bool _gps_has_basestation_position; gps_data _gps_basestation_data; void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); void update_ubx(const struct gps_data *d); uint8_t nmea_checksum(const char *s); void nmea_printf(const char *fmt, ...); void update_nmea(const struct gps_data *d); void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); void update_sbp(const struct gps_data *d); void update_sbp2(const struct gps_data *d); void update_msp(const struct gps_data *d); #if AP_SIM_GPS_FILE_ENABLED void update_file(); #endif void update_nova(const struct gps_data *d); void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen); uint32_t CRC32Value(uint32_t icrc); uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); // If gsof gets data in, handle it. // Simply, it should respond to this: https://receiverhelp.trimble.com/oem-gnss/index.html#API_TestingComms.html void on_data_gsof(); void update_gsof(const struct gps_data *d); void send_gsof(const uint8_t *buf, const uint16_t size); uint64_t pack_double_into_gsof_packet(const double& src) WARN_IF_UNUSED; uint32_t pack_float_into_gsof_packet(const float& src) WARN_IF_UNUSED; // get delayed data gps_data interpolate_data(const gps_data &d, uint32_t delay_ms); }; } #endif // HAL_SIM_GPS_ENABLED