fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 20:18:53 +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/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+10
View File
@@ -56,6 +56,7 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s sta
_yaw_sp_prev = _yaw;
_updateTrajConstraints();
_initEkfResetCounters();
return ret;
}
@@ -68,6 +69,7 @@ void FlightTaskAutoLineSmoothVel::reActivate()
}
_trajectory[2].reset(0.f, 0.7f, _position(2));
_initEkfResetCounters();
}
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
@@ -115,6 +117,14 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
return math::constrain(val, min, max);
}
void FlightTaskAutoLineSmoothVel::_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 FlightTaskAutoLineSmoothVel::_checkEkfResetCounters()
{
// Check if a reset event has happened.