AP_VisualOdom: add handle_vision_position_estimate

also add pre_arm_check

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -147,6 +147,73 @@ void AP_VisualOdom::handle_msg(const mavlink_message_t &msg)
     }
 }
 
+// general purpose method to consume position estimate data and send to EKF
+// distances in meters, roll, pitch and yaw are in radians
+void AP_VisualOdom::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)
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    // call backend
+    if (_driver != nullptr) {
+        _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, roll, pitch, yaw);
+    }
+}
+
+// general purpose method to consume position estimate data and send to EKF
+void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    // call backend
+    if (_driver != nullptr) {
+        _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude);
+    }
+}
+
+// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
+void AP_VisualOdom::align_sensor_to_vehicle()
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    // call backend
+    if (_driver != nullptr) {
+        _driver->align_sensor_to_vehicle();
+    }
+}
+
+// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
+bool AP_VisualOdom::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return true;
+    }
+
+    // check healthy
+    if (!healthy()) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom not healthy");
+        return false;
+    }
+
+    // if no backend we must have failed to create because out of memory
+    if (_driver == nullptr) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom out of memory");
+        return false;
+    }
+
+    // call backend specific arming check
+    return _driver->pre_arm_check(failure_msg, failure_msg_len);
+}
+
 // singleton instance
 AP_VisualOdom *AP_VisualOdom::_singleton;
 

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -70,6 +70,17 @@ public:
     // consume data from MAVLink messages
     void handle_msg(const mavlink_message_t &msg);
 
+    // general purpose methods to consume position estimate data and send to EKF
+    // 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);
+    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude);
+
+    // calibrate camera attitude to align with vehicle's AHRS/EKF attitude
+    void align_sensor_to_vehicle();
+
+    // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
+    bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:

libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp

@@ -14,6 +14,10 @@
  */
 
 #include "AP_VisualOdom_Backend.h"
