dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'navigator_new' into navigator_new_vector

This commit is contained in:
Anton Babushkin 2013-12-31 19:31:16 +04:00
parent 5c84479ce9 7f9a7ffe82
commit 549fd4f0e8
3 changed files with 70 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/mavlink/mavlink.c
View File

@ -220,17 +220,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
} else if (v_status.main_state == MAIN_STATE_AUTO) { } else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
// TODO review
if (control_mode.nav_state == NAV_STATE_NONE) { if (control_mode.nav_state == NAV_STATE_NONE) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (control_mode.nav_state == NAV_STATE_LOITER) { } else if (control_mode.nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == NAV_STATE_MISSION) { } else if (control_mode.nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (control_mode.nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} }
} }
*mavlink_custom_mode = custom_mode.data; *mavlink_custom_mode = custom_mode.data;

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

@ -165,6 +165,8 @@ private:
class Mission _mission; class Mission _mission;
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _rtl_done;
bool _waypoint_position_reached; bool _waypoint_position_reached;
bool _waypoint_yaw_reached; bool _waypoint_yaw_reached;
uint64_t _time_first_inside_orbit; uint64_t _time_first_inside_orbit;
@ -260,9 +262,9 @@ private:
void start_none(); void start_none();
void start_loiter(); void start_loiter();
void start_mission(); void start_mission();
void start_mission_loiter(); void finish_mission();
void start_rtl(); void start_rtl();
void start_rtl_loiter(); void finish_rtl();
/** /**
* Guards offboard mission * Guards offboard mission
@ -354,6 +356,8 @@ Navigator::Navigator() :
_fence_valid(false), _fence_valid(false),
_inside_fence(true), _inside_fence(true),
_mission(), _mission(),
_reset_loiter_pos(true),
_rtl_done(false),
_waypoint_position_reached(false), _waypoint_position_reached(false),
_waypoint_yaw_reached(false), _waypoint_yaw_reached(false),
_time_first_inside_orbit(0), _time_first_inside_orbit(0),
@ -590,7 +594,7 @@ Navigator::task_main()
/* RC signal available, use control switches to set mode */ /* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */ /* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
dispatch(EVENT_RTL_REQUESTED); dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED);
stick_mode = true; stick_mode = true;
} else { } else {
/* MISSION switch */ /* MISSION switch */
@ -637,7 +641,7 @@ Navigator::task_main()
break; break;
case NAV_STATE_RTL: case NAV_STATE_RTL:
dispatch(EVENT_RTL_REQUESTED); dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED);
break; break;
default: default:
@ -698,16 +702,21 @@ Navigator::task_main()
/* global position updated */ /* global position updated */
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
global_position_update(); global_position_update();
/* only check if waypoint has been reached in Mission or RTL mode */ /* only check if waypoint has been reached in MISSION */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (mission_item_reached()) { if (mission_item_reached()) {
/* advance by one mission item */ if (myState == NAV_STATE_MISSION) {
_mission.move_to_next(); /* advance by one mission item */
_mission.move_to_next();
/* if no more mission items available send this to state machine and start loiter at the last WP */ /* if no more mission items available send this to state machine and start loiter at the last WP */
if (_mission.current_mission_available()) { if (_mission.current_mission_available()) {
advance_mission(); advance_mission();
} else {
dispatch(EVENT_MISSION_FINISHED);
}
} else { } else {
/* RTL finished */
dispatch(EVENT_MISSION_FINISHED); dispatch(EVENT_MISSION_FINISHED);
} }
} }
@ -788,15 +797,9 @@ Navigator::status()
case NAV_STATE_MISSION: case NAV_STATE_MISSION:
warnx("State: Mission"); warnx("State: Mission");
break; break;
case NAV_STATE_MISSION_LOITER:
warnx("State: Loiter after Mission");
break;
case NAV_STATE_RTL: case NAV_STATE_RTL:
warnx("State: RTL"); warnx("State: RTL");
break; break;
case NAV_STATE_RTL_LOITER:
warnx("State: Loiter after RTL");
break;
default: default:
warnx("State: Unknown"); warnx("State: Unknown");
break; break;
@ -920,39 +923,19 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER}, /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_mission), NAV_STATE_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
}, },
{
/* STATE_MISSION_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
},
{ {
/* STATE_RTL */ /* STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER}, /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_rtl), NAV_STATE_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
},
{
/* STATE_RTL_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
}, },
}; };
@ -963,35 +946,42 @@ Navigator::start_none()
_mission_item_triplet.current_valid = false; _mission_item_triplet.current_valid = false;
_mission_item_triplet.next_valid = false; _mission_item_triplet.next_valid = false;
_reset_loiter_pos = true;
_rtl_done = false;
publish_mission_item_triplet(); publish_mission_item_triplet();
} }
void void
Navigator::start_loiter() Navigator::start_loiter()
{ {
/* set loiter position if needed */
if (_reset_loiter_pos || !_mission_item_triplet.current_valid) {
_reset_loiter_pos = false;
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw
_mission_item_triplet.current.altitude_is_relative = false;
float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
/* Use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl) {
_mission_item_triplet.current.altitude = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
_mission_item_triplet.current.altitude = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
}
}
_mission_item_triplet.previous_valid = false; _mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = true; _mission_item_triplet.current_valid = true;
_mission_item_triplet.next_valid = false; _mission_item_triplet.next_valid = false;
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw
get_loiter_item(&_mission_item_triplet.current); get_loiter_item(&_mission_item_triplet.current);
// TODO use relative altitude to allow flying without global reference (?)
_mission_item_triplet.current.altitude_is_relative = false;
float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
/* Use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl) {
_mission_item_triplet.current.altitude = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
_mission_item_triplet.current.altitude = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
}
publish_mission_item_triplet(); publish_mission_item_triplet();
} }
@ -999,7 +989,10 @@ Navigator::start_loiter()
void void
Navigator::start_mission() Navigator::start_mission()
{ {
/* leave previous mission item as is */ // TODO set prev item to current position?
_reset_loiter_pos = true;
_rtl_done = false;
int ret; int ret;
bool onboard; bool onboard;
unsigned index; unsigned index;
@ -1012,9 +1005,9 @@ Navigator::start_mission()
_mission_item_triplet.current_valid = true; _mission_item_triplet.current_valid = true;
if (onboard) { if (onboard) {
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index);
} else { } else {
mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index);
} }
} else { } else {
/* since a mission is started without knowledge if there are more mission items available, this can fail */ /* since a mission is started without knowledge if there are more mission items available, this can fail */
@ -1044,6 +1037,8 @@ Navigator::advance_mission()
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
_reset_loiter_pos = true;
int ret; int ret;
bool onboard; bool onboard;
unsigned index; unsigned index;
@ -1082,23 +1077,21 @@ Navigator::advance_mission()
} }
void void
Navigator::start_mission_loiter() Navigator::finish_mission()
{ {
/* make sure the current WP is valid */ /* loiter at last waypoint */
if (!_mission_item_triplet.current_valid) { _reset_loiter_pos = false;
warnx("ERROR: cannot switch to offboard mission loiter");
}
get_loiter_item(&_mission_item_triplet.current); mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
publish_mission_item_triplet(); start_loiter();
mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP");
} }
void void
Navigator::start_rtl() Navigator::start_rtl()
{ {
_reset_loiter_pos = true;
_rtl_done = false;
/* discard all mission item and insert RTL item */ /* discard all mission item and insert RTL item */
_mission_item_triplet.previous_valid = false; _mission_item_triplet.previous_valid = false;
@ -1118,26 +1111,19 @@ Navigator::start_rtl()
publish_mission_item_triplet(); publish_mission_item_triplet();
mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
} }
void void
Navigator::start_rtl_loiter() Navigator::finish_rtl()
{ {
_mission_item_triplet.previous_valid = false; /* loiter at home position */
_mission_item_triplet.current_valid = true; _reset_loiter_pos = false;
_mission_item_triplet.next_valid = false; _rtl_done = true;
_mission_item_triplet.current.lat = _home_pos.lat; mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
_mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
get_loiter_item(&_mission_item_triplet.current);
publish_mission_item_triplet(); start_loiter();
mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL");
} }
bool bool

2
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -65,9 +65,7 @@ typedef enum {
NAV_STATE_NONE = 0, NAV_STATE_NONE = 0,
NAV_STATE_LOITER, NAV_STATE_LOITER,
NAV_STATE_MISSION, NAV_STATE_MISSION,
NAV_STATE_MISSION_LOITER,
NAV_STATE_RTL, NAV_STATE_RTL,
NAV_STATE_RTL_LOITER,
NAV_STATE_MAX NAV_STATE_MAX
} nav_state_t; } nav_state_t;