PX4 Firmware
PX4 Autopilot Software http://px4.io
rtl.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2019 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 rtl.cpp
35  *
36  * Helper class to access RTL
37  *
38  * @author Julian Oes <julian@oes.ch>
39  * @author Anton Babushkin <anton.babushkin@me.com>
40  */
41 
42 #include "rtl.h"
43 #include "navigator.h"
44 #include <dataman/dataman.h>
45 
46 
47 static constexpr float DELAY_SIGMA = 0.01f;
48 
50  MissionBlock(navigator),
51  ModuleParams(navigator)
52 {
53 }
54 
55 void
57 {
58  // Reset RTL state.
59  _rtl_state = RTL_STATE_NONE;
60 
62 
63 }
64 
65 void
67 {
68  // get home position:
69  home_position_s &home_landing_position = *_navigator->get_home_position();
70  // get global position
71  const vehicle_global_position_s &global_position = *_navigator->get_global_position();
72 
73  // set destination to home per default, then check if other valid landing spot is closer
74  _destination.set(home_landing_position);
75  // get distance to home position
76  double dlat = home_landing_position.lat - global_position.lat;
77  double dlon = home_landing_position.lon - global_position.lon;
78  double min_dist_squared = dlat * dlat + dlon * dlon;
79 
81 
82  // consider the mission landing if not RTL_HOME type set
84  double mission_landing_lat = _navigator->get_mission_landing_lat();
85  double mission_landing_lon = _navigator->get_mission_landing_lon();
86 
87  // compare home position to landing position to decide which is closer
88  dlat = mission_landing_lat - global_position.lat;
89  dlon = mission_landing_lon - global_position.lon;
90  double dist_squared = dlat * dlat + dlon * dlon;
91 
92  // set destination to mission landing if closest or in RTL_LAND or RTL_MISSION (so not in RTL_CLOSEST)
93  if (dist_squared < min_dist_squared || rtl_type() != RTL_CLOSEST) {
94  min_dist_squared = dist_squared;
99 
100  }
101  }
102 
103  // do not consider rally point if RTL type is set to RTL_MISSION, so exit function and use either home or mission landing
104  if (rtl_type() == RTL_MISSION) {
105  return;
106  }
107 
108  // compare to safe landing positions
109  mission_safe_point_s closest_safe_point {} ;
110  mission_stats_entry_s stats;
111  int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
112  int num_safe_points = 0;
113 
114  if (ret == sizeof(mission_stats_entry_s)) {
115  num_safe_points = stats.num_items;
116  }
117 
118  // check if a safe point is closer than home or landing
119  int closest_index = 0;
120 
121  for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
122  mission_safe_point_s mission_safe_point;
123 
124  if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) !=
125  sizeof(mission_safe_point_s)) {
126  PX4_ERR("dm_read failed");
127  continue;
128  }
129 
130  // TODO: take altitude into account for distance measurement
131  dlat = mission_safe_point.lat - global_position.lat;
132  dlon = mission_safe_point.lon - global_position.lon;
133  double dist_squared = dlat * dlat + dlon * dlon;
134 
135  if (dist_squared < min_dist_squared) {
136  closest_index = current_seq;
137  min_dist_squared = dist_squared;
138  closest_safe_point = mission_safe_point;
139  }
140  }
141 
142  if (closest_index > 0) {
144 
145  // There is a safe point closer than home/mission landing
146  // TODO: handle all possible mission_safe_point.frame cases
147  switch (closest_safe_point.frame) {
148  case 0: // MAV_FRAME_GLOBAL
149  _destination.lat = closest_safe_point.lat;
150  _destination.lon = closest_safe_point.lon;
151  _destination.alt = closest_safe_point.alt;
152  _destination.yaw = home_landing_position.yaw;
153  break;
154 
155  case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
156  _destination.lat = closest_safe_point.lat;
157  _destination.lon = closest_safe_point.lon;
158  _destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home
159  _destination.yaw = home_landing_position.yaw;
160  break;
161 
162  default:
163  mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME");
164  break;
165  }
166  }
167 
168 }
169 
170 int
172 {
173  return _param_rtl_type.get();
174 }
175 
176 void
178 {
179 
180  // output the correct message, depending on where the RTL destination is
181  switch (_destination.type) {
183  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.");
184  break;
185 
187  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.");
188  break;
189 
191  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.");
192  break;
193  }
194 
195  const vehicle_global_position_s &global_position = *_navigator->get_global_position();
196 
197  _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
198 
200  // For safety reasons don't go into RTL if landed.
201  _rtl_state = RTL_STATE_LANDED;
202 
204  // RTL straight to RETURN state, but mission will takeover for landing.
205 
206  } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
207 
208  // If lower than return altitude, climb up first.
209  // If rtl_alt_min is true then forcing altitude change even if above.
210  _rtl_state = RTL_STATE_CLIMB;
211 
212  } else {
213  // Otherwise go straight to return
214  _rtl_state = RTL_STATE_RETURN;
215  }
216 
217  set_rtl_item();
218 }
219 
220 void
222 {
223  if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
224  advance_rtl();
225  set_rtl_item();
226  }
227 }
228 
229 void
231 {
232  _rtl_alt_min = min;
233 }
234 
235 void
237 {
238  // RTL_TYPE: mission landing.
239  // Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination.
240  // After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing.
242  if (_rtl_state > RTL_STATE_CLIMB) {
244  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing");
245  return;
246 
247  } else {
248  // Otherwise use regular RTL.
249  mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing");
250  }
251  }
252  }
253 
255 
257 
259 
260  // Check if we are pretty close to the destination already.
261  const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
262 
263  // Compute the loiter altitude.
264  const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
265 
266  switch (_rtl_state) {
267  case RTL_STATE_CLIMB: {
268 
270  _mission_item.lat = gpos.lat;
271  _mission_item.lon = gpos.lon;
276  _mission_item.time_inside = 0.0f;
279 
280  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)",
281  (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt));
282  break;
283  }
284 
285  case RTL_STATE_RETURN: {
286 
287  // Don't change altitude.
293 
294  // Use destination yaw if close to _destination.
295  // Check if we are pretty close to the destination already.
296  if (destination_dist < _param_rtl_min_dist.get()) {
298 
299  } else {
300  // Use current heading to _destination.
302  }
303 
305  _mission_item.time_inside = 0.0f;
308 
309  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)",
310  (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
311 
312  break;
313  }
314 
316  set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
317  break;
318  }
319 
320  case RTL_STATE_DESCEND: {
324  _mission_item.altitude = loiter_altitude;
326 
327  // Except for vtol which might be still off here and should point towards this location.
328  const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
329 
330  if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
332 
333  } else {
335  }
336 
338  _mission_item.time_inside = 0.0f;
341 
342  // Disable previous setpoint to prevent drift.
343  pos_sp_triplet->previous.valid = false;
344 
345  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)",
346  (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
347  break;
348  }
349 
350  case RTL_STATE_LOITER: {
351  const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON);
352 
353  // Don't change altitude.
356  _mission_item.altitude = loiter_altitude;
361  _mission_item.time_inside = math::max(_param_rtl_land_delay.get(), 0.0f);
362  _mission_item.autocontinue = autoland;
364 
366 
367  if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) {
370  (double)get_time_inside(_mission_item));
371 
372  } else {
374  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering");
375  }
376 
377  break;
378  }
379 
380  case RTL_STATE_LAND: {
381  // Land at destination.
389  _mission_item.time_inside = 0.0f;
392 
393  mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination");
394  break;
395  }
396 
397  case RTL_STATE_LANDED: {
399  set_return_alt_min(false);
400  break;
401  }
402 
403  default:
404  break;
405  }
406 
408 
409  // Execute command if set. This is required for commands like VTOL transition.
412  }
413 
414  // Convert mission item to current position setpoint and make it valid.
416 
419  }
420 }
421 
422 void
424 {
425  switch (_rtl_state) {
426  case RTL_STATE_CLIMB:
427  _rtl_state = RTL_STATE_RETURN;
428  break;
429 
430  case RTL_STATE_RETURN:
431 
432  // Descend to desired altitude if delay is set, directly land otherwise
433  if (_param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA) {
434  _rtl_state = RTL_STATE_DESCEND;
435 
436  } else {
437  _rtl_state = RTL_STATE_LAND;
438  }
439 
442  _rtl_state = RTL_STATE_TRANSITION_TO_MC;
443  }
444 
445  break;
446 
448  _rtl_state = RTL_STATE_RETURN;
449  break;
450 
451  case RTL_STATE_DESCEND:
452 
453  // Only go to land if autoland is enabled.
454  if (_param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA) {
455  _rtl_state = RTL_STATE_LOITER;
456 
457  } else {
458  _rtl_state = RTL_STATE_LAND;
459  }
460 
461  break;
462 
463  case RTL_STATE_LOITER:
464  _rtl_state = RTL_STATE_LAND;
465  break;
466 
467  case RTL_STATE_LAND:
468  _rtl_state = RTL_STATE_LANDED;
469  break;
470 
471  default:
472  break;
473  }
474 }
475 
476 
477 float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
478 {
480 
481  // horizontal distance to destination
482  const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
483 
484  float rtl_altitude;
485 
486  if (destination_dist <= _param_rtl_min_dist.get()) {
487  rtl_altitude = _destination.alt + _param_rtl_descend_alt.get();
488 
489  } else if (gpos.alt > _destination.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
490  rtl_altitude = gpos.alt;
491 
492  } else if (cone_half_angle_deg <= 0) {
493  rtl_altitude = _destination.alt + _param_rtl_return_alt.get();
494 
495  } else {
496 
497  // constrain cone half angle to meaningful values. All other cases are already handled above.
498  const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f));
499 
500  // minimum height above destination required
501  float height_above_destination_min = destination_dist / tanf(cone_half_angle_rad);
502 
503  // minimum altitude we need in order to be within the user defined cone
504  const float altitude_min = math::constrain(height_above_destination_min + _destination.alt, _destination.alt,
505  _destination.alt + _param_rtl_return_alt.get());
506 
507  if (gpos.alt < altitude_min) {
508  rtl_altitude = altitude_min;
509 
510  } else {
511  rtl_altitude = gpos.alt;
512  }
513  }
514 
515  // always demand altitude which is higher or equal the RTL descend altitude
516  rtl_altitude = math::max(rtl_altitude, _destination.alt + _param_rtl_descend_alt.get());
517 
518  return rtl_altitude;
519 }
#define VEHICLE_TYPE_FIXED_WING
void advance_rtl()
Move to next RTL item.
Definition: rtl.cpp:423
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
bool get_mission_start_land_available()
Definition: navigator.h:271
float get_acceptance_radius()
Get the acceptance radius.
void set(const home_position_s &home_position)
Definition: rtl.h:118
float altitude
altitude in meters (AMSL)
Definition: navigation.h:160
uint16_t autocontinue
true if next waypoint should follow after this one
Definition: navigation.h:177
float acceptance_radius
default radius in which the mission is accepted as reached in meters
Definition: navigation.h:155
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the distance to the next waypoint in meters.
Definition: geo.cpp:270
struct home_position_s * get_home_position()
Getters.
Definition: navigator.h:151
double get_mission_landing_lon()
Definition: navigator.h:274
static constexpr float DELAY_SIGMA
Definition: rtl.cpp:47
uint16_t origin
how the mission item was generated
Definition: navigation.h:177
struct position_setpoint_s previous
struct vehicle_land_detected_s * get_land_detected()
Definition: navigator.h:157
#define FLT_EPSILON
double lon
longitude in degrees
Definition: navigation.h:147
void set_return_alt_min(bool min)
Definition: rtl.cpp:230
Navigator * _navigator
double get_mission_landing_lat()
Definition: navigator.h:273
void set_rtl_item()
Set the RTL item.
Definition: rtl.cpp:236
struct position_setpoint_s current
__EXPORT ssize_t dm_read(dm_item_t item, unsigned index, void *buf, size_t count)
Retrieve from the data manager file.
Definition: dataman.cpp:1104
void on_inactive() override
This function is called while the mode is inactive.
Definition: rtl.cpp:56
struct vehicle_global_position_s * get_global_position()
Definition: navigator.h:156
mission_item_s _mission_item
float yaw
Definition: rtl.h:114
uint16_t nav_cmd
navigation command
Definition: navigation.h:165
RTLDestinationType type
Definition: rtl.h:116
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void mission_apply_limitation(mission_item_s &item)
General function used to adjust the mission item based on vehicle specific limitations.
DATAMANAGER driver.
float _rtl_alt
Definition: rtl.h:131
dataman housekeeping information for a specific item.
Definition: navigation.h:194
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the bearing to the next waypoint in radians.
Definition: geo.cpp:320
double lon
Definition: rtl.h:112
void reset_mission_item_reached()
Reset all reached flags.
float time_inside
time that the MAV should stay inside the radius before advancing in seconds
Definition: navigation.h:151
Safe Point (Rally Point).
Definition: navigation.h:223
float alt
Definition: rtl.h:113
double lat
Definition: rtl.h:111
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
RTLPosition _destination
the RTL position to fly to (typically the home position or a safe point)
Definition: rtl.h:129
float get_time_inside(const mission_item_s &item) const
struct position_setpoint_triplet_s * get_position_setpoint_triplet()
Definition: navigator.h:153
uint16_t num_items
total number of items stored (excluding this one)
Definition: navigation.h:195
bool _rtl_alt_min
Definition: rtl.h:132
void issue_command(const mission_item_s &item)
void find_RTL_destination()
Definition: rtl.cpp:66
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
Convert a mission item to a position setpoint.
bool is_mission_item_reached()
Check if mission item has been reached.
Helper class for RTL.
bool start_mission_landing()
Definition: navigator.h:270
void on_active() override
This function is called while the mode is active.
Definition: rtl.cpp:221
float get_loiter_radius()
Definition: navigator.h:171
static bool item_contains_position(const mission_item_s &item)
bool on_mission_landing()
Definition: navigator.h:269
orb_advert_t * get_mavlink_log_pub()
Definition: navigator.h:261
enum RTL::RTLState RTL_STATE_NONE
float get_mission_landing_alt()
Definition: navigator.h:275
struct vehicle_status_s * get_vstatus()
Definition: navigator.h:159
void on_activation() override
This function is called one time when mode becomes active, pos_sp_triplet must be initialized here...
Definition: rtl.cpp:177
RTL(Navigator *navigator)
Definition: rtl.cpp:49
int rtl_type() const
Definition: rtl.cpp:171
float yaw
in radians NED -PI..+PI, NAN means don&#39;t change yaw
Definition: navigation.h:157
float loiter_radius
loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise
Definition: navigation.h:156
void set_can_loiter_at_sp(bool can_loiter)
Setters.
Definition: navigator.h:144
void set_position_setpoint_triplet_updated()
Definition: navigator.h:145
void set_idle_item(struct mission_item_s *item)
Set idle mission item.
struct vehicle_local_position_s * get_local_position()
Definition: navigator.h:158
float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
Definition: rtl.cpp:477
uint16_t altitude_is_relative
true if altitude is relative from start point
Definition: navigation.h:177
void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode)
Set vtol transition item.
double lat
latitude in degrees
Definition: navigation.h:146