wip rtcm parse

jschall · Oct 16, 2023 · 7e85fdf · 7e85fdf
1 parent 1f9d543
commit 7e85fdf
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 3 deletions.
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
@@ -1650,7 +1650,6 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t &msg)
     }
 
     handle_gps_rtcm_fragment(packet.flags, packet.data, packet.len);
-    hal.console->printf("time: %u\n",(unsigned)AP_HAL::millis());
 }
 
 void AP_GPS::Write_AP_Logger_Log_Startup_messages()
@@ -2329,6 +2328,7 @@ void AP_GPS::rtcm_data_for_mavlink_send(uint8_t flags,uint32_t len, const uint8_
     RTCMPacketData packet;
     packet.flags = flags;
     packet.frag_len = len;
+    //hal.console->printf("%d",packet.data);
     memcpy(packet.data,data,len);
     _rtcmdatabuffer->push(packet);
     //try and send packet as many packets as possible

diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
@@ -558,7 +558,28 @@ void AP_GPS_DroneCAN::handle_moving_baseline_msg(const ardupilot_gnss_MovingBase
 
             const uint8_t* bytes;
             auto len = rtcm3_parser->get_len(bytes);
+            //print 1st byte and high 4 bits 
+            if ((unsigned)rtcm3_parser->get_id() == 1077){
+                hal.console->printf("%u %u\n",(unsigned)AP_HAL::millis(),(unsigned)rtcm3_parser->get_id());
+                hal.console->printf("%x %x %x %x %x %x %x %x %x %x %x\n",bytes[0],bytes[1],bytes[2],bytes[3],bytes[4],bytes[5],bytes[6],bytes[7],bytes[8],bytes[9],bytes[10]);
+                uint32_t gps_epoch = bytes[6];
+                for(uint8_t j = 6; j < 9;j++){
+                   gps_epoch = (gps_epoch << 8 | bytes[j+1]);
+                }
+                gps_epoch = gps_epoch >> 2;
+                hal.console->printf("%lu\n",gps_epoch);
+                hal.console->printf("%lx\n",gps_epoch);
 
+                if(gps_epoch % 1000 < 100 ){
+                   send_rtcm_packet = true;    
+                }
+                else{
+                    send_rtcm_packet = false;
+                }
+            }
+            if (send_rtcm_packet){
+                hal.console->printf("%u sending\n",rtcm3_parser->get_id());
+            }
             for (uint32_t ofs=0; ofs<len; ofs+=180) {
                 uint8_t frag_id = ofs/180;
                 uint8_t flags = 0;
@@ -572,8 +593,11 @@ void AP_GPS_DroneCAN::handle_moving_baseline_msg(const ardupilot_gnss_MovingBase
                     frag_len = 180;
                 }
                 // hal.console->printf("calling rtcm_data_for_mavlink_send\n");
-                gps.rtcm_data_for_mavlink_send(flags,frag_len,&bytes[ofs]);
+                if (send_rtcm_packet){
+                    gps.rtcm_data_for_mavlink_send(flags,frag_len,&bytes[ofs]);
+                }
                 //mavlink_msg_gps_rtcm_data_send(chan,flags,frag_len, &bytes[ofs]);
+
 
             }
         }

diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h
@@ -68,7 +68,7 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
 #endif
 
 private:
-
+    bool send_rtcm_packet = false;
     bool param_configured = true;
     enum config_step {
         STEP_SET_TYPE = 0,