diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 6b832cd782..6d958a3486 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -274,6 +274,7 @@ public: void Write_Beacon(AP_Beacon &beacon); void Write_Proximity(AP_Proximity &proximity); void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); + void Write_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest); void Write(const char *name, const char *labels, const char *fmt, ...); void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 79805db99b..45d3202f3a 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -1694,3 +1694,17 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points }; WriteBlock(&pkt_srtl, sizeof(pkt_srtl)); } + +void AP_Logger::Write_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest) +{ + struct log_OA pkt = { + LOG_PACKET_HEADER_INIT(LOG_OA_MSG), + time_us : AP_HAL::micros64(), + algorithm : algorithm, + final_lat : final_dest.lat, + final_lng : final_dest.lng, + oa_lat : oa_dest.lat, + oa_lng : oa_dest.lng, + }; + WriteBlock(&pkt, sizeof(pkt)); +} diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index c373b1d135..503a918bea 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1125,6 +1125,16 @@ struct PACKED log_SRTL { float D; }; +struct PACKED log_OA { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t algorithm; + int32_t final_lat; + int32_t final_lng; + int32_t oa_lat; + int32_t oa_lng; +}; + struct PACKED log_DSTL { LOG_PACKET_HEADER; uint64_t time_us; @@ -1337,7 +1347,9 @@ struct PACKED log_Arm_Disarm { { LOG_PERFORMANCE_MSG, sizeof(log_Performance), \ "PM", "QHHIIHIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,IntErr,SPICnt,I2CCnt", "s---b%--", "F---0A--" }, \ { LOG_SRTL_MSG, sizeof(log_SRTL), \ - "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" } + "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \ + { LOG_OA_MSG, sizeof(log_OA), \ + "OA","QBLLLL","TimeUS,Algo,DLat,DLng,OALat,OALng", "s-----", "F-GGGG" } // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \ @@ -1681,6 +1693,7 @@ enum LogMessages : uint8_t { LOG_ERROR_MSG, LOG_ADSB_MSG, LOG_ARM_DISARM_MSG, + LOG_OA_MSG, _LOG_LAST_MSG_ };