PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTaskAuto.hpp
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 /**
35  * @file FlightTaskAuto.hpp
36  *
37  * Map from global triplet to local quadruple.
38  */
39 
40 #pragma once
41 
42 #include "FlightTask.hpp"
48 #include <lib/ecl/geo/geo.h>
49 #include <ObstacleAvoidance.hpp>
50 
51 /**
52  * This enum has to agree with position_setpoint_s type definition
53  * The only reason for not using the struct position_setpoint is because
54  * of the size
55  */
56 enum class WaypointType : int {
57  position = position_setpoint_s::SETPOINT_TYPE_POSITION,
58  velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY,
59  loiter = position_setpoint_s::SETPOINT_TYPE_LOITER,
60  takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
61  land = position_setpoint_s::SETPOINT_TYPE_LAND,
62  idle = position_setpoint_s::SETPOINT_TYPE_IDLE,
63  offboard = position_setpoint_s::SETPOINT_TYPE_OFFBOARD, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks
64  follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET,
65 };
66 
67 enum class State {
68  offtrack, /**< Vehicle is more than cruise speed away from track */
69  target_behind, /**< Vehicle is in front of target. */
70  previous_infront, /**< Vehilce is behind previous waypoint.*/
71  none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */
72 };
73 
74 class FlightTaskAuto : public FlightTask
75 {
76 public:
78 
79  virtual ~FlightTaskAuto() = default;
80  bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
81  bool updateInitialize() override;
82  bool updateFinalize() override;
83 
84  /**
85  * Sets an external yaw handler which can be used to implement a different yaw control strategy.
86  */
87  void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
88 
89 protected:
90  void _setDefaultConstraints() override;
91  matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
92  void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
93  bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
94 
95  matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
96  matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
97  matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/
98  matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
99  float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
100  WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
101 
102  uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
103  uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
104  uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
105 
106  State _current_state{State::none};
107  float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
108  int _mission_gear{landing_gear_s::GEAR_KEEP};
109 
110  float _yaw_sp_prev{NAN};
111  bool _yaw_sp_aligned{false};
112 
113  ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
114 
115  DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
116  (ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
117  (ParamFloat<px4::params::MPC_CRUISE_90>) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line
118  (ParamFloat<px4::params::NAV_MC_ALT_RAD>)
119  _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
120  (ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
121  (ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
122  (ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
123  (ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
124  (ParamBool<px4::params::WV_EN>) _param_wv_en // enable/disable weather vane (VTOL)
125  );
126 
127 private:
128  matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
129  bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */
130 
131  uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
132 
134  _triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
136  _triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/
138  _triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
139  matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
140 
141  map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
142  float _reference_altitude{NAN}; /**< Altitude relative to ground. */
143  hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
144 
145  WeatherVane *_ext_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
146 
147 
148  void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
149  bool _evaluateTriplets(); /**< Checks and sets triplets. */
150  bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
151  bool _evaluateGlobalReference(); /**< Check is global reference is available. */
152  State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
153  void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */
154 };
Definition of geo / math functions to perform geodesic calculations.
matrix::Vector2f _closest_pt
closest point to the vehicle position on the line previous - target
matrix::Vector3f _triplet_target
current triplet from navigator which may differ from the intenal one (_target) depending on the vehic...
Vehicle is in normal tracking mode from triplet previous to triplet target.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
WaypointType
This enum has to agree with position_setpoint_s type definition The only reason for not using the str...
Vehilce is behind previous waypoint.
Abstract base class for different advanced flight tasks like orbit, follow me, ...
matrix::Vector3f _triplet_next_wp
next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle...
void setYawHandler(WeatherVane *ext_yaw_handler) override
Sets an external yaw handler which can be used to implement a different yaw control strategy...
ObstacleAvoidance _obstacle_avoidance
class adjusting setpoints according to external avoidance module&#39;s input
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
matrix::Vector3f _triplet_prev_wp
previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the veh...
Vehicle is more than cruise speed away from track.
Vehicle is in front of target.
This class is used to inject the setpoints of an obstacle avoidance system into the FlightTasks...