diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 1c7eadb4dcd02..9ba58a2969693 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -281,7 +281,7 @@ class AP_Logger 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 296134865fe0b..79d9df0245c4f 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 9d9dae937d5d7..b15803c36cf5d 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; // meters + float ang_err; // radians uint8_t reset_counter; }; @@ -2126,7 +2128,7 @@ struct PACKED log_Arm_Disarm { // @LoggerMessage: VISP // @Description: Vision Position // @Field: TimeUS: System time -// @Field: RemTimeUS: Remote system time +// @Field: RTimeUS: Remote system time // @Field: CTimeMS: Corrected system time // @Field: PX: Position X-axis (North-South) // @Field: PY: Position Y-axis (East-West) @@ -2134,18 +2136,20 @@ struct PACKED log_Arm_Disarm { // @Field: Roll: Roll lean angle // @Field: Pitch: Pitch lean angle // @Field: Yaw: Yaw angle -// @Field: ResetCnt: Position reset counter +// @Field: PErr: Position estimate error +// @Field: AErr: Attitude estimate error +// @Field: RstCnt: Position reset counter // @LoggerMessage: VISV // @Description: Vision Velocity // @Field: TimeUS: System time -// @Field: RemTimeUS: Remote system time +// @Field: RTimeUS: Remote system time // @Field: CTimeMS: Corrected system time // @Field: VX: Velocity X-axis (North-South) // @Field: VY: Velocity Y-axis (East-West) // @Field: VZ: Velocity Z-axis (Down-Up) // @Field: VErr: Velocity estimate error -// @Field: ResetCnt: Position reset counter +// @Field: RstCnt: Velocity reset counter // @LoggerMessage: WENC // @Description: Wheel encoder measurements @@ -2504,9 +2508,9 @@ 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,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,RstCnt", "sssmmmddhmd-", "FFC00000000-" }, \ { LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \ - "VISV", "QQIffffb", "TimeUS,RemTimeUS,CTimeMS,VX,VY,VZ,VErr,ResetCnt", "sssnnnn-", "FFC0000-" }, \ + "VISV", "QQIffffb", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,RstCnt", "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), \ diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index e7442495f8634..9577283c005be 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -788,6 +788,7 @@ void NavEKF2_core::fuseEulerYaw() measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data + R_YAW = sq(extNavDataDelayed.angErr); extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z); measured_yaw = euler321.z; } else { @@ -847,6 +848,7 @@ void NavEKF2_core::fuseEulerYaw() measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination()); } else if (extNavUsedForYaw) { // Get the yaw angle from the external vision data + R_YAW = sq(extNavDataDelayed.angErr); euler312 = extNavDataDelayed.quat.to_vector312(); measured_yaw = euler312.z; } else { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index d59a6b7bd2584..85a65da9a3164 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -909,11 +909,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, extNavDataNew.pos = pos; extNavDataNew.quat = quat; - if (posErr > 0) { - extNavDataNew.posErr = posErr; - } else { - extNavDataNew.posErr = frontend->_gpsHorizPosNoise; - } + extNavDataNew.posErr = posErr; extNavDataNew.angErr = angErr; timeStamp_ms = timeStamp_ms - delay_ms; // Correct for the average intersampling delay due to the filter updaterate diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 34a8702c1222e..4f60e5ddca40a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -963,11 +963,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, } extNavDataNew.pos = pos; - if (posErr > 0) { - extNavDataNew.posErr = posErr; - } else { - extNavDataNew.posErr = frontend->_gpsHorizPosNoise; - } + extNavDataNew.posErr = posErr; // calculate timestamp timeStamp_ms = timeStamp_ms - delay_ms; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index edeede66113ee..d975e91fac2d6 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -90,6 +90,22 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = { // @User: Advanced AP_GROUPINFO("_VEL_M_NSE", 5, AP_VisualOdom, _vel_noise, 0.1), + // @Param: _POS_M_NSE + // @DisplayName: Visual odometry position measurement noise + // @Description: Visual odometry position measurement noise minimum (meters). This value will be used if the sensor provides a lower noise value (or no noise value) + // @Units: m + // @Range: 0.1 10.0 + // @User: Advanced + AP_GROUPINFO("_POS_M_NSE", 6, AP_VisualOdom, _pos_noise, 0.2f), + + // @Param: _YAW_M_NSE + // @DisplayName: Visual odometry yaw measurement noise + // @Description: Visual odometry yaw measurement noise minimum (radians), This value will be used if the sensor provides a lower noise value (or no noise value) + // @Units: rad + // @Range: 0.05 1.0 + // @User: Advanced + AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f), + AP_GROUPEND }; @@ -156,7 +172,7 @@ 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_vision_position_estimate(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_VisualOdom::handle_vision_position_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) { // exit immediately if not enabled if (!enabled()) { @@ -168,7 +184,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin // convert attitude to quaternion and call backend Quaternion attitude; attitude.from_euler(roll, pitch, yaw); - _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter); + _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter); } } @@ -182,7 +198,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin // call backend if (_driver != nullptr) { - _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter); + _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, 0, 0, reset_counter); } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 5fe410c10bc13..66c53eedc2024 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -73,13 +73,19 @@ class AP_VisualOdom // return velocity measurement noise in m/s float get_vel_noise() const { return _vel_noise; } + + // return position measurement noise in m + float get_pos_noise() const { return _pos_noise; } + + // return yaw measurement noise in rad + float get_yaw_noise() const { return _yaw_noise; } // consume vision_position_delta mavlink messages void handle_vision_position_delta_msg(const mavlink_message_t &msg); // general purpose methods to consume position estimate data and send to EKF // distances in meters, roll, pitch and yaw are in radians - void handle_vision_position_estimate(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 handle_vision_position_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_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter); // general purpose methods to consume velocity estimate data and send to EKF @@ -105,6 +111,8 @@ class AP_VisualOdom AP_Float _pos_scale; // position scale factor applied to sensor values AP_Int16 _delay_ms; // average delay relative to inertial measurements 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 // 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 0ad1160651c4a..479b3571884b7 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -31,7 +31,7 @@ class AP_VisualOdom_Backend void handle_vision_position_delta_msg(const mavlink_message_t &msg); // consume vision position estimate data and send to EKF. distances in meters - virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) = 0; + virtual void handle_vision_position_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; // 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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 2f44431097135..f91360ed9d6b4 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -24,7 +24,7 @@ extern const AP_HAL::HAL& hal; // consume vision position estimate data and send to EKF. distances in meters -void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) +void AP_VisualOdom_IntelT265::handle_vision_position_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) { const float scale_factor = _frontend.get_pos_scale(); Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor}; @@ -41,9 +41,9 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti rotate_and_correct_position(pos); rotate_attitude(att); + 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 - const float posErr = 0; // parameter required? - const float angErr = 0; // parameter required? AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter)); // calculate euler orientation for logging @@ -53,7 +53,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti att.to_euler(roll, pitch, yaw); // log sensor data - AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), reset_counter); + AP::logger().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); // store corrected attitude for use in pre-arm checks _attitude_last = att; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 1a01bb5535334..9c6961b2bfa29 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -12,7 +12,7 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend using AP_VisualOdom_Backend::AP_VisualOdom_Backend; // consume vision position estimate data and send to EKF. distances in meters - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override; + void handle_vision_position_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; // 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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 7173b99bc33ab..84a81bf221e47 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -30,14 +30,14 @@ AP_VisualOdom_MAV::AP_VisualOdom_MAV(AP_VisualOdom &frontend) : } // consume vision position estimate data and send to EKF. distances in meters -void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) +void AP_VisualOdom_MAV::handle_vision_position_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) { 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 - const float posErr = 0; // parameter required? - const float angErr = 0; // parameter required? AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter)); // calculate euler orientation for logging @@ -47,7 +47,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, attitude.to_euler(roll, pitch, yaw); // log sensor data - AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), reset_counter); + AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter); // record time for health monitoring _last_update_ms = AP_HAL::millis(); diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h index 99c288bd7800e..02556d1922e4c 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h @@ -12,7 +12,7 @@ class AP_VisualOdom_MAV : public AP_VisualOdom_Backend AP_VisualOdom_MAV(AP_VisualOdom &frontend); // consume vision position estimate data and send to EKF. distances in meters - void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override; + void handle_vision_position_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; // 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; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index d1a01b0b589e3..cf785d88a37d2 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -771,6 +771,7 @@ class GCS_MAVLINK const float roll, const float pitch, const float yaw, + const float covariance[21], const uint8_t reset_counter, const uint16_t payload_size); void handle_vision_speed_estimate(const mavlink_message_t &msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 56c14ffa40717..092efd45051da 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2926,7 +2926,7 @@ void GCS_MAVLINK::handle_vision_position_estimate(const mavlink_message_t &msg) mavlink_vision_position_estimate_t m; mavlink_msg_vision_position_estimate_decode(&msg, &m); - handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter, + handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter, PAYLOAD_SIZE(chan, VISION_POSITION_ESTIMATE)); } @@ -2935,7 +2935,7 @@ void GCS_MAVLINK::handle_global_vision_position_estimate(const mavlink_message_t mavlink_global_vision_position_estimate_t m; mavlink_msg_global_vision_position_estimate_decode(&msg, &m); - handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter, + handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter, PAYLOAD_SIZE(chan, GLOBAL_VISION_POSITION_ESTIMATE)); } @@ -2945,7 +2945,7 @@ void GCS_MAVLINK::handle_vicon_position_estimate(const mavlink_message_t &msg) mavlink_msg_vicon_position_estimate_decode(&msg, &m); // vicon position estimate does not include reset counter - handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, 0, + handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, 0, PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE)); } @@ -2959,10 +2959,13 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use const float roll, const float pitch, const float yaw, + const float covariance[21], const uint8_t reset_counter, const uint16_t payload_size) { #if HAL_VISUALODOM_ENABLED + float posErr = 0; + float angErr = 0; // correct offboard timestamp to be in local ms since boot uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec, payload_size); @@ -2970,7 +2973,13 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use if (visual_odom == nullptr) { return; } - visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, reset_counter); + + if (!isnan(covariance[0])) { + posErr = cbrtf(sq(covariance[0])+sq(covariance[6])+sq(covariance[11])); + angErr = cbrtf(sq(covariance[15])+sq(covariance[18])+sq(covariance[20])); + } + + visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter); #endif }