#include "SIM_config.h" #if AP_SIM_GPS_TRIMBLE_ENABLED #include "SIM_GPS_Trimble.h" #include #include #include #include using namespace SITL; void GPS_Trimble::publish(const GPS_Data *d) { // This logic is to populate output buffer only with enabled channels. // It also only sends each channel at the configured rate. const uint64_t now = AP_HAL::millis(); uint8_t buf[MAX_PAYLOAD_SIZE] = {}; uint8_t payload_sz = 0; uint8_t offset = 0; if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){ const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; if (now - last_time > RateToPeriodMs(desired_rate)) { // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 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 { uint8_t(Gsof_Msg_Record_Type::POSITION_TIME), 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, "Trimble size check failed"); payload_sz += sizeof(pos_time); memcpy(&buf[offset], &pos_time, sizeof(pos_time)); offset += sizeof(pos_time); } } if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){ const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)]; const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)]; if (now - last_time > RateToPeriodMs(desired_rate)) { // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20 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 { uint8_t(Gsof_Msg_Record_Type::LLH), GSOF_POS_LEN, gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), gsof_pack_double(static_cast(d->altitude)) }; static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN, "Trimble size check failed"); payload_sz += sizeof(pos); memcpy(&buf[offset], &pos, sizeof(pos)); offset += sizeof(pos); } } if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){ const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; if (now - last_time > RateToPeriodMs(desired_rate)) { // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html // 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 { uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA), GSOF_VEL_LEN, vel_flags, gsof_pack_float(d->speed_2d()), gsof_pack_float(d->ground_track_rad()), // Trimble API has ambiguous direction here. // Intentionally narrow from double. gsof_pack_float(static_cast(d->speedD)) }; static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN, "Trimble size check failed"); payload_sz += sizeof(vel); memcpy(&buf[offset], &vel, sizeof(vel)); offset += sizeof(vel); } } if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){ const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; if (now - last_time > RateToPeriodMs(desired_rate)) { // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 constexpr uint8_t GSOF_DOP_LEN = 0x10; const struct PACKED gsof_dop { const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) }; 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, "gsof_dop must be 8 bytes"); constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); static_assert(dop_record_type_size == 1, "gsof_dop::OUTPUT_RECORD_TYPE must be 1 byte"); constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); static_assert(len_size == 1, "gsof_dop::RECORD_LEN must be 1 bytes"); constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); static_assert(dop_payload_size == GSOF_DOP_LEN, "dop_payload_size must be GSOF_DOP_LEN bytes"); payload_sz += sizeof(dop); memcpy(&buf[offset], &dop, sizeof(dop)); offset += sizeof(dop); } } if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){ const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; if (now - last_time > RateToPeriodMs(desired_rate)) { // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; const struct PACKED gsof_pos_sigma { const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) }; 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, "Trimble size check failed"); payload_sz += sizeof(pos_sigma); memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); offset += sizeof(pos_sigma); } } assert(offset == payload_sz); // Don't send empy frames when all channels are disabled; if (payload_sz > 0) { send_gsof(buf, payload_sz); } } bool DCOL_Parser::dcol_parse(const char data_in) { bool ret = false; switch (parse_state) { case Parse_State::WAITING_ON_STX: if (data_in == STX) { reset(); parse_state = Parse_State::WAITING_ON_STATUS; } break; case Parse_State::WAITING_ON_STATUS: if (data_in != (uint8_t)Status::OK) { // Invalid, status should always be OK. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } else { status = static_cast(data_in); parse_state = Parse_State::WAITING_ON_PACKET_TYPE; } break; case Parse_State::WAITING_ON_PACKET_TYPE: if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) { packet_type = Packet_Type::COMMAND_APPFILE; } else { // Unsupported command in this simulator. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } parse_state = Parse_State::WAITING_ON_LENGTH; break; case Parse_State::WAITING_ON_LENGTH: expected_payload_length = data_in; parse_state = Parse_State::WAITING_ON_PACKET_DATA; break; case Parse_State::WAITING_ON_PACKET_DATA: payload[cur_payload_idx] = data_in; if (++cur_payload_idx == expected_payload_length) { parse_state = Parse_State::WAITING_ON_CSUM; } break; case Parse_State::WAITING_ON_CSUM: expected_csum = data_in; parse_state = Parse_State::WAITING_ON_ETX; break; case Parse_State::WAITING_ON_ETX: if (data_in != ETX) { reset(); } if (!valid_csum()) { // GSOF driver sent a packet with invalid CSUM. // In real GSOF driver, the packet is simply ignored with no reply. // In the SIM, treat this as a coding error. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } else { ret = parse_payload(); } reset(); break; } return ret; } uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) { uint32_t min_period_ms = 0; switch (rate) { case Output_Rate::OFF: min_period_ms = 0; break; case Output_Rate::FREQ_10_HZ: min_period_ms = 100; break; case Output_Rate::FREQ_50_HZ: min_period_ms = 20; break; case Output_Rate::FREQ_100_HZ: min_period_ms = 10; break; } return min_period_ms; } bool DCOL_Parser::valid_csum() { uint8_t sum = (uint8_t)status; sum += (uint8_t)packet_type; sum += expected_payload_length; sum += crc_sum_of_bytes(payload, expected_payload_length); return sum == expected_csum; } bool DCOL_Parser::parse_payload() { bool success = false; if (packet_type == Packet_Type::COMMAND_APPFILE) { success = parse_cmd_appfile(); } return success; } bool DCOL_Parser::parse_cmd_appfile() { // A file control info block contains: // * version // * device type // * start application file flag // * factory settings flag constexpr uint8_t file_control_info_block_sz = 4; // An appfile header contains: // * transmisison number // * page index // * max page index constexpr uint8_t appfile_header_sz = 3; constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz; if (expected_payload_length < min_cmd_appfile_sz) { return false; } // For now, ignore appfile_trans_num, return success regardless. // If the driver doesn't send the right value, it's not clear what the behavior is supposed to be. // Also would need to handle re-sync. // For now, just store it for debugging. appfile_trans_num = payload[0]; // File control information block parsing: // https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html constexpr uint8_t expected_app_file_spec_version = 0x03; constexpr uint8_t file_ctrl_idx = appfile_header_sz; if (payload[file_ctrl_idx] != expected_app_file_spec_version) { // Only version 3 is supported at this time. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } constexpr uint8_t expected_dev_type = 0x00; if (payload[file_ctrl_idx+1] != expected_dev_type) { // Only "all" device type is supported. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } constexpr uint8_t expected_start_flag = 0x01; if (payload[file_ctrl_idx+2] != expected_start_flag) { // Only "immediate" start type is supported. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } constexpr uint8_t expected_factory_settings_flag = 0x00; if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) { // Factory settings restore before appfile is not supported. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz; const uint8_t record_type = payload[app_file_records_idx]; if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) { // Serial port baud/format // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html // Ignore serial port index (COM Port) since there's only one in SITL. // Ignore baud rate because you can't change baud yet in SITL. // Ignore parity because it can't be changed in SITL. // Ignore flow control because it's not yet in SITL. } else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){ // Output Message // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html // Ignore record length to save flash. // Ignore port index since SITL is only one port. if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) { const auto gsof_submessage_type = payload[app_file_records_idx + 6]; const auto rate = payload[app_file_records_idx + 4]; if (rate == (uint8_t)Output_Rate::OFF) { channel_rates[gsof_submessage_type] = static_cast(rate); } else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) { channel_rates[gsof_submessage_type] = static_cast(rate); } else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) { channel_rates[gsof_submessage_type] = static_cast(rate); } else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) { channel_rates[gsof_submessage_type] = static_cast(rate); } else { // Unsupported GSOF rate in SITL. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } else { // Only some data output protocols are supported in SITL. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } else { // Other application file packets are not yet supported. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } return true; } void DCOL_Parser::reset() { cur_payload_idx = 0; expected_payload_length = 0; parse_state = Parse_State::WAITING_ON_STX; // To be pedantic, one could zero the bytes in the payload, // but this is skipped because it's extra CPU. // Note - appfile_trans_number is intended to persist over parser resets. } void GPS_Trimble::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 // 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]; } 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_Trimble::gsof_pack_double(const double& src) { uint64_t dst; static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size"); memcpy(&dst, &src, sizeof(dst)); dst = htobe64(dst); return dst; } uint32_t GPS_Trimble::gsof_pack_float(const float& src) { uint32_t dst; static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size"); memcpy(&dst, &src, sizeof(dst)); dst = htobe32(dst); return dst; } void GPS_Trimble::update_read() { // Technically, the max command is slightly larger. // This will only slightly slow the response for packets that big. char c[MAX_PAYLOAD_SIZE]; const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE); if (n_read > 0) { for (uint8_t i = 0; i < n_read; i++) { if (dcol_parse(c[i])) { constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK}; write_to_autopilot((char*)response, sizeof(response)); } // TODO handle NACK for failure } } } #endif // AP_SIM_GPS_TRIMBLE_ENABLED