dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightTaskAutoMapper: move remaining methods into FlightTaskAutoLineSmooth

This commit is contained in:
Matthias Grob 2021-11-10 21:35:38 +01:00
parent 0211ef3ba1
commit f5183348a6
4 changed files with 103 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -36,6 +36,7 @@
*/
#include "FlightTaskAutoLineSmoothVel.hpp"
#include <mathlib/mathlib.h>
using namespace matrix;
@ -313,3 +314,95 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
_position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get());
}
}
void FlightTaskAutoLineSmoothVel::_prepareIdleSetpoints()
{
// Send zero thrust setpoint
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
_velocity_setpoint.setNaN();
_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
}
void FlightTaskAutoLineSmoothVel::_prepareLandSetpoints()
{
_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
// Slow down automatic descend close to ground
float land_speed = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _constraints.speed_down);
if (_type_previous != WaypointType::land) {
// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
_land_position = Vector3f(_target(0), _target(1), NAN);
_land_heading = _yaw_setpoint;
_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
}
// User input assisted landing
if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
// Stick full up -1 -> stop, stick full down 1 -> double the speed
land_speed *= (1 + _sticks.getPositionExpo()(2));
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
_velocity_setpoint_feedback.xy(), _deltatime);
_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
_yaw_sp_aligned = true;
}
} else {
// Make sure we have a valid land position even in the case we loose RC while amending it
if (!PX4_ISFINITE(_land_position(0))) {
_land_position.xy() = Vector2f(_position);
}
}
_position_setpoint = _land_position; // The last element of the land position has to stay NAN
_yaw_setpoint = _land_heading;
_velocity_setpoint(2) = land_speed;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoLineSmoothVel::_prepareTakeoffSetpoints()
{
// Takeoff is completely defined by target position
_position_setpoint = _target;
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoLineSmoothVel::_prepareVelocitySetpoints()
{
// XY Velocity waypoint
// TODO : Rewiew that. What is the expected behavior?
_position_setpoint = Vector3f(NAN, NAN, _position(2));
Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
}
void FlightTaskAutoLineSmoothVel::_preparePositionSetpoints()
{
// Simple waypoint navigation: go to xyz target, with standard limitations
_position_setpoint = _target;
_velocity_setpoint.setNaN(); // No special velocity limitations
}
void FlightTaskAutoLineSmoothVel::updateParams()
{
FlightTaskAuto::updateParams();
// make sure that alt1 is above alt2
_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
}
bool FlightTaskAutoLineSmoothVel::_highEnoughForLandingGear()
{
// return true if altitude is above two meters
return _dist_to_ground > 2.0f;
}

9
src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
View File

@ -83,6 +83,15 @@ protected:
bool _checkTakeoff() override { return _want_takeoff; };
bool _want_takeoff{false};
void _prepareIdleSetpoints();
void _prepareLandSetpoints();
void _prepareVelocitySetpoints();
void _prepareTakeoffSetpoints();
void _preparePositionSetpoints();
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
void updateParams() override; /**< See ModuleParam class */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight

95
src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp
View File

@ -36,103 +36,8 @@
*/
#include "FlightTaskAutoMapper.hpp"
#include <mathlib/mathlib.h>
using namespace matrix;
FlightTaskAutoMapper::FlightTaskAutoMapper() :
_sticks(this),
_stick_acceleration_xy(this)
{}
void FlightTaskAutoMapper::_prepareIdleSetpoints()
{
// Send zero thrust setpoint
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
_velocity_setpoint.setNaN();
_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
}
void FlightTaskAutoMapper::_prepareLandSetpoints()
{
_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
// Slow down automatic descend close to ground
float land_speed = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _constraints.speed_down);
if (_type_previous != WaypointType::land) {
// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
_land_position = Vector3f(_target(0), _target(1), NAN);
_land_heading = _yaw_setpoint;
_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
}
// User input assisted landing
if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
// Stick full up -1 -> stop, stick full down 1 -> double the speed
land_speed *= (1 + _sticks.getPositionExpo()(2));
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
_velocity_setpoint_feedback.xy(), _deltatime);
_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
_yaw_sp_aligned = true;
}
} else {
// Make sure we have a valid land position even in the case we loose RC while amending it
if (!PX4_ISFINITE(_land_position(0))) {
_land_position.xy() = Vector2f(_position);
}
}
_position_setpoint = _land_position; // The last element of the land position has to stay NAN
_yaw_setpoint = _land_heading;
_velocity_setpoint(2) = land_speed;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_prepareTakeoffSetpoints()
{
// Takeoff is completely defined by target position
_position_setpoint = _target;
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_prepareVelocitySetpoints()
{
// XY Velocity waypoint
// TODO : Rewiew that. What is the expected behavior?
_position_setpoint = Vector3f(NAN, NAN, _position(2));
Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
}
void FlightTaskAutoMapper::_preparePositionSetpoints()
{
// Simple waypoint navigation: go to xyz target, with standard limitations
_position_setpoint = _target;
_velocity_setpoint.setNaN(); // No special velocity limitations
}
void FlightTaskAutoMapper::updateParams()
{
FlightTaskAuto::updateParams();
// make sure that alt1 is above alt2
_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
}
bool FlightTaskAutoMapper::_highEnoughForLandingGear()
{
// return true if altitude is above two meters
return _dist_to_ground > 2.0f;
}

9
src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp
View File

@ -52,13 +52,6 @@ public:
virtual ~FlightTaskAutoMapper() = default;
protected:
void _prepareIdleSetpoints();
void _prepareLandSetpoints();
void _prepareVelocitySetpoints();
void _prepareTakeoffSetpoints();
void _preparePositionSetpoints();
void updateParams() override; /**< See ModuleParam class */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
@ -80,5 +73,5 @@ protected:
matrix::Vector3f _land_position;
float _land_heading;
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
};