forked from Archive/PX4-Autopilot
StickYaw: use consistently for all flight tasks
- 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:
parent
0c1f340154
commit
21d580293a
|
@ -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
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -243,9 +243,9 @@ void FlightTaskAuto::_prepareLandSetpoints()
|
||||||
vertical_speed *= (1 + _sticks.getPositionExpo()(2));
|
vertical_speed *= (1 + _sticks.getPositionExpo()(2));
|
||||||
|
|
||||||
// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
|
// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
|
||||||
if (!_weathervane.isActive() || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
|
if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
|
||||||
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
|
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
|
||||||
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
|
_deltatime);
|
||||||
}
|
}
|
||||||
|
|
||||||
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
|
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -142,7 +142,7 @@ protected:
|
||||||
Vector3f _unsmoothed_velocity_setpoint;
|
Vector3f _unsmoothed_velocity_setpoint;
|
||||||
Sticks _sticks{this};
|
Sticks _sticks{this};
|
||||||
StickAccelerationXY _stick_acceleration_xy{this};
|
StickAccelerationXY _stick_acceleration_xy{this};
|
||||||
StickYaw _stick_yaw;
|
StickYaw _stick_yaw{this};
|
||||||
matrix::Vector3f _land_position;
|
matrix::Vector3f _land_position;
|
||||||
float _land_heading;
|
float _land_heading;
|
||||||
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
|
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
|
||||||
|
@ -176,8 +176,7 @@ protected:
|
||||||
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
|
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
|
||||||
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
|
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
|
||||||
(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
|
(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
|
||||||
_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp
|
_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
|
||||||
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
|
|
||||||
);
|
);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -63,8 +63,8 @@ bool FlightTaskManualAcceleration::update()
|
||||||
{
|
{
|
||||||
bool ret = FlightTaskManualAltitudeSmoothVel::update();
|
bool ret = FlightTaskManualAltitudeSmoothVel::update();
|
||||||
|
|
||||||
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
|
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
|
||||||
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
|
_deltatime);
|
||||||
|
|
||||||
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint,
|
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint,
|
||||||
_position,
|
_position,
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -58,7 +58,7 @@ private:
|
||||||
void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
|
void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
|
||||||
|
|
||||||
StickAccelerationXY _stick_acceleration_xy{this};
|
StickAccelerationXY _stick_acceleration_xy{this};
|
||||||
StickYaw _stick_yaw;
|
StickYaw _stick_yaw{this};
|
||||||
|
|
||||||
WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
|
WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
|
||||||
};
|
};
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -42,10 +42,6 @@
|
||||||
|
|
||||||
using namespace matrix;
|
using namespace matrix;
|
||||||
|
|
||||||
FlightTaskManualAltitude::FlightTaskManualAltitude() :
|
|
||||||
_sticks(this)
|
|
||||||
{}
|
|
||||||
|
|
||||||
bool FlightTaskManualAltitude::updateInitialize()
|
bool FlightTaskManualAltitude::updateInitialize()
|
||||||
{
|
{
|
||||||
bool ret = FlightTask::updateInitialize();
|
bool ret = FlightTask::updateInitialize();
|
||||||
|
@ -95,8 +91,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
|
||||||
void FlightTaskManualAltitude::_scaleSticks()
|
void FlightTaskManualAltitude::_scaleSticks()
|
||||||
{
|
{
|
||||||
// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
|
// 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());
|
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
|
||||||
_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
|
_is_yaw_good_for_control, _deltatime);
|
||||||
|
|
||||||
// Use sticks input with deadzone and exponential curve for vertical velocity
|
// 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() :
|
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);
|
_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()
|
void FlightTaskManualAltitude::_updateAltitudeLock()
|
||||||
{
|
{
|
||||||
// Depending on stick inputs and velocity, position is locked.
|
// Depending on stick inputs and velocity, position is locked.
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -40,14 +40,15 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "FlightTask.hpp"
|
#include "FlightTask.hpp"
|
||||||
#include "Sticks.hpp"
|
|
||||||
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
|
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
|
||||||
|
#include "Sticks.hpp"
|
||||||
|
#include "StickYaw.hpp"
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
|
|
||||||
class FlightTaskManualAltitude : public FlightTask
|
class FlightTaskManualAltitude : public FlightTask
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
FlightTaskManualAltitude();
|
FlightTaskManualAltitude() = default;
|
||||||
virtual ~FlightTaskManualAltitude() = default;
|
virtual ~FlightTaskManualAltitude() = default;
|
||||||
bool activate(const trajectory_setpoint_s &last_setpoint) override;
|
bool activate(const trajectory_setpoint_s &last_setpoint) override;
|
||||||
bool updateInitialize() override;
|
bool updateInitialize() override;
|
||||||
|
@ -73,7 +74,8 @@ protected:
|
||||||
*/
|
*/
|
||||||
void _updateAltitudeLock();
|
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
|
bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
|
||||||
|
|
||||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
|
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
|
||||||
|
@ -81,8 +83,6 @@ protected:
|
||||||
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
|
(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_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_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_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_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 */
|
(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 _unlockYaw();
|
||||||
void _lockYaw();
|
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.
|
* Terrain following.
|
||||||
* During terrain following, the position setpoint is adjusted
|
* During terrain following, the position setpoint is adjusted
|
||||||
|
@ -132,7 +124,6 @@ private:
|
||||||
|
|
||||||
void setGearAccordingToSwitch();
|
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 */
|
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_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 */
|
bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
|
||||||
|
|
|
@ -38,5 +38,5 @@ px4_add_library(FlightTaskUtility
|
||||||
StickYaw.cpp
|
StickYaw.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning)
|
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib)
|
||||||
target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -31,23 +31,19 @@
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
|
||||||
* @file StickYaw.cpp
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "StickYaw.hpp"
|
#include "StickYaw.hpp"
|
||||||
|
|
||||||
#include <px4_platform_common/defines.h>
|
#include <px4_platform_common/defines.h>
|
||||||
|
|
||||||
StickYaw::StickYaw()
|
StickYaw::StickYaw(ModuleParams *parent) :
|
||||||
{
|
ModuleParams(parent)
|
||||||
_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
|
{}
|
||||||
}
|
|
||||||
|
|
||||||
void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed,
|
void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw,
|
||||||
const float yaw, const bool is_yaw_good_for_control, const float deltatime)
|
const float yaw, const bool is_yaw_good_for_control, const float deltatime)
|
||||||
{
|
{
|
||||||
yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime);
|
_yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get());
|
||||||
|
yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get()));
|
||||||
yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control);
|
yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -39,19 +39,20 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "SlewRate.hpp"
|
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
|
||||||
|
#include <px4_platform_common/module_params.h>
|
||||||
|
|
||||||
class StickYaw
|
class StickYaw : public ModuleParams
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
StickYaw();
|
StickYaw(ModuleParams *parent);
|
||||||
~StickYaw() = default;
|
~StickYaw() = default;
|
||||||
|
|
||||||
void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float desired_yawspeed, float yaw,
|
void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw,
|
||||||
bool is_yaw_good_for_control, float deltatime);
|
bool is_yaw_good_for_control, float deltatime);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
SlewRate<float> _yawspeed_slew_rate;
|
AlphaFilter<float> _yawspeed_filter;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Lock yaw when not currently turning
|
* Lock yaw when not currently turning
|
||||||
|
@ -65,4 +66,9 @@ private:
|
||||||
* @return yaw setpoint to execute to have a yaw lock at the correct moment in time
|
* @return yaw setpoint to execute to have a yaw lock at the correct moment in time
|
||||||
*/
|
*/
|
||||||
static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control);
|
static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control);
|
||||||
|
|
||||||
|
DEFINE_PARAMETERS(
|
||||||
|
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection
|
||||||
|
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau ///< time constant for yaw speed filtering
|
||||||
|
)
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue