AP_GPS: add support for raw callback when we receive TIM-TM2 message

CubePilot · May 10, 2024 · ff84fc5 · ff84fc5
1 parent 7290d4c
commit ff84fc5
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -1468,6 +1468,22 @@ AP_GPS_UBLOX::_parse_gps(void)
 
 #if UBLOX_TIM_TM2_LOGGING
     if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
+        if (gps._gps_raw_cb) {
+            uint8_t buf[sizeof(_buffer.rxm_rawx) + 8];
+            buf[0] = PREAMBLE1;
+            buf[1] = PREAMBLE2;
+            buf[2] = _class;
+            buf[3] = _msg_id;
+            buf[4] = _payload_length & 0xff;
+            buf[5] = _payload_length >> 8;
+            memcpy(&buf[6], &_buffer.tim_tm2, _payload_length);
+            // checksum
+            uint8_t ck_a = 0, ck_b = 0;
+            _update_checksum(&buf[2], _payload_length + 4, ck_a, ck_b);
+            buf[_payload_length+6] = ck_a;
+            buf[_payload_length+7] = ck_b;
+            gps._gps_raw_cb(buf, _payload_length + 8);
+        }
         log_tim_tm2();
         return false;
     }