diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index edeede6611..cae2c06956 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 in meters + // @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 in radians + // @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 5fe410c10b..66c53eedc2 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -73,13 +73,19 @@ public: // 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 @@ private: 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 0ad1160651..479b357188 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -31,7 +31,7 @@ public: 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 2f44431097..f91360ed9d 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 1a01bb5535..9c6961b2bf 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -12,7 +12,7 @@ public: 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 7173b99bc3..84a81bf221 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 99c288bd78..02556d1922 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h @@ -12,7 +12,7 @@ public: 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;