yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Trajectory manual - Handle EKF xy reset

Browse Source
This commit is contained in:
bresch
2018-12-13 15:30:11 +01:00
parent e1472818dc
commit a2d5485c7f
2 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@@ -147,6 +147,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
if (_position_lock_xy_active) {
// Check if a reset event has happened.
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
// Reset the XY axes
_smoothing[0].setCurrentPosition(_position(0));
_smoothing[1].setCurrentPosition(_position(1));
_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
}
}
Vector3f pos_sp_smooth;
Vector3f accel_sp_smooth;

1
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
View File

@@ -70,4 +70,5 @@ private:
matrix::Vector3f _vel_sp_smooth;
bool _position_lock_xy_active{false};
matrix::Vector2f _position_setpoint_xy_locked;
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
};