diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index ac45d68828cdd8..7e7c362876c101 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -986,198 +986,447 @@ uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_ } void GPS_GSOF::publish(const 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) - }; +{ + // 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); + } - // 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, - }; + 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); + + payload_sz += sizeof(pos_time); + memcpy(&buf[offset], &pos_time, sizeof(pos_time)); + offset += sizeof(pos_time); + } + } - // 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); + 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); + + payload_sz += sizeof(pos); + memcpy(&buf[offset], &pos, sizeof(pos)); + offset += sizeof(pos); + } } - 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, - 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); - - // 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); + 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->heading()), + // 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); + + 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); + 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); + + 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); + payload_sz += sizeof(pos_sigma); + memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); + offset += sizeof(pos_sigma); + } } - 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, - gsof_pack_float(d->speed_2d()), - gsof_pack_float(d->heading()), - // 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); - - // 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)); + + // 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. } @@ -1189,7 +1438,6 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) // * 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; @@ -1210,8 +1458,8 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) }; ++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. + // 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 @@ -1242,7 +1490,6 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) csum += buf[i]; } - constexpr uint8_t ETX = 0x03; const uint8_t dcol_trailer[2] = { csum, ETX @@ -1277,6 +1524,22 @@ uint32_t GPS_GSOF::gsof_pack_float(const float& src) return dst; } +void GPS_GSOF::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 + } + } +} + /* send MSP GPS data */ @@ -1474,7 +1737,7 @@ void GPS::update() if (!init_sitl_pointer()) { return; } - + check_backend_allocation(); if (backend == nullptr) { return; @@ -1516,13 +1779,16 @@ void GPS::update() // simulate delayed lock times bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL); - // run at configured GPS rate (default 5Hz) - if ((now_ms - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) { + // Only let physics run and GPS write at configured GPS rate (default 5Hz). + if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) { + // Reading runs every iteration. + // Beware- physics don't update every iteration with this approach. + // Currently, none of the drivers rely on quickly updated physics. backend->update_read(); return; } - last_update = now_ms; + last_write_update_ms = now_ms; d.latitude = latitude; d.longitude = longitude; @@ -1540,6 +1806,10 @@ void GPS::update() d.speedD = speedD + (velErrorNED.z * rand_float()); d.have_lock = have_lock; + + + + if (_sitl->gps_drift_alt[idx] > 0) { // add slow altitude drift controlled by a slow sine wave d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f); @@ -1593,9 +1863,9 @@ void GPS::update() void GPS_Backend::update_read() { - // swallow any config bytes - char c; - read_from_autopilot(&c, 1); + // swallow any config bytes + char c; + read_from_autopilot(&c, 1); } /* diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 235234fd6ad7a0..c9f8c34e415480 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -98,13 +98,132 @@ class GPS_FILE : public GPS_Backend { void publish(const GPS_Data *d) override; }; -class GPS_GSOF : public GPS_Backend { +class DCOL_Parser { + // The DCOL parser is used by Trimble GSOF devices. + // It's used for doing configuration. + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html +public: + // Feed data in to the DCOL parser. + // If the data reaches a parse state that needs to write ACK/NACK back out, + // the function returns true with a populated data_out value. + // Otherwise, it returns false waiting for more data. + bool dcol_parse(const char data_in); + + static constexpr uint8_t STX = 0x02; + static constexpr uint8_t ETX = 0x03; + + // Receiver status code + enum class Status : uint8_t { + OK = 0x00, + }; + + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html + enum class Command_Response : uint8_t { + ACK = 0x06, + NACK = 0x15, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Frequenc + enum class Output_Rate : uint8_t { + OFF = 0, + FREQ_10_HZ = 1, + FREQ_50_HZ = 15, + FREQ_100_HZ = 16, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_ApplicationFilePackets.html?tocpath=API%20Documentation%7CCommand%20and%20report%20packets%7CApplication%20file%20packets%7C_____0 + enum class Packet_Type : uint8_t { + COMMAND_APPFILE = 0x64, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + enum class Appfile_Record_Type : uint8_t { + SERIAL_PORT_BAUD_RATE_FORMAT = 0x02, + OUTPUT_MESSAGE = 0x07, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output + enum class Output_Msg_Msg_Type : uint8_t { + GSOF = 10, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + enum class Gsof_Msg_Record_Type : uint8_t { + POSITION_TIME = 1, + LLH = 2, + VELOCITY_DATA = 8, + PDOP_INFO = 9, + POSITION_SIGMA_INFO = 12, + }; + +protected: + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPacketStructure.html + static constexpr uint8_t MAX_PAYLOAD_SIZE = 255; + + // GSOF supports this many different packet types. + // Only a fraction are supported by the simulator. + // Waste some RAM and allocate arrays for the whole set. + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + static constexpr uint8_t MAX_CHANNEL_NUM = 70; + // Rates of dynamically enabled channels. + // Assume factory behavior of no enabled channels. + // Each channel can send data out at its own rate. + Output_Rate channel_rates[MAX_CHANNEL_NUM] = {Output_Rate::OFF}; + + // Last publish time of dynamically enabled channels. + uint32_t last_publish_ms[MAX_CHANNEL_NUM]; + + static uint32_t RateToPeriodMs(const Output_Rate rate); + +private: + + // Internal parser implementation state + enum class Parse_State { + WAITING_ON_STX, + WAITING_ON_STATUS, + WAITING_ON_PACKET_TYPE, + WAITING_ON_LENGTH, + WAITING_ON_PACKET_DATA, + WAITING_ON_CSUM, + WAITING_ON_ETX, + }; + + bool valid_csum(); + bool parse_payload(); + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + bool parse_cmd_appfile(); + + + // states for currently parsing packet + Status status; + Parse_State parse_state = {Parse_State::WAITING_ON_STX}; + Packet_Type packet_type; + // This is the length in the header. + uint8_t expected_payload_length; + // This is the increasing tally of bytes per packet. + uint8_t cur_payload_idx; + // This is the expected packet checksum in the trailer. + uint8_t expected_csum; + + // The application file record transmission number + uint8_t appfile_trans_num; + + uint8_t payload[MAX_PAYLOAD_SIZE]; + + // Clear all parser state/flags for handling a fresh packet. + void reset(); +}; + +class GPS_GSOF : public GPS_Backend, public DCOL_Parser { public: CLASS_NO_COPY(GPS_GSOF); using GPS_Backend::GPS_Backend; + + // GPS_Backend overrides void publish(const GPS_Data *d) override; + void update_read() override; private: void send_gsof(const uint8_t *buf, const uint16_t size); @@ -235,7 +354,8 @@ class GPS : public SerialDevice { int ext_fifo_fd; - uint32_t last_update; // milliseconds + // The last time GPS data was written [mS] + uint32_t last_write_update_ms; // last 20 samples, allowing for up to 20 samples of delay GPS_Data _gps_history[20]; diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h index 00f58ab93511b1..d04e573506c8ae 100644 --- a/libraries/SITL/SIM_SerialDevice.h +++ b/libraries/SITL/SIM_SerialDevice.h @@ -46,7 +46,7 @@ class SerialDevice { ByteBuffer *to_autopilot; ByteBuffer *from_autopilot; - bool init_sitl_pointer(); + bool init_sitl_pointer() WARN_IF_UNUSED; private: