diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-01 16:43:54 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-01 16:43:54 +0100 |
commit | 09f29d0972dc381817c65eceb0a7072a3e40d59c (patch) | |
tree | 349ddc2206a63948e7d19ded60bfab353e30900f | |
parent | ec8bc6c0208a5eb9ad3decf7bec196c5bf113dd4 (diff) | |
parent | 3bc94f1fe6edba3a48bcb799f97c51168a16bf76 (diff) | |
download | px4-firmware-09f29d0972dc381817c65eceb0a7072a3e40d59c.tar.gz px4-firmware-09f29d0972dc381817c65eceb0a7072a3e40d59c.tar.bz2 px4-firmware-09f29d0972dc381817c65eceb0a7072a3e40d59c.zip |
Merge remote-tracking branch 'upstream/navigator_new' into navigator_new_fw
-rw-r--r-- | src/modules/commander/commander.cpp | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink.c | 7 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 285 | ||||
-rw-r--r-- | src/modules/uORB/topics/home_position.h | 2 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_control_mode.h | 5 |
5 files changed, 145 insertions, 156 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 03d3c02d1..1e5318121 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bc191f30d..d4e4c027b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -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; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4176cee1b..982ebefcc 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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()) { - - 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(); + /* 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 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; @@ -886,16 +898,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}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -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); - - publish_mission_item_triplet(); + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - 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; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 1cf37e29c..3e2fee84e 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -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 */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5ce968f67..9d5dad9f9 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -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; |