From d9b90bf19f3c85ec80ec066ca1bc1ae1e964bff9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 21 Sep 2020 19:52:44 +0900 Subject: [PATCH] AP_Logger: VISP and VISV get ignored field --- libraries/AP_Logger/AP_Logger.h | 4 ++-- libraries/AP_Logger/LogFile.cpp | 10 ++++++---- libraries/AP_Logger/LogStructure.h | 12 ++++++++---- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index f1d59a3ac2..adf3f3276d 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -279,8 +279,8 @@ 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, float pos_err, float ang_err, uint8_t reset_counter); - void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter); + void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored); + void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, bool ignored); 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 3deed38b80..7b6273a98a 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -930,7 +930,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, float pos_err, float ang_err, uint8_t reset_counter) +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, float pos_err, float ang_err, uint8_t reset_counter, bool ignored) { const struct log_VisualPosition pkt_visualpos { LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG), @@ -945,13 +945,14 @@ void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, yaw : yaw, pos_err : pos_err, ang_err : ang_err, - reset_counter : reset_counter + reset_counter : reset_counter, + ignored : (uint8_t)ignored }; WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition)); } // Write visual velocity sensor data, velocity in NED meters per second -void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter) +void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, bool ignored) { const struct log_VisualVelocity pkt_visualvel { LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG), @@ -962,7 +963,8 @@ void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, vel_y : vel.y, vel_z : vel.z, vel_err : vel_err, - reset_counter : reset_counter + reset_counter : reset_counter, + ignored : (uint8_t)ignored }; WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity)); } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index f9cf584c56..8d9552969c 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -648,6 +648,7 @@ struct PACKED log_VisualPosition { float pos_err; // meters float ang_err; // radians uint8_t reset_counter; + uint8_t ignored; }; struct PACKED log_VisualVelocity { @@ -660,6 +661,7 @@ struct PACKED log_VisualVelocity { float vel_z; float vel_err; uint8_t reset_counter; + uint8_t ignored; }; struct PACKED log_ekfBodyOdomDebug { @@ -2268,7 +2270,8 @@ struct PACKED log_PSC { // @Field: Yaw: Yaw angle // @Field: PErr: Position estimate error // @Field: AErr: Attitude estimate error -// @Field: RstCnt: Position reset counter +// @Field: Rst: Position reset counter +// @Field: Ign: Ignored // @LoggerMessage: VISV // @Description: Vision Velocity @@ -2279,7 +2282,8 @@ struct PACKED log_PSC { // @Field: VY: Velocity Y-axis (East-West) // @Field: VZ: Velocity Z-axis (Down-Up) // @Field: VErr: Velocity estimate error -// @Field: RstCnt: Velocity reset counter +// @Field: Rst: Velocity reset counter +// @Field: Ign: Ignored // @LoggerMessage: WENC // @Description: Wheel encoder measurements @@ -2685,9 +2689,9 @@ struct PACKED log_PSC { { 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", "QQIffffffffB", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,RstCnt", "sssmmmddhmd-", "FFC00000000-" }, \ + "VISP", "QQIffffffffBB", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,Rst,Ign", "sssmmmddhmd--", "FFC00000000--" }, \ { LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \ - "VISV", "QQIffffB", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,RstCnt", "sssnnnn-", "FFC0000-" }, \ + "VISV", "QQIffffBB", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign", "sssnnnn--", "FFC0000--" }, \ { LOG_OPTFLOW_MSG, sizeof(log_Optflow), \ "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" }, \ { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \