diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index f95d5922c31dfe..fe41069ef22e5b 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -25,6 +25,7 @@ #include "SIM_GPS_SBP2.h" #include "SIM_GPS_SBP.h" #include "SIM_GPS_UBLOX.h" +#include "SIM_GPS_SBF.h" #include @@ -268,6 +269,12 @@ void GPS::check_backend_allocation() break; #endif +#if AP_SIM_GPS_SBF_ENABLED + case Type::SBF: + backend = NEW_NOTHROW GPS_SBF(*this, instance); + break; +#endif + #if AP_SIM_GPS_TRIMBLE_ENABLED case Type::TRIMBLE: backend = NEW_NOTHROW GPS_Trimble(*this, instance); diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 954bbd45acef34..9e762252e1de47 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -120,6 +120,9 @@ class GPS : public SerialDevice { #if AP_SIM_GPS_SBP2_ENABLED SBP2 = 9, #endif +#if AP_SIM_GPS_SBF_ENABLED + SBF = 10, //matches GPS_TYPE +#endif #if AP_SIM_GPS_TRIMBLE_ENABLED TRIMBLE = 11, // matches GPS1_TYPE #endif diff --git a/libraries/SITL/SIM_GPS_SBF.cpp b/libraries/SITL/SIM_GPS_SBF.cpp new file mode 100644 index 00000000000000..c705de440b0984 --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBF.cpp @@ -0,0 +1,135 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_SBF_ENABLED + +#include "SIM_GPS_SBF.h" + +#include +#include + +using namespace SITL; + +static const uint16_t CRC16_LOOK_UP[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, + 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, + 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, + 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, + 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, + 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, + 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, + 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, + 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, + 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, + 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, + 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, + 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, + 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, + 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, + 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, + 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8, + 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, + 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, + 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, + 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, + 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, + 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, + 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 +}; + +void GPS_SBF::send_sbf(uint16_t msgid, uint8_t *buf, uint16_t size, uint8_t *tmstp) { + + //HEADER + const uint8_t PREAMBLE1 = 0x24; + const uint8_t PREAMBLE2 = 0x40; + uint8_t hdr[8]; + uint16_t crc = 0; + hdr[0] = PREAMBLE1; + hdr[1] = PREAMBLE2; + hdr[4] = msgid & 0xFF; + hdr[5] = (msgid >> 8) & 0xFF; + hdr[6] = size & 0xFF; + hdr[7] = (size >> 8) & 0xFF; + crc16_ccitt(hdr+4, 4, crc); + for (uint8_t i = 4; i<8; i++) { + crc = (crc << 8) ^ CRC16_LOOK_UP[uint8_t((crc >> 8) ^ hdr[i])]; //include id and length + } + for (uint16_t i = 0; i> 8) ^ buf[i])]; + } + hdr[2] = crc & 0xFF; + hdr[3] = (crc >> 8) & 0xFF; + + write_to_autopilot((char*)hdr, sizeof(hdr)); + write_to_autopilot((char*)tmstp, sizeof(tmstp)); + write_to_autopilot((char*)buf, sizeof(buf)); +} + +void GPS_SBF::publish(const GPS_Data *d) { + + const double DNU_DOUBLE = -2e10; + const float DNU_FLOAT = -2e10; + const uint8_t DNU_UINT8 = 255; + const uint16_t DNU_UINT16 = 65535; + + struct PACKED timestamp_t { + uint64_t tow; + uint16_t wnc; + } time_stamp {}; + + struct PACKED sbf_pvtGeod_t + { + uint8_t mode; + uint8_t error; + double latitude; + double longitude; + double height; + float undulation; + float vn; + float ve; + float vu; + float cog; + double rxclkbias; + float rxclkdrift; + uint8_t timesystem; + uint8_t datum; + uint8_t nrsv; + uint8_t wacorrinfo; + uint16_t referenceid; + uint16_t meancorrage; + uint64_t signalinfo; + uint8_t alertflag; + } pvtGeod_buf {} ; + + const uint16_t PVTGEO_0_MSG_ID = 0x0FA7; + + const auto gps_tow = gps_time(); + time_stamp.tow = gps_tow.ms; + time_stamp.wnc = gps_tow.week; + + pvtGeod_buf.mode = 4; //Mode: default to rtk fixed + pvtGeod_buf.error= 0; //Error: no error + pvtGeod_buf.latitude = radians(_sitl->state.latitude); + pvtGeod_buf.longitude = radians(_sitl->state.longitude); + pvtGeod_buf.height = d->altitude; + pvtGeod_buf.undulation = DNU_DOUBLE; + pvtGeod_buf.vn = d->speedN; + pvtGeod_buf.ve = d->speedE; + pvtGeod_buf.vu = -d->speedD; + pvtGeod_buf.cog = degrees(d->ground_track_rad()); + pvtGeod_buf.rxclkbias = DNU_DOUBLE; + pvtGeod_buf.rxclkdrift = DNU_FLOAT; + pvtGeod_buf.timesystem = DNU_UINT8; + pvtGeod_buf.datum = DNU_UINT8; + pvtGeod_buf.nrsv = d->num_sats; + pvtGeod_buf.wacorrinfo = 0; //default value + pvtGeod_buf.referenceid = DNU_UINT16; + pvtGeod_buf.meancorrage = DNU_UINT16; + pvtGeod_buf.signalinfo = 0; + pvtGeod_buf.alertflag = 0; + + send_sbf(PVTGEO_0_MSG_ID, (uint8_t*)&pvtGeod_buf, sizeof(pvtGeod_buf), (uint8_t*)&time_stamp); +} + +#endif //AP_SIM_GPS_SBF_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBF.h b/libraries/SITL/SIM_GPS_SBF.h new file mode 100644 index 00000000000000..11a7e0207805fc --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBF.h @@ -0,0 +1,27 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_SBF_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_SBF : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_SBF); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; + +private: + + void send_sbf(uint16_t msgid, uint8_t *buf, uint16_t size, uint8_t *tmstp); + +}; + +}; + +#endif // AP_SIM_GPS_SBF_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 8ec2d7ff42c48c..aa281c45f753a7 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -80,6 +80,10 @@ #define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) #endif +#ifndef AP_SIM_GPS_SBF_ENABLED +#define AP_SIM_GPS_SBF_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_SIM_GPS_TRIMBLE_ENABLED #define AP_SIM_GPS_TRIMBLE_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED #endif