AP_VisualOdom: support VISION_SPEED_ESTIMATE

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()
 {

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;

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() {}
 

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) {
+    if (_use_posvel_rotation) {
-        position = _pos_rotation * position;
+        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
+    // create position and velocity rotation from yaw trim
-    _use_pos_rotation = false;
+    _use_posvel_rotation = false;
     if (!is_zero(_yaw_trim)) {
-        _pos_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
+        _posvel_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
-        _use_pos_rotation = true;
+        _use_posvel_rotation = true;
     }
 
     // recalculate position with new rotation

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)

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

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