fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'navigator_rewrite' into navigator_rewrite_offboard2_merge

Browse Source
This commit is contained in:
Julian Oes 2014-06-27 12:55:42 +02:00
commit 332f03fa67
14 changed files with 242 additions and 516 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/commander/commander.cpp
View File

@ -1645,10 +1645,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break; // changed successfully or already in this state
}
// else fallback to ALTCTL
print_reject_mode(status, "POSCTL");
}
// fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
@ -1659,7 +1659,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
print_reject_mode(status, "ALTCTL");
}
// else fallback to MANUAL
// fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
@ -1671,28 +1671,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode(status, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode(status, "AUTO_LOITER");
} else {
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode(status, "AUTO_MISSION");
}
// else fallback to ALTCTL (POSCTL likely will not work too)
// fallback to POSCTL
res = main_state_transition(status, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to MANUAL
// fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;

6
src/modules/navigator/loiter.cpp
View File

@ -53,8 +53,7 @@
#include "loiter.h"
Loiter::Loiter(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator)
MissionBlock(navigator, name)
{
/* load initial params */
updateParams();
@ -70,11 +69,10 @@ bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(false, pos_sp_triplet);;
return set_loiter_item(pos_sp_triplet);
}
void
Loiter::on_inactive()
{
}

2
src/modules/navigator/loiter.h
View File

@ -47,7 +47,7 @@
#include "navigator_mode.h"
#include "mission_block.h"
class Loiter : public NavigatorMode, MissionBlock
class Loiter : public MissionBlock
{
public:
/**

23
src/modules/navigator/mission.cpp
View File

@ -58,8 +58,7 @@
#include "mission.h"
Mission::Mission(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_onboard_mission({0}),
_offboard_mission({0}),
@ -223,7 +222,7 @@ Mission::advance_mission()
void
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
{
set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
set_previous_pos_setpoint(pos_sp_triplet);
/* try setting onboard mission item */
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
@ -233,7 +232,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: onboard mission running");
}
_mission_type = MISSION_TYPE_ONBOARD;
_navigator->set_is_in_loiter(false);
_navigator->set_can_loiter_at_sp(false);
/* try setting offboard mission item */
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@ -243,7 +242,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: offboard mission running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
_navigator->set_is_in_loiter(false);
_navigator->set_can_loiter_at_sp(false);
} else {
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
@ -253,24 +252,14 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: no mission available");
}
_mission_type = MISSION_TYPE_NONE;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
set_loiter_item(use_current_pos_sp, pos_sp_triplet);
set_loiter_item(pos_sp_triplet);
reset_mission_item_reached();
report_mission_finished();
}
}
void
Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp)
{
/* reuse current setpoint as previous setpoint */
if (current_pos_sp->valid) {
memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
}
}
bool
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{

8
src/modules/navigator/mission.h
View File

@ -62,7 +62,7 @@
class Navigator;
class Mission : public NavigatorMode, MissionBlock
class Mission : public MissionBlock
{
public:
/**
@ -106,12 +106,6 @@ private:
*/
void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Set previous position setpoint
*/
void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp);
/**
* Try to set the current position setpoint from an onboard mission item
* @return true if mission item successfully set

117
src/modules/navigator/mission_block.cpp
View File

@ -52,13 +52,13 @@
#include "mission_block.h"
MissionBlock::MissionBlock(Navigator *navigator) :
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_mission_item({0}),
_mission_item_valid(false),
_navigator_priv(navigator)
_mission_item_valid(false)
{
}
@ -69,20 +69,15 @@ MissionBlock::~MissionBlock()
bool
MissionBlock::is_mission_item_reached()
{
/* don't check landed WPs */
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return false;
return _navigator->get_vstatus()->condition_landed;
}
/* TODO: count turns */
#if 0
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
/* TODO: count turns */
if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
return false;
}
#endif
hrt_abstime now = hrt_absolute_time();
@ -93,24 +88,24 @@ MissionBlock::is_mission_item_reached()
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator_priv->get_home_position()->alt
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator_priv->get_global_position()->lat,
_navigator_priv->get_global_position()->lon,
_navigator_priv->get_global_position()->alt,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
if (_navigator_priv->get_global_position()->alt >
altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {
@ -124,10 +119,10 @@ MissionBlock::is_mission_item_reached()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
@ -172,54 +167,76 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
sp->type = SETPOINT_TYPE_TAKEOFF;
break;
} else if (item->nav_cmd == NAV_CMD_LAND) {
case NAV_CMD_LAND:
sp->type = SETPOINT_TYPE_LAND;
break;
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = SETPOINT_TYPE_LOITER;
break;
} else {
default:
sp->type = SETPOINT_TYPE_POSITION;
break;
}
}
void
MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* reuse current setpoint as previous setpoint */
if (pos_sp_triplet->current.valid) {
memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
}
}
bool
MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
{
if (_navigator_priv->get_is_in_loiter()) {
/* already loitering, bail out */
return false;
}
if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
/* leave position setpoint as is */
} else {
/* don't change setpoint if 'can_loiter_at_sp' flag set */
if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
/* use current position */
pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
_navigator->set_can_loiter_at_sp(true);
}
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
|| pos_sp_triplet->next.valid) {
/* position setpoint triplet should be updated */
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
_navigator_priv->set_is_in_loiter(true);
return true;
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
return true;
}
return false;
}

