51 ModuleParams(navigator)
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;
88 dlat = mission_landing_lat - global_position.
lat;
89 dlon = mission_landing_lon - global_position.
lon;
90 double dist_squared = dlat * dlat + dlon * dlon;
94 min_dist_squared = dist_squared;
112 int num_safe_points = 0;
119 int closest_index = 0;
121 for (
int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
126 PX4_ERR(
"dm_read failed");
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;
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;
142 if (closest_index > 0) {
147 switch (closest_safe_point.frame) {
173 return _param_rtl_type.get();
266 switch (_rtl_state) {
296 if (destination_dist < _param_rtl_min_dist.get()) {
351 const bool autoland = (_param_rtl_land_delay.get() >
FLT_EPSILON);
425 switch (_rtl_state) {
486 if (destination_dist <= _param_rtl_min_dist.get()) {
489 }
else if (gpos.
alt >
_destination.
alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
490 rtl_altitude = gpos.
alt;
492 }
else if (cone_half_angle_deg <= 0) {
501 float height_above_destination_min = destination_dist / tanf(cone_half_angle_rad);
507 if (gpos.
alt < altitude_min) {
508 rtl_altitude = altitude_min;
511 rtl_altitude = gpos.
alt;
#define VEHICLE_TYPE_FIXED_WING
void advance_rtl()
Move to next RTL item.
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
bool get_mission_start_land_available()
Helper class to access missions.
float get_acceptance_radius()
Get the acceptance radius.
void set(const home_position_s &home_position)
float altitude
altitude in meters (AMSL)
uint16_t autocontinue
true if next waypoint should follow after this one
float acceptance_radius
default radius in which the mission is accepted as reached in meters
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.
struct home_position_s * get_home_position()
Getters.
double get_mission_landing_lon()
static constexpr float DELAY_SIGMA
uint16_t origin
how the mission item was generated
struct position_setpoint_s previous
struct vehicle_land_detected_s * get_land_detected()
#define mavlink_and_console_log_info(_pub, _text,...)
Send a mavlink emergency message and print to console.
double lon
longitude in degrees
void set_return_alt_min(bool min)
double get_mission_landing_lat()
void set_rtl_item()
Set the RTL item.
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.
void on_inactive() override
This function is called while the mode is inactive.
struct vehicle_global_position_s * get_global_position()
mission_item_s _mission_item
uint16_t nav_cmd
navigation command
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void mission_apply_limitation(mission_item_s &item)
General function used to adjust the mission item based on vehicle specific limitations.
dataman housekeeping information for a specific item.
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.
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
Safe Point (Rally Point).
constexpr _Tp min(_Tp a, _Tp b)
RTLPosition _destination
the RTL position to fly to (typically the home position or a safe point)
float get_time_inside(const mission_item_s &item) const
struct position_setpoint_triplet_s * get_position_setpoint_triplet()
uint16_t num_items
total number of items stored (excluding this one)
void issue_command(const mission_item_s &item)
void find_RTL_destination()
constexpr _Tp max(_Tp a, _Tp b)
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.
bool start_mission_landing()
void on_active() override
This function is called while the mode is active.
float get_loiter_radius()
static bool item_contains_position(const mission_item_s &item)
bool on_mission_landing()
orb_advert_t * get_mavlink_log_pub()
enum RTL::RTLState RTL_STATE_NONE
float get_mission_landing_alt()
struct vehicle_status_s * get_vstatus()
void on_activation() override
This function is called one time when mode becomes active, pos_sp_triplet must be initialized here...
RTL(Navigator *navigator)
float yaw
in radians NED -PI..+PI, NAN means don't change yaw
float loiter_radius
loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise
void set_can_loiter_at_sp(bool can_loiter)
Setters.
void set_position_setpoint_triplet_updated()
void set_idle_item(struct mission_item_s *item)
Set idle mission item.
struct vehicle_local_position_s * get_local_position()
float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
uint16_t altitude_is_relative
true if altitude is relative from start point
void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode)
Set vtol transition item.
double lat
latitude in degrees