AP_VisualOdom: add IntelT265 backend

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -0,0 +1,193 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_VisualOdom_IntelT265.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;
+
+// consume vision position estimate data and send to EKF. distances in meters
+void AP_VisualOdom_IntelT265::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;
+
+    // record time for health monitoring
+    _last_update_ms = AP_HAL::millis();
+}
+
+// apply rotation and correction to position
+void AP_VisualOdom_IntelT265::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_IntelT265::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_IntelT265::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)) {
+        _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_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
+{
+    // exit immediately if not healthy
+    if (!healthy()) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "VisualOdom not healthy");
+        return false;
+    }
+
+    // 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_IntelT265.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "AP_VisualOdom_Backend.h"
+
+class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend
+{
+
+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) override;
+
+    // handle request to align camera's attitude with vehicle's AHRS/EKF attitude
+    void align_sensor_to_vehicle() override { _align_camera = true; }
+
+    // arming check
+    bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
+
+protected:
+
+    // 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);
+
+    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 _error_orientation;                    // true if the orientation is not supported
+    Quaternion _attitude_last;                  // last attitude received from camera (used for arming checks)
+};