fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Trajectory auto - Handle EKF xy reset

Browse Source
This commit is contained in:
bresch 2018-12-13 17:45:14 +01:00
parent a2d5485c7f
commit df58e161d1
2 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -123,6 +123,14 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
// that one is used as a velocity limit.
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
// Check if a reset event has happened.
if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) {
// Reset the XY axes
_trajectory[0].setCurrentPosition(_position(0));
_trajectory[1].setCurrentPosition(_position(1));
_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
}
if (PX4_ISFINITE(_position_setpoint(0)) &&
PX4_ISFINITE(_position_setpoint(1))) {
// Use position setpoints to generate velocity setpoints

1
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
View File

@ -76,4 +76,5 @@ protected:
void _generateTrajectory();
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
float _yaw_sp_prev;
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
};