From 5e5a0d21110e6e6be34f475ef9774ebead74dc95 Mon Sep 17 00:00:00 2001 From: chobits Date: Thu, 4 Jun 2020 15:54:19 +0800 Subject: [PATCH] AP_Logger: log ext nav pos err and ang err --- libraries/AP_Logger/AP_Logger.h | 2 +- libraries/AP_Logger/LogFile.cpp | 4 +++- libraries/AP_Logger/LogStructure.h | 6 +++++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index f5ba692e51..45b58a08a2 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, 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); 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); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 296134865f..79d9df0245 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -911,7 +911,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, 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) { const struct log_VisualPosition pkt_visualpos { LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG), @@ -924,6 +924,8 @@ void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, roll : roll, pitch : pitch, yaw : yaw, + pos_err : pos_err, + ang_err : ang_err, 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 9d9dae937d..0eaebb2f8d 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -625,6 +625,8 @@ struct PACKED log_VisualPosition { float roll; // degrees float pitch; // degrees float yaw; // degrees + float pos_err; + float ang_err; uint8_t reset_counter; }; @@ -2134,6 +2136,8 @@ struct PACKED log_Arm_Disarm { // @Field: Roll: Roll lean angle // @Field: Pitch: Pitch lean angle // @Field: Yaw: Yaw angle +// @Field: PosErr: Position estimate error +// @Field: AngErr: Attitude estimate error // @Field: ResetCnt: Position reset counter // @LoggerMessage: VISV @@ -2504,7 +2508,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", "QQIffffffb", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,ResetCnt", "sssmmmddh-", "FFC000000-" }, \ + "VISP", "QQIffffffffb", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PosErr,AngErr,ResetCnt", "sssmmmddhmd-", "FFC00000000-" }, \ { LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \ "VISV", "QQIffffb", "TimeUS,RemTimeUS,CTimeMS,VX,VY,VZ,VErr,ResetCnt", "sssnnnn-", "FFC0000-" }, \ { LOG_OPTFLOW_MSG, sizeof(log_Optflow), \