AP_Logger: VISP message gets velocity error field

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);

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));

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), \