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

FlightTaskAuto: hotfix filter for yawspeed feed-forward

Browse Source 
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
2021-10-06 14:06:58 -04:00
committed by Daniel Agar
parent 3e6ff2451b
commit b40dbd3d6f
2 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+4 -2
View File
@@ -109,8 +109,10 @@ void FlightTaskAuto::_limitYawRate()
_yaw_setpoint = yaw_setpoint_sat; _yaw_setpoint = yaw_setpoint_sat;
if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) { if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
// Create a feedforward // Create a feedforward using the filtered derivative
_yawspeed_setpoint = dyaw / _deltatime; _yawspeed_filter.setParameters(_deltatime, .2f);
_yawspeed_filter.update(dyaw);
_yawspeed_setpoint = _yawspeed_filter.getState() / _deltatime;
} }
} }
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+2
View File
@@ -46,6 +46,7 @@
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <lib/geo/geo.h> #include <lib/geo/geo.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
// TODO: make this switchable in the board config, like a module // TODO: make this switchable in the board config, like a module
#if CONSTRAINED_FLASH #if CONSTRAINED_FLASH
@@ -114,6 +115,7 @@ protected:
int _mission_gear{landing_gear_s::GEAR_KEEP}; int _mission_gear{landing_gear_s::GEAR_KEEP};
float _yaw_sp_prev{NAN}; float _yaw_sp_prev{NAN};
AlphaFilter<float> _yawspeed_filter;
bool _yaw_sp_aligned{false}; bool _yaw_sp_aligned{false};
ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */ ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */