AP_VisualOdom: move visual odometry update function into AP_VisualOdom

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -84,6 +84,35 @@ bool AP_VisualOdom::enabled() const
     return ((_type != AP_VisualOdom_Type_None) && (_driver != nullptr));
 }
 
+// update visual odometry sensor
+void AP_VisualOdom::update()
+{
+    if (!enabled()) {
+        return;
+    }
+
+    // check for updates
+    if (_state.last_processed_sensor_update_ms == _state.last_sensor_update_ms) {
+        // This reading has already been processed
+        return;
+    }
+    _state.last_processed_sensor_update_ms = _state.last_sensor_update_ms;
+
+    const float time_delta_sec = get_time_delta_usec() / 1000000.0f;
+
+    AP::ahrs_navekf().writeBodyFrameOdom(get_confidence(),
+                                         get_position_delta(),
+                                         get_angle_delta(),
+                                         time_delta_sec,
+                                         get_last_update_ms(),
+                                         get_pos_offset());
+    // log sensor data
+    AP::logger().Write_VisualOdom(time_delta_sec,
+                                  get_angle_delta(),
+                                  get_position_delta(),
+                                  get_confidence());
+}
+
 // return true if sensor is basically healthy (we are receiving data)
 bool AP_VisualOdom::healthy() const
 {
@@ -92,7 +121,7 @@ bool AP_VisualOdom::healthy() const
     }
 
     // healthy if we have received sensor messages within the past 300ms
-    return ((AP_HAL::millis() - _state.last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
+    return ((AP_HAL::millis() - _state.last_sensor_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
 }
 
 // consume VISION_POSITION_DELTA MAVLink message

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -49,29 +49,28 @@ public:
         Vector3f position_delta;    // position delta (in meters) of most recent update
         uint64_t time_delta_usec;   // time delta (in usec) between previous and most recent update
         float confidence;           // confidence expressed as a value from 0 (no confidence) to 100 (very confident)
-        uint32_t last_update_ms;    // system time (in milliseconds) of last update from sensor
+        uint32_t last_sensor_update_ms;    // system time (in milliseconds) of last update from sensor
+        uint32_t last_processed_sensor_update_ms; // timestamp of last sensor update that was processed
+
     };
 
     // detect and initialise any sensors
     void init();
 
+    // should be called really, really often.  The faster you call
+    // this the lower the latency of the data fed to the estimator.
+    void update();
+
     // return true if sensor is enabled
     bool enabled() const;
 
     // return true if sensor is basically healthy (we are receiving data)
     bool healthy() const;
 
-    // state accessors
-    const Vector3f &get_angle_delta() const { return _state.angle_delta; }
-    const Vector3f &get_position_delta() const { return _state.position_delta; }
-    uint64_t get_time_delta_usec() const { return _state.time_delta_usec; }
-    float get_confidence() const { return _state.confidence; }
-    uint32_t get_last_update_ms() const { return _state.last_update_ms; }
-
     // return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
     const Vector3f &get_pos_offset(void) const { return _pos_offset; }
 
-    // consume VISUAL_POSITION_DELTA data from MAVLink messages
+    // consume data from MAVLink messages
     void handle_msg(mavlink_message_t *msg);
 
     static const struct AP_Param::GroupInfo var_info[];
@@ -80,6 +79,13 @@ private:
 
     static AP_VisualOdom *_singleton;
 
+    // state accessors
+    const Vector3f &get_angle_delta() const { return _state.angle_delta; }
+    const Vector3f &get_position_delta() const { return _state.position_delta; }
+    uint64_t get_time_delta_usec() const { return _state.time_delta_usec; }
+    float get_confidence() const { return _state.confidence; }
+    uint32_t get_last_update_ms() const { return _state.last_sensor_update_ms; }
+
     // parameters
     AP_Int8 _type;
     AP_Vector3f _pos_offset;    // position offset of the camera in the body frame

libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp

@@ -37,5 +37,5 @@ void AP_VisualOdom_Backend::set_deltas(const Vector3f &angle_delta, const Vector
 
     _frontend._state.time_delta_usec = time_delta_usec;
     _frontend._state.confidence = confidence;
-    _frontend._state.last_update_ms = AP_HAL::millis();
+    _frontend._state.last_sensor_update_ms = AP_HAL::millis();
 }