19
src/modules/navigator/mission_block.h
View File

@ -47,17 +47,17 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "navigator_mode.h"
class Navigator;
class MissionBlock
class MissionBlock : public NavigatorMode
{
public:
/**
* Constructor
*
* @param pointer to parent class
*/
MissionBlock(Navigator *navigator);
MissionBlock(Navigator *navigator, const char *name);
/**
* Destructor
@ -82,14 +82,18 @@ public:
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
/**
* Set previous position setpoint to current setpoint
*/
void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
*
* @param true if the current position setpoint should be re-used
* @param the position setpoint triplet to set
* @return true if setpoint has changed
*/
bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@ -97,9 +101,6 @@ public:
mission_item_s _mission_item;
bool _mission_item_valid;
private:
Navigator *_navigator_priv;
};
#endif

10
src/modules/navigator/navigator.h
View File

@ -103,7 +103,7 @@ public:
/**
* Setters
*/
void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
/**
* Getters
@ -117,9 +117,9 @@ public:
int get_offboard_mission_sub() { return _offboard_mission_sub; }
int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
Geofence& get_geofence() { return _geofence; }
bool get_is_in_loiter() { return _is_in_loiter; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
float get_acceptance_radius() { return _param_acceptance_radius.get(); }
int get_mavlink_fd() { return _mavlink_fd; }
private:
@ -167,11 +167,11 @@ private:
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
bool _is_in_loiter; /**< flags if current position SP can be used to loiter */
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
/**
* Retrieve global position
*/

330
src/modules/navigator/navigator_main.cpp
View File

