39 #include <mathlib/mathlib.h> 53 _setDefaultConstraints();
55 _updateAltitudeAboveGround();
61 if (follow_line && !follow_line_prev) {
70 _thrust_setpoint =
Vector3f(NAN, NAN, NAN);
75 if (_highEnoughForLandingGear()) {
76 _gear.landing_gear = _mission_gear;
80 _generateIdleSetpoints();
83 _generateLandSetpoints();
85 }
else if (follow_line) {
89 _generateTakeoffSetpoints();
92 _generateVelocitySetpoints();
95 if (_param_com_obs_avoid.get()) {
96 _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (
int)_type);
97 _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
102 _type_previous = _type;
110 _velocity_setpoint = _velocity;
111 _position_setpoint = _position;
117 _position_setpoint =
Vector3f(NAN, NAN, NAN);
118 _velocity_setpoint =
Vector3f(NAN, NAN, NAN);
119 _thrust_setpoint.zero();
125 _position_setpoint =
Vector3f(_target(0), _target(1), NAN);
126 _velocity_setpoint =
Vector3f(
Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
128 _constraints.tilt =
math::radians(_param_mpc_tiltmax_lnd.get());
129 _constraints.speed_down = _param_mpc_land_speed.get();
130 _gear.landing_gear = landing_gear_s::GEAR_DOWN;
136 _position_setpoint = _target;
137 _velocity_setpoint =
Vector3f(NAN, NAN, NAN);
140 _constraints.speed_up =
math::gradual(_alt_above_ground, _param_mpc_land_alt2.get(),
141 _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up);
143 _gear.landing_gear = landing_gear_s::GEAR_DOWN;
150 _position_setpoint =
Vector3f(NAN, NAN, _position(2));
152 _velocity_setpoint =
Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
158 _alt_above_ground = -_position(2);
160 if (PX4_ISFINITE(_dist_to_bottom)) {
162 _alt_above_ground = _dist_to_bottom;
164 }
else if (_sub_home_position.get().valid_alt) {
166 _alt_above_ground = -_position(2) + _sub_home_position.get().z;
172 FlightTaskAuto::updateParams();
175 _param_mpc_land_alt1.set(
math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
181 return _alt_above_ground > 2.0f;
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
Call once on the event where you switch to the task.
bool update() override
To be called regularly in the control loop cycle to execute the task.
const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
constexpr T radians(T degrees)
void _updateAltitudeAboveGround()
Computes altitude above ground based on sensors available.
Vector2< float > Vector2f
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
Call once on the event where you switch to the task.
bool _highEnoughForLandingGear()
Checks if gears can be lowered.
Vector3< float > Vector3f
void _generateLandSetpoints()
constexpr _Tp max(_Tp a, _Tp b)
void _generateIdleSetpoints()
void _generateTakeoffSetpoints()
void _reset()
Resets member variables to current vehicle state.
void updateParams() override
See ModuleParam class.
Abstract Flight task which generates local setpoints based on the triplet type.
void _generateVelocitySetpoints()