From 4f639481c1f874c5c078305a259d649feff10d6f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 11 Apr 2020 09:52:57 +0900 Subject: [PATCH] AP_Logger: VISP msg adds reset_counter --- libraries/AP_Logger/AP_Logger.h | 2 +- libraries/AP_Logger/LogFile.cpp | 5 +++-- libraries/AP_Logger/LogStructure.h | 4 +++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 2e7e5734bf..5782d83c3d 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -281,7 +281,7 @@ public: uint8_t sequence, const RallyLocation &rally_point); void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence); - void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw); + void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter); void Write_AOA_SSA(AP_AHRS &ahrs); void Write_Beacon(AP_Beacon &beacon); void Write_Proximity(AP_Proximity &proximity); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 6ea69d13ff..20f8007ad3 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -915,7 +915,7 @@ void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, } // Write visual position sensor data. x,y,z are in meters, angles are in degrees -void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw) +void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter) { const struct log_VisualPosition pkt_visualpos { LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG), @@ -927,7 +927,8 @@ void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, pos_z : z, roll : roll, pitch : pitch, - yaw : yaw + yaw : yaw, + reset_counter : reset_counter }; WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index b745ff3502..cde873942a 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -662,6 +662,7 @@ struct PACKED log_VisualPosition { float roll; // degrees float pitch; // degrees float yaw; // degrees + uint8_t reset_counter; }; struct PACKED log_ekfBodyOdomDebug { @@ -2020,6 +2021,7 @@ struct PACKED log_Arm_Disarm { // @Field: Roll: Roll lean angle // @Field: Pitch: Pitch lean angle // @Field: Yaw: Yaw angle +// @Field: ResetCnt: Position reset counter // @LoggerMessage: WENC // @Description: Wheel encoder measurements @@ -2250,7 +2252,7 @@ struct PACKED log_Arm_Disarm { { LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \ "VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \ { LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \ - "VISP", "QQIffffff", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw", "sssmmmddh", "FFC000000" }, \ + "VISP", "QQIffffffb", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,ResetCnt", "sssmmmddh-", "FFC000000-" }, \ { LOG_OPTFLOW_MSG, sizeof(log_Optflow), \ "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" }, \ { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \