diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index d5687624b1..ba4f96454c 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 5e6499f67d..63ea267fa3 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/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: diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 4fff810948..efeaad3b89 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -14,6 +14,10 @@ */ #include "AP_VisualOdom_Backend.h" +#include +#include + +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; +} diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index 8dac2f36a1..3c9e9eecb2 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -16,6 +16,7 @@ #include #include +#include #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) };