AP_VisualOdom: use Odometry quality

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -106,6 +106,14 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f),
 
+    // @Param: _QUAL_MIN
+    // @DisplayName: Visual odometry minimum quality
+    // @Description: Visual odometry will only be sent to EKF if over this value. -1 to always send (even bad values), 0 to send if good or unknown
+    // @Units: %
+    // @Range: -1 100
+    // @User: Advanced
+    AP_GROUPINFO("_QUAL_MIN", 8, AP_VisualOdom, _quality_min, 0),
+
     AP_GROUPEND
 };
 
@@ -161,6 +169,16 @@ bool AP_VisualOdom::healthy() const
     return _driver->healthy();
 }
 
+// return quality as a measure from 0 ~ 100
+// -1 means failed, 0 means unknown, 1 is worst, 100 is best
+int8_t AP_VisualOdom::quality() const
+{
+    if (_driver == nullptr) {
+        return 0;
+    }
+    return _driver->quality();
+}
+
 #if HAL_GCS_ENABLED
 // consume vision_position_delta mavlink messages
 void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &msg)
@@ -179,7 +197,8 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms
 
 // general purpose method to consume position estimate data and send to EKF
 // distances in meters, roll, pitch and yaw are in radians
-void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     // exit immediately if not enabled
     if (!enabled()) {
@@ -191,12 +210,13 @@ void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_
         // convert attitude to quaternion and call backend
         Quaternion attitude;
         attitude.from_euler(roll, pitch, yaw);
-        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
+        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
     }
 }
 
 // general purpose method to consume position estimate data and send to EKF
-void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     // exit immediately if not enabled
     if (!enabled()) {
@@ -205,11 +225,14 @@ void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_
 
     // call backend
     if (_driver != nullptr) {
-        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
+        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
     }
 }
 
-void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
+// general purpose methods to consume velocity estimate data and send to EKF
+// velocity in NED meters per second
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
 {
     // exit immediately if not enabled
     if (!enabled()) {
@@ -218,7 +241,7 @@ void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32
 
     // call backend
     if (_driver != nullptr) {
-        _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter);
+        _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality);
     }
 }
 

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -83,6 +83,13 @@ public:
     // return yaw measurement noise in rad
     float get_yaw_noise() const { return _yaw_noise; }
 
+    // return quality threshold
+    int8_t get_quality_min() const { return _quality_min; }
+
+    // return quality as a measure from -1 ~ 100
+    // -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    int8_t quality() const;
+
 #if HAL_GCS_ENABLED
     // consume vision_position_delta mavlink messages
     void handle_vision_position_delta_msg(const mavlink_message_t &msg);
@@ -90,12 +97,14 @@ public:
 
     // general purpose methods to consume position estimate data and send to EKF
     // distances in meters, roll, pitch and yaw are in radians
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter);
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter);
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
     
     // general purpose methods to consume velocity estimate data and send to EKF
     // velocity in NED meters per second
-    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter);
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality);
 
     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
     void request_align_yaw_to_ahrs();
@@ -126,6 +135,7 @@ private:
     AP_Float _vel_noise;        // velocity measurement noise in m/s
     AP_Float _pos_noise;        // position measurement noise in meters
     AP_Float _yaw_noise;        // yaw measurement noise in radians
+    AP_Int8 _quality_min;       // positions and velocities will only be sent to EKF if over this value.  if 0 all values sent to EKF
 
     // reference to backends
     AP_VisualOdom_Backend *_driver;

libraries/AP_VisualOdom/AP_VisualOdom_Backend.h

@@ -29,16 +29,22 @@ public:
     // return true if sensor is basically healthy (we are receiving data)
     bool healthy() const;
 
+    // return quality as a measure from -1 ~ 100
+    // -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    int8_t quality() const { return _quality; }
+
 #if HAL_GCS_ENABLED
     // consume vision_position_delta mavlink messages
     void handle_vision_position_delta_msg(const mavlink_message_t &msg);
 #endif
 
     // consume vision pose estimate data and send to EKF. distances in meters
-    virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) = 0;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-    virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) = 0;
 
     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
     virtual void request_align_yaw_to_ahrs() {}
@@ -62,8 +68,8 @@ protected:
 #if HAL_LOGGING_ENABLED
     // Logging Functions
     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, bool ignored);
+    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, int8_t quality);
-    void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored);
+    void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality);
 #endif
 
     AP_VisualOdom &_frontend;   // reference to frontend
@@ -72,6 +78,9 @@ protected:
     // reset counter handling
     uint8_t _last_reset_counter;    // last sensor reset counter received
     uint32_t _reset_timestamp_ms;   // time reset counter was received
+
+    // quality
+    int8_t _quality;                // last recorded quality
 };
 
 #endif  // HAL_VISUALODOM_ENABLED

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -28,7 +28,8 @@
 extern const AP_HAL::HAL& hal;
 
 // consume vision pose estimate data and send to EKF. distances in meters
-void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     const float scale_factor = _frontend.get_pos_scale();
     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
@@ -59,8 +60,11 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
     posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
     angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
 
+    // record quality
+    _quality = quality;
+
     // check for recent position reset
-    bool consume = should_consume_sensor_data(true, reset_counter);
+    bool consume = should_consume_sensor_data(true, reset_counter) && (_quality >= _frontend.get_quality_min());
     if (consume) {
         // send attitude and position to EKF
         AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
@@ -74,7 +78,7 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
 
 #if HAL_LOGGING_ENABLED
     // log sensor data
-    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume);
+    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume, _quality);
 #endif
 
     // store corrected attitude for use in pre-arm checks
