Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: check dataflash to see if we should log backend gps messages

Browse Source
This commit is contained in:
Peter Barker 2017-06-27 18:12:45 +10:00 committed by Francisco Ferreira
parent 7cd794201e
commit 7258cfd20b
8 changed files with 53 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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;

11
libraries/AP_GPS/AP_GPS.h
View File

@ -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

4
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -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

10
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -393,7 +393,7 @@ void
AP_GPS_SBP::logging_log_full_update()
{
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
if (!should_df_log()) {
return;
}
@ -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,7 +413,7 @@ 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()) {
if (!should_df_log()) {
return;
}
@ -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));
}
};

14
libraries/AP_GPS/AP_GPS_SBP2.cpp
View File

@ -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));
};

22
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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);

17
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -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;
}

2
libraries/AP_GPS/GPS_Backend.h
View File

@ -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;
};