diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index f7d7057bfb..a621616877 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -11,7 +11,6 @@ #include #include #include -#include #include #include #include @@ -219,7 +218,6 @@ public: void Write_Error(LogErrorSubsystem sub_system, LogErrorCode error_code); void Write_GPS(uint8_t instance, uint64_t time_us=0); - void Write_RFND(const RangeFinder &rangefinder); void Write_IMU(); void Write_IMUDT(uint64_t time_us, uint8_t imu_mask); bool Write_ISBH(uint16_t seqno, diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 1e861fa55e..79805db99b 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include "AP_Logger.h" @@ -176,25 +175,6 @@ void AP_Logger::Write_GPS(uint8_t i, uint64_t time_us) } -// Write an RFND (rangefinder) packet -void AP_Logger::Write_RFND(const RangeFinder &rangefinder) -{ - AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0); - AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1); - - struct log_RFND pkt = { - LOG_PACKET_HEADER_INIT((uint8_t)(LOG_RFND_MSG)), - time_us : AP_HAL::micros64(), - dist1 : s0 ? s0->distance_cm() : (uint16_t)0, - status1 : s0 ? (uint8_t)s0->status() : (uint8_t)0, - orient1 : s0 ? s0->orientation() : ROTATION_NONE, - dist2 : s1 ? s1->distance_cm() : (uint16_t)0, - status2 : s1 ? (uint8_t)s1->status() : (uint8_t)0, - orient2 : s1 ? s1->orientation() : ROTATION_NONE, - }; - WriteBlock(&pkt, sizeof(pkt)); -} - // Write an RCIN packet void AP_Logger::Write_RCIN(void) {