AP_VisualOdom: T265 supports vision-position-delta

implementation is the same for both backends so move to AP_VisualOdom_Backend

libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp

@@ -38,6 +38,39 @@ bool AP_VisualOdom_Backend::healthy() const
     return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
 }
 
+// consume vision_position_delta mavlink messages
+void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_message_t &msg)
+{
+    // decode message
+    mavlink_vision_position_delta_t packet;
+    mavlink_msg_vision_position_delta_decode(&msg, &packet);
+
+    // apply rotation to angle and position delta
+    const enum Rotation rot = _frontend.get_orientation();
+    Vector3f angle_delta = Vector3f(packet.angle_delta[0], packet.angle_delta[1], packet.angle_delta[2]);
+    angle_delta.rotate(rot);
+    Vector3f position_delta = Vector3f(packet.position_delta[0], packet.position_delta[1], packet.position_delta[2]);
+    position_delta.rotate(rot);
+
+    const uint32_t now_ms = AP_HAL::millis();
+    _last_update_ms = now_ms;
+
+    // send to EKF
+    const float time_delta_sec = packet.time_delta_usec / 1000000.0f;
+    AP::ahrs_navekf().writeBodyFrameOdom(packet.confidence,
+                                         position_delta,
+                                         angle_delta,
+                                         time_delta_sec,
+                                         now_ms,
+                                         _frontend.get_pos_offset());
+
+    // log sensor data
+    AP::logger().Write_VisualOdom(time_delta_sec,
+                                  angle_delta,
+                                  position_delta,
+                                  packet.confidence);
+}
+
 // returns the system time of the last reset if reset_counter has not changed
 // updates the reset timestamp to the current system time if the reset_counter has changed
 uint32_t AP_VisualOdom_Backend::get_reset_timestamp_ms(uint8_t reset_counter)

libraries/AP_VisualOdom/AP_VisualOdom_Backend.h

@@ -28,7 +28,7 @@ public:
 	bool healthy() const;
 
 	// consume vision_position_delta mavlink messages
-	virtual void handle_vision_position_delta_msg(const mavlink_message_t &msg) = 0;
+	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;

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h

@@ -11,9 +11,6 @@ public:
 
     using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
 
-    // ignore vision-position-delta messages from T265
-    void handle_vision_position_delta_msg(const mavlink_message_t &msg) override {};
-
     // 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;
 

libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

@@ -29,39 +29,6 @@ AP_VisualOdom_MAV::AP_VisualOdom_MAV(AP_VisualOdom &frontend) :
 {
 }
 
-// consume vision_position_delta mavlink messages
-void AP_VisualOdom_MAV::handle_vision_position_delta_msg(const mavlink_message_t &msg)
-{
-    // decode message
-    mavlink_vision_position_delta_t packet;
-    mavlink_msg_vision_position_delta_decode(&msg, &packet);
-
-    // apply rotation to angle and position delta
-    const enum Rotation rot = _frontend.get_orientation();
-    Vector3f angle_delta = Vector3f(packet.angle_delta[0], packet.angle_delta[1], packet.angle_delta[2]);
-    angle_delta.rotate(rot);
-    Vector3f position_delta = Vector3f(packet.position_delta[0], packet.position_delta[1], packet.position_delta[2]);
-    position_delta.rotate(rot);
-
-    const uint32_t now_ms = AP_HAL::millis();
-    _last_update_ms = now_ms;
-
-    // send to EKF
-    const float time_delta_sec = packet.time_delta_usec / 1000000.0f;
-    AP::ahrs_navekf().writeBodyFrameOdom(packet.confidence,
-                                         position_delta,
-                                         angle_delta,
-                                         time_delta_sec,
-                                         now_ms,
-                                         _frontend.get_pos_offset());
-
-    // log sensor data
-    AP::logger().Write_VisualOdom(time_delta_sec,
-                                  angle_delta,
-                                  position_delta,
-                                  packet.confidence);
-}
-
 // 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)
 {

libraries/AP_VisualOdom/AP_VisualOdom_MAV.h

@@ -11,9 +11,6 @@ public:
     // constructor
     AP_VisualOdom_MAV(AP_VisualOdom &frontend);
 
-    // consume vision_position_delta mavlink messages
-    void handle_vision_position_delta_msg(const mavlink_message_t &msg) override;
-
     // 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;
 };