diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index c457627a4176bb..a8551de10ea2b5 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -273,6 +273,22 @@ void AP_VisualOdom::align_position_to_ahrs(bool align_xy, bool align_z) } } +// align position to the given position expressed in meters offset from EKF origin +// and the given yaw expressed in radians from North +void AP_VisualOdom::align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad) +{ + // exit immediately if not enabled + if (!enabled()) { + return; + } + + // call backend + if (_driver != nullptr) { + _driver->align_position_and_yaw(pos_ned, yaw_rad); + } +} + + // 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 { diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h index 3f7323bf7d2f8e..488db9d1fda319 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom.h @@ -113,6 +113,10 @@ class AP_VisualOdom // should only be called when this library is not being used as the position source void align_position_to_ahrs(bool align_xy, bool align_z); + // align position to the given position expressed in meters offset from EKF origin + // and the given yaw expressed in radians from North + void align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad); + // 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; diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 683971140bfa71..f6c4f0ea32629b 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -90,4 +90,10 @@ uint32_t AP_VisualOdom_Backend::get_reset_timestamp_ms(uint8_t reset_counter) return _reset_timestamp_ms; } +// force an update of the reset timestamp (used when external caller updates position or yaw) +void AP_VisualOdom_Backend::force_update_reset_timestamp_ms() +{ + _reset_timestamp_ms = AP_HAL::millis(); +} + #endif diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index c669b8babb329e..b442fa75fe6cf6 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -52,6 +52,10 @@ class AP_VisualOdom_Backend // handle request to align position with AHRS virtual void align_position_to_ahrs(bool align_xy, bool align_z) {} + // align position to the given position expressed in meters offset from EKF origin + // and the given yaw expressed in radians from North + virtual void align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad) {}; + // arming check - by default no checks performed virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; } @@ -61,6 +65,9 @@ class AP_VisualOdom_Backend // updates the reset timestamp to the current system time if the reset_counter has changed uint32_t get_reset_timestamp_ms(uint8_t reset_counter); + // force an update of the reset timestamp (used when external caller updates position or yaw) + void force_update_reset_timestamp_ms(); + AP_VisualOdom::VisualOdom_Type get_type(void) const { return _frontend.get_type(); } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 744fac017b5147..b1f65d977f51e5 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -50,6 +50,14 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint } } + // handle external request to align position and yaw + if (ext_align_request.align) { + align_yaw(pos, attitude, ext_align_request.yaw_rad); + align_position(pos, ext_align_request.pos_ned, true, true); + ext_align_request.align = false; + force_update_reset_timestamp_ms(); + } + // rotate position and attitude to align with vehicle rotate_and_correct_position(pos); rotate_attitude(att); @@ -251,6 +259,15 @@ void AP_VisualOdom_IntelT265::align_position(const Vector3f &sensor_pos, const V } } +// align position to the given position expressed in meters offset from EKF origin +// and the given yaw expressed in radians from North +void AP_VisualOdom_IntelT265::align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad) +{ + // record new position and yaw + // offsets will be adjusted on the next update from the sensor + ext_align_request = {pos_ned, yaw_rad, 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 { diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 6103ec05dfa1b6..9728a5f67cfa8d 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -28,6 +28,10 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend // should only be called when this library is not being used as the position source void align_position_to_ahrs(bool align_xy, bool align_z) override { _align_posxy = align_xy; _align_posz = align_z; } + // align position to the given position expressed in meters offset from EKF origin + // and the given yaw expressed in radians from North + void align_position_and_yaw(const Vector3f& pos_ned, float yaw_rad) override; + // arming check bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; @@ -86,6 +90,11 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend Quaternion _attitude_last; // last attitude received from camera (used for arming checks) uint8_t _pos_reset_counter_last; // last vision-position-estimate reset counter value uint32_t _pos_reset_ignore_start_ms; // system time we start ignoring sensor information, 0 if sensor data is not being ignored + struct { + Vector3f pos_ned; // externally requested position in NED frame (meters offset from EKF origin) + float yaw_rad; // externally requested yaw in radians + bool align; // true when an extenrally requested pos_ned and yaw_rad have been provided + } ext_align_request; // voxl reset jump handling variables uint8_t _voxl_reset_counter_last; // last reset counter from voxl camera (only used for origin jump handling)