+#include <AP_Logger/AP_Logger.h>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL &hal;
 
 /*
   base class constructor. 
@@ -39,3 +43,182 @@ void AP_VisualOdom_Backend::set_deltas(const Vector3f &angle_delta, const Vector
     _frontend._state.confidence = confidence;
     _frontend._state.last_sensor_update_ms = AP_HAL::millis();
 }
+
+// general purpose method to send position estimate data to EKF
+// distances in meters, roll, pitch and yaw are in radians
+void AP_VisualOdom_Backend::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)
+{
+    Quaternion attitude;
+    attitude.from_euler(roll, pitch, yaw);
+    handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude);
+}
+
+// general purpose method to send position estimate data to EKF
+void AP_VisualOdom_Backend::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
+{
+    Vector3f pos = {x, y, z};
+    Quaternion att = attitude;
+
+    // handle user request to align camera
+    if (_align_camera) {
+        if (align_sensor_to_vehicle(pos, attitude)) {
+            _align_camera = false;
+        }
+    }
+
+    // rotate position and attitude to align with vehicle
+    rotate_and_correct_position(pos);
+    rotate_attitude(att);
+
+    // send attitude and position to EKF
+    const float posErr = 0; // parameter required?
+    const float angErr = 0; // parameter required?
+    const uint32_t reset_timestamp_ms = 0; // no data available
+    AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, reset_timestamp_ms);
+
+    // calculate euler orientation for logging
+    float roll;
+    float pitch;
+    float yaw;
+    att.to_euler(roll, pitch, yaw);
+
+    // log sensor data
+    AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw));
+
+    // store corrected attitude for use in pre-arm checks
+    _attitude_last = att;
+    _have_attitude = true;
+}
+
+// apply rotation and correction to position
+void AP_VisualOdom_Backend::rotate_and_correct_position(Vector3f &position) const
+{
+    if (_use_pos_rotation) {
+        position = _pos_rotation * position;
+    }
+    position += _pos_correction;
+}
+
+// rotate attitude using _yaw_trim
+void AP_VisualOdom_Backend::rotate_attitude(Quaternion &attitude) const
+{
+    // apply orientation rotation
+    if (_use_att_rotation) {
+        attitude *= _att_rotation;
+    }
+
+    // apply earth-frame yaw rotation
+    if (!is_zero(_yaw_trim)) {
+        attitude = _yaw_rotation * attitude;
+    }
+    return;
+}
+
+// use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude
+bool AP_VisualOdom_Backend::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude)
+{
+    // fail immediately if ahrs cannot provide attitude
+    Quaternion ahrs_quat;
+    if (!AP::ahrs().get_quaternion(ahrs_quat)) {
+        return false;
+    }
+
+    // if ahrs's yaw is from the compass, wait until it has been initialised
+    if (!AP::ahrs().is_ext_nav_used_for_yaw() && !AP::ahrs().yaw_initialised()) {
+        return false;
+    }
+
+    // clear any existing errors
+    _error_orientation = false;
+
+    // create rotation quaternion to correct for orientation
+    const Rotation rot = _frontend.get_orientation();
+    _att_rotation.initialise();
+    _use_att_rotation = false;
+    if (rot != Rotation::ROTATION_NONE) {
+        _att_rotation.rotate(rot);
+        _att_rotation.invert();
+        _use_att_rotation = true;
+    }
+
+    Quaternion att_corrected = attitude;
+    att_corrected *= _att_rotation;
+
+    // extract sensor's corrected yaw
+    const float sens_yaw = att_corrected.get_euler_yaw();
+
+    // trim yaw by difference between ahrs and sensor yaw
+    Vector3f angle_diff;
+    ahrs_quat.angular_difference(att_corrected).to_axis_angle(angle_diff);
+    _yaw_trim = angle_diff.z;
+    gcs().send_text(MAV_SEVERITY_CRITICAL, "VisOdom: yaw shifted %d to %d deg",
+                    //(int)degrees(_yaw_trim - yaw_trim_orig),
+                    (int)degrees(_yaw_trim),
+                    (int)degrees(sens_yaw + _yaw_trim));
+
+    // convert _yaw_trim to _yaw_rotation to speed up processing later
+    _yaw_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
+
+    // calculate position with current rotation and correction
+    Vector3f pos_orig = position;
+    rotate_and_correct_position(pos_orig);
+
+    // create position rotation from yaw trim
+    _use_pos_rotation = false;
+    if (!is_zero(_yaw_trim)) {
+        Matrix3f trim_matrix;
+        _pos_rotation.from_euler(0.0f, 0.0f, _yaw_trim);
+        _use_pos_rotation = true;
+    }
+
+    // recalculate position with new rotation
+    Vector3f pos_new = position;
+    rotate_and_correct_position(pos_new);
+
+    // update position correction to remove change due to rotation
+    _pos_correction += (pos_orig - pos_new);
+
+    return true;
+}
+
+// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
+bool AP_VisualOdom_Backend::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
+{
+    // exit immediately if no attitude to check
+    if (!_have_attitude) {
+        return true;
+    }
+
+    // check for unsupported orientation
+    if (_error_orientation) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "check VISO_ORIENT parameter");
+        return false;
+    }
+
+    // get ahrs attitude
+    Quaternion ahrs_quat;
+    if (!AP::ahrs().get_quaternion(ahrs_quat)) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom waiting for AHRS attitude");
+        return false;
+    }
+
+    // get angular difference between AHRS and camera attitude
+    Vector3f angle_diff;
+    _attitude_last.angular_difference(ahrs_quat).to_axis_angle(angle_diff);
+
+    // check if roll and pitch is different by > 10deg (using NED so cannot determine whether roll or pitch specifically)
+    const float rp_diff_deg = degrees(safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)));
+    if (rp_diff_deg > 10.0f) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
+        return false;
+    }
+
+    // check if yaw is different by > 10deg
+    const float yaw_diff_deg = degrees(fabsf(angle_diff.z));
+    if (yaw_diff_deg > 10.0f) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);
+        return false;
+    }
+
+    return true;
+}

libraries/AP_VisualOdom/AP_VisualOdom_Backend.h

@@ -16,6 +16,7 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
 #include "AP_VisualOdom.h"
 
 class AP_VisualOdom_Backend
@@ -27,13 +28,43 @@ public:
     // consume VISION_POSITION_DELTA MAVLink message
 	virtual void handle_msg(const mavlink_message_t &msg) {};
 
+	// general purpose methods to consume position estimate data and send to EKF
+	// 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);
+    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude);
+
+    // calibrate camera attitude to align with vehicle's AHRS/EKF attitude
+    void align_sensor_to_vehicle() { _align_camera = true; }
+
+    // arming check
+    bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
+
 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;
+
+    // rotate attitude using _yaw_trim
+    void rotate_attitude(Quaternion &attitude) const;
+
+    // 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:
 
-    // references
-    AP_VisualOdom &_frontend;
+    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
+    Matrix3f _pos_rotation;                     // rotation to align position from sensor to earth frame.  use when _use_pos_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 _align_camera = true;                  // true if camera should be aligned to AHRS/EKF
+    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)
 };