From d1360c14f8e2b0ccfc545c57f5b771dfbb7f6ec7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 20 Feb 2024 16:08:30 +0900 Subject: [PATCH] AP_VisualOdom: use Odometry quality --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 35 +++++++++++++++---- libraries/AP_VisualOdom/AP_VisualOdom.h | 16 +++++++-- .../AP_VisualOdom/AP_VisualOdom_Backend.h | 17 ++++++--- .../AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 20 +++++++---- .../AP_VisualOdom/AP_VisualOdom_IntelT265.h | 6 ++-- .../AP_VisualOdom/AP_VisualOdom_Logging.cpp | 10 +++--- libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 32 ++++++++++++----- libraries/AP_VisualOdom/AP_VisualOdom_MAV.h | 6 ++-- libraries/AP_VisualOdom/LogStructure.h | 14 +++++--- 9 files changed, 116 insertions(+), 40 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index d8cbcb05c4..35c97d4875 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/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); } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 47606abb97..3f7323bf7d 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/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); - 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); + // 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, 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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 9f5225aa9e..c669b8babb 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/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_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, 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, 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 diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index cc063d8590..c26bd38b71 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/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 } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 8a78bd1b7a..6103ec05df 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/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; } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp index 4a92a98360..80b9e1d70f 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp +++ b/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)); } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 5e4af959fa..48b6ec04e0 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -24,15 +24,23 @@ #include // 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 - AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms()); + // record quality + _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 } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h index 18d5fed709..c1d1c131dc 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h +++ b/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 diff --git a/libraries/AP_VisualOdom/LogStructure.h b/libraries/AP_VisualOdom/LogStructure.h index cb15e181c4..776c3f82b5 100644 --- a/libraries/AP_VisualOdom/LogStructure.h +++ b/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: Pitch: Pitch lean angle -// @Field: Yaw: Yaw angle +// @Field: R: Roll lean angle +// @Field: P: Pitch lean 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