From 070ad226ce3a58e968089bfd70f12ee139c93683 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Fri, 26 Feb 2021 04:31:57 -0500 Subject: [PATCH] AP_Logger: OABR correct altitude logging frame and specifier --- libraries/AP_Logger/LogFile.cpp | 8 ++++++-- libraries/AP_Logger/LogStructure.h | 6 +++--- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 003b9d9817..2d5e961a32 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -627,6 +627,10 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points void AP_Logger::Write_OABendyRuler(uint8_t type, bool active, float target_yaw, float target_pitch, bool resist_chg, float margin, const Location &final_dest, const Location &oa_dest) { + int32_t oa_dest_alt, final_alt; + bool got_oa_dest = oa_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, oa_dest_alt); + bool got_final_dest = final_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, final_alt); + const struct log_OABendyRuler pkt{ LOG_PACKET_HEADER_INIT(LOG_OA_BENDYRULER_MSG), time_us : AP_HAL::micros64(), @@ -639,10 +643,10 @@ void AP_Logger::Write_OABendyRuler(uint8_t type, bool active, float target_yaw, margin : margin, final_lat : final_dest.lat, final_lng : final_dest.lng, - final_alt : final_dest.alt, + final_alt : got_final_dest ? final_alt : final_dest.alt, oa_lat : oa_dest.lat, oa_lng : oa_dest.lng, - oa_alt : oa_dest.alt + oa_alt : got_oa_dest ? oa_dest_alt : oa_dest.alt }; WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 932808724b..cdbe2e64dc 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1148,10 +1148,10 @@ struct PACKED log_PSCZ { // @Field: Mar: Margin from path to obstacle on best yaw chosen // @Field: DLt: Destination latitude // @Field: DLg: Destination longitude -// @Field: DAlt: Desired alt +// @Field: DAlt: Desired alt above EKF Origin // @Field: OLt: Intermediate location chosen for avoidance // @Field: OLg: Intermediate location chosen for avoidance -// @Field: OAlt: Intermediate alt chosen for avoidance +// @Field: OAlt: Intermediate alt chosen for avoidance above EKF origin // @LoggerMessage: OADJ // @Description: Object avoidance (Dijkstra) diagnostics @@ -1499,7 +1499,7 @@ LOG_STRUCTURE_FROM_CAMERA \ { LOG_SRTL_MSG, sizeof(log_SRTL), \ "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \ { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \ - "OABR","QBBHHHBfLLfLLf","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s-bddd-mDUmDUm", "F-------GGBGGB" }, \ + "OABR","QBBHHHBfLLiLLi","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s-bddd-mDUmDUm", "F-------GGBGGB" }, \ { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \ "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \ { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \