From 8f94a0cfb0a5d8ea9d6ecdccfbe2a76f8163e39d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 3 Apr 2020 13:38:37 +0900 Subject: [PATCH] AP_VisualOdom: integrate IntelT265 backend --- libraries/AP_VisualOdom/AP_VisualOdom.cpp | 13 +- libraries/AP_VisualOdom/AP_VisualOdom.h | 5 +- .../AP_VisualOdom/AP_VisualOdom_Backend.cpp | 173 ------------------ .../AP_VisualOdom/AP_VisualOdom_Backend.h | 31 +--- 4 files changed, 23 insertions(+), 199 deletions(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index f8ec6b4763..4d7997fc00 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -16,6 +16,7 @@ #include "AP_VisualOdom.h" #include "AP_VisualOdom_Backend.h" #include "AP_VisualOdom_MAV.h" +#include "AP_VisualOdom_IntelT265.h" #include #include @@ -27,7 +28,7 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = { // @Param: _TYPE // @DisplayName: Visual odometry camera connection type // @Description: Visual odometry camera connection type - // @Values: 0:None,1:MAV + // @Values: 0:None,1:MAV,2:IntelT265 // @User: Advanced // @RebootRequired: True AP_GROUPINFO_FLAGS("_TYPE", 0, AP_VisualOdom, _type, 0, AP_PARAM_FLAG_ENABLE), @@ -82,8 +83,16 @@ AP_VisualOdom::AP_VisualOdom() void AP_VisualOdom::init() { // create backend - if (_type == AP_VisualOdom_Type_MAV) { + switch (_type) { + case AP_VisualOdom_Type_None: + // do nothing + break; + case AP_VisualOdom_Type_MAV: _driver = new AP_VisualOdom_MAV(*this); + break; + case AP_VisualOdom_Type_IntelT265: + _driver = new AP_VisualOdom_IntelT265(*this); + break; } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 83e30c68d4..aaaf76b979 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -36,8 +36,9 @@ public: // external position backend types (used by _TYPE parameter) enum AP_VisualOdom_Type { - AP_VisualOdom_Type_None = 0, - AP_VisualOdom_Type_MAV = 1 + AP_VisualOdom_Type_None = 0, + AP_VisualOdom_Type_MAV = 1, + AP_VisualOdom_Type_IntelT265 = 2 }; // detect and initialise any sensors diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 6e7752f7ad..038a176d9f 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -34,176 +34,3 @@ bool AP_VisualOdom_Backend::healthy() const // 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 -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; - - // record time for health monitoring - _last_update_ms = AP_HAL::millis(); -} - -// 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; -} diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 229b488c64..5f2aa9c09a 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -29,17 +29,16 @@ public: bool healthy() const; // consume vision_position_delta mavlink messages - virtual void handle_vision_position_delta_msg(const mavlink_message_t &msg) {}; + virtual void handle_vision_position_delta_msg(const mavlink_message_t &msg) = 0; - // 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, const Quaternion &attitude); + // 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) = 0; - // calibrate camera attitude to align with vehicle's AHRS/EKF attitude - void align_sensor_to_vehicle() { _align_camera = true; } + // handle request to align camera's attitude with vehicle's AHRS/EKF attitude + virtual void align_sensor_to_vehicle() {} - // arming check - bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const; + // arming check - by default no checks performed + virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; } protected: @@ -52,18 +51,6 @@ 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); - 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) - uint32_t _last_update_ms; // system time of last update from sensor (used by health checks) + AP_VisualOdom &_frontend; // reference to frontend + uint32_t _last_update_ms; // system time of last update from sensor (used by health checks) };