AP_DAL: fixed GPS backend for multiple sensors

libraries/AP_DAL/AP_DAL.h

@@ -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_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 {
     AP_DAL &dal();

libraries/AP_DAL/AP_DAL_GPS.cpp

@@ -23,16 +23,11 @@ void AP_DAL_GPS::start_frame()
 
     for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
         log_RGPI &RGPI = _RGPI[i];
+        const log_RGPI old_RGPI = RGPI;
 
         RGPI.status = (GPS_Status)gps.status(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);
         RGPI.last_message_time_ms = last_message_time_ms;
         RGPI.lat = loc.lat;
@@ -45,14 +40,15 @@ void AP_DAL_GPS::start_frame()
         RGPI.hdop = gps.get_hdop(i);
         RGPI.num_sats = gps.num_sats(i);
         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];
+        const log_RGPJ old_RGPJ = RGPJ;
 
         RGPJ.velocity = gps.velocity(i);
         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);
-        WRITE_REPLAY_BLOCK(RGPJ, RGPJ);
+        WRITE_REPLAY_BLOCK_IFCHANGD(RGPJ, RGPJ, old_RGPJ);
 
         // also fetch antenna offset for this frame
         antenna_offset[i] = gps.get_antenna_offset(i);

libraries/AP_DAL/AP_DAL_GPS.h

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