diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 9c5ca366b2..c999a45ab7 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -21,6 +21,8 @@ #include #include +#define VISUALODOM_RESET_IGNORE_DURATION_MS 1000 // sensor data is ignored for 1sec after a position reset + extern const AP_HAL::HAL& hal; // consume vision position estimate data and send to EKF. distances in meters @@ -43,8 +45,13 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f); angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f); - // send attitude and position to EKF - AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter)); + + // check for recent position reset + bool consume = should_consume_sensor_data(true, reset_counter); + if (consume) { + // send attitude and position to EKF + AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter)); + } // calculate euler orientation for logging float roll; @@ -53,7 +60,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti att.to_euler(roll, pitch, yaw); // log sensor data - AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter); + AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume); // store corrected attitude for use in pre-arm checks _attitude_last = att; @@ -69,13 +76,17 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_ Vector3f vel_corrected = vel; rotate_velocity(vel_corrected); - // send velocity to EKF - AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms()); + // check for recent position reset + bool consume = should_consume_sensor_data(false, reset_counter); + if (consume) { + // send velocity to EKF + AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms()); + } // record time for health monitoring _last_update_ms = AP_HAL::millis(); - AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, _frontend.get_vel_noise(), reset_counter); + AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, _frontend.get_vel_noise(), reset_counter, !consume); } // apply rotation and correction to position @@ -219,4 +230,27 @@ bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_m return true; } +// returns true if sensor data should be consumed, false if it should be ignored +// set vision_position_estimate to true if reset_counter is from the VISION_POSITION_ESTIMATE source, false otherwise +// only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored +bool AP_VisualOdom_IntelT265::should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter) +{ + uint32_t now_ms = AP_HAL::millis(); + + // set ignore start time if reset counter has changed + if (vision_position_estimate) { + if (reset_counter != _pos_reset_counter_last) { + _pos_reset_counter_last = reset_counter; + _pos_reset_ignore_start_ms = now_ms; + } + } + + // check if 1 second has passed since the last reset + if ((now_ms - _pos_reset_ignore_start_ms) > VISUALODOM_RESET_IGNORE_DURATION_MS) { + _pos_reset_ignore_start_ms = 0; + } + + return (_pos_reset_ignore_start_ms == 0); +} + #endif diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h index 9c6961b2bf..aeea477be7 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h @@ -37,6 +37,11 @@ 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); + // returns true if sensor data should be consumed, false if it should be ignored + // set vision_position_estimate to true if reset_counter is from the VISION_POSITION_ESTIMATE source, false otherwise + // only the VISION_POSITION_ESTIMATE message's reset_counter is used to determine if sensor data should be ignored + bool should_consume_sensor_data(bool vision_position_estimate, uint8_t reset_counter); + 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 @@ -47,6 +52,8 @@ protected: 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) + 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 }; #endif diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 84a81bf221..23f813bd59 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -47,7 +47,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, attitude.to_euler(roll, pitch, yaw); // log sensor data - AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter); + AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false); // record time for health monitoring _last_update_ms = AP_HAL::millis(); @@ -61,7 +61,7 @@ void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, ui // record time for health monitoring _last_update_ms = AP_HAL::millis(); - AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel, _frontend.get_vel_noise(), reset_counter); + AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel, _frontend.get_vel_noise(), reset_counter, false); } #endif