diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index d3d17d3a56..dd5cbcad80 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -14,10 +14,12 @@ #define ALLOW_DOUBLE_MATH_FUNCTIONS +#include #include #include #include #include +#include // simulated CAN GPS devices get fed from our SITL estimates: #if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED @@ -81,6 +83,7 @@ uint32_t GPS::device_baud() const #if AP_SIM_GPS_FILE_ENABLED case Type::FILE: #endif + case Type::GSOF: return 0; // 0 meaning unset } return 0; // 0 meaning unset @@ -187,7 +190,7 @@ void GPS::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) } /* - return GPS time of week in milliseconds + return GPS time of week */ static GPS_TOW gps_time() { @@ -390,7 +393,7 @@ void GPS::update_ubx(const struct gps_data *d) memset(&sol, 0, sizeof(sol)); sol.fix_type = d->have_lock?3:0; sol.fix_status = 221; - sol.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; + sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3; sol.time = gps_tow.ms; sol.week = gps_tow.week; @@ -416,7 +419,7 @@ void GPS::update_ubx(const struct gps_data *d) pvt.fix_type = d->have_lock? 0x3 : 0; pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok pvt.flags2 =0; - pvt.num_sv = d->have_lock?_sitl->gps_numsats[instance]:3; + pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3; pvt.lon = d->longitude * 1.0e7; pvt.lat = d->latitude * 1.0e7; pvt.height = d->altitude * 1000.0f; @@ -695,7 +698,7 @@ void GPS::update_sbp(const struct gps_data *d) pos.height = d->altitude; pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.n_sats = _sitl->gps_numsats[instance]; + pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; // Send single point position solution pos.flags = 0; @@ -710,7 +713,7 @@ void GPS::update_sbp(const struct gps_data *d) velned.d = 1e3 * d->speedD; velned.h_accuracy = 5e3; velned.v_accuracy = 5e3; - velned.n_sats = _sitl->gps_numsats[instance]; + velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; velned.flags = 0; sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); @@ -809,7 +812,7 @@ void GPS::update_sbp2(const struct gps_data *d) pos.height = d->altitude; pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.n_sats = _sitl->gps_numsats[instance]; + pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; // Send single point position solution pos.flags = 1; @@ -824,7 +827,7 @@ void GPS::update_sbp2(const struct gps_data *d) velned.d = 1e3 * d->speedD; velned.h_accuracy = 5e3; velned.v_accuracy = 5e3; - velned.n_sats = _sitl->gps_numsats[instance]; + velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; velned.flags = 1; sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); @@ -969,7 +972,7 @@ void GPS::update_nova(const struct gps_data *d) bestpos.lat = d->latitude; bestpos.lng = d->longitude; bestpos.hgt = d->altitude; - bestpos.svsused = _sitl->gps_numsats[instance]; + bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3; bestpos.latsdev=0.2; bestpos.lngsdev=0.2; bestpos.hgtsdev=0.2; @@ -1014,6 +1017,300 @@ uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc return( crc ); } +void GPS::update_gsof(const struct gps_data *d) +{ + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 + constexpr uint8_t GSOF_POS_TIME_TYPE { 0x01 }; + constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A }; + // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE + const uint8_t bootcount = 17; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + enum class POS_FLAGS_1 : uint8_t { + NEW_POSITION = 1U << 0, + CLOCK_FIX_CALULATED = 1U << 1, + HORIZ_FROM_THIS_POS = 1U << 2, + HEIGHT_FROM_THIS_POS = 1U << 3, + RESERVED_4 = 1U << 4, + LEAST_SQ_POSITION = 1U << 5, + RESERVED_6 = 1U << 6, + POSITION_L1_PSEUDORANGES = 1U << 7 + }; + const uint8_t pos_flags_1 { + uint8_t(POS_FLAGS_1::NEW_POSITION) | + uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) | + uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::RESERVED_4) | + uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) | + uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES) + }; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + enum class POS_FLAGS_2 : uint8_t { + DIFFERENTIAL_POS = 1U << 0, + DIFFERENTIAL_POS_PHASE_RTK = 1U << 1, + POSITION_METHOD_FIXED_PHASE = 1U << 2, + OMNISTAR_ACTIVE = 1U << 3, + DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4, + NETWORK_RTK = 1U << 5, + DITHERED_RTK = 1U << 6, + BEACON_DGNSS = 1U << 7, + }; + + // Simulate a GPS without RTK in SIM since there is no RTK SIM params. + // This means these flags are unset: + // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS + uint8_t pos_flags_2 {0}; + if(d->have_lock) { + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK); + pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT); + } + + const auto gps_tow = gps_time(); + const struct PACKED gsof_pos_time { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint32_t time_week_ms; + uint16_t time_week; + uint8_t num_sats; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + uint8_t pos_flags_1; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + uint8_t pos_flags_2; + uint8_t initialized_num; + } pos_time { + GSOF_POS_TIME_TYPE, + GSOF_POS_TIME_LEN, + htobe32(gps_tow.ms), + htobe16(gps_tow.week), + d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3), + pos_flags_1, + pos_flags_2, + bootcount + }; + static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN); + + constexpr uint8_t GSOF_POS_TYPE = 0x02; + constexpr uint8_t GSOF_POS_LEN = 0x18; + + const struct PACKED gsof_pos { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint64_t lat; + uint64_t lng; + uint64_t alt; + } pos { + GSOF_POS_TYPE, + GSOF_POS_LEN, + pack_double_into_gsof_packet(d->latitude * DEG_TO_RAD_DOUBLE), + pack_double_into_gsof_packet(d->longitude * DEG_TO_RAD_DOUBLE), + pack_double_into_gsof_packet(static_cast(d->altitude)) + }; + static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html + constexpr uint8_t GSOF_VEL_TYPE = 0x08; + // use the smaller packet by ignoring local coordinate system + constexpr uint8_t GSOF_VEL_LEN = 0x0D; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + enum class VEL_FIELDS : uint8_t { + VALID = 1U << 0, + CONSECUTIVE_MEASUREMENTS = 1U << 1, + HEADING_VALID = 1U << 2, + RESERVED_3 = 1U << 3, + RESERVED_4 = 1U << 4, + RESERVED_5 = 1U << 5, + RESERVED_6 = 1U << 6, + RESERVED_7 = 1U << 7, + }; + uint8_t vel_flags {0}; + if(d->have_lock) { + vel_flags |= uint8_t(VEL_FIELDS::VALID); + vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS); + vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID); + } + + const struct PACKED gsof_vel { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + uint8_t flags; + uint32_t horiz_m_p_s; + uint32_t heading_rad; + uint32_t vertical_m_p_s; + } vel { + GSOF_VEL_TYPE, + GSOF_VEL_LEN, + vel_flags, + pack_float_into_gsof_packet(d->speed_2d()), + pack_float_into_gsof_packet(d->heading()), + // Trimble API has ambiguous direction here. + // Intentionally narrow from double. + pack_float_into_gsof_packet(static_cast(d->speedD)) + }; + static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); + + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 + constexpr uint8_t GSOF_DOP_TYPE = 0x09; + constexpr uint8_t GSOF_DOP_LEN = 0x10; + const struct PACKED gsof_dop { + const uint8_t OUTPUT_RECORD_TYPE { GSOF_DOP_TYPE }; + const uint8_t RECORD_LEN { GSOF_DOP_LEN }; + uint32_t pdop = htobe32(1); + uint32_t hdop = htobe32(1); + uint32_t vdop = htobe32(1); + uint32_t tdop = htobe32(1); + } dop {}; + // Check the payload size calculation in the compiler + constexpr auto dop_size = sizeof(gsof_dop); + static_assert(dop_size == 18); + constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); + static_assert(dop_record_type_size == 1); + constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); + static_assert(len_size == 1); + constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); + static_assert(dop_payload_size == GSOF_DOP_LEN); + + constexpr uint8_t GSOF_POS_SIGMA_TYPE = 0x0C; + constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; + const struct PACKED gsof_pos_sigma { + const uint8_t OUTPUT_RECORD_TYPE { GSOF_POS_SIGMA_TYPE }; + const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN }; + uint32_t pos_rms = htobe32(0); + uint32_t sigma_e = htobe32(0); + uint32_t sigma_n = htobe32(0); + uint32_t cov_en = htobe32(0); + uint32_t sigma_up = htobe32(0); + uint32_t semi_major_axis = htobe32(0); + uint32_t semi_minor_axis = htobe32(0); + uint32_t orientation = htobe32(0); + uint32_t unit_variance = htobe32(0); + uint16_t n_epocs = htobe32(1); // Always 1 for kinematic. + } pos_sigma {}; + static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN); + + // TODO add GSOF49 + const uint8_t payload_sz = sizeof(pos_time) + sizeof(pos) + sizeof(vel) + sizeof(dop) + sizeof(pos_sigma); + uint8_t buf[payload_sz] = {}; + uint8_t offset = 0; + memcpy(&buf[offset], &pos_time, sizeof(pos_time)); + offset += sizeof(pos_time); + memcpy(&buf[offset], &pos, sizeof(pos)); + offset += sizeof(pos); + memcpy(&buf[offset], &vel, sizeof(vel)); + offset += sizeof(vel); + memcpy(&buf[offset], &dop, sizeof(dop)); + offset += sizeof(dop); + memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); + offset += sizeof(pos_sigma); + assert(offset == payload_sz); + send_gsof(buf, sizeof(buf)); +} + + +void GPS::send_gsof(const uint8_t *buf, const uint16_t size) +{ + // All Trimble "Data Collector" packets, including GSOF, are comprised of three fields: + // * A fixed-length packet header (dcol_header) + // * A variable-length data frame (buf) + // * A fixed-length packet trailer (dcol_trailer) + // Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0 + + const uint8_t STX = 0x02; + // status bitfield + // https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1 + const uint8_t STATUS = 0xa8; + const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT) + + // Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame. + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2 + + static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number + // Most messages, even GSOF49, only take one page. For SIM, assume it. + assert(size < 0xFA); // GPS SIM doesn't yet support paging + constexpr uint8_t PAGE_INDEX = 0; + constexpr uint8_t MAX_PAGE_INDEX = 0; + const uint8_t gsof_header[3] = { + TRANSMISSION_NUMBER, + PAGE_INDEX, + MAX_PAGE_INDEX, + + }; + ++TRANSMISSION_NUMBER; + + // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes. + // A captured GSOF49 packet from BD940 has total bytes of 115 bytes. + // Thus, the following 5 bytes are not counted. + // 1) STX + // 2) STATUS + // 3) PACKET TYPE + // 4) LENGTH + // 5) CHECKSUM + // 6) ETX + // This aligns with manual's idea of data bytes: + // "Each message begins with a 4-byte header, followed by the bytes of data in each packet. The packet ends with a 2-byte trailer." + // Thus, for this implementation with single-page single-record per DCOL packet, + // the length is simply the sum of data packet size, the gsof_header size. + const uint8_t length = size + sizeof(gsof_header); + const uint8_t dcol_header[4] { + STX, + STATUS, + PACKET_TYPE, + length + }; + + + + // Sum bytes (status + type + length + data bytes) and modulo 256 the summation + // Because it's a uint8, use natural overflow + uint8_t csum = STATUS + PACKET_TYPE + length; + for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) { + csum += gsof_header[i]; + } + for (size_t i = 0; i < size; i++) { + csum += buf[i]; + } + + constexpr uint8_t ETX = 0x03; + const uint8_t dcol_trailer[2] = { + csum, + ETX + }; + + write_to_autopilot((char*)dcol_header, sizeof(dcol_header)); + write_to_autopilot((char*)gsof_header, sizeof(gsof_header)); + write_to_autopilot((char*)buf, size); + write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer)); + const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer); + // Validate length based on everything but DCOL h + if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } +} + +uint64_t GPS::pack_double_into_gsof_packet(const double& src) +{ + uint64_t dst; + static_assert(sizeof(src) == sizeof(dst)); + memcpy(&dst, &src, sizeof(dst)); + dst = htobe64(dst); + return dst; +} + +uint32_t GPS::pack_float_into_gsof_packet(const float& src) +{ + uint32_t dst; + static_assert(sizeof(src) == sizeof(dst)); + memcpy(&dst, &src, sizeof(dst)); + dst = htobe32(dst); + return dst; +} + /* send MSP GPS data */ @@ -1065,7 +1362,7 @@ void GPS::update_msp(const struct gps_data *d) msp_gps.gps_week = t.week; msp_gps.ms_tow = t.ms; msp_gps.fix_type = d->have_lock?3:0; - msp_gps.satellites_in_view = d->have_lock?_sitl->gps_numsats[instance]:3; + msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3; msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100; msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100; msp_gps.horizontal_vel_accuracy = 30; @@ -1186,7 +1483,7 @@ void GPS::update() const uint8_t idx = instance; // alias to avoid code churn - struct gps_data d; + struct gps_data d {}; // simulate delayed lock times bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL); @@ -1297,6 +1594,10 @@ void GPS::update() update_msp(&d); break; + case Type::GSOF: + update_gsof(&d); + break; + #if AP_SIM_GPS_FILE_ENABLED case Type::FILE: update_file(); diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 0ed610a6e0..cbcd5d90fb 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -59,7 +59,8 @@ public: #endif NOVA = 8, SBP2 = 9, - MSP = 19, + GSOF = 11, // matches GPS_TYPE + MSP = 19, }; GPS(uint8_t _instance); @@ -136,6 +137,14 @@ private: 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); };