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

FlightTaskManualAltitude: add an optional stick tilt input filter

Browse Source
This commit is contained in:
Matthias Grob 2020-05-14 17:11:43 +02:00
parent 1a3c692e4e
commit 17d4fd064f
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@ -341,6 +341,10 @@ void FlightTaskManualAltitude::_updateSetpoints()
// setpoint along z-direction, which is computed in PositionControl.cpp.
Vector2f sp(&_sticks(0));
_man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get());
_man_input_filter.update(sp);
sp = _man_input_filter.getState();
_rotateIntoHeadingFrame(sp);
if (sp.length() > 1.0f) {

6
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
View File

@ -40,6 +40,7 @@
#pragma once
#include "FlightTaskManual.hpp"
#include <lib/ecl/EKF/AlphaFilter.hpp>
class FlightTaskManualAltitude : public FlightTaskManual
{
@ -83,7 +84,8 @@ protected:
(ParamFloat<px4::params::MPC_LAND_SPEED>)
_param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
(ParamFloat<px4::params::MPC_TKO_SPEED>)
_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
_param_mpc_tko_speed, /**< desired upwards speed when still close to the ground */
(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau
)
private:
bool _isYawInput();
@ -137,4 +139,6 @@ private:
* _dist_to_ground_lock.
*/
float _dist_to_ground_lock = NAN;
AlphaFilter<matrix::Vector2f> _man_input_filter;
};