aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DAL: fixed GPS backend for multiple sensors

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-07 07:46:15 +11:00
parent bf61910c7c
commit 1848491ac2
3 changed files with 7 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_DAL/AP_DAL.h
View File

@ -308,7 +308,9 @@ private:
}; };
#define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end)) #define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))
#define WRITE_REPLAY_BLOCK_IFCHANGD(sname,v,old) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end)) #define WRITE_REPLAY_BLOCK_IFCHANGD(sname,v,old) do { static_assert(sizeof(v) == sizeof(old), "types must match"); \
AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end)); } \
while (0)
namespace AP { namespace AP {
AP_DAL &dal(); AP_DAL &dal();

12
libraries/AP_DAL/AP_DAL_GPS.cpp
View File

@ -23,16 +23,11 @@ void AP_DAL_GPS::start_frame()
for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) { for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
log_RGPI &RGPI = _RGPI[i]; log_RGPI &RGPI = _RGPI[i];
const log_RGPI old_RGPI = RGPI;
RGPI.status = (GPS_Status)gps.status(i); RGPI.status = (GPS_Status)gps.status(i);
const uint32_t last_message_time_ms = gps.last_message_time_ms(i); const uint32_t last_message_time_ms = gps.last_message_time_ms(i);
if (last_message_time_ms == _last_logged_message_time_ms[i]) {
// assume that no other state changes if we don't get a message.
return;
}
_last_logged_message_time_ms[i] = last_message_time_ms;
const Location &loc = gps.location(i); const Location &loc = gps.location(i);
RGPI.last_message_time_ms = last_message_time_ms; RGPI.last_message_time_ms = last_message_time_ms;
RGPI.lat = loc.lat; RGPI.lat = loc.lat;
@ -45,14 +40,15 @@ void AP_DAL_GPS::start_frame()
RGPI.hdop = gps.get_hdop(i); RGPI.hdop = gps.get_hdop(i);
RGPI.num_sats = gps.num_sats(i); RGPI.num_sats = gps.num_sats(i);
RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec); RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);
WRITE_REPLAY_BLOCK(RGPI, RGPI); WRITE_REPLAY_BLOCK_IFCHANGD(RGPI, RGPI, old_RGPI);
log_RGPJ &RGPJ = _RGPJ[i]; log_RGPJ &RGPJ = _RGPJ[i];
const log_RGPJ old_RGPJ = RGPJ;
RGPJ.velocity = gps.velocity(i); RGPJ.velocity = gps.velocity(i);
RGPJ.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc); RGPJ.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc);
RGPJ.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg); RGPJ.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg);
WRITE_REPLAY_BLOCK(RGPJ, RGPJ); WRITE_REPLAY_BLOCK_IFCHANGD(RGPJ, RGPJ, old_RGPJ);
// also fetch antenna offset for this frame // also fetch antenna offset for this frame
antenna_offset[i] = gps.get_antenna_offset(i); antenna_offset[i] = gps.get_antenna_offset(i);

2
libraries/AP_DAL/AP_DAL_GPS.h
View File

@ -150,6 +150,4 @@ private:
struct log_RGPJ _RGPJ[GPS_MAX_INSTANCES]; struct log_RGPJ _RGPJ[GPS_MAX_INSTANCES];
Vector3f antenna_offset[GPS_MAX_INSTANCES]; Vector3f antenna_offset[GPS_MAX_INSTANCES];
uint32_t _last_logged_message_time_ms[GPS_MAX_INSTANCES];
}; };