37 #include <mathlib/mathlib.h> 46 _sub_triplet_setpoint.update();
49 ret = ret && _sub_triplet_setpoint.get().current.valid;
52 return ret && PX4_ISFINITE(_position(0))
53 && PX4_ISFINITE(_position(1))
54 && PX4_ISFINITE(_velocity(0))
55 && PX4_ISFINITE(_velocity(1));
61 _position_setpoint = _position;
62 _velocity_setpoint.setZero();
63 _position_lock.setAll(NAN);
72 if (!_sub_triplet_setpoint.get().current.valid) {
73 _setDefaultConstraints();
74 _position_setpoint = _position;
79 if (_sub_triplet_setpoint.get().current.yaw_valid) {
81 _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
83 if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
85 _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
88 }
else if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
90 _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
97 if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
99 if (!PX4_ISFINITE(_position_lock(0))) {
100 _position_setpoint = _position_lock = _position;
103 _position_setpoint = _position_lock;
110 _position_lock.setAll(NAN);
114 if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
116 if (!PX4_ISFINITE(_position_lock(0))) {
117 _position_setpoint = _position_lock = _position;
118 _position_setpoint(2) = _position_lock(2) = _position(2) - _param_mis_takeoff_alt.get();
121 _position_setpoint = _position_lock;
128 _position_lock.setAll(NAN);
132 if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
134 if (!PX4_ISFINITE(_position_lock(0))) {
135 _position_setpoint = _position_lock = _position;
136 _position_setpoint(2) = _position_lock(2) = NAN;
137 _velocity_setpoint(2) = _param_mpc_land_speed.get();
140 _position_setpoint = _position_lock;
141 _velocity_setpoint(2) = _param_mpc_land_speed.get();
148 _position_lock.setAll(NAN);
152 if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
153 _thrust_setpoint.zero();
162 const bool position_ctrl_xy = _sub_triplet_setpoint.get().current.position_valid
163 && _sub_vehicle_local_position.get().xy_valid;
164 const bool position_ctrl_z = _sub_triplet_setpoint.get().current.alt_valid
165 && _sub_vehicle_local_position.get().z_valid;
166 const bool velocity_ctrl_xy = _sub_triplet_setpoint.get().current.velocity_valid
167 && _sub_vehicle_local_position.get().v_xy_valid;
168 const bool velocity_ctrl_z = _sub_triplet_setpoint.get().current.velocity_valid
169 && _sub_vehicle_local_position.get().v_z_valid;
170 const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy;
171 const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z;
172 const bool acceleration_ctrl = _sub_triplet_setpoint.get().current.acceleration_valid;
175 if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) {
180 if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) {
185 if (feedforward_ctrl_xy) {
186 _position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
187 _position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
188 _velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
189 _velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
191 }
else if (position_ctrl_xy) {
192 _position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
193 _position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
195 }
else if (velocity_ctrl_xy) {
197 if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
199 _velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
200 _velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
202 }
else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
205 _velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf(
206 _yaw) * _sub_triplet_setpoint.get().current.vy;
207 _velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf(
208 _yaw) * _sub_triplet_setpoint.get().current.vy;
217 if (feedforward_ctrl_z) {
218 _position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
219 _velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
221 }
else if (position_ctrl_z) {
222 _position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
224 }
else if (velocity_ctrl_z) {
225 _velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
230 if (_sub_triplet_setpoint.get().current.acceleration_valid) {
231 _thrust_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
232 _thrust_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
233 _thrust_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
237 _constraints.want_takeoff = _checkTakeoff();
virtual bool activate(vehicle_local_position_setpoint_s last_setpoint)
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.
virtual bool updateInitialize()
Call before activate() or update() to initialize time and input data.
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
Call once on the event where you switch to the task.
bool updateInitialize() override
Call before activate() or update() to initialize time and input data.