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

StickYaw: use consistently for all flight tasks

Browse Source 
- Switching to the first order filter that was previously
only in FlightTaskManualAltitude.
- Moving the scaling of full stick deflection to
radians per second into the class.
This commit is contained in:
Matthias Grob
2023-03-02 12:06:40 +01:00
parent 0c1f340154
commit 21d580293a
10 changed files with 42 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+3 -15
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -42,10 +42,6 @@
using namespace matrix;
FlightTaskManualAltitude::FlightTaskManualAltitude() :
_sticks(this)
{}
bool FlightTaskManualAltitude::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
@@ -95,8 +91,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
void FlightTaskManualAltitude::_scaleSticks()
{
// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
_is_yaw_good_for_control, _deltatime);
// Use sticks input with deadzone and exponential curve for vertical velocity
const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() :
@@ -104,14 +100,6 @@ void FlightTaskManualAltitude::_scaleSticks()
_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
}
float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
{
const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f);
const float alpha = _deltatime / den;
_yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target;
return _yawspeed_filter_state;
}
void FlightTaskManualAltitude::_updateAltitudeLock()
{
// Depending on stick inputs and velocity, position is locked.
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+6 -15
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -40,14 +40,15 @@
#pragma once
#include "FlightTask.hpp"
#include "Sticks.hpp"
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include "Sticks.hpp"
#include "StickYaw.hpp"
#include <uORB/Subscription.hpp>
class FlightTaskManualAltitude : public FlightTask
{
public:
FlightTaskManualAltitude();
FlightTaskManualAltitude() = default;
virtual ~FlightTaskManualAltitude() = default;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
@@ -73,7 +74,8 @@ protected:
*/
void _updateAltitudeLock();
Sticks _sticks;
Sticks _sticks{this};
StickYaw _stick_yaw{this};
bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
@@ -81,8 +83,6 @@ protected:
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below which to land with land speed */
@@ -97,14 +97,6 @@ private:
void _unlockYaw();
void _lockYaw();
/**
* Filter between stick input and yaw setpoint
*
* @param yawspeed_target yaw setpoint desired by the stick
* @return filtered value from independent filter state
*/
float _applyYawspeedFilter(float yawspeed_target);
/**
* Terrain following.
* During terrain following, the position setpoint is adjusted
@@ -132,7 +124,6 @@ private:
void setGearAccordingToSwitch();
float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */