From 714975662d845018a6e9e7846df99e62ec8b0dd9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 29 May 2020 10:24:26 +0900 Subject: [PATCH] AP_Logger: VISP message gets velocity error field --- libraries/AP_Logger/AP_Logger.h | 2 +- libraries/AP_Logger/LogFile.cpp | 3 ++- libraries/AP_Logger/LogStructure.h | 3 ++- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index e5da130453..1c7eadb4dc 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -282,7 +282,7 @@ public: 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, uint8_t reset_counter); - void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, 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_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 d68024bbbf..296134865f 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -930,7 +930,7 @@ void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, } // 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, 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) { const struct log_VisualVelocity pkt_visualvel { LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG), @@ -940,6 +940,7 @@ void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, vel_x : vel.x, vel_y : vel.y, vel_z : vel.z, + vel_err : vel_err, reset_counter : reset_counter }; WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity)); diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 9d1aac13a5..f8718fca71 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -636,6 +636,7 @@ struct PACKED log_VisualVelocity { float vel_x; float vel_y; float vel_z; + float vel_err; uint8_t reset_counter; }; @@ -2494,7 +2495,7 @@ struct PACKED log_Arm_Disarm { { LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \ "VISP", "QQIffffffb", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,ResetCnt", "sssmmmddh-", "FFC000000-" }, \ { LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \ - "VISV", "QQIfffb", "TimeUS,RemTimeUS,CTimeMS,VX,VY,VZ,ResetCnt", "sssnnn-", "FFC000-" }, \ + "VISV", "QQIffffb", "TimeUS,RemTimeUS,CTimeMS,VX,VY,VZ,VErr,ResetCnt", "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), \