AP_VisualOdom: handle_msg directly updates EKF

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -90,36 +90,7 @@ void AP_VisualOdom::init()
 // return true if sensor is enabled
 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 ((_type != AP_VisualOdom_Type_None));
 }
 
 // return true if sensor is basically healthy (we are receiving data)
@@ -129,8 +100,10 @@ bool AP_VisualOdom::healthy() const
         return false;
     }
 
-    // healthy if we have received sensor messages within the past 300ms
-    return ((AP_HAL::millis() - _state.last_sensor_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
+    if (_driver == nullptr) {
+        return false;
+    }
+    return _driver->healthy();
 }
 
 // consume VISION_POSITION_DELTA MAVLink message

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -27,6 +27,7 @@ class AP_VisualOdom
 {
 public:
     friend class AP_VisualOdom_Backend;
+    friend class AP_VisualOdom_MAV;
 
     AP_VisualOdom();
 
@@ -41,23 +42,9 @@ public:
         AP_VisualOdom_Type_MAV    = 1
     };
 
-    // The VisualOdomState structure is filled in by the backend driver
-    struct VisualOdomState {
-        Vector3f angle_delta;       // attitude delta (in radians) of most recent update
-        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_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;
 
@@ -87,12 +74,8 @@ 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; }
+    // get user defined orientation
+    enum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); }
 
     // parameters
     AP_Int8 _type;
@@ -101,9 +84,6 @@ private:
 
     // reference to backends
     AP_VisualOdom_Backend *_driver;
-
-    // state of backend
-    VisualOdomState _state;
 };
 
 namespace AP {

libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp

@@ -28,20 +28,11 @@ AP_VisualOdom_Backend::AP_VisualOdom_Backend(AP_VisualOdom &frontend) :
 {
 }
 
-// set deltas (used by backend to update state)
-void AP_VisualOdom_Backend::set_deltas(const Vector3f &angle_delta, const Vector3f& position_delta, uint64_t time_delta_usec, float confidence)
+// return true if sensor is basically healthy (we are receiving data)
+bool AP_VisualOdom_Backend::healthy() const
 {
-    // rotate and store angle_delta
-    _frontend._state.angle_delta = angle_delta;
-    _frontend._state.angle_delta.rotate((enum Rotation)_frontend._orientation.get());
-
-    // rotate and store position_delta
-    _frontend._state.position_delta = position_delta;
-    _frontend._state.position_delta.rotate((enum Rotation)_frontend._orientation.get());
-
-    _frontend._state.time_delta_usec = time_delta_usec;
-    _frontend._state.confidence = confidence;
-    _frontend._state.last_sensor_update_ms = AP_HAL::millis();
+    // healthy if we have received sensor messages within the past 300ms
+    return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
 }
 
 // general purpose method to send position estimate data to EKF
@@ -88,6 +79,9 @@ void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time
     // store corrected attitude for use in pre-arm checks
     _attitude_last = att;
     _have_attitude = true;
+
+    // record time for health monitoring
+    _last_update_ms = AP_HAL::millis();
 }
 
 // apply rotation and correction to position

libraries/AP_VisualOdom/AP_VisualOdom_Backend.h

@@ -25,6 +25,9 @@ public:
     // constructor. This incorporates initialisation as well.
 	AP_VisualOdom_Backend(AP_VisualOdom &frontend);
 
+	// return true if sensor is basically healthy (we are receiving data)
+	bool healthy() const;
+
     // consume VISION_POSITION_DELTA MAVLink message
 	virtual void handle_msg(const mavlink_message_t &msg) {};
 
@@ -41,9 +44,6 @@ public:
 
 protected:
 
-    // set deltas (used by backend to update state)
-    void set_deltas(const Vector3f &angle_delta, const Vector3f& position_delta, uint64_t time_delta_usec, float confidence);
-
     // apply rotation and correction to position
     void rotate_and_correct_position(Vector3f &position) const;
 
@@ -53,9 +53,8 @@ protected:
     // use sensor provided position and attitude to calculate rotation to align sensor with AHRS/EKF attitude
     bool align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude);
 
-private:
-
     AP_VisualOdom &_frontend;                   // reference to frontend
+
     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
@@ -67,4 +66,5 @@ private:
     bool _have_attitude;                        // true if we have received an attitude from the camera (used for arming checks)
     bool _error_orientation;                    // true if the orientation is not supported
     Quaternion _attitude_last;                  // last attitude received from camera (used for arming checks)
+    uint32_t _last_update_ms;                   // system time of last update from sensor (used by health checks)
 };

libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

@@ -13,9 +13,10 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <AP_HAL/AP_HAL.h>
 #include "AP_VisualOdom_MAV.h"
-#include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -32,7 +33,27 @@ void AP_VisualOdom_MAV::handle_msg(const mavlink_message_t &msg)
     mavlink_vision_position_delta_t packet;
     mavlink_msg_vision_position_delta_decode(&msg, &packet);
 
-    const Vector3f angle_delta(packet.angle_delta[0], packet.angle_delta[1], packet.angle_delta[2]);
-    const Vector3f position_delta(packet.position_delta[0], packet.position_delta[1], packet.position_delta[2]);
-    set_deltas(angle_delta, position_delta, packet.time_delta_usec, packet.confidence);
+    // 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
+    AP::ahrs_navekf().writeBodyFrameOdom(packet.confidence,
+                                         angle_delta,
+                                         position_delta,
+                                         packet.time_delta_usec,
+                                         now_ms,
+                                         _frontend.get_pos_offset());
+
+    // log sensor data
+    AP::logger().Write_VisualOdom(packet.time_delta_usec / 1000000.0f,
+                                  angle_delta,
+                                  position_delta,
+                                  packet.confidence);
 }