fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 11:53:34 +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-16 13:09:09 +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/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+8
View File
@@ -50,6 +50,7 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint
_smoothing.reset(state_prev.acc_z, state_prev.vz, state_prev.z);
_initEkfResetCounters();
_resetPositionLock();
return ret;
@@ -61,6 +62,7 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
// using the generated jerk, reset the z derivatives to zero
_smoothing.reset(0.f, 0.f, _position(2));
_initEkfResetCounters();
_resetPositionLock();
}
@@ -71,6 +73,12 @@ void FlightTaskManualAltitudeSmoothVel::_resetPositionLock()
_position_setpoint_z_locked = NAN;
}
void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters()
{
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
}
void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
{
if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {