void AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type, uint16_t sender_id, uint8_t msg_len, uint8_t *msg_buff) { if (!should_df_log()) { return; } //MASK OUT MESSAGES WE DON'T WANT TO LOG if (( ((uint16_t) gps._sbp_logmask) & msg_type) == 0) { return; } uint64_t time_us = AP_HAL::micros64(); uint8_t pages = 1; if (msg_len > 48) { pages += (msg_len - 48) / 104 + 1; } struct log_SbpRAWH pkt = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAWH), time_us : time_us, msg_type : msg_type, sender_id : sender_id, index : 1, pages : pages, msg_len : msg_len, };
void AP_GPS_SBF::log_ExtEventPVTGeodetic(const msg4007 &temp) { if (!should_df_log()) { return; } uint64_t now = AP_HAL::micros64(); struct log_GPS_SBF_EVENT header = { LOG_PACKET_HEADER_INIT(LOG_GPS_SBF_EVENT_MSG), time_us:now, TOW:temp.TOW, WNc:temp.WNc, Mode:temp.Mode, Error:temp.Error, Latitude:ToDeg(temp.Latitude), Longitude:ToDeg(temp.Longitude), Height:temp.Height, Undulation:temp.Undulation, Vn:temp.Vn, Ve:temp.Ve, Vu:temp.Vu, COG:temp.COG }; DataFlash_Class::instance()->WriteBlock(&header, sizeof(header)); }
// Private Methods ///////////////////////////////////////////////////////////// void AP_GPS_UBLOX::log_mon_hw(void) { if (!should_df_log()) { return; } struct log_Ubx1 pkt = { LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX1_MSG)), time_us : AP_HAL::micros64(), instance : state.instance, noisePerMS : _buffer.mon_hw_60.noisePerMS, jamInd : _buffer.mon_hw_60.jamInd, aPower : _buffer.mon_hw_60.aPower, agcCnt : _buffer.mon_hw_60.agcCnt, config : _unconfigured_messages, };
void AP_GPS_SBP::logging_log_full_update() { if (!should_df_log()) { return; } struct log_SbpHealth pkt = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH), time_us : AP_HAL::micros64(), crc_error_counter : crc_error_counter, last_injected_data_ms : last_injected_data_ms, last_iar_num_hypotheses : last_iar_num_hypotheses, };
void AP_GPS_SBP2::logging_log_full_update() { if (!should_df_log()) { return; } //TODO: Expand with heartbeat info. //TODO: Get rid of IAR NUM HYPO struct log_SbpHealth pkt = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH), time_us : AP_HAL::micros64(), crc_error_counter : crc_error_counter, last_injected_data_ms : last_injected_data_ms, last_iar_num_hypotheses : 0, }; DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt)); };