dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightTaskAuto: hotfix filter for yawspeed feed-forward 

to get rid of derivative spikes when navigator is
continuously updating the yaw setpoint in the
triplet for a POI but is running at a lower rate.

The proper solution is to generate that yaw setpoint
with high rate in the flight task and have the triplet
just guid to the next waypoint at low rate.
This commit is contained in:
Matthias Grob 2020-08-18 15:47:31 +02:00 committed by Daniel Agar
parent 3e6ff2451b
commit b40dbd3d6f
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -109,8 +109,10 @@ void FlightTaskAuto::_limitYawRate()
_yaw_setpoint = yaw_setpoint_sat;
if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
// Create a feedforward
_yawspeed_setpoint = dyaw / _deltatime;
// Create a feedforward using the filtered derivative
_yawspeed_filter.setParameters(_deltatime, .2f);
_yawspeed_filter.update(dyaw);
_yawspeed_setpoint = _yawspeed_filter.getState() / _deltatime;
}
}

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -46,6 +46,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
// TODO: make this switchable in the board config, like a module
#if CONSTRAINED_FLASH
@ -114,6 +115,7 @@ protected:
int _mission_gear{landing_gear_s::GEAR_KEEP};
float _yaw_sp_prev{NAN};
AlphaFilter<float> _yawspeed_filter;
bool _yaw_sp_aligned{false};
ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */