From 66a5f645d79d26435ea037ff927e649831693952 Mon Sep 17 00:00:00 2001 From: chobits Date: Wed, 13 May 2020 16:30:40 +0800 Subject: [PATCH] AP_VisualOdom: support VISION_SPEED_ESTIMATE --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 21 +++++++++++ libraries/AP_VisualOdom/AP_VisualOdom.h | 8 +++++ .../AP_VisualOdom/AP_VisualOdom_Backend.h | 3 ++ .../AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 36 +++++++++++++++---- .../AP_VisualOdom/AP_VisualOdom_IntelT265.h | 10 ++++-- libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 11 ++++++ libraries/AP_VisualOdom/AP_VisualOdom_MAV.h | 3 ++ 7 files changed, 84 insertions(+), 8 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index 2f62b4186b..edeede6611 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -82,6 +82,14 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = { // @User: Advanced AP_GROUPINFO("_DELAY_MS", 4, AP_VisualOdom, _delay_ms, 10), + // @Param: _VEL_M_NSE + // @DisplayName: Visual odometry velocity measurement noise + // @Description: Visual odometry velocity measurement noise in m/s + // @Units: m/s + // @Range: 0.05 5.0 + // @User: Advanced + AP_GROUPINFO("_VEL_M_NSE", 5, AP_VisualOdom, _vel_noise, 0.1), + AP_GROUPEND }; @@ -178,6 +186,19 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin } } +void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) +{ + // exit immediately if not enabled + if (!enabled()) { + return; + } + + // call backend + if (_driver != nullptr) { + _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter); + } +} + // calibrate camera attitude to align with vehicle's AHRS/EKF attitude void AP_VisualOdom::align_sensor_to_vehicle() { diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 62f673a532..5fe410c10b 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -71,6 +71,9 @@ public: // return the sensor delay in milliseconds (see _DELAY_MS parameter) uint16_t get_delay_ms() const { return MAX(0, _delay_ms); } + // return velocity measurement noise in m/s + float get_vel_noise() const { return _vel_noise; } + // consume vision_position_delta mavlink messages void handle_vision_position_delta_msg(const mavlink_message_t &msg); @@ -78,6 +81,10 @@ public: // 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, const Quaternion &attitude, uint8_t reset_counter); + + // 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); // calibrate camera attitude to align with vehicle's AHRS/EKF attitude void align_sensor_to_vehicle(); @@ -97,6 +104,7 @@ private: AP_Int8 _orientation; // camera orientation on vehicle frame 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 // 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 5656e9dd95..0ad1160651 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -33,6 +33,9 @@ public: // 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; + // 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; + // handle request to align camera's attitude with vehicle's AHRS/EKF attitude virtual void align_sensor_to_vehicle() {} diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 62bcc2605f..53868bb0a0 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -62,15 +62,39 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti _last_update_ms = AP_HAL::millis(); } +// 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) +{ + // rotate velocity to align with vehicle + Vector3f vel_corrected = vel; + rotate_velocity(vel_corrected); + + // send velocity to EKF + AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms()); + + // record time for health monitoring + _last_update_ms = AP_HAL::millis(); + + AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter); +} + // apply rotation and correction to position void AP_VisualOdom_IntelT265::rotate_and_correct_position(Vector3f &position) const { - if (_use_pos_rotation) { - position = _pos_rotation * position; + if (_use_posvel_rotation) { + position = _posvel_rotation * position; } position += _pos_correction; } +// apply rotation to velocity +void AP_VisualOdom_IntelT265::rotate_velocity(Vector3f &velocity) const +{ + if (_use_posvel_rotation) { + velocity = _posvel_rotation * velocity; + } +} + // rotate attitude using _yaw_trim void AP_VisualOdom_IntelT265::rotate_attitude(Quaternion &attitude) const { @@ -135,11 +159,11 @@ bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, Vector3f pos_orig = position; rotate_and_correct_position(pos_orig); - // create position rotation from yaw trim - _use_pos_rotation = false; + // create position and velocity rotation from yaw trim + _use_posvel_rotation = false; if (!is_zero(_yaw_trim)) { - _pos_rotation.from_euler(0.0f, 0.0f, _yaw_trim); - _use_pos_rotation = true; + _posvel_rotation.from_euler(0.0f, 0.0f, _yaw_trim); + _use_posvel_rotation = true; } // recalculate position with new rotation diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index f0a9afd003..1a01bb5535 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -14,6 +14,9 @@ public: // 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; + // 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; + // handle request to align camera's attitude with vehicle's AHRS/EKF attitude void align_sensor_to_vehicle() override { _align_camera = true; } @@ -25,6 +28,9 @@ protected: // apply rotation and correction to position void rotate_and_correct_position(Vector3f &position) const; + // apply rotation to velocity + void rotate_velocity(Vector3f &velocity) const; + // rotate attitude using _yaw_trim void rotate_attitude(Quaternion &attitude) const; @@ -34,10 +40,10 @@ protected: float _yaw_trim; // yaw angle trim (in radians) to align camera's yaw to ahrs/EKF's Quaternion _yaw_rotation; // earth-frame yaw rotation to align heading of sensor with vehicle. use when _yaw_trim is non-zero Quaternion _att_rotation; // body-frame rotation corresponding to ORIENT parameter. use when get_orientation != NONE - Matrix3f _pos_rotation; // rotation to align position from sensor to earth frame. use when _use_pos_rotation is true + Matrix3f _posvel_rotation; // rotation to align position and/or velocity from sensor to earth frame. use when _use_posvel_rotation is true Vector3f _pos_correction; // position correction that should be added to position reported from sensor bool _use_att_rotation; // true if _att_rotation should be applied to sensor's attitude data - bool _use_pos_rotation; // true if _pos_rotation should be applied to sensor's position data + bool _use_posvel_rotation; // true if _posvel_rotation should be applied to sensor's position and/or velocity data bool _align_camera = true; // true if camera should be aligned to AHRS/EKF bool _error_orientation; // true if the orientation is not supported Quaternion _attitude_last; // last attitude received from camera (used for arming checks) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index cb94c71185..5a39c6c8cf 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -53,4 +53,15 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, _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) +{ + // send velocity to EKF + 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(); + + AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter); +} + #endif diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h index 824038f336..99c288bd78 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h @@ -13,6 +13,9 @@ public: // 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; + + // 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; }; #endif