PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTask.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 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 /**
35  * @file FlightTask.hpp
36  *
37  * Abstract base class for different advanced flight tasks like orbit, follow me, ...
38  *
39  * @author Matthias Grob <maetugr@gmail.com>
40  */
41 
42 #pragma once
43 
44 #include <px4_platform_common/module_params.h>
45 #include <drivers/drv_hrt.h>
46 #include <matrix/matrix/math.hpp>
47 #include <uORB/Subscription.hpp>
56 
57 class FlightTask : public ModuleParams
58 {
59 public:
61  ModuleParams(nullptr)
62  {
65  }
66 
67  virtual ~FlightTask() = default;
68 
69  /**
70  * Call once on the event where you switch to the task
71  * @param state of the previous task
72  * @return true on success, false on error
73  */
74  virtual bool activate(vehicle_local_position_setpoint_s last_setpoint);
75 
76  /**
77  * Call this to reset an active Flight Task
78  */
79  virtual void reActivate();
80 
81  /**
82  * To be called to adopt parameters from an arrived vehicle command
83  * @param command received command message containing the parameters
84  * @return true if accepted, false if declined
85  */
86  virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; }
87 
88  /**
89  * Call before activate() or update()
90  * to initialize time and input data
91  * @return true on success, false on error
92  */
93  virtual bool updateInitialize();
94 
95  /**
96  * To be called regularly in the control loop cycle to execute the task
97  * @return true on success, false on error
98  */
99  virtual bool update() = 0;
100 
101  /**
102  * Call after update()
103  * to constrain the generated setpoints in order to comply
104  * with the constraints of the current mode
105  * @return true on success, false on error
106  */
107  virtual bool updateFinalize() { return true; };
108 
109  /**
110  * Get the output data
111  * @return task output setpoints that get executed by the positon controller
112  */
114 
115  /**
116  * Get vehicle constraints.
117  * The constraints can vary with task.
118  * @return constraints
119  */
121 
122  /**
123  * Get landing gear position.
124  * The constraints can vary with task.
125  * @return landing gear
126  */
127  const landing_gear_s &getGear() { return _gear; }
128 
129  /**
130  * Get avoidance desired waypoint
131  * @return desired waypoints
132  */
134 
135  /**
136  * Empty setpoint.
137  * All setpoints are set to NAN.
138  */
140 
141  /**
142  * Empty constraints.
143  * All constraints are set to NAN.
144  */
146 
147  /**
148  * default landing gear state
149  */
151 
152  /**
153  * Call this whenever a parameter update notification is received (parameter_update uORB message)
154  */
156  {
157  updateParams();
158  }
159 
160  /**
161  * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
162  * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
163  */
164  virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
165 
167  const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
168 
169 protected:
170 
173 
174  /** Reset all setpoints to NAN */
175  void _resetSetpoints();
176 
177  /** Check and update local position */
179 
180  /** Set constraints to default values */
181  virtual void _setDefaultConstraints();
182 
183  /** Determine when to trigger a takeoff (ignored in flight) */
184  virtual bool _checkTakeoff();
185 
186  /**
187  * Monitor the EKF reset counters and
188  * call the appropriate handling functions in case of a reset event
189  */
190  void _initEkfResetCounters();
191  void _checkEkfResetCounters();
192  virtual void _ekfResetHandlerPositionXY() {};
193  virtual void _ekfResetHandlerVelocityXY() {};
194  virtual void _ekfResetHandlerPositionZ() {};
195  virtual void _ekfResetHandlerVelocityZ() {};
196  virtual void _ekfResetHandlerHeading(float delta_psi) {};
197 
198  /* Time abstraction */
199  static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
200  float _time = 0; /**< passed time in seconds since the task was activated */
201  float _deltatime = 0; /**< passed time in seconds since the task was last updated */
202  hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */
203  hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */
204  hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */
205 
206  /* Current vehicle state */
207  matrix::Vector3f _position; /**< current vehicle position */
208  matrix::Vector3f _velocity; /**< current vehicle velocity */
209  float _yaw = 0.f; /**< current vehicle yaw heading */
210  float _dist_to_bottom = 0.0f; /**< current height above ground level */
211 
212  /**
213  * Setpoints which the position controller has to execute.
214  * Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time.
215  * If more than one type of setpoint is set, then order of control is a as follow: position, velocity,
216  * acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the
217  * _velocity_setpoint is used as feedforward.
218  * _acceleration_setpoint and _jerk_setpoint are currently not supported.
219  */
227 
230 
231  /* Counters for estimator local position resets */
232  struct {
233  uint8_t xy = 0;
234  uint8_t vxy = 0;
235  uint8_t z = 0;
236  uint8_t vz = 0;
237  uint8_t quat = 0;
238  } _reset_counters;
239 
240  /**
241  * Vehicle constraints.
242  * The constraints can vary with tasks.
243  */
245 
247 
248  /**
249  * Desired waypoints.
250  * Goals set by the FCU to be sent to the obstacle avoidance system.
251  */
253 
254  DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
255  (ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
256  (ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
257  (ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
258  (ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air
259  )
260 };
matrix::Vector3f _acceleration_setpoint
Definition: FlightTask.hpp:222
vehicle_trajectory_waypoint_s _desired_waypoint
Desired waypoints.
Definition: FlightTask.hpp:252
matrix::Vector3f _velocity
current vehicle velocity
Definition: FlightTask.hpp:208
static const vehicle_local_position_setpoint_s empty_setpoint
Empty setpoint.
Definition: FlightTask.hpp:139
void handleParameterUpdate()
Call this whenever a parameter update notification is received (parameter_update uORB message) ...
Definition: FlightTask.hpp:155
float _yawspeed_setpoint
Definition: FlightTask.hpp:226
hrt_abstime _time_stamp_activate
time stamp when task was activated
Definition: FlightTask.hpp:202
void _checkEkfResetCounters()
Definition: FlightTask.cpp:51
float _yaw_setpoint
Definition: FlightTask.hpp:225
const landing_gear_s & getGear()
Get landing gear position.
Definition: FlightTask.hpp:127
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
matrix::Vector3f _velocity_setpoint_feedback
Definition: FlightTask.hpp:228
virtual void _setDefaultConstraints()
Set constraints to default values.
Definition: FlightTask.cpp:163
float _deltatime
passed time in seconds since the task was last updated
Definition: FlightTask.hpp:201
static const landing_gear_s empty_landing_gear_default_keep
default landing gear state
Definition: FlightTask.hpp:150
virtual void _ekfResetHandlerVelocityXY()
Definition: FlightTask.hpp:193
virtual ~FlightTask()=default
virtual void _ekfResetHandlerPositionXY()
Definition: FlightTask.hpp:192
const vehicle_trajectory_waypoint_s & getAvoidanceWaypoint()
Get avoidance desired waypoint.
Definition: FlightTask.hpp:133
virtual void _ekfResetHandlerHeading(float delta_psi)
Definition: FlightTask.hpp:196
hrt_abstime _time_stamp_current
time stamp at the beginning of the current task update
Definition: FlightTask.hpp:203
matrix::Vector3f _position_setpoint
Setpoints which the position controller has to execute.
Definition: FlightTask.hpp:220
High-resolution timer with callouts and timekeeping.
virtual bool update()=0
To be called regularly in the control loop cycle to execute the task.
float _dist_to_bottom
current height above ground level
Definition: FlightTask.hpp:210
const vehicle_constraints_s & getConstraints()
Get vehicle constraints.
Definition: FlightTask.hpp:120
static constexpr uint64_t _timeout
maximal time in us before a loop or data times out
Definition: FlightTask.hpp:199
virtual void _ekfResetHandlerVelocityZ()
Definition: FlightTask.hpp:195
landing_gear_s _gear
Definition: FlightTask.hpp:246
matrix::Vector3f _jerk_setpoint
Definition: FlightTask.hpp:223
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
virtual bool updateFinalize()
Call after update() to constrain the generated setpoints in order to comply with the constraints of t...
Definition: FlightTask.hpp:107
matrix::Vector3f _thrust_setpoint
Definition: FlightTask.hpp:224
virtual void _ekfResetHandlerPositionZ()
Definition: FlightTask.hpp:194
uint8_t quat
Definition: FlightTask.hpp:237
struct FlightTask::@63 _reset_counters
virtual bool updateInitialize()
Call before activate() or update() to initialize time and input data.
Definition: FlightTask.cpp:27
uint8_t vxy
Definition: FlightTask.hpp:234
void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp)
Definition: FlightTask.hpp:166
matrix::Vector3f _thrust_setpoint_feedback
Definition: FlightTask.hpp:229
matrix::Vector3f _position
current vehicle position
Definition: FlightTask.hpp:207
float _time
passed time in seconds since the task was activated
Definition: FlightTask.hpp:200
matrix::Vector3f _velocity_setpoint
Definition: FlightTask.hpp:221
uORB::SubscriptionData< vehicle_local_position_s > _sub_vehicle_local_position
Definition: FlightTask.hpp:171
virtual void reActivate()
Call this to reset an active Flight Task.
Definition: FlightTask.cpp:22
vehicle_constraints_s _constraints
Vehicle constraints.
Definition: FlightTask.hpp:244
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
virtual bool _checkTakeoff()
Determine when to trigger a takeoff (ignored in flight)
Definition: FlightTask.cpp:174
virtual bool applyCommandParameters(const vehicle_command_s &command)
To be called to adopt parameters from an arrived vehicle command.
Definition: FlightTask.hpp:86
void _evaluateVehicleLocalPosition()
Check and update local position.
Definition: FlightTask.cpp:114
void _resetSetpoints()
Reset all setpoints to NAN.
Definition: FlightTask.cpp:104
const vehicle_local_position_setpoint_s getPositionSetpoint()
Get the output data.
Definition: FlightTask.cpp:81
uint8_t z
Definition: FlightTask.hpp:235
float _yaw
current vehicle yaw heading
Definition: FlightTask.hpp:209
uORB::SubscriptionData< vehicle_attitude_s > _sub_attitude
Definition: FlightTask.hpp:172
uint8_t xy
Definition: FlightTask.hpp:233
static const vehicle_constraints_s empty_constraints
Empty constraints.
Definition: FlightTask.hpp:145
void _initEkfResetCounters()
Monitor the EKF reset counters and call the appropriate handling functions in case of a reset event...
Definition: FlightTask.cpp:42
virtual void setYawHandler(WeatherVane *ext_yaw_handler)
Sets an external yaw handler which can be used by any flight task to implement a different yaw contro...
Definition: FlightTask.hpp:164
hrt_abstime _time_stamp_last
time stamp when task was last updated
Definition: FlightTask.hpp:204
uint8_t vz
Definition: FlightTask.hpp:236