diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index c4276fe8968a52..b0f00af502e213 100644 --- 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 index d6feb1d515cc9e..a429a24c1d961f 100644 --- 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; ofsprintf("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 index 9d77a8d6eba978..174c7ca10e5b67 100644 --- 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,