PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTaskAutoMapper.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 /**
35  * @file FlightAutoMapper.cpp
36  */
37 
38 #include "FlightTaskAutoMapper.hpp"
39 #include <mathlib/mathlib.h>
40 
41 using namespace matrix;
42 
44 {
45  bool ret = FlightTaskAuto::activate(last_setpoint);
46  _reset();
47  return ret;
48 }
49 
51 {
52  // always reset constraints because they might change depending on the type
53  _setDefaultConstraints();
54 
55  _updateAltitudeAboveGround();
56 
57  bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
58  bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
59 
60  // 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state.
61  if (follow_line && !follow_line_prev) {
62  _reset();
63  }
64 
65  // The only time a thrust set-point is sent out is during
66  // idle. Hence, reset thrust set-point to NAN in case the
67  // vehicle exits idle.
68 
69  if (_type_previous == WaypointType::idle) {
70  _thrust_setpoint = Vector3f(NAN, NAN, NAN);
71  }
72 
73  // during mission and reposition, raise the landing gears but only
74  // if altitude is high enough
75  if (_highEnoughForLandingGear()) {
76  _gear.landing_gear = _mission_gear;
77  }
78 
79  if (_type == WaypointType::idle) {
80  _generateIdleSetpoints();
81 
82  } else if (_type == WaypointType::land) {
83  _generateLandSetpoints();
84 
85  } else if (follow_line) {
86  _generateSetpoints();
87 
88  } else if (_type == WaypointType::takeoff) {
89  _generateTakeoffSetpoints();
90 
91  } else if (_type == WaypointType::velocity) {
92  _generateVelocitySetpoints();
93  }
94 
95  if (_param_com_obs_avoid.get()) {
96  _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
97  _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
98  _yawspeed_setpoint);
99  }
100 
101  // update previous type
102  _type_previous = _type;
103 
104  return true;
105 }
106 
108 {
109  // Set setpoints equal current state.
110  _velocity_setpoint = _velocity;
111  _position_setpoint = _position;
112 }
113 
115 {
116  // Send zero thrust setpoint
117  _position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints
118  _velocity_setpoint = Vector3f(NAN, NAN, NAN);
119  _thrust_setpoint.zero();
120 }
121 
123 {
124  // Keep xy-position and go down with landspeed
125  _position_setpoint = Vector3f(_target(0), _target(1), NAN);
126  _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
127  // set constraints
128  _constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
129  _constraints.speed_down = _param_mpc_land_speed.get();
130  _gear.landing_gear = landing_gear_s::GEAR_DOWN;
131 }
132 
134 {
135  // Takeoff is completely defined by target position
136  _position_setpoint = _target;
137  _velocity_setpoint = Vector3f(NAN, NAN, NAN);
138 
139  // limit vertical speed during takeoff
140  _constraints.speed_up = math::gradual(_alt_above_ground, _param_mpc_land_alt2.get(),
141  _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up);
142 
143  _gear.landing_gear = landing_gear_s::GEAR_DOWN;
144 }
145 
147 {
148  // TODO: Remove velocity force logic from navigator, since
149  // navigator should only send out waypoints.
150  _position_setpoint = Vector3f(NAN, NAN, _position(2));
151  Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
152  _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
153 }
154 
156 {
157  // Altitude above ground is by default just the negation of the current local position in D-direction.
158  _alt_above_ground = -_position(2);
159 
160  if (PX4_ISFINITE(_dist_to_bottom)) {
161  // We have a valid distance to ground measurement
162  _alt_above_ground = _dist_to_bottom;
163 
164  } else if (_sub_home_position.get().valid_alt) {
165  // if home position is set, then altitude above ground is relative to the home position
166  _alt_above_ground = -_position(2) + _sub_home_position.get().z;
167  }
168 }
169 
171 {
172  FlightTaskAuto::updateParams();
173 
174  // make sure that alt1 is above alt2
175  _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
176 }
177 
179 {
180  // return true if altitude is above two meters
181  return _alt_above_ground > 2.0f;
182 }
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
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.
const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
Definition: Functions.hpp:139
constexpr T radians(T degrees)
Definition: Limits.hpp:85
void _updateAltitudeAboveGround()
Computes altitude above ground based on sensors available.
Vector2< float > Vector2f
Definition: Vector2.hpp:69
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
Call once on the event where you switch to the task.
bool _highEnoughForLandingGear()
Checks if gears can be lowered.
Vector3< float > Vector3f
Definition: Vector3.hpp:136
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
void _reset()
Resets member variables to current vehicle state.
void updateParams() override
See ModuleParam class.
Abstract Flight task which generates local setpoints based on the triplet type.