UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-26 16:28:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAltitude: remove duplicate, deprecated yaw stick handling

Browse Source
This commit is contained in:
Matthias Grob
2023-06-30 11:46:33 +02:00
parent 4d465533b5
commit b7e2a9c320
2 changed files with 2 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@@ -90,10 +90,6 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
void FlightTaskManualAltitude::_scaleSticks()
{
// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
_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() :
_param_mpc_z_vel_max_up.get();
@@ -273,40 +269,6 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
}
}
void FlightTaskManualAltitude::_updateHeadingSetpoints()
{
if (_isYawInput() || !_is_yaw_good_for_control) {
_unlockYaw();
} else {
_lockYaw();
}
}
bool FlightTaskManualAltitude::_isYawInput()
{
/*
* A threshold larger than FLT_EPSILON is required because the
* _yawspeed_setpoint comes from an IIR filter and takes too much
* time to reach zero.
*/
return fabsf(_yawspeed_setpoint) > 0.001f;
}
void FlightTaskManualAltitude::_unlockYaw()
{
// no fixed heading when rotating around yaw by stick
_yaw_setpoint = NAN;
}
void FlightTaskManualAltitude::_lockYaw()
{
// hold the current heading when no more rotation commanded
if (!PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint = _yaw;
}
}
void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
{
// Only reset the yaw setpoint when the heading is locked
@@ -317,7 +279,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
void FlightTaskManualAltitude::_updateSetpoints()
{
_updateHeadingSetpoints(); // get yaw setpoint
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
_is_yaw_good_for_control, _deltatime);
_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
_yaw_setpoint);
_updateAltitudeLock();

5
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
View File

@@ -56,7 +56,6 @@ public:
bool update() override;
protected:
void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */
virtual void _updateSetpoints(); /**< updates all setpoints */
virtual void _scaleSticks(); /**< scales sticks to velocity in z */
@@ -89,10 +88,6 @@ protected:
_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
)
private:
bool _isYawInput();
void _unlockYaw();
void _lockYaw();
/**
* Terrain following.
* During terrain following, the position setpoint is adjusted