Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

RTL check alt_max before using #8272

Merged
merged 2 commits into from Nov 14, 2017
Merged

RTL check alt_max before using #8272

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9c3c557
RTL check alt_max before using
dagar Nov 12, 2017
36f4717
Navigator RTL fully initialize mission item in each state
dagar Nov 12, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
165 changes: 70 additions & 95 deletions src/modules/navigator/rtl.cpp
View file
Open in desktop
Expand Up @@ -40,29 +40,21 @@
#include "rtl.h"
#include "navigator.h"

#include <geo/geo.h>
#include <cfloat>

#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/uORB.h>

static constexpr float DELAY_SIGMA = 0.01f;
using math::max;
using math::min;

RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RTL_RETURN_ALT", false),
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
_param_land_delay(this, "RTL_LAND_DELAY", false),
_param_rtl_min_dist(this, "RTL_MIN_DIST", false)
{
/* load initial params */
updateParams();

/* initial reset */
on_inactive();
}

void
Expand All @@ -72,39 +64,23 @@ RTL::on_inactive()
_rtl_state = RTL_STATE_NONE;
}

float
RTL::get_rtl_altitude()
{
return math::min(_param_return_alt.get(), _navigator->get_land_detected()->alt_max);
}

void
RTL::on_activation()
{
set_current_position_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->next.valid = false;

/* for safety reasons don't go into RTL if landed */
if (_navigator->get_land_detected()->landed) {
// for safety reasons don't go into RTL if landed
_rtl_state = RTL_STATE_LANDED;

} else if ((_rtl_alt_min
|| _navigator->get_global_position()->alt < (_navigator->get_home_position()->alt + get_rtl_altitude()))) {
} else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get())
|| _rtl_alt_min) {

/* if lower than return altitude, climb up first */
// if lower than return altitude, climb up first
// if rtl_alt_min is true then forcing altitude change even if above
_rtl_state = RTL_STATE_CLIMB;

} else {
/* otherwise go straight to return */
// otherwise go straight to return
_rtl_state = RTL_STATE_RETURN;

/* set altitude setpoint to current altitude */
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}

set_rtl_item();
Expand All @@ -128,75 +104,72 @@ RTL::set_return_alt_min(bool min)
void
RTL::set_rtl_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

_navigator->set_can_loiter_at_sp(false);

const home_position_s &home = *_navigator->get_home_position();
const vehicle_global_position_s &gpos = *_navigator->get_global_position();

position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

switch (_rtl_state) {
case RTL_STATE_CLIMB: {

// check if we are pretty close to home already
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);

// if we are close to home we do not climb as high, otherwise we climb to return alt
float climb_alt = _navigator->get_home_position()->alt + get_rtl_altitude();
float climb_alt = home.alt + _param_return_alt.get();

// we are close to home, limit climb to min
if (home_dist < _param_rtl_min_dist.get()) {
climb_alt = _navigator->get_home_position()->alt + _param_descend_alt.get();
climb_alt = home.alt + _param_descend_alt.get();
}

_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = gpos.lat;
_mission_item.lon = gpos.lon;
_mission_item.altitude = climb_alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;

mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
(int)(climb_alt),
(int)(climb_alt - _navigator->get_home_position()->alt));
(int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt));
break;
}

case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;

// don't change altitude
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = gpos.alt;
_mission_item.altitude_is_relative = false;

// use home yaw if close to home
/* check if we are pretty close to home already */
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);

if (home_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.yaw = home.yaw;

} else {
// use current heading to home
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon);
}

_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;

mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
(int)(_mission_item.altitude), (int)(_mission_item.altitude - home.alt));

break;
}

Expand All @@ -207,31 +180,22 @@ RTL::set_rtl_item()
}

case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = min(home.alt + _param_descend_alt.get(), gpos.alt);
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

// check if we are already lower - then we will just stay there
if (_mission_item.altitude > _navigator->get_global_position()->alt) {
_mission_item.altitude = _navigator->get_global_position()->alt;
}
// except for vtol which might be still off here and should point towards this location
const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);

_mission_item.yaw = _navigator->get_home_position()->yaw;
if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);

// except for vtol which might be still off here and should point towards this location
float d_current = get_distance_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);

if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
} else {
_mission_item.yaw = home.yaw;
}

_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
Expand All @@ -241,41 +205,52 @@ RTL::set_rtl_item()
pos_sp_triplet->previous.valid = false;

mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
(int)(_mission_item.altitude), (int)(_mission_item.altitude - home.alt));
break;
}

case RTL_STATE_LOITER: {
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
const bool autoland = (_param_land_delay.get() > FLT_EPSILON);

_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.altitude = gpos.alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = home.yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.time_inside = max(_param_land_delay.get(), 0.0f);
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;

_navigator->set_can_loiter_at_sp(true);

if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
(double)get_time_inside(_mission_item));

} else {
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering");
}

break;
}

case RTL_STATE_LAND: {
set_land_item(&_mission_item, false);
_mission_item.yaw = _navigator->get_home_position()->yaw;
// land at home position
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.yaw = home.yaw;
_mission_item.altitude = home.alt;
_mission_item.altitude_is_relative = false;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;

mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
break;
Expand All @@ -300,10 +275,10 @@ RTL::set_rtl_item()

/* convert mission item to current position setpoint and make it valid */
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;

_navigator->set_position_setpoint_triplet_updated();
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
_navigator->set_position_setpoint_triplet_updated();
}
}

void
Expand All @@ -330,7 +305,7 @@ RTL::advance_rtl()
case RTL_STATE_DESCEND:

/* only go to land if autoland is enabled */
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
if (_param_land_delay.get() > FLT_EPSILON) {
_rtl_state = RTL_STATE_LOITER;

} else {
Expand Down
13 changes: 0 additions & 13 deletions src/modules/navigator/rtl.h
View file
Open in desktop
Expand Up @@ -41,13 +41,6 @@
#ifndef NAVIGATOR_RTL_H
#define NAVIGATOR_RTL_H

#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>

#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>

#include "navigator_mode.h"
#include "mission_block.h"

Expand Down Expand Up @@ -76,12 +69,6 @@ class RTL : public MissionBlock
*/
void advance_rtl();

/**
* Get RTL altitude
*/
float get_rtl_altitude();


enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
Expand Down