@ -126,7 +126,7 @@ Navigator::Navigator() :
_offboard(this, "OFF"),
_update_triplet(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
_param_acceptance_radius(this, "ACC_RAD")
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@ -352,7 +352,7 @@ Navigator::task_main()
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
_navigation_mode = nullptr;
_is_in_loiter = false;
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
_navigation_mode = &_mission;
@ -373,7 +373,7 @@ Navigator::task_main()
break;
default:
_navigation_mode = nullptr;
_is_in_loiter = false;
_can_loiter_at_sp = false;
break;
}
@ -394,7 +394,7 @@ Navigator::task_main()
_update_triplet = true;
}
if (_update_triplet ) {
if (_update_triplet) {
publish_position_setpoint_triplet();
_update_triplet = false;
}
@ -454,329 +454,7 @@ Navigator::status()
warnx("Geofence not set");
}
}
#if 0
bool
Navigator::start_none_on_ground()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
return true;
}
bool
Navigator::start_none_in_air()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
return true;
}
bool
Navigator::start_auto_on_ground()
{
reset_reached();
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
_update_triplet = true;
return true;
}
bool
Navigator::start_loiter()
{
/* if no existing item available, use current position */
if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw
_pos_sp_triplet.current.alt = _global_pos.alt;
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
_update_triplet = true;
return true;
}
bool
Navigator::start_mission()
{
/* start fresh */
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
return set_mission_items();
}
bool
Navigator::advance_mission()
{
/* tell mission to move by one */
_mission.move_to_next();
/* now try to set the new mission items, if it fails, it will dispatch loiter */
return set_mission_items();
}
bool
Navigator::set_mission_items()
{
if (_pos_sp_triplet.current.valid) {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_pos_sp_triplet.previous.valid = true;
}
bool onboard;
int index;
/* if we fail to set the current mission, continue to loiter */
if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
return false;
}
/* if we got an RTL mission item, switch to RTL mode and give up */
if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
return false;
}
_mission_item_valid = true;
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.current.valid = true;
mission_item_s next_mission_item;
bool last_wp = false;
/* now try to set the next mission item as well, if there is no more next
* this means we're heading to the last waypoint */
if (_mission.get_next_mission_item(&next_mission_item)) {
/* convert the next mission item and set it valid */
mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
_pos_sp_triplet.next.valid = true;
} else {
last_wp = true;
}
/* notify user about what happened */
mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
_update_triplet = true;
reset_reached();
return true;
}
bool
Navigator::start_rtl()
{
if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
_mission_item_valid = true;
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.current.valid = true;
reset_reached();
_update_triplet = true;
return true;
}
/* if RTL doesn't work, fallback to loiter */
return false;
}
bool
Navigator::advance_rtl()
{
/* tell mission to move by one */
_rtl.move_to_next();
/* now try to set the new mission items, if it fails, it will dispatch loiter */
if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
_mission_item_valid = true;
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.current.valid = true;
reset_reached();
_update_triplet = true;
return true;
}
return false;
}
bool
Navigator::start_land()
{
/* TODO: verify/test */
reset_reached();
/* this state can be requested by commander even if no global position available,
* in his case controller must perform landing without position control */
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_mission_item_valid = true;
mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
return true;
}
bool
Navigator::check_mission_item_reached()
{
/* only check if there is actually a mission item to check */
if (!_mission_item_valid) {
return false;
}
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return _vstatus.condition_landed;
}
/* XXX TODO count turns */
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
// return false;
// }
uint64_t now = hrt_absolute_time();
if (!_waypoint_position_reached) {
float acceptance_radius;
if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
acceptance_radius = _mission_item.acceptance_radius;
} else {
acceptance_radius = _parameters.acceptance_radius;
}
if (_do_takeoff) {
/* require only altitude for takeoff */
if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
_waypoint_position_reached = true;
}
} else {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
/* calculate AMSL altitude for this waypoint */
float wp_alt_amsl = _mission_item.altitude;
if (_mission_item.altitude_is_relative)
wp_alt_amsl += _home_pos.alt;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
&dist_xy, &dist_z);
if (dist >= 0.0f && dist <= acceptance_radius) {
_waypoint_position_reached = true;
}
}
}
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
}
/* check if the current waypoint was reached */
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
if (_mission_item.time_inside > 0.01f) {
mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
(double)_mission_item.time_inside);
}
}
/* check if the MAV was long enough inside the waypoint orbit */
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
return true;
}
}
return false;
}
void
Navigator::reset_reached()
{
_time_first_inside_orbit = 0;
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
}
#endif
void
Navigator::publish_position_setpoint_triplet()
{

6
src/modules/navigator/navigator_params.c
View File

@ -55,12 +55,12 @@
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Takeoff Acceptance Radius (FW only)
* Acceptance Radius
*
* Acceptance radius for fixedwing.
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
*
* @unit meters
* @min 1.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);