@@ -85,14 +89,18 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
 }
 
 // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
 {
     // rotate velocity to align with vehicle
     Vector3f vel_corrected = vel;
     rotate_velocity(vel_corrected);
 
+    // record quality
+    _quality = quality;
+
     // check for recent position reset
-    bool consume = should_consume_sensor_data(false, reset_counter);
+    bool consume = should_consume_sensor_data(false, reset_counter) && (_quality >= _frontend.get_quality_min());
     if (consume) {
         // send velocity to EKF
         AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
@@ -102,7 +110,7 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_
     _last_update_ms = AP_HAL::millis();
 
 #if HAL_LOGGING_ENABLED
-    Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume);
+    Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume, _quality);
 #endif
 }
 

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h

@@ -14,10 +14,12 @@ public:
     using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
 
     // consume vision pose estimate data and send to EKF. distances in meters
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) override;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
 
     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
     void request_align_yaw_to_ahrs() override { _align_yaw = true; }

libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp

@@ -24,7 +24,7 @@ void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &a
 }
 
 // Write visual position sensor data.  x,y,z are in meters, angles are in degrees
-void AP_VisualOdom_Backend::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 AP_VisualOdom_Backend::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, int8_t quality)
 {
     const struct log_VisualPosition pkt_visualpos {
         LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
@@ -40,13 +40,14 @@ void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32
         pos_err         : pos_err,
         ang_err         : ang_err,
         reset_counter   : reset_counter,
-        ignored         : (uint8_t)ignored
+        ignored         : (uint8_t)ignored,
+        quality         : quality
     };
     AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
 }
 
 // Write visual velocity sensor data, velocity in NED meters per second
-void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored)
+void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality)
 {
     const struct log_VisualVelocity pkt_visualvel {
         LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
@@ -58,7 +59,8 @@ void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32
         vel_z           : vel.z,
         vel_err         : _frontend.get_vel_noise(),
         reset_counter   : reset_counter,
-        ignored         : (uint8_t)ignored
+        ignored         : (uint8_t)ignored,
+        quality         : quality
     };
     AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
 }

libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

@@ -24,15 +24,23 @@
 #include <AP_Logger/AP_Logger.h>
 
 // consume vision pose estimate data and send to EKF. distances in meters
-void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     const float scale_factor =  _frontend.get_pos_scale();
     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
 
     posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
     angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
-    // send attitude and position to EKF
+
-    AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
+    // record quality
+    _quality = quality;
+
+    // send attitude and position to EKF if quality OK
+    bool consume = (_quality >= _frontend.get_quality_min());
+    if (consume) {
+        AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
+    }
 
     // calculate euler orientation for logging
     float roll;
@@ -42,23 +50,31 @@ void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t t
 
 #if HAL_LOGGING_ENABLED
     // log sensor data
-    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
+    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, !consume, _quality);
 #endif
 
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();
 }
 
-void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
+// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
 {
-    // send velocity to EKF
+    // record quality
-    AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
+    _quality = quality;
+
+    // send velocity to EKF if quality OK
+    bool consume = (_quality >= _frontend.get_quality_min());
+    if (consume) {
+        AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
+    }
 
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();
 
 #if HAL_LOGGING_ENABLED
-    Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false);
+    Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, !consume, _quality);
 #endif
 }
 

libraries/AP_VisualOdom/AP_VisualOdom_MAV.h

@@ -14,10 +14,12 @@ public:
     using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
 
     // consume vision pose estimate data and send to EKF. distances in meters
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) override;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
 };
 
 #endif  // AP_VISUALODOM_MAV_ENABLED

libraries/AP_VisualOdom/LogStructure.h

@@ -40,13 +40,14 @@ struct PACKED log_VisualOdom {
 // @Field: PX: Position X-axis (North-South)
 // @Field: PY: Position Y-axis (East-West)
 // @Field: PZ: Position Z-axis (Down-Up)
-// @Field: Roll: Roll lean angle
+// @Field: R: Roll lean angle
-// @Field: Pitch: Pitch lean angle
+// @Field: P: Pitch lean angle
-// @Field: Yaw: Yaw angle
+// @Field: Y: Yaw angle
 // @Field: PErr: Position estimate error
 // @Field: AErr: Attitude estimate error
 // @Field: Rst: Position reset counter
 // @Field: Ign: Ignored
+// @Field: Q: Quality
 struct PACKED log_VisualPosition {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -62,6 +63,7 @@ struct PACKED log_VisualPosition {
     float ang_err;  // radians
     uint8_t reset_counter;
     uint8_t ignored;
+    int8_t quality;
 };
 
 // @LoggerMessage: VISV
@@ -75,6 +77,7 @@ struct PACKED log_VisualPosition {
 // @Field: VErr: Velocity estimate error
 // @Field: Rst: Velocity reset counter
 // @Field: Ign: Ignored
+// @Field: Q: Quality
 struct PACKED log_VisualVelocity {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -86,6 +89,7 @@ struct PACKED log_VisualVelocity {
     float vel_err;
     uint8_t reset_counter;
     uint8_t ignored;
+    int8_t quality;
 };
 
 #if HAL_VISUALODOM_ENABLED
@@ -93,9 +97,9 @@ struct PACKED log_VisualVelocity {
     { 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", "QQIffffffffBB", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,Rst,Ign", "sssmmmddhmd--", "FFC00000000--" }, \
+      "VISP", "QQIffffffffBBb", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,R,P,Y,PErr,AErr,Rst,Ign,Q", "sssmmmddhmd--%", "FFC00000000--0" }, \
     { LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \
-      "VISV", "QQIffffBB", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign", "sssnnnn--", "FFC0000--" },
+      "VISV", "QQIffffBBb", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign,Q", "sssnnnn--%", "FFC0000--0" },
 #else
 #define LOG_STRUCTURE_FROM_VISUALODOM
 #endif