forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/navigator_new' into navigator_new_fw
This commit is contained in:
Thomas Gubler
commit
09f29d0972
|
|
@ -605,7 +605,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
memset(&armed, 0, sizeof(armed));
|
||||
|
||||
status.main_state = MAIN_STATE_MANUAL;
|
||||
status.set_nav_state = NAV_STATE_INIT;
|
||||
status.set_nav_state = NAV_STATE_NONE;
|
||||
status.set_nav_state_timestamp = 0;
|
||||
status.arming_state = ARMING_STATE_INIT;
|
||||
status.hil_state = HIL_STATE_OFF;
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
*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;
|
||||
// TODO review
|
||||
if (control_mode.nav_state == NAV_STATE_NONE) {
|
||||
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
|
||||
} else if (control_mode.nav_state == NAV_STATE_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) {
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -165,6 +165,8 @@ private:
|
|||
|
||||
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_yaw_reached;
|
||||
uint64_t _time_first_inside_orbit;
|
||||
|
|
@ -260,9 +262,9 @@ private:
|
|||
void start_none();
|
||||
void start_loiter();
|
||||
void start_mission();
|
||||
void start_mission_loiter();
|
||||
void finish_mission();
|
||||
void start_rtl();
|
||||
void start_rtl_loiter();
|
||||
void finish_rtl();
|
||||
|
||||
/**
|
||||
* Guards offboard mission
|
||||
|
|
@ -354,6 +356,8 @@ Navigator::Navigator() :
|
|||
_fence_valid(false),
|
||||
_inside_fence(true),
|
||||
_mission(),
|
||||
_reset_loiter_pos(true),
|
||||
_rtl_done(false),
|
||||
_waypoint_position_reached(false),
|
||||
_waypoint_yaw_reached(false),
|
||||
_time_first_inside_orbit(0),
|
||||
|
|
@ -375,7 +379,8 @@ Navigator::Navigator() :
|
|||
memset(&_mission_result, 0, sizeof(struct mission_result_s));
|
||||
|
||||
/* Initialize state machine */
|
||||
myState = NAV_STATE_INIT;
|
||||
myState = NAV_STATE_NONE;
|
||||
start_none();
|
||||
}
|
||||
|
||||
Navigator::~Navigator()
|
||||
|
|
@ -496,11 +501,14 @@ Navigator::task_main_trampoline(int argc, char *argv[])
|
|||
void
|
||||
Navigator::task_main()
|
||||
{
|
||||
|
||||
/* inform about start */
|
||||
warnx("Initializing..");
|
||||
fflush(stdout);
|
||||
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] started");
|
||||
|
||||
_fence_valid = load_fence(GEOFENCE_MAX_VERTICES);
|
||||
|
||||
/*
|
||||
|
|
@ -526,7 +534,10 @@ Navigator::task_main()
|
|||
/* rate limit position updates to 50 Hz */
|
||||
orb_set_interval(_global_pos_sub, 20);
|
||||
|
||||
unsigned prevState = 0;
|
||||
unsigned prevState = NAV_STATE_NONE;
|
||||
bool pub_control_mode = true;
|
||||
hrt_abstime mavlink_open_time = 0;
|
||||
const hrt_abstime mavlink_open_interval = 500000;
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[7];
|
||||
|
|
@ -565,10 +576,16 @@ Navigator::task_main()
|
|||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* only update vehicle status if it changed */
|
||||
if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
|
||||
/* try to reopen the mavlink log device with specified interval */
|
||||
mavlink_open_time = hrt_abstime() + mavlink_open_interval;
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
}
|
||||
|
||||
/* vehicle status updated */
|
||||
if (fds[6].revents & POLLIN) {
|
||||
|
||||
vehicle_status_update();
|
||||
pub_control_mode = true;
|
||||
|
||||
/* Evaluate state machine from commander and set the navigator mode accordingly */
|
||||
if (_vstatus.main_state == MAIN_STATE_AUTO) {
|
||||
|
|
@ -577,23 +594,21 @@ Navigator::task_main()
|
|||
/* RC signal available, use control switches to set mode */
|
||||
/* RETURN switch, overrides MISSION switch */
|
||||
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
|
||||
dispatch(EVENT_RTL_REQUESTED);
|
||||
dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED);
|
||||
stick_mode = true;
|
||||
} else {
|
||||
/* MISSION switch */
|
||||
if (!stick_mode) {
|
||||
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
|
||||
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
stick_mode = true;
|
||||
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
|
||||
/* switch to mission only if available */
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
stick_mode = true;
|
||||
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
|
||||
/* switch to mission only if available */
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
stick_mode = true;
|
||||
}
|
||||
stick_mode = true;
|
||||
}
|
||||
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
|
||||
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
|
||||
|
|
@ -609,7 +624,6 @@ Navigator::task_main()
|
|||
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
|
||||
|
||||
switch (_vstatus.set_nav_state) {
|
||||
case NAV_STATE_INIT:
|
||||
case NAV_STATE_NONE:
|
||||
/* nothing to do */
|
||||
break;
|
||||
|
|
@ -619,11 +633,15 @@ Navigator::task_main()
|
|||
break;
|
||||
|
||||
case NAV_STATE_MISSION:
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
} else {
|
||||
dispatch(EVENT_LOITER_REQUESTED);
|
||||
}
|
||||
break;
|
||||
|
||||
case NAV_STATE_RTL:
|
||||
dispatch(EVENT_RTL_REQUESTED);
|
||||
dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -632,8 +650,8 @@ Navigator::task_main()
|
|||
}
|
||||
|
||||
} else {
|
||||
/* on first switch to AUTO try mission, if none is available fallback to loiter instead */
|
||||
if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) {
|
||||
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
|
||||
if (myState == NAV_STATE_NONE) {
|
||||
if (_mission.current_mission_available()) {
|
||||
dispatch(EVENT_MISSION_REQUESTED);
|
||||
} else {
|
||||
|
|
@ -647,73 +665,76 @@ Navigator::task_main()
|
|||
/* not in AUTO */
|
||||
dispatch(EVENT_NONE_REQUESTED);
|
||||
}
|
||||
|
||||
/* XXX Hack to get mavlink output going, try opening the fd with 5Hz */
|
||||
if (_mavlink_fd < 0) {
|
||||
/* try to open the mavlink log device every once in a while */
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
}
|
||||
}
|
||||
|
||||
/* only update parameters if it changed */
|
||||
/* parameters updated */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
parameters_update();
|
||||
/* note that these new parameters won't be in effect until a mission triplet is published again */
|
||||
}
|
||||
|
||||
/* only update craft capabilities if they have changed */
|
||||
/* navigation capabilities updated */
|
||||
if (fds[3].revents & POLLIN) {
|
||||
navigation_capabilities_update();
|
||||
}
|
||||
|
||||
/* offboard mission updated */
|
||||
if (fds[4].revents & POLLIN) {
|
||||
offboard_mission_update(_vstatus.is_rotary_wing);
|
||||
// XXX check if mission really changed
|
||||
dispatch(EVENT_MISSION_CHANGED);
|
||||
}
|
||||
|
||||
/* onboard mission updated */
|
||||
if (fds[5].revents & POLLIN) {
|
||||
onboard_mission_update();
|
||||
// XXX check if mission really changed
|
||||
dispatch(EVENT_MISSION_CHANGED);
|
||||
}
|
||||
|
||||
/* home position updated */
|
||||
if (fds[2].revents & POLLIN) {
|
||||
home_position_update();
|
||||
// XXX check if home position really changed
|
||||
dispatch(EVENT_HOME_POSITION_CHANGED);
|
||||
}
|
||||
|
||||
/* only run controller if position changed */
|
||||
/* global position updated */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
global_position_update();
|
||||
/* only check if waypoint has been reached in Mission or RTL mode */
|
||||
if (mission_item_reached()) {
|
||||
/* only check if waypoint has been reached in MISSION */
|
||||
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
|
||||
if (mission_item_reached()) {
|
||||
if (myState == NAV_STATE_MISSION) {
|
||||
/* advance by one mission item */
|
||||
_mission.move_to_next();
|
||||
|
||||
if (_vstatus.main_state == MAIN_STATE_AUTO &&
|
||||
(myState == NAV_STATE_MISSION)) {
|
||||
|
||||
/* 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 (_mission.current_mission_available()) {
|
||||
advance_mission();
|
||||
/* if no more mission items available send this to state machine and start loiter at the last WP */
|
||||
if (_mission.current_mission_available()) {
|
||||
advance_mission();
|
||||
} else {
|
||||
dispatch(EVENT_MISSION_FINISHED);
|
||||
}
|
||||
} else {
|
||||
/* RTL finished */
|
||||
dispatch(EVENT_MISSION_FINISHED);
|
||||
}
|
||||
} else {
|
||||
dispatch(EVENT_MISSION_FINISHED);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* notify user about state changes */
|
||||
if (myState != prevState) {
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
|
||||
prevState = myState;
|
||||
pub_control_mode = true;
|
||||
}
|
||||
|
||||
publish_control_mode();
|
||||
/* publish control mode if updated */
|
||||
if (pub_control_mode) {
|
||||
publish_control_mode();
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
|
@ -767,9 +788,6 @@ Navigator::status()
|
|||
}
|
||||
|
||||
switch (myState) {
|
||||
case NAV_STATE_INIT:
|
||||
warnx("State: Init");
|
||||
break;
|
||||
case NAV_STATE_NONE:
|
||||
warnx("State: None");
|
||||
break;
|
||||
|
|
@ -779,15 +797,9 @@ Navigator::status()
|
|||
case NAV_STATE_MISSION:
|
||||
warnx("State: Mission");
|
||||
break;
|
||||
case NAV_STATE_MISSION_LOITER:
|
||||
warnx("State: Loiter after Mission");
|
||||
break;
|
||||
case NAV_STATE_RTL:
|
||||
warnx("State: RTL");
|
||||
break;
|
||||
case NAV_STATE_RTL_LOITER:
|
||||
warnx("State: Loiter after RTL");
|
||||
break;
|
||||
default:
|
||||
warnx("State: Unknown");
|
||||
break;
|
||||
|
|
@ -885,16 +897,6 @@ Navigator::fence_point(int argc, char *argv[])
|
|||
|
||||
|
||||
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
|
||||
{
|
||||
/* STATE_INIT */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
|
||||
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT},
|
||||
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
|
||||
},
|
||||
{
|
||||
/* STATE_NONE */
|
||||
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
|
||||
|
|
@ -921,39 +923,19 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
|
|||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
|
||||
/* 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_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 */
|
||||
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
|
||||
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
|
||||
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
|
||||
/* 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_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), 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},
|
||||
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
|
||||
},
|
||||
};
|
||||
|
||||
|
|
@ -964,33 +946,42 @@ Navigator::start_none()
|
|||
_mission_item_triplet.current_valid = false;
|
||||
_mission_item_triplet.next_valid = false;
|
||||
|
||||
_reset_loiter_pos = true;
|
||||
_rtl_done = false;
|
||||
|
||||
publish_mission_item_triplet();
|
||||
}
|
||||
|
||||
void
|
||||
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.current_valid = true;
|
||||
_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 = 0.0f; // TODO use current yaw sp here or set to undefined?
|
||||
|
||||
get_loiter_item(&_mission_item_triplet.current);
|
||||
|
||||
float global_min_alt = _parameters.min_altitude + _home_pos.altitude;
|
||||
|
||||
/* Use current altitude if above min altitude set by parameter */
|
||||
if (_global_pos.alt < global_min_alt) {
|
||||
_mission_item_triplet.current.altitude = global_min_alt;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt));
|
||||
} else {
|
||||
_mission_item_triplet.current.altitude = _global_pos.alt;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter here");
|
||||
}
|
||||
|
||||
publish_mission_item_triplet();
|
||||
}
|
||||
|
||||
|
|
@ -998,7 +989,9 @@ Navigator::start_loiter()
|
|||
void
|
||||
Navigator::start_mission()
|
||||
{
|
||||
/* leave previous mission item as isas is */
|
||||
// TODO set prev item to current position?
|
||||
_reset_loiter_pos = true;
|
||||
_rtl_done = false;
|
||||
|
||||
int ret;
|
||||
bool onboard;
|
||||
|
|
@ -1012,9 +1005,9 @@ Navigator::start_mission()
|
|||
_mission_item_triplet.current_valid = true;
|
||||
|
||||
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 {
|
||||
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 {
|
||||
/* 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));
|
||||
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
|
||||
|
||||
_reset_loiter_pos = true;
|
||||
|
||||
int ret;
|
||||
bool onboard;
|
||||
unsigned index;
|
||||
|
|
@ -1082,23 +1077,21 @@ Navigator::advance_mission()
|
|||
}
|
||||
|
||||
void
|
||||
Navigator::start_mission_loiter()
|
||||
Navigator::finish_mission()
|
||||
{
|
||||
/* make sure the current WP is valid */
|
||||
if (!_mission_item_triplet.current_valid) {
|
||||
warnx("ERROR: cannot switch to offboard mission loiter");
|
||||
}
|
||||
/* loiter at last waypoint */
|
||||
_reset_loiter_pos = false;
|
||||
|
||||
get_loiter_item(&_mission_item_triplet.current);
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
|
||||
|
||||
publish_mission_item_triplet();
|
||||
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP");
|
||||
start_loiter();
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::start_rtl()
|
||||
{
|
||||
_reset_loiter_pos = true;
|
||||
_rtl_done = false;
|
||||
|
||||
/* discard all mission item and insert RTL item */
|
||||
_mission_item_triplet.previous_valid = false;
|
||||
|
|
@ -1108,36 +1101,29 @@ Navigator::start_rtl()
|
|||
_mission_item_triplet.current.lat = _home_pos.lat;
|
||||
_mission_item_triplet.current.lon = _home_pos.lon;
|
||||
_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
|
||||
_mission_item_triplet.current.yaw = 0.0f;
|
||||
_mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint?
|
||||
_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
|
||||
_mission_item_triplet.current.loiter_direction = 1;
|
||||
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
|
||||
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
|
||||
_mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number
|
||||
_mission_item_triplet.current.autocontinue = false;
|
||||
_mission_item_triplet.current_valid = true;
|
||||
|
||||
publish_mission_item_triplet();
|
||||
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] return to launch");
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Navigator::start_rtl_loiter()
|
||||
Navigator::finish_rtl()
|
||||
{
|
||||
_mission_item_triplet.previous_valid = false;
|
||||
_mission_item_triplet.current_valid = true;
|
||||
_mission_item_triplet.next_valid = false;
|
||||
/* loiter at home position */
|
||||
_reset_loiter_pos = false;
|
||||
_rtl_done = true;
|
||||
|
||||
_mission_item_triplet.current.lat = _home_pos.lat;
|
||||
_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);
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
|
||||
|
||||
publish_mission_item_triplet();
|
||||
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL");
|
||||
start_loiter();
|
||||
}
|
||||
|
||||
bool
|
||||
|
|
@ -1187,8 +1173,12 @@ Navigator::mission_item_reached()
|
|||
float dist_z = -1.0f;
|
||||
|
||||
// if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
|
||||
|
||||
// current relative or AMSL altitude depending on _mission_item_triplet.current.altitude_is_relative
|
||||
float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt;
|
||||
|
||||
dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude,
|
||||
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
|
||||
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
// warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude);
|
||||
|
|
@ -1207,13 +1197,18 @@ Navigator::mission_item_reached()
|
|||
// // XXX TODO
|
||||
// }
|
||||
|
||||
if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
|
||||
_waypoint_position_reached = true;
|
||||
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
/* require only altitude for takeoff */
|
||||
if (current_alt > _mission_item_triplet.current.altitude)
|
||||
_waypoint_position_reached = true;
|
||||
} else {
|
||||
if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* check if required yaw reached */
|
||||
float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw);
|
||||
float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw);
|
||||
float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
|
||||
|
||||
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
|
||||
_waypoint_yaw_reached = true;
|
||||
|
|
@ -1223,7 +1218,7 @@ Navigator::mission_item_reached()
|
|||
if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */
|
||||
|
||||
if (_time_first_inside_orbit == 0) {
|
||||
/* XXX announcment? */
|
||||
/* XXX announcement? */
|
||||
_time_first_inside_orbit = now;
|
||||
}
|
||||
|
||||
|
|
@ -1337,17 +1332,17 @@ Navigator::publish_control_mode()
|
|||
}
|
||||
|
||||
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
|
||||
if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON &&
|
||||
fabsf(a.lat - b.lat) < FLT_EPSILON &&
|
||||
fabsf(a.lon - b.lon) < FLT_EPSILON &&
|
||||
if (a.altitude_is_relative == b.altitude_is_relative &&
|
||||
fabs(a.lat - b.lat) < FLT_EPSILON &&
|
||||
fabs(a.lon - b.lon) < FLT_EPSILON &&
|
||||
fabsf(a.altitude - b.altitude) < FLT_EPSILON &&
|
||||
fabsf(a.yaw - b.yaw) < FLT_EPSILON &&
|
||||
fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON &&
|
||||
fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON &&
|
||||
fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON &&
|
||||
a.loiter_direction == b.loiter_direction &&
|
||||
a.nav_cmd == b.nav_cmd &&
|
||||
fabsf(a.radius - b.radius) < FLT_EPSILON &&
|
||||
fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
|
||||
fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) {
|
||||
a.autocontinue == b.autocontinue) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ struct home_position_s
|
|||
{
|
||||
uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
|
||||
|
||||
bool altitude_is_relative;
|
||||
//bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then
|
||||
double lat; /**< Latitude in degrees */
|
||||
double lon; /**< Longitude in degrees */
|
||||
float altitude; /**< Altitude in meters */
|
||||
|
|
|
|||
|
|
@ -62,13 +62,10 @@
|
|||
*/
|
||||
|
||||
typedef enum {
|
||||
NAV_STATE_INIT = 0,
|
||||
NAV_STATE_NONE,
|
||||
NAV_STATE_NONE = 0,
|
||||
NAV_STATE_LOITER,
|
||||
NAV_STATE_MISSION,
|
||||
NAV_STATE_MISSION_LOITER,
|
||||
NAV_STATE_RTL,
|
||||
NAV_STATE_RTL_LOITER,
|
||||
NAV_STATE_MAX
|
||||
} nav_state_t;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue