PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTasks.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 flight_taks.h
36  *
37  * Library class to hold and manage all implemented flight task instances
38  *
39  * @author Matthias Grob <maetugr@gmail.com>
40  */
41 
42 #pragma once
43 
44 #include "FlightTask.hpp"
45 #include "FlightTasks_generated.hpp"
48 #include <uORB/Subscription.hpp>
51 
52 #include <new>
53 
54 enum class FlightTaskError : int {
55  NoError = 0,
56  InvalidTask = -1,
57  ActivationFailed = -2
58 };
59 
61 {
62 public:
63  FlightTasks();
64 
66  {
67  if (_current_task.task) {
68  _current_task.task->~FlightTask();
69  }
70  }
71 
72  /**
73  * Call regularly in the control loop cycle to execute the task
74  * @return true on success, false on error
75  */
76  bool update();
77 
78  /**
79  * Get the output data from the current task
80  * @return output setpoint, to be executed by position control
81  */
82  const vehicle_local_position_setpoint_s getPositionSetpoint();
83 
84  /**
85  * Get task dependent constraints
86  * @return setpoint constraints that has to be respected by the position controller
87  */
88  const vehicle_constraints_s getConstraints();
89 
90  /**
91  * Get landing gear position.
92  * @return landing gear
93  */
94  const landing_gear_s getGear();
95 
96  /**
97  * Get task avoidance desired waypoints
98  * @return auto triplets in the mc_pos_control
99  */
100  const vehicle_trajectory_waypoint_s getAvoidanceWaypoint();
101 
102  /**
103  * Get empty avoidance desired waypoints
104  * @return empty triplets in the mc_pos_control
105  */
106  const vehicle_trajectory_waypoint_s &getEmptyAvoidanceWaypoint();
107 
108  /**
109  * Switch to the next task in the available list (for testing)
110  * @return 0 on success, <0 on error
111  */
112  FlightTaskError switchTask() { return switchTask(static_cast<int>(_current_task.index) + 1); }
113 
114  /**
115  * Switch to a specific task (for normal usage)
116  * @param task index to switch to
117  * @return 0 on success, <0 on error
118  */
119  FlightTaskError switchTask(FlightTaskIndex new_task_index);
120  FlightTaskError switchTask(int new_task_index);
121 
122  /**
123  * Get the number of the active task
124  * @return number of active task, -1 if there is none
125  */
126  int getActiveTask() const { return static_cast<int>(_current_task.index); }
127 
128  /**
129  * Check if any task is active
130  * @return true if a task is active, false if not
131  */
132  bool isAnyTaskActive() const { return _current_task.task; }
133 
134  /**
135  * Call this whenever a parameter update notification is received (parameter_update uORB message)
136  */
137  void handleParameterUpdate();
138 
139  /**
140  * Call this method to get the description of a task error.
141  */
142  const char *errorToString(const FlightTaskError error);
143 
144  /**
145  * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy.
146  */
147  void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);}
148 
149  /**
150  * This method will re-activate current task.
151  */
152  void reActivate();
153 
154  void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); }
155 
156 private:
157 
158  /**
159  * Union with all existing tasks: we use it to make sure that only the memory of the largest existing
160  * task is needed, and to avoid using dynamic memory allocations.
161  */
162  TaskUnion _task_union; /**< storage for the currently active task */
163 
164  struct flight_task_t {
166  FlightTaskIndex index;
167  };
168  flight_task_t _current_task = {nullptr, FlightTaskIndex::None};
169 
170  struct task_error_t {
171  int error;
172  const char *msg;
173  };
174 
175  static constexpr int _numError = 3;
176  /**
177  * Map from Error int to user friendly string.
178  */
179  task_error_t _taskError[_numError] = {
180  {static_cast<int>(FlightTaskError::NoError), "No Error"},
181  {static_cast<int>(FlightTaskError::InvalidTask), "Invalid Task "},
182  {static_cast<int>(FlightTaskError::ActivationFailed), "Activation Failed"}
183  };
184 
185  /**
186  * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
187  */
188  void _updateCommand();
189  FlightTaskIndex switchVehicleCommand(const int command);
190 
191  uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */
192 
193  uORB::PublicationQueued<vehicle_command_ack_s> _pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};
194 
195  int _initTask(FlightTaskIndex task_index);
196 };
FlightTaskError
Definition: FlightTasks.hpp:54
void setYawHandler(WeatherVane *ext_yaw_handler)
Sets an external yaw handler.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Abstract base class for different advanced flight tasks like orbit, follow me, ...
void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp)
bool isAnyTaskActive() const
Check if any task is active.
int getActiveTask() const
Get the number of the active task.
FlightTaskError switchTask()
Switch to the next task in the available list (for testing)
TaskUnion _task_union
Union with all existing tasks: we use it to make sure that only the memory of the largest existing ta...