mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 09:43:57 -04:00
AP_GPS: check dataflash to see if we should log backend gps messages
This commit is contained in:
parent
7cd794201e
commit
7258cfd20b
@ -268,9 +268,8 @@ AP_GPS::AP_GPS()
|
||||
}
|
||||
|
||||
/// Startup initialisation.
|
||||
void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager)
|
||||
void AP_GPS::init(const AP_SerialManager& serial_manager)
|
||||
{
|
||||
_DataFlash = dataflash;
|
||||
primary_instance = 0;
|
||||
|
||||
// search for serial ports with gps protocol
|
||||
@ -1023,10 +1022,6 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t *msg)
|
||||
|
||||
void AP_GPS::Write_DataFlash_Log_Startup_messages()
|
||||
{
|
||||
if (_DataFlash == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (uint8_t instance=0; instance<num_instances; instance++) {
|
||||
if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
|
||||
continue;
|
||||
|
@ -40,7 +40,6 @@
|
||||
|
||||
#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)
|
||||
|
||||
class DataFlash_Class;
|
||||
class AP_GPS_Backend;
|
||||
|
||||
/// @class AP_GPS
|
||||
@ -146,7 +145,7 @@ public:
|
||||
};
|
||||
|
||||
/// Startup initialisation.
|
||||
void init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager);
|
||||
void init(const AP_SerialManager& serial_manager);
|
||||
|
||||
/// Update GPS state based on possible bytes received from the module.
|
||||
/// This routine must be called periodically (typically at 10Hz or
|
||||
@ -370,10 +369,10 @@ public:
|
||||
|
||||
void Write_DataFlash_Log_Startup_messages();
|
||||
|
||||
protected:
|
||||
// indicate which bit in LOG_BITMASK indicates gps logging enabled
|
||||
void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; }
|
||||
|
||||
// dataflash for logging, if available
|
||||
DataFlash_Class *_DataFlash;
|
||||
protected:
|
||||
|
||||
// configuration parameters
|
||||
AP_Int8 _type[GPS_MAX_RECEIVERS];
|
||||
@ -395,6 +394,8 @@ protected:
|
||||
AP_Int8 _blend_mask;
|
||||
AP_Float _blend_tc;
|
||||
|
||||
uint32_t _log_gps_bit = -1;
|
||||
|
||||
private:
|
||||
// returns the desired gps update rate in milliseconds
|
||||
// this does not provide any gurantee that the GPS is updating at the requested
|
||||
|
@ -161,7 +161,7 @@ AP_GPS_SBF::parse(uint8_t temp)
|
||||
void
|
||||
AP_GPS_SBF::log_ExtEventPVTGeodetic(const msg4007 &temp)
|
||||
{
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -184,7 +184,7 @@ AP_GPS_SBF::log_ExtEventPVTGeodetic(const msg4007 &temp)
|
||||
COG:temp.COG
|
||||
};
|
||||
|
||||
gps._DataFlash->WriteBlock(&header, sizeof(header));
|
||||
DataFlash_Class::instance()->WriteBlock(&header, sizeof(header));
|
||||
}
|
||||
|
||||
bool
|
||||
|
@ -393,8 +393,8 @@ void
|
||||
AP_GPS_SBP::logging_log_full_update()
|
||||
{
|
||||
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
return;
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
struct log_SbpHealth pkt = {
|
||||
@ -404,8 +404,8 @@ AP_GPS_SBP::logging_log_full_update()
|
||||
last_injected_data_ms : last_injected_data_ms,
|
||||
last_iar_num_hypotheses : last_iar_num_hypotheses,
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
};
|
||||
|
||||
void
|
||||
@ -413,8 +413,8 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
||||
uint16_t sender_id,
|
||||
uint8_t msg_len,
|
||||
uint8_t *msg_buff) {
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
return;
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
//MASK OUT MESSAGES WE DON'T WANT TO LOG
|
||||
@ -439,7 +439,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
||||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt.data, msg_buff, MIN(msg_len, 48));
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
for (uint8_t i = 0; i < pages - 1; i++) {
|
||||
struct log_SbpRAWM pkt2 = {
|
||||
@ -452,7 +452,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
||||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt2.data, &msg_buff[48 + i * 104], MIN(msg_len - (48 + i * 104), 104));
|
||||
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -432,7 +432,7 @@ AP_GPS_SBP2::_detect(struct SBP2_detect_state &state, uint8_t data)
|
||||
void
|
||||
AP_GPS_SBP2::logging_log_full_update()
|
||||
{
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -446,7 +446,7 @@ AP_GPS_SBP2::logging_log_full_update()
|
||||
last_injected_data_ms : last_injected_data_ms,
|
||||
last_iar_num_hypotheses : 0,
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
};
|
||||
|
||||
void
|
||||
@ -454,7 +454,7 @@ AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type,
|
||||
uint16_t sender_id,
|
||||
uint8_t msg_len,
|
||||
uint8_t *msg_buff) {
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -480,7 +480,7 @@ AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type,
|
||||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt.data, msg_buff, MIN(msg_len, 48));
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
for (uint8_t i = 0; i < pages - 1; i++) {
|
||||
struct log_SbpRAWM pkt2 = {
|
||||
@ -493,13 +493,13 @@ AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type,
|
||||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt2.data, &msg_buff[48 + i * 104], MIN(msg_len - (48 + i * 104), 104));
|
||||
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
}
|
||||
};
|
||||
|
||||
void
|
||||
AP_GPS_SBP2::logging_ext_event() {
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -512,5 +512,5 @@ AP_GPS_SBP2::logging_ext_event() {
|
||||
level : last_event.flags.level,
|
||||
quality : last_event.flags.quality,
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
};
|
||||
|
@ -483,7 +483,7 @@ AP_GPS_UBLOX::read(void)
|
||||
// Private Methods /////////////////////////////////////////////////////////////
|
||||
void AP_GPS_UBLOX::log_mon_hw(void)
|
||||
{
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
struct log_Ubx1 pkt = {
|
||||
@ -501,12 +501,12 @@ void AP_GPS_UBLOX::log_mon_hw(void)
|
||||
pkt.aPower = _buffer.mon_hw_68.aPower;
|
||||
pkt.agcCnt = _buffer.mon_hw_68.agcCnt;
|
||||
}
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void AP_GPS_UBLOX::log_mon_hw2(void)
|
||||
{
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -519,15 +519,16 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
|
||||
ofsQ : _buffer.mon_hw2.ofsQ,
|
||||
magQ : _buffer.mon_hw2.magQ,
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
#if UBLOX_RXM_RAW_LOGGING
|
||||
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
|
||||
{
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t now = AP_HAL::micros64();
|
||||
for (uint8_t i=0; i<raw.numSV; i++) {
|
||||
struct log_GPS_RAW pkt = {
|
||||
@ -544,15 +545,16 @@ void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
|
||||
cno : raw.svinfo[i].cno,
|
||||
lli : raw.svinfo[i].lli
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
|
||||
{
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
if (!should_df_log()) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t now = AP_HAL::micros64();
|
||||
|
||||
struct log_GPS_RAWH header = {
|
||||
@ -564,7 +566,7 @@ void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
|
||||
numMeas : raw.numMeas,
|
||||
recStat : raw.recStat
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&header, sizeof(header));
|
||||
DataFlash_Class::instance()->WriteBlock(&header, sizeof(header));
|
||||
|
||||
for (uint8_t i=0; i<raw.numMeas; i++) {
|
||||
struct log_GPS_RAWS pkt = {
|
||||
@ -583,7 +585,7 @@ void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
|
||||
doStdev : raw.svinfo[i].doStdev,
|
||||
trkStat : raw.svinfo[i].trkStat
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
#endif // UBLOX_RXM_RAW_LOGGING
|
||||
@ -1335,7 +1337,7 @@ void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const
|
||||
AP_GPS_Backend::Write_DataFlash_Log_Startup_messages();
|
||||
|
||||
if (_have_version) {
|
||||
gps._DataFlash->Log_Write_MessageF("u-blox %d HW: %s SW: %s",
|
||||
DataFlash_Class::instance()->Log_Write_MessageF("u-blox %d HW: %s SW: %s",
|
||||
state.instance+1,
|
||||
_version.hwVersion,
|
||||
_version.swVersion);
|
||||
|
@ -167,5 +167,20 @@ void AP_GPS_Backend::Write_DataFlash_Log_Startup_messages() const
|
||||
{
|
||||
char buffer[64];
|
||||
_detection_message(buffer, sizeof(buffer));
|
||||
gps._DataFlash->Log_Write_Message(buffer);
|
||||
DataFlash_Class::instance()->Log_Write_Message(buffer);
|
||||
}
|
||||
|
||||
bool AP_GPS_Backend::should_df_log() const
|
||||
{
|
||||
DataFlash_Class *instance = DataFlash_Class::instance();
|
||||
if (instance == nullptr) {
|
||||
return false;
|
||||
}
|
||||
if (gps._log_gps_bit == (uint32_t)-1) {
|
||||
return false;
|
||||
}
|
||||
if (!instance->should_log(gps._log_gps_bit)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
@ -82,4 +82,6 @@ protected:
|
||||
void make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds);
|
||||
|
||||
void _detection_message(char *buffer, uint8_t buflen) const;
|
||||
|
||||
bool should_df_log() const;
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user