PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTaskOffboard.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 /**
34  * @file FlightTaskAuto.cpp
35  */
36 #include "FlightTaskOffboard.hpp"
37 #include <mathlib/mathlib.h>
38 #include <float.h>
39 
40 using namespace matrix;
41 
43 {
44  bool ret = FlightTask::updateInitialize();
45 
46  _sub_triplet_setpoint.update();
47 
48  // require a valid triplet
49  ret = ret && _sub_triplet_setpoint.get().current.valid;
50 
51  // require valid position / velocity in xy
52  return ret && PX4_ISFINITE(_position(0))
53  && PX4_ISFINITE(_position(1))
54  && PX4_ISFINITE(_velocity(0))
55  && PX4_ISFINITE(_velocity(1));
56 }
57 
59 {
60  bool ret = FlightTask::activate(last_setpoint);
61  _position_setpoint = _position;
62  _velocity_setpoint.setZero();
63  _position_lock.setAll(NAN);
64  return ret;
65 }
66 
68 {
69  // reset setpoint for every loop
70  _resetSetpoints();
71 
72  if (!_sub_triplet_setpoint.get().current.valid) {
73  _setDefaultConstraints();
74  _position_setpoint = _position;
75  return false;
76  }
77 
78  // Yaw / Yaw-speed
79  if (_sub_triplet_setpoint.get().current.yaw_valid) {
80  // yaw control required
81  _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
82 
83  if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
84  // yawspeed is used as feedforward
85  _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
86  }
87 
88  } else if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
89  // only yawspeed required
90  _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
91  // set yaw setpoint to NAN since not used
92  _yaw_setpoint = NAN;
93 
94  }
95 
96  // Loiter
97  if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
98  // loiter just means that the vehicle should keep position
99  if (!PX4_ISFINITE(_position_lock(0))) {
100  _position_setpoint = _position_lock = _position;
101 
102  } else {
103  _position_setpoint = _position_lock;
104  }
105 
106  // don't have to continue
107  return true;
108 
109  } else {
110  _position_lock.setAll(NAN);
111  }
112 
113  // Takeoff
114  if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
115  // just do takeoff to default altitude
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();
119 
120  } else {
121  _position_setpoint = _position_lock;
122  }
123 
124  // don't have to continue
125  return true;
126 
127  } else {
128  _position_lock.setAll(NAN);
129  }
130 
131  // Land
132  if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
133  // land with landing speed, but keep position in xy
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();
138 
139  } else {
140  _position_setpoint = _position_lock;
141  _velocity_setpoint(2) = _param_mpc_land_speed.get();
142  }
143 
144  // don't have to continue
145  return true;
146 
147  } else {
148  _position_lock.setAll(NAN);
149  }
150 
151  // IDLE
152  if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
153  _thrust_setpoint.zero();
154  return true;
155  }
156 
157  // Possible inputs:
158  // 1. position setpoint
159  // 2. position setpoint + velocity setpoint (velocity used as feedforward)
160  // 3. velocity setpoint
161  // 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported
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;
173 
174  // if nothing is valid in xy, then exit offboard
175  if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) {
176  return false;
177  }
178 
179  // if nothing is valid in z, then exit offboard
180  if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) {
181  return false;
182  }
183 
184  // XY-direction
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;
190 
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;
194 
195  } else if (velocity_ctrl_xy) {
196 
197  if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
198  // in local frame: don't require any transformation
199  _velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
200  _velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
201 
202  } else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
203  // in body frame: need to transorm first
204  // Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy
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;
209 
210  } else {
211  // no valid frame
212  return false;
213  }
214  }
215 
216  // Z-direction
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;
220 
221  } else if (position_ctrl_z) {
222  _position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
223 
224  } else if (velocity_ctrl_z) {
225  _velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
226  }
227 
228  // Acceleration
229  // Note: this is not supported yet and will be mapped to normalized thrust directly.
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;
234  }
235 
236  // use default conditions of upwards position or velocity to take off
237  _constraints.want_takeoff = _checkTakeoff();
238 
239  return true;
240 }
virtual bool activate(vehicle_local_position_setpoint_s last_setpoint)
Call once on the event where you switch to the task.
Definition: FlightTask.cpp:12
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.
Definition: FlightTask.cpp:27
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.