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

FlightTask smoothVel - Initialize ekf reset counters when task is activated

Browse Source
This commit is contained in:
bresch
2019-07-02 16:39:48 +02:00
committed by Mathieu Bresciani
parent 1414f50cea
commit 24811cf550
6 changed files with 30 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+9
View File
@@ -52,6 +52,7 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint
_smoothing[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
}
_initEkfResetCounters();
_resetPositionLock();
return ret;
@@ -80,6 +81,14 @@ void FlightTaskManualPositionSmoothVel::_resetPositionLock()
_position_setpoint_z_locked = NAN;
}
void FlightTaskManualPositionSmoothVel::_initEkfResetCounters()
{
_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
}
void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
{
// Check if a reset event has happened.