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

FlightTask Smoothing: check dt before dividing

Browse Source
This commit is contained in:
ChristophTobler 2018-08-10 09:47:38 +02:00 committed by Daniel Agar
parent 9a23295a40
commit d3d549b8e6
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
View File

@ -197,7 +197,11 @@ void
ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
{
// Adjust velocity setpoint if demand exceeds acceleration. /
matrix::Vector2f acc = (vel_sp - _vel_sp_prev) / dt;
matrix::Vector2f acc{};
if (dt > FLT_EPSILON) {
acc = (vel_sp - _vel_sp_prev) / dt;
}
if (acc.length() > _acc_state_dependent) {
vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev;

7
src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp
View File

@ -146,7 +146,12 @@ void
ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt)
{
// limit vertical acceleration
float acc = (vel_sp - _vel_sp_prev) / dt;
float acc = 0.f;
if (dt > FLT_EPSILON) {
acc = (vel_sp - _vel_sp_prev) / dt;
}
float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent;
if (fabsf(acc) > fabsf(max_acc)) {