188
src/modules/navigator/rtl.cpp
View File

@ -44,6 +44,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@ -52,14 +53,14 @@
#include "navigator.h"
#include "rtl.h"
#define DELAY_SIGMA 0.01f
RTL::RTL(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY"),
_param_acceptance_radius(this, "ACCEPT_RAD")
_param_land_delay(this, "LAND_DELAY")
{
/* load initial params */
updateParams();
@ -75,7 +76,11 @@ void
RTL::on_inactive()
{
_first_run = true;
_rtl_state = RTL_STATE_NONE;
/* reset RTL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_rtl_state = RTL_STATE_NONE;
}
}
bool
@ -84,14 +89,33 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
bool updated = false;
if (_first_run) {
_first_run = false;
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item(pos_sp_triplet);
updated = true;
_first_run = false;
}
if ((_rtl_state == RTL_STATE_CLIMB
|| _rtl_state == RTL_STATE_RETURN)
&& is_mission_item_reached()) {
} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
@ -106,31 +130,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
/* make sure we have the latest params */
updateParams();
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_FINISHED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
_rtl_state = RTL_STATE_RETURN;
}
}
/* if switching directly to return state, set altitude setpoint to current altitude */
if (_rtl_state == RTL_STATE_RETURN) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
set_previous_pos_setpoint(pos_sp_triplet);
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
_mission_item.lat = _navigator->get_global_position()->lat;
@ -141,50 +145,49 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_RETURN: {
case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
/* TODO: add this again */
// don't change altitude
// if (_pos_sp_triplet.previous.valid) {
// /* if previous setpoint is valid then use it to calculate heading to home */
// _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
if (pos_sp_triplet->previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
_mission_item.lat, _mission_item.lon);
// } else {
// /* else use current position */
// _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
// }
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_DESCEND: {
case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
@ -193,21 +196,46 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(true);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_LAND: {
case RTL_STATE_LOITER: {
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_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.pitch_min = 0.0f;
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
}
break;
}
case RTL_STATE_LAND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
@ -216,36 +244,44 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
break;
}
case RTL_STATE_FINISHED: {
/* nothing to do, report fail */
case RTL_STATE_LANDED: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_IDLE;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
break;
}
default:
break;
}
if (_rtl_state == RTL_STATE_FINISHED) {
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
} else {
/* if not finished, convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
}
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
}
void
@ -262,18 +298,20 @@ RTL::advance_rtl()
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
if (_param_land_delay.get() < 0) {
_rtl_state = RTL_STATE_FINISHED;
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
_rtl_state = RTL_STATE_LOITER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_FINISHED;
case RTL_STATE_LOITER:
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_FINISHED:
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_LANDED;
break;
default:

6
src/modules/navigator/rtl.h
View File

@ -54,7 +54,7 @@
class Navigator;
class RTL : public NavigatorMode, MissionBlock
class RTL : public MissionBlock
{
public:
/**
@ -97,14 +97,14 @@ private:
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LOITER,
RTL_STATE_LAND,
RTL_STATE_FINISHED,
RTL_STATE_LANDED,
} _rtl_state;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
control::BlockParamFloat _param_acceptance_radius;
};
#endif

12
src/modules/navigator/rtl_params.c
View File

@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
/**
* RTL acceptance radius
*
* Acceptance radius for waypoints set for RTL
*
* @unit meters
* @min 1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);

1
src/modules/uORB/topics/mission.h
View File

@ -50,6 +50,7 @@
/* compatible to mavlink MAV_CMD */
enum NAV_CMD {
NAV_CMD_IDLE=0,
NAV_CMD_WAYPOINT=16,
NAV_CMD_LOITER_UNLIMITED=17,
NAV_CMD_LOITER_TURN_COUNT=18,