diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 66767b1290..be6422739e 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include "DataFlash.h" #include "DataFlash_File.h" @@ -290,13 +291,16 @@ void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, uint64_t time_ // Write an RFND (rangefinder) packet void DataFlash_Class::Log_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 : rangefinder.distance_cm(0), - orient1 : rangefinder.get_orientation(0), - dist2 : rangefinder.distance_cm(1), - orient2 : rangefinder.get_orientation(1) + dist1 : s0 ? s0->distance_cm() : (uint16_t)0, + orient1 : s0 ? s0->orientation() : ROTATION_NONE, + dist2 : s1 ? s1->distance_cm() : (uint16_t)0, + orient2 : s1 ? s1->orientation() : ROTATION_NONE, }; WriteBlock(&pkt, sizeof(pkt)); }