From 705b10fcafcb04440edc306cc07c7430e89ad788 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Apr 2014 18:39:42 +0200 Subject: navigator: support DO_JUMP misison items --- src/modules/navigator/navigator_mission.cpp | 94 +++++++++++++++++------------ src/modules/navigator/navigator_mission.h | 2 + 2 files changed, 56 insertions(+), 40 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 72dddebfe..ac7e604ef 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -142,22 +142,19 @@ Mission::next_mission_available() int Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) { + int ret = ERROR; + /* try onboard mission first */ if (current_onboard_mission_available()) { - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; + ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item); + if (ret == OK) { + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; } - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - /* otherwise fallback to offboard */ - + /* otherwise fallback to offboard */ } else if (current_offboard_mission_available()) { dm_item_t dm_current; @@ -169,42 +166,34 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _current_mission_type = MISSION_TYPE_NONE; - return ERROR; + ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item); + if (ret == OK) { + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; } - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - } else { /* happens when no more mission items can be added as a next item */ _current_mission_type = MISSION_TYPE_NONE; - return ERROR; + ret == ERROR; } - return OK; + return ret; } int Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { + int ret = ERROR; + /* try onboard mission first */ if (next_onboard_mission_available()) { - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* otherwise fallback to offboard */ + unsigned next_onboard_mission_index = _current_onboard_mission_index + 1; + ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item); + /* otherwise fallback to offboard */ } else if (next_offboard_mission_available()) { dm_item_t dm_current; @@ -216,19 +205,14 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } + unsigned next_offboard_mission_index = _current_offboard_mission_index + 1; + ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item); } else { /* happens when no more mission items can be added as a next item */ - return ERROR; + ret = ERROR; } - - return OK; + return ret; } @@ -316,3 +300,33 @@ Mission::publish_mission_result() /* reset reached bool */ _mission_result.mission_reached = false; } + +int +Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + /* check for DO_JUMP item */ + if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { + /* if it's not a DO_JUMP, then we were successful */ + return OK; + } + } + + /* we have given up, we don't want to cycle forever */ + warnx("ERROR: cycling through mission items without success"); + return ERROR; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 2bd4da82e..fef145410 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -77,6 +77,8 @@ public: void publish_mission_result(); private: + int get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item); + bool current_onboard_mission_available(); bool current_offboard_mission_available(); bool next_onboard_mission_available(); -- cgit v1.2.3 From 654ab4635ad0fb70d6c0cb601d56e3d97691bdac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 18 Apr 2014 11:15:40 +0200 Subject: navigator: wrong mission topic was copied, clearer naming of offboard mission now --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 6 +++--- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 072f16aae..36d47b7ee 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -832,10 +832,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { - orb_publish(ORB_ID(mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1bb5d33a1..494266dd3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; - if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -543,7 +543,7 @@ Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -611,7 +611,7 @@ Navigator::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index c8a589bb7..90675bb2e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 1ccdb7181..dfc461ae4 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -107,7 +107,7 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission); +ORB_DECLARE(offboard_mission); ORB_DECLARE(onboard_mission); #endif -- cgit v1.2.3 From c3c0328e8bb9211580dbe5a52ecb23e0452cb402 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Apr 2014 17:36:59 +0200 Subject: navigator: lot's of cleanup (WIP) --- src/modules/commander/commander.cpp | 57 +- src/modules/commander/state_machine_helper.cpp | 6 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +- src/modules/mavlink/mavlink_main.cpp | 7 +- src/modules/mavlink/mavlink_messages.cpp | 5 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/navigator/geofence_params.c | 1 - src/modules/navigator/mission.cpp | 297 +++++++ src/modules/navigator/mission.h | 98 +++ src/modules/navigator/module.mk | 6 +- src/modules/navigator/navigator_main.cpp | 934 +++++++-------------- src/modules/navigator/navigator_mission.cpp | 332 -------- src/modules/navigator/navigator_mission.h | 106 --- src/modules/navigator/navigator_params.c | 41 - src/modules/navigator/navigator_state.h | 21 - src/modules/navigator/rtl.cpp | 253 ++++++ src/modules/navigator/rtl.h | 92 ++ src/modules/navigator/rtl_params.c | 87 ++ src/modules/systemlib/state_table.h | 23 +- src/modules/uORB/topics/mission.h | 2 + .../uORB/topics/position_setpoint_triplet.h | 12 +- src/modules/uORB/topics/vehicle_global_position.h | 12 +- src/modules/uORB/topics/vehicle_status.h | 13 +- 23 files changed, 1221 insertions(+), 1194 deletions(-) create mode 100644 src/modules/navigator/mission.cpp create mode 100644 src/modules/navigator/mission.h delete mode 100644 src/modules/navigator/navigator_mission.cpp delete mode 100644 src/modules/navigator/navigator_mission.h delete mode 100644 src/modules/navigator/navigator_state.h create mode 100644 src/modules/navigator/rtl.cpp create mode 100644 src/modules/navigator/rtl.h create mode 100644 src/modules/navigator/rtl_params.c (limited to 'src/modules/navigator') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fa..c2d24a883 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -547,15 +547,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAV_STATE_LOITER; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAV_STATE_MISSION; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -668,8 +666,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; - status.set_nav_state_timestamp = 0; + status.set_nav_state = NAVIGATION_STATE_NONE; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -944,7 +941,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; - if (status.is_rotary_wing && status.condition_local_altitude_valid) { + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1222,30 +1219,30 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + if (sp_man.mode_switch == SWITCH_POS_ON) { - } else { - /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAV_STATE_LOITER; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAVIGATION_STATE_RTL; + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAVIGATION_STATE_LOITER; + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAVIGATION_STATE_MISSION; + } + /* XXX: I don't understand this */ + //else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && + // pos_sp_triplet.nav_state == NAVIGATION_STATE_RTL) { + // /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + // status.set_nav_state = NAV_STATE_MISSION; + // } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e6f245cf9..54f0f13f4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -450,8 +450,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAV_STATE_RTL; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_RTL; ret = TRANSITION_CHANGED; } @@ -461,8 +460,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAV_STATE_LAND; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LAND; ret = TRANSITION_CHANGED; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df785..a00a388a8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -150,8 +150,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ double _loiter_hold_lat; @@ -393,7 +391,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), /* states */ - _setpoint_valid(false), _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), @@ -663,7 +660,6 @@ FixedwingPositionControl::vehicle_setpoint_poll() if (pos_sp_triplet_updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - _setpoint_valid = true; } } @@ -708,7 +704,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -781,7 +777,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. - if (_control_mode.flag_control_position_enabled) { + if (pos_sp_triplet.current.valid) { /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 36d47b7ee..c60c85495 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -872,7 +872,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item break; case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; - /* TODO: also save param2 (repeat count) */ + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; @@ -889,6 +889,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + return OK; } @@ -908,7 +911,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - /* TODO: also save param2 (repeat count) */ + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 648228e3b..3cf69bf7c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -117,7 +117,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND + || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { /* use main state when navigator is not active */ if (status->main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); @@ -142,7 +143,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *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; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2bd2d356a..7d3d39d18 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -645,7 +645,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = true; - _pos_sp_triplet.nav_state = NAV_STATE_NONE; + // _pos_sp_triplet.nav_state = NAV_STATE_NONE; _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; _pos_sp_triplet.current.lat = _lat_sp; _pos_sp_triplet.current.lon = _lon_sp; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 5831a0ca9..653b1ad84 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp new file mode 100644 index 000000000..e95bcfd43 --- /dev/null +++ b/src/modules/navigator/mission.cpp @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.cpp + * + * Helper class to access missions + * + * @author Julian Oes + */ + +#include +#include + +#include +#include + +#include +#include + +#include "mission.h" + + +Mission::Mission() : + + _offboard_dataman_id(-1), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE), + _mission_result_pub(-1), + _mission_result({}) +{ +} + +Mission::~Mission() +{ +} + +void +Mission::set_offboard_dataman_id(const int new_id) +{ + _offboard_dataman_id = new_id; +} + +void +Mission::set_offboard_mission_count(int new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(int new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::command_current_offboard_mission_index(const int new_index) +{ + if (new_index >= 0) { + _current_offboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= _offboard_mission_item_count) { + _current_offboard_mission_index = 0; + } + } + report_current_offboard_mission_item(); +} + +bool +Mission::command_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= _onboard_mission_item_count) { + _current_onboard_mission_index = 0; + } + } + // TODO: implement this for onboard missions as well + // report_current_mission_item(); +} + +bool +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +{ + *onboard = false; + *index = -1; + + /* try onboard mission first */ + if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + return true; + } + } + + /* otherwise fallback to offboard */ + if (_current_offboard_mission_index < _offboard_mission_item_count) { + + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { + + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + return true; + } + } + + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + + return false; +} + +bool +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + int next_temp_mission_index = _current_onboard_mission_index + 1; + + /* try onboard mission first */ + if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* then try offboard mission */ + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + next_temp_mission_index = _current_offboard_mission_index + 1; + if (next_temp_mission_index < _offboard_mission_item_count) { + if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* both failed, bail out */ + return false; +} + +bool +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ + if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { + return false; + } + + } else { + /* if it's not a DO_JUMP, then we were successful */ + return true; + } + } + + /* we have given up, we don't want to cycle forever */ + warnx("ERROR: cycling through mission items without success"); + return false; +} + +void +Mission::move_to_next() +{ + report_mission_item_reached(); + + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } +} + +void +Mission::report_mission_item_reached() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } + publish_mission_result(); +} + +void +Mission::report_current_offboard_mission_item() +{ + _mission_result.index_current_mission = _current_offboard_mission_index; + publish_mission_result(); +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.mission_reached = false; +} + diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h new file mode 100644 index 000000000..0fa2ff3fa --- /dev/null +++ b/src/modules/navigator/mission.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include +#include +#include + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor + */ + ~Mission(); + + void set_offboard_dataman_id(const int new_id); + void set_offboard_mission_count(const int new_count); + void set_onboard_mission_count(const int new_count); + void set_onboard_mission_allowed(const bool allowed); + + bool command_current_offboard_mission_index(const int new_index); + bool command_current_onboard_mission_index(const int new_index); + + bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index); + bool get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + +private: + bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + + void report_mission_item_reached(); + void report_current_offboard_mission_item(); + + void publish_mission_result(); + + int _offboard_dataman_id; + int _current_offboard_mission_index; + int _current_onboard_mission_index; + int _offboard_mission_item_count; /** number of offboard mission items available */ + int _onboard_mission_item_count; /** number of onboard mission items available */ + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; + + orb_advert_t _mission_result_pub; /**< publish mission result topic */ + mission_result_s _mission_result; /**< mission result for commander/mavlink */ +}; + +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 55f8a4caa..5f113f686 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -39,7 +39,9 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp \ + mission.cpp \ + rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 494266dd3..34a28aec3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Jean Cyr - * @author Julian Oes - * @author Anton Babushkin + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,19 +31,20 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * - * Handles missions, geo fencing and failsafe navigation behavior. - * Published the mission item triplet for the position controller. + * Handles mission items, geo fencing and failsafe navigation behavior. + * Published the position setpoint triplet for the position controller. * * @author Lorenz Meier * @author Jean Cyr - * @author Julian Oes + * @author Julian Oes * @author Anton Babushkin */ #include + #include #include #include @@ -58,9 +55,13 @@ #include #include #include +#include +#include + #include #include #include + #include #include #include @@ -71,20 +72,19 @@ #include #include #include + #include #include #include #include #include #include -#include #include +#include #include -#include -#include -#include "navigator_state.h" -#include "navigator_mission.h" +#include "mission.h" +#include "rtl.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -130,12 +130,12 @@ public: /** * Add point to geofence */ - void add_fence_point(int argc, char *argv[]); + void add_fence_point(int argc, char *argv[]); /** * Load fence from file */ - void load_fence_from_file(const char *filename); + void load_fence_from_file(const char *filename); private: @@ -154,15 +154,15 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_item_s _mission_item; /**< current mission item */ + + bool _mission_item_valid; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -174,56 +174,41 @@ private: struct navigation_capabilities_s _nav_caps; - class Mission _mission; + Mission _mission; + + RTL _rtl; - bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ - MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - - bool _pos_sp_triplet_updated; - - const char *nav_states_str[NAV_STATE_MAX]; + bool _update_triplet; struct { - float min_altitude; float acceptance_radius; float loiter_radius; int onboard_mission_enabled; float takeoff_alt; - float land_alt; - float rtl_alt; - float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { - param_t min_altitude; param_t acceptance_radius; param_t loiter_radius; param_t onboard_mission_enabled; param_t takeoff_alt; - param_t land_alt; - param_t rtl_alt; - param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { EVENT_NONE_REQUESTED, - EVENT_READY_REQUESTED, EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, - EVENT_LAND_REQUESTED, + EVENT_TAKEN_OFF, + EVENT_LANDED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, + EVENT_MISSION_ITEM_REACHED, MAX_EVENT }; @@ -232,15 +217,6 @@ private: */ static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND - }; - - enum RTLState _rtl_state; - /** * Update our local parameter cache. */ @@ -264,7 +240,7 @@ private: /** * Retrieve offboard mission. */ - void offboard_mission_update(bool isrotaryWing); + void offboard_mission_update(); /** * Retrieve onboard mission. @@ -296,19 +272,20 @@ private: /** * Functions that are triggered when a new state is entered. */ - void start_none(); - void start_ready(); - void start_loiter(); - void start_mission(); - void start_rtl(); - void start_land(); - void start_land_home(); + bool start_none_on_ground(); + bool start_none_in_air(); + bool start_auto_on_ground(); + bool start_loiter(); + bool start_mission(); + bool advance_mission(); + bool start_rtl(); + bool advance_rtl(); + bool start_land(); /** * Fork for state transitions */ - void request_loiter_or_ready(); - void request_mission_if_available(); + // void request_loiter_or_ready(); /** * Guards offboard mission @@ -333,12 +310,12 @@ private: /** * Perform actions when current mission item reached. */ - void on_mission_item_reached(); + // void on_mission_item_reached(); /** * Move to next waypoint */ - void set_mission_item(); + bool set_mission_items(); /** * Switch to next RTL state @@ -348,7 +325,7 @@ private: /** * Set position setpoint for mission item */ - void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); /** * Helper function to get a takeoff item @@ -377,12 +354,9 @@ Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), - _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), - -/* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), @@ -391,53 +365,29 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - -/* publications */ _pos_sp_triplet_pub(-1), - -/* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _pos_sp_triplet({}), + _mission_item({}), _mission_item_valid(false), - _global_pos_valid(false), - _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _need_takeoff(true), - _do_takeoff(false), - _set_nav_state_timestamp(0), - _pos_sp_triplet_updated(false), -/* states */ - _rtl_state(RTL_STATE_NONE) + _update_triplet(false) { - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); - _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - - memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_item, 0, sizeof(struct mission_item_s)); - - memset(&nav_states_str, 0, sizeof(nav_states_str)); - nav_states_str[0] = "NONE"; - nav_states_str[1] = "READY"; - nav_states_str[2] = "LOITER"; - nav_states_str[3] = "MISSION"; - nav_states_str[4] = "RTL"; - nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = NAV_STATE_NONE; - start_none(); + myState = NAV_STATE_NONE_ON_GROUND; + + start_none_on_ground(); } Navigator::~Navigator() @@ -472,16 +422,12 @@ Navigator::parameters_update() struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); - param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); - param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); - param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); + _mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); _geofence.updateParams(); } @@ -496,6 +442,8 @@ void Navigator::home_position_update() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + + _rtl.set_home_position(&_home_pos); } void @@ -506,7 +454,7 @@ Navigator::navigation_capabilities_update() void -Navigator::offboard_mission_update(bool isrotaryWing) +Navigator::offboard_mission_update() { struct mission_s offboard_mission; @@ -523,19 +471,17 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_offboard_mission_count(offboard_mission.count); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.command_current_offboard_mission_index(offboard_mission.current_index); } else { _mission.set_offboard_mission_count(0); - _mission.set_current_offboard_mission_index(0); + _mission.command_current_offboard_mission_index(0); } - - _mission.publish_mission_result(); } void @@ -546,11 +492,11 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); - _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.command_current_onboard_mission_index(onboard_mission.current_index); } else { _mission.set_onboard_mission_count(0); - _mission.set_current_onboard_mission_index(0); + _mission.command_current_onboard_mission_index(0); } } @@ -626,13 +572,13 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(_vstatus.is_rotary_wing); + offboard_mission_update(); onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; + unsigned prevState = NAV_STATE_NONE_ON_GROUND; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -690,55 +636,45 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state requested by commander */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } - } - - } else { - /* navigator shouldn't act */ + /* commander requested new navigation mode, forward it to state machine */ + switch (_vstatus.set_nav_state) { + case NAVIGATION_STATE_NONE: dispatch(EVENT_NONE_REQUESTED); + break; + + case NAVIGATION_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAVIGATION_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; + + case NAVIGATION_STATE_RTL: + /* TODO: what is this for? */ + // if (!(_rtl_state == RTL_STATE_DESCEND && + // (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + // _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); + // } + break; + + case NAVIGATION_STATE_LAND: + /* TODO: add this */ + // dispatch(EVENT_LAND_REQUESTED); + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + /* commander sets in-air/on-ground flag as well */ + if (_vstatus.condition_landed) { + dispatch(EVENT_LANDED); + } else { + dispatch(EVENT_TAKEN_OFF); + } } /* parameters updated */ @@ -754,74 +690,49 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { - offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + offboard_mission_update(); + /* TODO: 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 + /* TODO: 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 + /* TODO check if home position really changed */ dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - - /* publish position setpoint triplet on each position update if navigator active */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - _pos_sp_triplet_updated = true; - - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { - /* got global position when landing, update setpoint */ - start_land(); - } - - _global_pos_valid = _global_pos.global_valid; - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } + if (check_mission_item_reached()) { + dispatch(EVENT_MISSION_ITEM_REACHED); } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { - //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } - } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } - /* publish position setpoint triplet if updated */ - if (_pos_sp_triplet_updated) { - _pos_sp_triplet_updated = false; + if (_update_triplet ) { publish_position_setpoint_triplet(); - } - - /* notify user about state changes */ - if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); - prevState = myState; + _update_triplet = false; } perf_end(_loop_perf); @@ -879,8 +790,12 @@ Navigator::status() } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); + case NAV_STATE_NONE_ON_GROUND: + warnx("State: None on ground"); + break; + + case NAV_STATE_NONE_IN_AIR: + warnx("State: None in air"); break; case NAV_STATE_LOITER: @@ -895,6 +810,10 @@ Navigator::status() warnx("State: RTL"); break; + case NAV_STATE_LAND: + warnx("State: LAND"); + break; + default: warnx("State: Unknown"); break; @@ -903,92 +822,119 @@ Navigator::status() StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { - /* NAV_STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* NAV_STATE_NONE_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + }, + { + /* NAV_STATE_NONE_IN_AIR */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, /* 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_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, }, { - /* NAV_STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* NAV_STATE_AUTO_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, }, { /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* 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_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, }, { /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* 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_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, }, { /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* 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_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, }, }; -void -Navigator::start_none() +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; - _mission_item_valid = false; - _reset_loiter_pos = true; - _do_takeoff = false; - _rtl_state = RTL_STATE_NONE; + _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; - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void -Navigator::start_ready() +bool +Navigator::start_auto_on_ground() { reset_reached(); @@ -998,46 +944,26 @@ Navigator::start_ready() _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - if (_rtl_state != RTL_STATE_DESCEND) { - /* reset RTL state if landed not at home */ - _rtl_state = RTL_STATE_NONE; - } + // if (_rtl_state != RTL_STATE_DESCEND) { + // reset RTL state if landed not at home + // _rtl_state = RTL_STATE_NONE; + // } - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void +bool Navigator::start_loiter() { - reset_reached(); - - _do_takeoff = false; - - /* set loiter position if needed */ - if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { - _reset_loiter_pos = false; + /* 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 - - float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; - - /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { - _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - - } else { - _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - } - + _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; @@ -1045,167 +971,146 @@ Navigator::start_loiter() _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - _pos_sp_triplet_updated = true; + mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); + + _update_triplet = true; + return true; } -void +bool Navigator::start_mission() { - _need_takeoff = true; + /* start fresh */ + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; - set_mission_item(); + return set_mission_items(); } -void -Navigator::set_mission_item() +bool +Navigator::advance_mission() { - reset_reached(); + /* tell mission to move by one */ + _mission.move_to_next(); - /* copy current mission to previous item */ - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + /* now try to set the new mission items, if it fails, it will dispatch loiter */ + return set_mission_items(); +} - _reset_loiter_pos = true; - _do_takeoff = false; +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; + } - int ret; bool onboard; - unsigned index; + int index; - ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); + /* if we fail to set the current mission, continue to loiter */ + if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - if (ret == OK) { - _mission.report_current_offboard_mission_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; + } - _mission_item_valid = true; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { - /* don't reset RTL state on RTL or LOITER items */ - _rtl_state = RTL_STATE_NONE; - } + /* if we got an RTL mission item, switch to RTL mode and give up */ + if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + dispatch(EVENT_RTL_REQUESTED); + return false; + } - if (_vstatus.is_rotary_wing) { - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { - /* do special TAKEOFF handling for VTOL */ - _need_takeoff = false; - - /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _pos_sp_triplet.current.alt; - - if (_vstatus.condition_landed) { - /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ - takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); - } + _mission_item_valid = true; - /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { - /* force TAKEOFF if landed or waypoint altitude is more than current */ - _do_takeoff = 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; + - /* move current position setpoint to next */ - memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + mission_item_s next_mission_item; - /* set current setpoint to takeoff */ + 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; + } - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = takeoff_alt_amsl; - _pos_sp_triplet.current.yaw = NAN; - _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; - } + /* notify user about what happened */ + mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d", + (last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index); - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - /* will need takeoff after landing */ - _need_takeoff = true; - } - } + _update_triplet = true; - if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); + reset_reached(); - } else { - if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); + return true; +} - } else { - mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); - } - } +bool +Navigator::start_rtl() +{ + /* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */ + if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - } else { - /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_valid = false; - _pos_sp_triplet.current.valid = false; - warnx("ERROR: current WP can't be set"); - } + _mission_item_valid = true; - if (!_do_takeoff) { - mission_item_s item_next; - ret = _mission.get_next_mission_item(&item_next); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; - if (ret == OK) { - position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + reset_reached(); - } else { - /* this will fail for the last WP */ - _pos_sp_triplet.next.valid = false; - } + _update_triplet = true; + return true; } + dispatch(EVENT_LOITER_REQUESTED); - _pos_sp_triplet_updated = true; + return false; } -void -Navigator::start_rtl() +bool +Navigator::advance_rtl() { - _do_takeoff = false; + /* tell mission to move by one */ + _rtl.move_to_next(); - /* decide if we need climb */ - if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { - _rtl_state = RTL_STATE_CLIMB; + /* 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)) { - } else { - _rtl_state = RTL_STATE_RETURN; - } - } + _mission_item_valid = true; - /* if switching directly to return state, reset altitude setpoint */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; + + reset_reached(); + + _update_triplet = true; + return true; } - _reset_loiter_pos = true; - set_rtl_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; } -void +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 */ - _do_takeoff = false; - _reset_loiter_pos = true; memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _global_pos.lat; _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; @@ -1220,185 +1125,22 @@ Navigator::start_land() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::start_land_home() -{ - reset_reached(); - - /* land to home position, should be called when hovering above home, from RTL state */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_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; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); _pos_sp_triplet.next.valid = false; -} - -void -Navigator::set_rtl_item() -{ - reset_reached(); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - } - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _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; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); - break; - } - - case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // 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); - - } 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); - } - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _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; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - default: { - mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::request_loiter_or_ready() -{ - /* XXX workaround: no landing detector for fixedwing yet */ - if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } -} - -void -Navigator::request_mission_if_available() -{ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - request_loiter_or_ready(); - } + _update_triplet = true; + return true; } void -Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { sp->valid = true; if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* set home position for RTL item */ - sp->lat = _home_pos.lat; - sp->lon = _home_pos.lon; - sp->alt = _home_pos.alt + _parameters.rtl_alt; if (_pos_sp_triplet.previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ @@ -1408,9 +1150,6 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ /* else use current position */ sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); } - sp->loiter_radius = _parameters.loiter_radius; - sp->loiter_direction = 1; - sp->pitch_min = 0.0f; } else { sp->lat = item->lat; @@ -1450,14 +1189,14 @@ Navigator::check_mission_item_reached() 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_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_TIME_LIMIT || + // _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + // _mission_item.loiter_radius > 0.01f) { - return false; - } + // return false; + // } uint64_t now = hrt_absolute_time(); @@ -1475,22 +1214,19 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _home_pos.alt : _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 = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _global_pos.lat, _global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_global_pos.alt > altitude_amsl - acceptance_radius) { _waypoint_position_reached = true; } - } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1499,7 +1235,10 @@ Navigator::check_mission_item_reached() } if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { + + /* 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); @@ -1514,24 +1253,23 @@ Navigator::check_mission_item_reached() /* 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); + 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) { - reset_reached(); return true; } } - return false; - } void @@ -1543,62 +1281,6 @@ Navigator::reset_reached() } -void -Navigator::on_mission_item_reached() -{ - if (myState == NAV_STATE_MISSION) { - - _mission.report_mission_item_reached(); - - if (_do_takeoff) { - /* takeoff completed */ - _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); - - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - if (_mission.current_mission_available()) { - set_mission_item(); - - } else { - /* if no more mission items available then finish mission */ - /* loiter at last waypoint */ - _reset_loiter_pos = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - request_loiter_or_ready(); - } - - } else if (myState == NAV_STATE_RTL) { - /* RTL completed */ - if (_rtl_state == RTL_STATE_DESCEND) { - /* hovering above home position, land if needed or loiter */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - - if (_mission_item.autocontinue) { - dispatch(EVENT_LAND_REQUESTED); - - } else { - _reset_loiter_pos = false; - dispatch(EVENT_LOITER_REQUESTED); - } - - } else { - /* next RTL step */ - _rtl_state = (RTLState)(_rtl_state + 1); - set_rtl_item(); - } - - } else if (myState == NAV_STATE_LAND) { - /* landing completed */ - mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); - dispatch(EVENT_READY_REQUESTED); - } - _mission.publish_mission_result(); -} - void Navigator::publish_position_setpoint_triplet() { @@ -1607,11 +1289,9 @@ Navigator::publish_position_setpoint_triplet() /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { - /* advertise and publish */ _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp deleted file mode 100644 index ac7e604ef..000000000 --- a/src/modules/navigator/navigator_mission.cpp +++ /dev/null @@ -1,332 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file navigator_mission.cpp - * Helper class to access missions - */ - -#include -#include -#include -#include -#include -#include -#include "navigator_mission.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - - -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), - _mission_result_pub(-1) -{ - memset(&_mission_result, 0, sizeof(struct mission_result_s)); -} - -Mission::~Mission() -{ - -} - -void -Mission::set_offboard_dataman_id(int new_id) -{ - _offboard_dataman_id = new_id; -} - -void -Mission::set_current_offboard_mission_index(int new_index) -{ - if (new_index != -1) { - warnx("specifically set to %d", new_index); - _current_offboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } - } - report_current_offboard_mission_item(); -} - -void -Mission::set_current_onboard_mission_index(int new_index) -{ - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } - } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); -} - -void -Mission::set_offboard_mission_count(unsigned new_count) -{ - _offboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_count(unsigned new_count) -{ - _onboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; -} - -bool -Mission::current_mission_available() -{ - return (current_onboard_mission_available() || current_offboard_mission_available()); - -} - -bool -Mission::next_mission_available() -{ - return (next_onboard_mission_available() || next_offboard_mission_available()); -} - -int -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (current_onboard_mission_available()) { - - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - } - - /* otherwise fallback to offboard */ - } else if (current_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - } - - } else { - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; - ret == ERROR; - } - - return ret; -} - -int -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (next_onboard_mission_available()) { - - unsigned next_onboard_mission_index = _current_onboard_mission_index + 1; - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item); - - /* otherwise fallback to offboard */ - } else if (next_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - unsigned next_offboard_mission_index = _current_offboard_mission_index + 1; - ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item); - - } else { - /* happens when no more mission items can be added as a next item */ - ret = ERROR; - } - return ret; -} - - -bool -Mission::current_onboard_mission_available() -{ - return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; -} - -bool -Mission::current_offboard_mission_available() -{ - return _offboard_mission_item_count > _current_offboard_mission_index; -} - -bool -Mission::next_onboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_ONBOARD) { - next = 1; - } - - return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; -} - -bool -Mission::next_offboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_OFFBOARD) { - next = 1; - } - - return _offboard_mission_item_count > (_current_offboard_mission_index + next); -} - -void -Mission::move_to_next() -{ - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::report_mission_item_reached() -{ - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; - } -} - -void -Mission::report_current_offboard_mission_item() -{ - _mission_result.index_current_mission = _current_offboard_mission_index; -} - -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.mission_reached = false; -} - -int -Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item) -{ - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ - for (int i=0; i<10; i++) { - - const ssize_t len = sizeof(struct mission_item_s); - - /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* check for DO_JUMP item */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; - } else { - /* if it's not a DO_JUMP, then we were successful */ - return OK; - } - } - - /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); - return ERROR; -} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h deleted file mode 100644 index fef145410..000000000 --- a/src/modules/navigator/navigator_mission.h +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file navigator_mission.h - * Helper class to access missions - */ - -#ifndef NAVIGATOR_MISSION_H -#define NAVIGATOR_MISSION_H - -#include -#include - - -class __EXPORT Mission -{ -public: - /** - * Constructor - */ - Mission(); - - /** - * Destructor, also kills the sensors task. - */ - ~Mission(); - - void set_offboard_dataman_id(int new_id); - void set_current_offboard_mission_index(int new_index); - void set_current_onboard_mission_index(int new_index); - void set_offboard_mission_count(unsigned new_count); - void set_onboard_mission_count(unsigned new_count); - - void set_onboard_mission_allowed(bool allowed); - - bool current_mission_available(); - bool next_mission_available(); - - int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); - int get_next_mission_item(struct mission_item_s *mission_item); - - void move_to_next(); - - void report_mission_item_reached(); - void report_current_offboard_mission_item(); - void publish_mission_result(); - -private: - int get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item); - - bool current_onboard_mission_available(); - bool current_offboard_mission_available(); - bool next_onboard_mission_available(); - bool next_offboard_mission_available(); - - int _offboard_dataman_id; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items available */ - unsigned _onboard_mission_item_count; /** number of onboard mission items available */ - - bool _onboard_mission_allowed; - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; - - int _mission_result_pub; - - struct mission_result_s _mission_result; -}; - -#endif diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9ef359c6d..49969a382 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,16 +52,6 @@ * Navigator parameters, accessible via MAVLink */ -/** - * Minimum altitude (fixed wing only) - * - * Minimum altitude above home for LOITER. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); - /** * Waypoint acceptance radius * @@ -101,37 +91,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); -/** - * Landing altitude - * - * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); - -/** - * Return-To-Launch altitude - * - * Minimum altitude above home position for going home in RTL mode. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); - -/** - * Return-To-Launch delay - * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. - * - * @unit seconds - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); - /** * Enable parachute deployment * diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h deleted file mode 100644 index 6a1475c9b..000000000 --- a/src/modules/navigator/navigator_state.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * navigator_state.h - * - * Created on: 27.01.2014 - * Author: ton - */ - -#ifndef NAVIGATOR_STATE_H_ -#define NAVIGATOR_STATE_H_ - -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - -#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp new file mode 100644 index 000000000..6e621e39d --- /dev/null +++ b/src/modules/navigator/rtl.cpp @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_rtl.cpp + * Helper class to access RTL + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "rtl.h" + + +RTL::RTL() : + SuperBlock(NULL, "RTL"), + _mavlink_fd(-1), + _rtl_state(RTL_STATE_NONE), + _home_position({}), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY"), + _loiter_radius(50), + _acceptance_radius(50) +{ + /* load initial params */ + updateParams(); +} + +RTL::~RTL() +{ +} + +void +RTL::set_home_position(const home_position_s *new_home_position) +{ + memcpy(&_home_position, new_home_position, sizeof(home_position_s)); +} + +bool +RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item) +{ + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* decide if we need climb */ + if (_rtl_state == RTL_STATE_NONE) { + if (global_position->alt < _home_position.alt + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + } else { + _rtl_state = RTL_STATE_RETURN; + } + } + + /* if switching directly to return state, set altitude setpoint to current altitude */ + if (_rtl_state == RTL_STATE_RETURN) { + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = global_position->alt; + } + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + float climb_alt = _home_position.alt + _param_return_alt.get(); + + /* TODO understand and fix this */ + // if (_vstatus.condition_landed) { + // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + // } + + new_mission_item->lat = global_position->lat; + new_mission_item->lon = global_position->lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = climb_alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_TAKEOFF; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _home_position.alt)); + break; + } + case RTL_STATE_RETURN: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + + /* 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 */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon); + + // } else { + // /* else use current position */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon); + // } + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_DESCEND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt + _param_descend_alt.get(); + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = _param_land_delay.get() > -0.001f; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_LAND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_LAND; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_FINISHED: { + /* nothing to do, report fail */ + return false; + } + + default: + return false; + } + + return true; +} + +bool +RTL::get_next_rtl_item(mission_item_s *new_mission_item) +{ + /* TODO implement */ + return false; +} + +void +RTL::move_to_next() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < 0) { + _rtl_state = RTL_STATE_FINISHED; + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_FINISHED; + break; + + case RTL_STATE_FINISHED: + break; + + default: + break; + } +} \ No newline at end of file diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h new file mode 100644 index 000000000..c761837fc --- /dev/null +++ b/src/modules/navigator/rtl.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + * @author Julian Oes + * @author Anton Babushkin + */ + +#ifndef NAVIGATOR_RTL_H +#define NAVIGATOR_RTL_H + +#include +#include + +#include +#include +#include + +class RTL : public control::SuperBlock +{ +public: + /** + * Constructor + */ + RTL(); + + /** + * Destructor + */ + ~RTL(); + + void set_home_position(const home_position_s *home_position); + + bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); + bool get_next_rtl_item(mission_item_s *mission_item); + + void move_to_next(); + +private: + int _mavlink_fd; + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LAND, + RTL_STATE_FINISHED, + } _rtl_state; + + home_position_s _home_position; + float _loiter_radius; + float _acceptance_radius; + + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c new file mode 100644 index 000000000..7a8b1d745 --- /dev/null +++ b/src/modules/navigator/rtl_params.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes + */ + +#include + +#include + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index f2709d29f..38378651b 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,7 @@ * @file state_table.h * * Finite-State-Machine helper class for state table + * @author: Julian Oes */ #ifndef __SYSTEMLIB_STATE_TABLE_H @@ -43,7 +44,7 @@ class StateTable { public: - typedef void (StateTable::*Action)(); + typedef bool (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; @@ -53,17 +54,23 @@ public: : myTable(table), myNsignals(nSignals), myNstates(nStates) {} #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) static_cast(_target) + #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} void dispatch(unsigned const sig) { - register Tran const *t = myTable + myState*myNsignals + sig; - (this->*(t->action))(); - - myState = t->nextState; + /* get transition using state table */ + Tran const *t = myTable + myState*myNsignals + sig; + /* first up change state, this allows to do further dispatchs in the state functions */ + + /* now execute state function, if it runs with success, accept new state */ + if ((this->*(t->action))()) { + myState = t->nextState; + } + } + bool doNothing() { + return true; } - void doNothing() {} protected: unsigned myState; private: diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index dfc461ae4..4532b329d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -93,6 +93,8 @@ struct mission_item_s { bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ int do_jump_mission_index; /**< the mission index that we want to jump to */ + int do_jump_repeat_count; /**< how many times the jump should be repeated */ + int do_jump_current_count; /**< how many times the jump has already been repeated */ }; struct mission_s diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 34aaa30dd..85e8ef8a5 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,7 +45,6 @@ #include #include #include "../uORB.h" -#include /** * @addtogroup topics @@ -74,6 +73,17 @@ struct position_setpoint_s float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; +typedef enum { + NAV_STATE_NONE_ON_GROUND = 0, + NAV_STATE_NONE_IN_AIR, + NAV_STATE_AUTO_ON_GROUND, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + /** * Global position setpoint triplet in WGS84 coordinates. * diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index adcd028f0..cfab695f8 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -36,7 +36,7 @@ * Definition of the global fused WGS84 position uORB topic. * * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes * @author Lorenz Meier */ @@ -66,16 +66,16 @@ struct vehicle_global_position_s { bool global_valid; /**< true if position satisfies validity criteria of estimator */ bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 48af4c9e2..c1bd828d8 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,8 +54,6 @@ #include #include "../uORB.h" -#include - /** * @addtogroup topics @{ */ @@ -93,6 +91,14 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; +typedef enum { + NAVIGATION_STATE_NONE = 0, + NAVIGATION_STATE_MISSION, + NAVIGATION_STATE_LOITER, + NAVIGATION_STATE_RTL, + NAVIGATION_STATE_LAND +} navigation_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -152,8 +158,7 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - unsigned int set_nav_state; /**< set navigation state machine to specified value */ - uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ + navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ -- cgit v1.2.3 From c0d4ecf2b629a90222cf26d904c7b1260b1147c4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 11:42:46 +0200 Subject: navigator: some warnings and cleanup --- src/modules/navigator/navigator_main.cpp | 36 ++++++++++++++++---------------- 1 file changed, 18 insertions(+), 18 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 34a28aec3..f39dc88e3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -159,8 +159,9 @@ private: vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ home_position_s _home_pos; /**< home position for RTL */ - position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_item_s _mission_item; /**< current mission item */ + navigation_capabilities_s _nav_caps; + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ bool _mission_item_valid; @@ -172,7 +173,6 @@ private: bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ - struct navigation_capabilities_s _nav_caps; Mission _mission; @@ -307,11 +307,6 @@ private: */ bool check_mission_item_reached(); - /** - * Perform actions when current mission item reached. - */ - // void on_mission_item_reached(); - /** * Move to next waypoint */ @@ -352,7 +347,7 @@ Navigator *g_navigator; Navigator::Navigator() : -/* state machine transition table */ + /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -366,18 +361,27 @@ Navigator::Navigator() : _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), + _vstatus({}), + _control_mode({}), + _global_pos({}), + _home_pos({}), + _mission_item({}), + _nav_caps({}), + _pos_sp_triplet({}), + _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), + _geofence({}), _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission(), - _pos_sp_triplet({}), - _mission_item({}), - _mission_item_valid(false), + _mission({}), + _rtl({}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _update_triplet(false) + _update_triplet(false), + _parameters({}), + _parameter_handles({}) { _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); @@ -530,12 +534,9 @@ Navigator::task_main() { /* inform about start */ warnx("Initializing.."); - fflush(stdout); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[navigator] started"); - /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager @@ -578,7 +579,6 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE_ON_GROUND; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -811,7 +811,7 @@ Navigator::status() break; case NAV_STATE_LAND: - warnx("State: LAND"); + warnx("State: Land"); break; default: -- cgit v1.2.3 From 9f857f86e44a6385daa383d9b02173ad3c36fa01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 12:24:52 +0200 Subject: navigator: corrected the RTL waypoint types --- src/modules/navigator/rtl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6e621e39d..b8ea06275 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -119,7 +119,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss new_mission_item->yaw = NAN; new_mission_item->loiter_radius = _loiter_radius; new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_TAKEOFF; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; new_mission_item->acceptance_radius = _acceptance_radius; new_mission_item->time_inside = 0.0f; new_mission_item->pitch_min = 0.0f; @@ -168,7 +168,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss new_mission_item->yaw = NAN; new_mission_item->loiter_radius = _loiter_radius; new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; new_mission_item->acceptance_radius = _acceptance_radius; new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); new_mission_item->pitch_min = 0.0f; @@ -250,4 +250,4 @@ RTL::move_to_next() default: break; } -} \ No newline at end of file +} -- cgit v1.2.3 From ea0142810a734bd8cade70668eefa562a495ec4a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 12:55:27 +0200 Subject: navigator: comments and whitespace --- src/modules/navigator/navigator_main.cpp | 94 +++++++++++--------------------- 1 file changed, 32 insertions(+), 62 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f39dc88e3..c3fc4e939 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,7 +142,7 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _navigator_task; /**< task handle for sensor task */ - int _mavlink_fd; + int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ int _home_pos_sub; /**< home position subscription */ @@ -160,30 +160,31 @@ private: vehicle_global_position_s _global_pos; /**< global vehicle position */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ - navigation_capabilities_s _nav_caps; + navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - bool _mission_item_valid; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - Geofence _geofence; - bool _geofence_violation_warning_sent; + Geofence _geofence; /**< class that handles the geofence */ + bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ - Mission _mission; + Mission _mission; /**< class that handles the missions */ - RTL _rtl; + RTL _rtl; /**< class that handles RTL */ - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - uint64_t _time_first_inside_orbit; - MissionFeasibilityChecker missionFeasiblityChecker; + bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ + bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ + uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ - bool _update_triplet; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + + bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ struct { float acceptance_radius; @@ -210,7 +211,7 @@ private: EVENT_HOME_POSITION_CHANGED, EVENT_MISSION_ITEM_REACHED, MAX_EVENT - }; + }; /**< possible events that can be thrown at state machine */ /** * State machine transition table @@ -267,8 +268,6 @@ private: */ void task_main(); - void publish_safepoints(unsigned points); - /** * Functions that are triggered when a new state is entered. */ @@ -283,22 +282,7 @@ private: bool start_land(); /** - * Fork for state transitions - */ - // void request_loiter_or_ready(); - - /** - * Guards offboard mission - */ - bool offboard_mission_available(unsigned relative_index); - - /** - * Guards onboard mission - */ - bool onboard_mission_available(unsigned relative_index); - - /** - * Reset all "reached" flags. + * Reset all "mission item reached" flags. */ void reset_reached(); @@ -308,27 +292,24 @@ private: bool check_mission_item_reached(); /** - * Move to next waypoint + * Set mission items by accessing the mission class. + * If failing, tell the state machine to loiter. */ bool set_mission_items(); /** - * Switch to next RTL state + * Set a RTL item by accessing the RTL class. + * If failing, tell the state machine to loiter. */ void set_rtl_item(); /** - * Set position setpoint for mission item + * Translate mission item to a position setpoint. */ void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); /** - * Helper function to get a takeoff item - */ - void get_takeoff_setpoint(position_setpoint_s *pos_sp); - - /** - * Publish a new mission item triplet for position controller + * Publish a new position setpoint triplet for position controllers */ void publish_position_setpoint_triplet(); }; @@ -608,13 +589,12 @@ Navigator::task_main() /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { + /* timed out - periodic check for _task_should_exit, etc. */ continue; - } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { + } else if (pret < 0) { + /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } @@ -651,16 +631,11 @@ Navigator::task_main() break; case NAVIGATION_STATE_RTL: - /* TODO: what is this for? */ - // if (!(_rtl_state == RTL_STATE_DESCEND && - // (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - // _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); - // } break; case NAVIGATION_STATE_LAND: - /* TODO: add this */ + /* TODO: add and test this */ // dispatch(EVENT_LAND_REQUESTED); break; @@ -781,6 +756,7 @@ Navigator::status() if (_fence_valid) { warnx("Geofence is valid"); + /* TODO: needed? */ // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); @@ -944,12 +920,6 @@ Navigator::start_auto_on_ground() _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - - // if (_rtl_state != RTL_STATE_DESCEND) { - // reset RTL state if landed not at home - // _rtl_state = RTL_STATE_NONE; - // } - _update_triplet = true; return true; } @@ -1057,7 +1027,6 @@ Navigator::set_mission_items() bool Navigator::start_rtl() { - /* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */ if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { _mission_item_valid = true; @@ -1070,8 +1039,9 @@ Navigator::start_rtl() _update_triplet = true; return true; } - dispatch(EVENT_LOITER_REQUESTED); + /* if RTL doesn't work, fallback to loiter */ + dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1189,7 +1159,7 @@ Navigator::check_mission_item_reached() return _vstatus.condition_landed; } - /* TODO count turns */ + /* TODO: count turns */ // 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) && @@ -1242,7 +1212,7 @@ Navigator::check_mission_item_reached() /* 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) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ _waypoint_yaw_reached = true; } -- cgit v1.2.3 From e882824ee15e0c5fff58c7f223ec7be181c7af8f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 23:31:15 +0200 Subject: eph and epv renaming, make this compile again --- src/drivers/gps/gps.cpp | 6 +++--- src/drivers/gps/mtk.cpp | 4 ++-- src/drivers/gps/ubx.cpp | 4 ++-- .../attitude_estimator_ekf_main.cpp | 4 ++-- .../fw_att_pos_estimator_main.cpp | 5 ++--- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- src/modules/navigator/navigator_main.cpp | 21 +++++++++++---------- .../position_estimator_inav_main.c | 12 ++++++------ src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/uORB/topics/vehicle_gps_position.h | 4 ++-- 11 files changed, 37 insertions(+), 37 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f..f05f4a409 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -282,8 +282,8 @@ GPS::task_main() _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 3.0f; - _report.epv_m = 7.0f; + _report.eph = 3.0f; + _report.epv = 7.0f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; @@ -446,7 +446,7 @@ GPS::print_info() warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); + warnx("eph: %.2fm, epv: %.2fm", _report.eph, _report.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c90ecbe28..99f88fb8a 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -253,8 +253,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; - _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->eph = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + _gps_position->epv = 0.0; //unknown in mtk custom mode _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 8a2afecb7..95965b60d 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -425,8 +425,8 @@ UBX::handle_message() _gps_position->lat = packet->lat; _gps_position->lon = packet->lon; _gps_position->alt = packet->height_msl; - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m _gps_position->timestamp_position = hrt_absolute_time(); _rate_count_lat_lon++; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c61b6ff3f..ec679f1ae 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -398,7 +398,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds hrt_abstime vel_t = 0; bool vel_valid = false; - if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { vel_valid = true; if (gps_updated) { vel_t = gps.timestamp_velocity; @@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { + } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index e5435e843..f47531b26 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1247,7 +1247,6 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1255,8 +1254,8 @@ FixedwingEstimator::task_main() _global_pos.yaw = _local_pos.yaw; - _global_pos.eph = _gps.eph_m; - _global_pos.epv = _gps.epv_m; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; _global_pos.timestamp = _local_pos.timestamp; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 27b1af046..67ded1230 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -541,8 +541,8 @@ protected: gps->lat, gps->lon, gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), + cm_uint16_from_m_float(gps->eph), + cm_uint16_from_m_float(gps->epv), gps->vel_m_s * 100.0f, _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, gps->satellites_visible); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33a4fef12..95314d56f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -661,12 +661,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m - hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c3fc4e939..d45446e5f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -743,16 +743,17 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); - - if (_global_pos.global_valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); - warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); - warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); - warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); - } + /* TODO: add this again */ + // warnx("Global position is %svalid", _global_pos_valid ? "" : "in"); + + // if (_global_pos.global_valid) { + // warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); + // warnx("Altitude %5.5f meters, altitude above home %5.5f meters", + // (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + // warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + // (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); + // } if (_fence_valid) { warnx("Geofence is valid"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 368424853..54c8a7d17 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -592,13 +592,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { + if (gps.eph > max_eph_epv * 1.5f || gps.epv > max_eph_epv * 1.5f || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { + if (gps.eph < max_eph_epv && gps.epv < max_eph_epv && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -673,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); - w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { @@ -951,8 +951,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.yaw = local_pos.yaw; // TODO implement dead-reckoning - global_pos.eph = gps.eph_m; - global_pos.epv = gps.epv_m; + global_pos.eph = gps.eph; + global_pos.epv = gps.epv; if (vehicle_global_position_pub < 0) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b74d4183b..5d49cc4c9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -972,8 +972,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf_gps_pos.eph_m; - log_msg.body.log_GPS.epv = buf_gps_pos.epv_m; + log_msg.body.log_GPS.eph = buf_gps_pos.eph; + log_msg.body.log_GPS.epv = buf_gps_pos.epv; log_msg.body.log_GPS.lat = buf_gps_pos.lat; log_msg.body.log_GPS.lon = buf_gps_pos.lon; log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 794c3f8bc..a75810278 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -65,8 +65,8 @@ struct vehicle_gps_position_s { float c_variance_rad; /**< course accuracy estimate rad */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float eph; /**< GPS HDOP horizontal dilution of position in m */ + float epv; /**< GPS VDOP horizontal dilution of position in m */ uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ float vel_m_s; /**< GPS ground speed (m/s) */ -- cgit v1.2.3 From ed6c2a5168ca891f20594687acfd3c6bbf7e1cf9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 May 2014 21:56:32 +0200 Subject: commander and navigator: lot's of changes, failsafe handling in commander, navigator only for execution (WIP) --- src/drivers/blinkm/blinkm.cpp | 5 +- src/drivers/gps/mtk.cpp | 4 +- src/modules/commander/commander.cpp | 472 ++++++++++----------- src/modules/commander/state_machine_helper.cpp | 414 ++++-------------- src/modules/commander/state_machine_helper.h | 16 +- src/modules/mavlink/mavlink_messages.cpp | 86 ++-- src/modules/navigator/navigator_main.cpp | 240 +---------- src/modules/systemlib/state_table.h | 28 +- src/modules/uORB/topics/mission_result.h | 1 + .../uORB/topics/position_setpoint_triplet.h | 15 +- src/modules/uORB/topics/vehicle_status.h | 35 +- 11 files changed, 411 insertions(+), 905 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..98c491ce6 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -655,13 +655,14 @@ BlinkM::led() /* indicate main control state */ if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) + /* TODO: add other Auto modes */ + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; - else + else led_color_4 = LED_OFF; led_color_5 = led_color_4; } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index ea4e95fb2..41716cd97 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -251,8 +251,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->lon = 0; // Indicate this data is not usable and bail out - _gps_position->eph_m = 1000.0f; - _gps_position->epv_m = 1000.0f; + _gps_position->eph = 1000.0f; + _gps_position->epv = 1000.0f; _gps_position->fix_type = 0; return; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 77d810a81..908384bb6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,11 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,8 +33,13 @@ /** * @file commander.cpp - * Main system state machine implementation. + * Main fail-safe handling. * + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -87,6 +88,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -387,103 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { - /* result of the command */ - enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } - /* only handle high-priority commands here */ + /* result of the command */ + enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t) cmd->param1; - uint8_t custom_main_mode = (uint8_t) cmd->param2; + uint8_t base_mode = (uint8_t)cmd->param1; + uint8_t custom_main_mode = (uint8_t)cmd->param2; - /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); - } + transition_result_t arming_ret = TRANSITION_NOT_CHANGED; - // Transition the arming state - transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + transition_result_t main_ret = TRANSITION_NOT_CHANGED; - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - if (hil_ret == OK) { - ret = true; - } - - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - - if (arming_res == TRANSITION_CHANGED) { - ret = true; - } + // Transition the arming state + arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_res = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status, MAIN_STATE_ACRO); } - } else { + } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } } } - } - if (main_res == TRANSITION_CHANGED) { - ret = true; - } + if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) { + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } - break; - } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. @@ -502,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } } } @@ -516,14 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_LOITER; + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_MISSION; + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -531,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; +#if 0 /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command @@ -538,15 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { /* reject parachute depoyment not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; +#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; @@ -560,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } else { @@ -574,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) { warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); @@ -609,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); + answer_command(*cmd, cmd_result); } /* send any requested ACKs */ - if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } @@ -682,7 +663,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_NONE; + status.set_nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -772,6 +753,11 @@ int commander_thread_main(int argc, char *argv[]) safety.safety_switch_available = false; safety.safety_off = false; + /* Subscribe to mission result topic */ + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -849,6 +835,13 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); + transition_result_t arming_ret; + + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = false; + bool main_state_changed = false; + bool failsafe_state_changed = false; + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { @@ -856,6 +849,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + arming_ret = TRANSITION_NOT_CHANGED; + + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -935,6 +931,7 @@ int commander_thread_main(int argc, char *argv[]) if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + arming_state_changed = true; } } } @@ -1131,12 +1128,21 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 1"); + arming_state_changed = true; + } } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 2"); + arming_state_changed = true; + } + } status_changed = true; } @@ -1144,11 +1150,18 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { - // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + /* TODO: check for sensors */ + warnx("arming status1: %d", status.arming_state); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + warnx("arming status2: %d", status.arming_state); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed"); + arming_state_changed = true; + } } else { - // XXX: Add emergency stuff if sensors are lost + /* TODO: Add emergency stuff if sensors are lost */ } @@ -1167,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + } + /* start RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1184,11 +1203,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t arming_res; // store all transitions results here - - /* arm/disarm by RC */ - arming_res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && @@ -1199,7 +1213,11 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 1"); + arming_state_changed = true; + } stick_off_counter = 0; } else { @@ -1221,7 +1239,11 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 2"); + arming_state_changed = true; + } } stick_on_counter = 0; @@ -1234,23 +1256,25 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (arming_res == TRANSITION_CHANGED) { + if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + arming_state_changed = true; - } else if (arming_res == TRANSITION_DENIED) { + } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - } + + // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + // /* recover from failsafe */ + // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + // } /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1258,102 +1282,41 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); + main_state_changed = true; } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAVIGATION_STATE_RTL; - - - } else { - - /* LOITER switch */ - if (sp_man.loiter_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAVIGATION_STATE_LOITER; - - } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - } - } - } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } + } - if (armed.armed) { - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - - if (auto_res == TRANSITION_NOT_CHANGED) { - last_auto_state_valid = hrt_absolute_time(); - } - - /* still invalid state after the timeout interval, execute failsafe */ - if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (auto_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_MISSION; - } + if (armed.armed) { + /* if RC is lost, switch to RTL_RC or Termination unless a mission is running + * and not yet finished) */ + if (status.rc_signal_lost + && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; } else { - /* failsafe for manual modes */ - transition_result_t manual_res = TRANSITION_DENIED; - - if (!status.condition_landed) { - /* vehicle is not landed, try to perform RTL */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - } - - if (manual_res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (manual_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_RTL; - } - } - - } else { - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* reset failsafe when disarmed */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + status.failsafe_state = FAILSAFE_STATE_LAND; } } - } - // TODO remove this hack - /* flight termination in manual mode if assist switch is on POSCTL position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { - if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(armed.armed); - } + /* if the data link is lost, switch to RTL_DL or Termination */ + /* TODO: add this */ + + } else { + /* reset failsafe when disarmed */ + status.failsafe_state = FAILSAFE_STATE_NORMAL; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1369,11 +1332,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check which state machines for changes, clear "changed" flag */ - bool arming_state_changed = check_arming_state_changed(); - bool main_state_changed = check_main_state_changed(); - bool failsafe_state_changed = check_failsafe_state_changed(); - hrt_abstime t1 = hrt_absolute_time(); /* print new state */ @@ -1408,18 +1366,24 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } was_armed = armed.armed; + /* now set nav state according to failsafe and main state */ + set_nav_state(&status); + if (main_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + main_state_changed = false; } if (failsafe_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + failsafe_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1608,6 +1572,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; + warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL @@ -1648,14 +1613,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // else fallback to ALTCTL (POSCTL likely will not work too) - print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1675,85 +1639,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } void - set_control_mode() { - /* set vehicle_control_mode according to main state and failsafe state */ + /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; + /* TODO: check this */ control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator should act */ - bool navigator_enabled = false; - - switch (status.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (status.main_state) { - case MAIN_STATE_MANUAL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - break; + switch (status.set_nav_state) { + case NAVIGATION_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - case MAIN_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - default: - break; - } + case NAVIGATION_STATE_ALTCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_POSCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_RTL: - navigator_enabled = true; + case NAVIGATION_STATE_AUTO_MISSION: + case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_LAND: - navigator_enabled = true; + case NAVIGATION_STATE_LAND: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_TERMINATION: + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -1769,21 +1741,6 @@ set_control_mode() default: break; } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - - /* in failsafe LAND mode position may be not available */ - control_mode.flag_control_position_enabled = status.condition_local_position_valid; - control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; - - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - } } void @@ -1927,7 +1884,6 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ - // XXX disable interrupts in arming_state_transition if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6f0e9794a..214480079 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -59,30 +60,20 @@ #include "state_machine_helper.h" #include "commander_helper.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -static bool arming_state_changed = true; -static bool main_state_changed = true; -static bool failsafe_state_changed = true; - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation @@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; - arming_state_changed = true; } } @@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet } } -bool -check_arming_state_changed() -{ - if (arming_state_changed) { - arming_state_changed = false; - return true; - - } else { - return false; - } -} - transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* transition may be denied even if requested the same state because conditions may be changed */ + /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { case MAIN_STATE_MANUAL: - ret = TRANSITION_CHANGED; - break; - case MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; case MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ + /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || (status->condition_local_altitude_valid || status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } - break; case MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } - break; - case MAIN_STATE_AUTO: - + case MAIN_STATE_AUTO_MISSION: + case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case MAIN_STATE_AUTO_RTL: + /* need global position and home position */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } break; - } + case MAIN_STATE_MAX: + default: + break; + } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { status->main_state = new_main_state; - main_state_changed = true; - } else { ret = TRANSITION_NOT_CHANGED; } @@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } -bool -check_main_state_changed() -{ - if (main_state_changed) { - main_state_changed = false; - return true; - - } else { - return false; - } -} - -bool -check_failsafe_state_changed() -{ - if (failsafe_state_changed) { - failsafe_state_changed = false; - return true; - - } else { - return false; - } -} - /** -* Transition from one hil state to another -*/ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_transition = false; - int ret = ERROR; + transition_result_t ret = TRANSITION_DENIED; if (current_status->hil_state == new_state) { - valid_transition = true; + ret = TRANSITION_NOT_CHANGED; } else { - switch (new_state) { - case HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - valid_transition = false; - + ret = TRANSITION_DENIED; break; case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - - // Disable publication of all attached sensors - + /* Disable publication of all attached sensors */ /* list directory */ DIR *d; d = opendir("/dev"); if (d) { - struct dirent *direntry; char devname[24]; @@ -388,288 +332,98 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } - closedir(d); + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + } else { /* failed opening dir */ - warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); - return 1; + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; } + } else { + mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + ret = TRANSITION_DENIED; } - break; default: - warnx("Unknown hil state"); + warnx("Unknown HIL state"); break; } } - if (valid_transition) { + if (ret = TRANSITION_CHANGED) { current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - // XXX also set lockdown here - - ret = OK; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } - return ret; } /** -* Transition from one failsafe state to another -*/ -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) + * Check failsafe and main status and set navigation status for navigator accordingly + */ +void set_nav_state(struct vehicle_status_s *status) { - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if requested the same state because conditions may be changed */ - if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { - /* transitions from TERMINATION to other states not allowed */ - if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { - ret = TRANSITION_NOT_CHANGED; - } - - } else { - switch (new_failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* always allowed (except from TERMINATION state) */ - ret = TRANSITION_CHANGED; + switch (status->failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_MANUAL: + status->set_nav_state = NAVIGATION_STATE_MANUAL; break; - case FAILSAFE_STATE_RTL: - - /* global position and home position required for RTL */ - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAVIGATION_STATE_RTL; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_ALTCTL: + status->set_nav_state = NAVIGATION_STATE_ALTCTL; + break; + case MAIN_STATE_POSCTL: + status->set_nav_state = NAVIGATION_STATE_POSCTL; break; - case FAILSAFE_STATE_LAND: + case MAIN_STATE_AUTO_MISSION: + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + break; - /* at least relative altitude estimate required for landing */ - if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAVIGATION_STATE_LAND; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO_LOITER: + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + break; + case MAIN_STATE_AUTO_RTL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; break; - case FAILSAFE_STATE_TERMINATION: - /* always allowed */ - ret = TRANSITION_CHANGED; + case MAIN_STATE_ACRO: + status->set_nav_state = NAVIGATION_STATE_ACRO; break; default: break; } + break; - if (ret == TRANSITION_CHANGED) { - if (status->failsafe_state != new_failsafe_state) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; - - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - } + case FAILSAFE_STATE_RTL_RC: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + break; - return ret; -} + case FAILSAFE_STATE_RTL_DL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + break; + case FAILSAFE_STATE_LAND: + status->set_nav_state = NAVIGATION_STATE_LAND; + break; + case FAILSAFE_STATE_TERMINATION: + status->set_nav_state = NAVIGATION_STATE_TERMINATION; + break; -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ -// -//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -//{ -// int ret = 1; -// -//// /* Switch on HIL if in standby and not already in HIL mode */ -//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) -//// && !current_status->flag_hil_enabled) { -//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { -//// /* Enable HIL on request */ -//// current_status->flag_hil_enabled = true; -//// ret = OK; -//// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); -//// -//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_fmu_armed) { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") -//// -//// } else { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") -//// } -//// } -// -// /* switch manual / auto */ -// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { -// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { -// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { -// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { -// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); -// } -// -// /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only arm in standby state */ -// // XXX REMOVE -// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); -// ret = OK; -// printf("[cmd] arming due to command request\n"); -// } -// } -// -// /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only disarm in ground ready */ -// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -// ret = OK; -// printf("[cmd] disarming due to command request\n"); -// } -// } -// -// /* NEVER actually switch off HIL without reboot */ -// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { -// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); -// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); -// ret = ERROR; -// } -// -// return ret; -//} + default: + break; + } +} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..594bf23a1 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,25 +56,17 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); - bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -bool check_arming_state_changed(); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -bool check_main_state_changed(); - transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); -bool check_navigation_state_changed(); - -bool check_failsafe_state_changed(); - -void set_navigation_state_changed(); +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); +void set_nav_state(struct vehicle_status_s *status); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 70087cf3e..31c0c8f79 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,51 +118,71 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND - || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - - } else if (status->main_state == MAIN_STATE_ALTCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + switch (status->set_nav_state) { - } else if (status->main_state == MAIN_STATE_POSCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - - } else if (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; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - } else if (status->main_state == MAIN_STATE_ACRO) { + case NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - } - - } else { - /* use navigation state when navigator is active */ - *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; + break; - if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + case NAVIGATION_STATE_AUTO_MISSION: + *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.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + case NAVIGATION_STATE_AUTO_LOITER: + *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.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + *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.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + case NAVIGATION_STATE_LAND: + *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.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } + break; + + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 74694447a..f30cd9a94 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ /** * @file navigator_main.cpp - * Implementation of the main navigation state machine. * * Handles mission items, geo fencing and failsafe navigation behavior. * Published the position setpoint triplet for the position controller. @@ -75,7 +74,6 @@ #include #include -#include #include #include #include @@ -102,7 +100,7 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator : public StateTable +class Navigator { public: /** @@ -116,7 +114,7 @@ public: ~Navigator(); /** - * Start the navigator task. + * Start the navigator task. * * @return OK on success. */ @@ -200,24 +198,6 @@ private: param_t takeoff_alt; } _parameter_handles; /**< handles for parameters */ - enum Event { - EVENT_NONE_REQUESTED, - EVENT_LOITER_REQUESTED, - EVENT_MISSION_REQUESTED, - EVENT_RTL_REQUESTED, - EVENT_TAKEN_OFF, - EVENT_LANDED, - EVENT_MISSION_CHANGED, - EVENT_HOME_POSITION_CHANGED, - EVENT_MISSION_ITEM_REACHED, - MAX_EVENT - }; /**< possible events that can be thrown at state machine */ - - /** - * State machine transition table - */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - /** * Update our local parameter cache. */ @@ -268,41 +248,6 @@ private: */ void task_main(); - /** - * Functions that are triggered when a new state is entered. - */ - bool start_none_on_ground(); - bool start_none_in_air(); - bool start_auto_on_ground(); - bool start_loiter(); - bool start_mission(); - bool advance_mission(); - bool start_rtl(); - bool advance_rtl(); - bool start_land(); - - /** - * Reset all "mission item reached" flags. - */ - void reset_reached(); - - /** - * Check if current mission item has been reached. - */ - bool check_mission_item_reached(); - - /** - * Set mission items by accessing the mission class. - * If failing, tell the state machine to loiter. - */ - bool set_mission_items(); - - /** - * Set a RTL item by accessing the RTL class. - * If failing, tell the state machine to loiter. - */ - void set_rtl_item(); - /** * Translate mission item to a position setpoint. */ @@ -329,7 +274,6 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -368,11 +312,6 @@ Navigator::Navigator() : _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - - /* Initialize state machine */ - myState = NAV_STATE_NONE_ON_GROUND; - - start_none_on_ground(); } Navigator::~Navigator() @@ -615,41 +554,6 @@ Navigator::task_main() /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - - /* commander requested new navigation mode, forward it to state machine */ - switch (_vstatus.set_nav_state) { - case NAVIGATION_STATE_NONE: - dispatch(EVENT_NONE_REQUESTED); - break; - - case NAVIGATION_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAVIGATION_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); - break; - - case NAVIGATION_STATE_RTL: - dispatch(EVENT_RTL_REQUESTED); - break; - - case NAVIGATION_STATE_LAND: - /* TODO: add and test this */ - // dispatch(EVENT_LAND_REQUESTED); - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - /* commander sets in-air/on-ground flag as well */ - if (_vstatus.condition_landed) { - dispatch(EVENT_LANDED); - } else { - dispatch(EVENT_TAKEN_OFF); - } } /* parameters updated */ @@ -666,30 +570,21 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); - /* TODO check if home position really changed */ - dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - if (check_mission_item_reached()) { - dispatch(EVENT_MISSION_ITEM_REACHED); - } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { @@ -765,125 +660,8 @@ Navigator::status() } else { warnx("Geofence not set"); } - - switch (myState) { - case NAV_STATE_NONE_ON_GROUND: - warnx("State: None on ground"); - break; - - case NAV_STATE_NONE_IN_AIR: - warnx("State: None in air"); - break; - - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - - case NAV_STATE_LAND: - warnx("State: Land"); - break; - - default: - warnx("State: Unknown"); - break; - } } - -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* NAV_STATE_NONE_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - }, - { - /* NAV_STATE_NONE_IN_AIR */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* 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_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - }, - { - /* NAV_STATE_AUTO_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - }, - { - /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, - }, - { - /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* 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_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, - }, - { - /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* 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_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, - }, - { - /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* 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_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, - }, -}; - +#if 0 bool Navigator::start_none_on_ground() { @@ -984,13 +762,11 @@ Navigator::set_mission_items() /* if we fail to set the current mission, continue to loiter */ if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - dispatch(EVENT_LOITER_REQUESTED); 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) { - dispatch(EVENT_RTL_REQUESTED); return false; } @@ -1042,7 +818,6 @@ Navigator::start_rtl() } /* if RTL doesn't work, fallback to loiter */ - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1066,7 +841,6 @@ Navigator::advance_rtl() return true; } - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1105,7 +879,7 @@ Navigator::start_land() _update_triplet = true; return true; } - +#endif void Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { @@ -1147,7 +921,7 @@ Navigator::mission_item_to_position_setpoint(const mission_item_s *item, positio sp->type = SETPOINT_TYPE_NORMAL; } } - +#if 0 bool Navigator::check_mission_item_reached() { @@ -1251,12 +1025,12 @@ Navigator::reset_reached() _waypoint_yaw_reached = false; } - +#endif void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - _pos_sp_triplet.nav_state = static_cast(myState); + /* TODO: set nav_state */ /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index 38378651b..e6011fdef 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -33,7 +33,7 @@ /** * @file state_table.h - * + * * Finite-State-Machine helper class for state table * @author: Julian Oes */ @@ -44,32 +44,32 @@ class StateTable { public: - typedef bool (StateTable::*Action)(); + typedef void (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; }; - + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) : myTable(table), myNsignals(nSignals), myNstates(nStates) {} - + #define NO_ACTION &StateTable::doNothing #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - + void dispatch(unsigned const sig) { /* get transition using state table */ Tran const *t = myTable + myState*myNsignals + sig; - /* first up change state, this allows to do further dispatchs in the state functions */ - - /* now execute state function, if it runs with success, accept new state */ - if ((this->*(t->action))()) { - myState = t->nextState; - } + + /* accept new state */ + myState = t->nextState; + + /* */ + (this->*(t->action))(); } - bool doNothing() { - return true; + void doNothing() { + return; } protected: unsigned myState; @@ -79,4 +79,4 @@ private: unsigned myNstates; }; -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 7c3921198..ad654a9ff 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -56,6 +56,7 @@ struct mission_result_s bool mission_reached; /**< true if mission has been reached */ unsigned mission_index_reached; /**< index of the mission which has been reached */ unsigned index_current_mission; /**< index of the current mission */ + bool mission_finished; /**< true if mission has been completed */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 85e8ef8a5..c2741a05b 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -74,14 +74,13 @@ struct position_setpoint_s }; typedef enum { - NAV_STATE_NONE_ON_GROUND = 0, - NAV_STATE_NONE_IN_AIR, - NAV_STATE_AUTO_ON_GROUND, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX + NAV_STATE_MANUAL = 0, + NAV_STATE_POSCTL, + NAV_STATE_AUTO, + NAV_STATE_RC_LOSS, + NAV_STATE_DL_LOSS, + NAV_STATE_TERMINATION, + MAX_NAV_STATE } nav_state_t; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index d902dc49e..259d7329e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,9 +63,11 @@ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, MAIN_STATE_POSCTL, - MAIN_STATE_AUTO, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, MAIN_STATE_ACRO, - MAIN_STATE_MAX + MAIN_STATE_MAX, } main_state_t; // If you change the order, add or remove arming_state_t states make sure to update the arrays @@ -78,7 +80,7 @@ typedef enum { ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE, - ARMING_STATE_MAX + ARMING_STATE_MAX, } arming_state_t; typedef enum { @@ -88,18 +90,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL, /**< Return To Launch */ - FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX + FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_NONE = 0, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND + NAVIGATION_STATE_MANUAL = 0, + NAVIGATION_STATE_ACRO, + NAVIGATION_STATE_ALTCTL, + NAVIGATION_STATE_POSCTL, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_RTL_RC, + NAVIGATION_STATE_AUTO_RTL_DL, + NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TERMINATION, } navigation_state_t; enum VEHICLE_MODE_FLAG { @@ -160,10 +169,10 @@ struct vehicle_status_s { uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; /**< main state machine */ + main_state_t main_state; /**< main state machine */ navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ - hil_state_t hil_state; /**< current hil state */ + hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ -- cgit v1.2.3 From 854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Jun 2014 16:01:28 +0200 Subject: navigator: mission class added (WIP) --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/navigator/mission.cpp | 438 +++++++++++++++------ src/modules/navigator/mission.h | 164 ++++++-- src/modules/navigator/module.mk | 1 - src/modules/navigator/navigator.h | 205 ++++++++++ src/modules/navigator/navigator_main.cpp | 338 ++-------------- src/modules/navigator/rtl.cpp | 31 +- src/modules/navigator/rtl.h | 21 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/topics/mission.h | 12 +- .../uORB/topics/position_setpoint_triplet.h | 30 +- 12 files changed, 749 insertions(+), 497 deletions(-) create mode 100644 src/modules/navigator/navigator.h (limited to 'src/modules/navigator') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a53118dec..b8d94fe18 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -861,7 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6b45736e9..48f91481d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index adbb0cfa2..f3a86666f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -38,30 +38,42 @@ * @author Julian Oes */ +#include #include #include +#include + +#include #include #include +#include #include #include +#include "navigator.h" #include "mission.h" -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), +Mission::Mission(Navigator *navigator) : + SuperBlock(NULL, "MIS"), + _navigator(navigator), + _first_run(true), + _param_onboard_enabled(this, "ONBOARD_EN"), + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), + _onboard_mission({0}), + _offboard_mission({0}), + _mission_item({0}), _mission_result_pub(-1), - _mission_result({}) + _mission_result({0}), + _mission_type(MISSION_TYPE_NONE) { + /* load initial params */ + updateParams(); + /* set initial mission items */ + reset(); } Mission::~Mission() @@ -69,134 +81,335 @@ Mission::~Mission() } void -Mission::set_offboard_dataman_id(const int new_id) +Mission::reset() { - _offboard_dataman_id = new_id; + _first_run = true; } -void -Mission::set_offboard_mission_count(int new_count) +bool +Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - _offboard_mission_item_count = new_count; + bool updated = false; + /* check if anything has changed, and reset mission items if needed */ + if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) { + set_mission_items(pos_sp_triplet); + updated = true; + _first_run = false; + } + + if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + advance_mission(); + set_mission_items(pos_sp_triplet); + updated = true; + } + return updated; } -void -Mission::set_onboard_mission_count(int new_count) +bool +Mission::is_mission_item_reached() { - _onboard_mission_item_count = new_count; + /* 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) { + + return false; + } +#endif + + hrt_abstime now = hrt_absolute_time(); + + if (!_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + float altitude_amsl = _mission_item.altitude_is_relative + ? _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->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) { + + /* require only altitude for takeoff */ + if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } + } + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + /* TODO: removed takeoff, why? */ + 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->get_global_position()->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 >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + return true; + } + } + return false; } void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; +Mission::reset_mission_item_reached() { + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; } -bool -Mission::command_current_offboard_mission_index(const int new_index) +void +Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { - if (new_index >= 0) { - _current_offboard_mission_index = (unsigned)new_index; - } else { + sp->valid = true; - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } + sp->lat = item->lat; + sp->lon = item->lon; + 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) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } 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) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_POSITION; } - report_current_offboard_mission_item(); } bool -Mission::command_current_onboard_mission_index(int new_index) +Mission::is_onboard_mission_updated() { - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { + bool updated; + orb_check(_onboard_mission_sub, &updated); - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } + if (!updated) { + return false; } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); + + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) { + _onboard_mission.count = 0; + _onboard_mission.current_index = 0; + } + return true; } bool -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +Mission::is_offboard_mission_updated() { - *onboard = false; - *index = -1; - - /* try onboard mission first */ - if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - return true; - } + bool updated; + orb_check(_offboard_mission_sub, &updated); + if (!updated) { + return false; } - /* otherwise fallback to offboard */ - if (_current_offboard_mission_index < _offboard_mission_item_count) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) { + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; - if (_offboard_dataman_id == 0) { + + if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, + (size_t)_offboard_mission.count, + _navigator->get_geofence()); + } else { + _offboard_mission.count = 0; + _offboard_mission.current_index = 0; + } + return true; +} - return true; - } + +void +Mission::advance_mission() +{ + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _onboard_mission.current_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _offboard_mission.current_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } +} + +void +Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + warnx("set mission items"); + + set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; + if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting onboard mission item */ + _mission_type = MISSION_TYPE_ONBOARD; + } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting offboard mission item */ + _mission_type = MISSION_TYPE_OFFBOARD; + } else { + _mission_type = MISSION_TYPE_NONE; + } +} + +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) +{ + if (_param_onboard_enabled.get() > 0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index, + &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; + + /* TODO: report this somehow */ + return true; + } + } return false; } bool -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - int next_temp_mission_index = _current_onboard_mission_index + 1; + if (_offboard_mission.current_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; - /* try onboard mission first */ - if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + report_current_offboard_mission_item(); return true; } } + return false; +} - /* then try offboard mission */ - dm_item_t dm_current; - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - next_temp_mission_index = _current_offboard_mission_index + 1; - if (next_temp_mission_index < _offboard_mission_item_count) { - if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { - return true; +void +Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + int next_temp_mission_index = _onboard_mission.current_index + 1; + + /* try if there is a next onboard mission */ + if (next_temp_mission_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; } } - /* both failed, bail out */ - return false; + /* give up */ + next_pos_sp->valid = false; + return; +} + +void +Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + /* try if there is a next offboard mission */ + int next_temp_mission_index = _offboard_mission.current_index + 1; + if (next_temp_mission_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; + } + } + /* give up */ + next_pos_sp->valid = false; + return; } bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item) { /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i=0; i<10; i++) { @@ -211,25 +424,25 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + return false; + } - /* only raise the repeat count if this is for the current mission item - * but not for the next mission item */ - if (is_current) { - (new_mission_item->do_jump_current_count)++; + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; - /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the dataman */ + return false; } - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; - } else { - return false; } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; } else { /* if it's not a DO_JUMP, then we were successful */ @@ -242,32 +455,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio return false; } -void -Mission::move_to_next() -{ - report_mission_item_reached(); - - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - void Mission::report_mission_item_reached() { - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + if (_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.mission_index_reached = _offboard_mission.current_index; } publish_mission_result(); } @@ -275,7 +468,7 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + _mission_result.index_current_mission = _offboard_mission.current_index; publish_mission_result(); } @@ -294,4 +487,3 @@ Mission::publish_mission_result() /* reset reached bool */ _mission_result.mission_reached = false; } - diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5e0e9702d..e86dd25bb 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -31,70 +31,172 @@ * ****************************************************************************/ /** - * @file navigator_mission.h + * @file mission.h * Helper class to access missions - * @author Julian Oes * - * @author Julian Oes + * @author Julian Oes */ #ifndef NAVIGATOR_MISSION_H #define NAVIGATOR_MISSION_H -#include -#include +#include + +#include +#include + #include +#include +#include +#include +#include + +#include "mission_feasibility_checker.h" + +class Navigator; -class __EXPORT Mission +class Mission : public control::SuperBlock { public: /** * Constructor */ - Mission(); + Mission(Navigator *navigator); /** * Destructor */ - ~Mission(); + virtual ~Mission(); - void set_offboard_dataman_id(const int new_id); - void set_offboard_mission_count(const int new_count); - void set_onboard_mission_count(const int new_count); - void set_onboard_mission_allowed(const bool allowed); + /** + * This function is called while the mode is inactive + */ + virtual void reset(); - bool command_current_offboard_mission_index(const int new_index); - bool command_current_onboard_mission_index(const int new_index); + /** + * This function is called while the mode is active + */ + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); - bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index); - bool get_next_mission_item(struct mission_item_s *mission_item); +protected: + /** + * Check if mission item has been reached + * @return true if successfully reached + */ + bool is_mission_item_reached(); + /** + * Reset all reached flags + */ + void reset_mission_item_reached(); - void move_to_next(); + /** + * Convert a mission item to a position setpoint + */ + void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp); + + class Navigator *_navigator; + + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; + + bool _first_run; private: - bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + /** + * Update onboard mission topic + * @return true if onboard mission has been updated + */ + bool is_onboard_mission_updated(); + + /** + * Update offboard mission topic + * @return true if offboard mission has been updated + */ + bool is_offboard_mission_updated(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * Set new mission items + */ + 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 + */ + bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + + /** + * Try to set the current position setpoint from an offboard mission item + * @return true if mission item successfully set + */ + bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp); - void report_mission_item_reached(); - void report_current_offboard_mission_item(); + /** + * Try to set the next position setpoint from an onboard mission item + */ + void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp); - void publish_mission_result(); + /** + * Try to set the next position setpoint from an offboard mission item + */ + void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp); - int _offboard_dataman_id; - int _current_offboard_mission_index; - int _current_onboard_mission_index; - int _offboard_mission_item_count; /** number of offboard mission items available */ - int _onboard_mission_item_count; /** number of onboard mission items available */ - bool _onboard_mission_allowed; + /** + * Read a mission item from the dataman and watch out for DO_JUMPS + * @return true if successful + */ + bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item); + + /** + * Report that a mission item has been reached + */ + void report_mission_item_reached(); + + /** + * Rport the current mission item + */ + void report_current_offboard_mission_item(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + + control::BlockParamFloat _param_onboard_enabled; + + int _onboard_mission_sub; + int _offboard_mission_sub; + + struct mission_s _onboard_mission; + struct mission_s _offboard_mission; + + struct mission_item_s _mission_item; + + orb_advert_t _mission_result_pub; + struct mission_result_s _mission_result; enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; + MISSION_TYPE_OFFBOARD + } _mission_type; + + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - mission_result_s _mission_result; /**< mission result for commander/mavlink */ }; #endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 8913d4d1c..2d07bc49c 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -41,7 +41,6 @@ SRCS = navigator_main.cpp \ navigator_params.c \ mission.cpp \ rtl.cpp \ - rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h new file mode 100644 index 000000000..1838fe32b --- /dev/null +++ b/src/modules/navigator/navigator.h @@ -0,0 +1,205 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator.h + * Helper class to access missions + * @author Julian Oes + * @author Anton Babushkin + */ + +#ifndef NAVIGATOR_H +#define NAVIGATOR_H + +#include + +#include +#include +#include +#include +#include + +#include "mission.h" +#include "rtl.h" +#include "geofence.h" + +class Navigator +{ +public: + /** + * Constructor + */ + Navigator(); + + /** + * Destructor, also kills the navigators task. + */ + ~Navigator(); + + /** + * Start the navigator task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the navigator status. + */ + void status(); + + /** + * Add point to geofence + */ + void add_fence_point(int argc, char *argv[]); + + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + + struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct home_position_s* get_home_position() { return &_home_pos; } + Geofence& get_geofence() { return _geofence; } + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _navigator_task; /**< task handle for sensor task */ + + int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ + + int _global_pos_sub; /**< global position subscription */ + int _home_pos_sub; /**< home position subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _offboard_mission_sub; /**< notification of offboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ + + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + mission_item_s _mission_item; /**< current mission item */ + navigation_capabilities_s _nav_caps; /**< navigation capabilities */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + + bool _mission_item_valid; /**< flags if the current mission item is valid */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Geofence _geofence; /**< class that handles the geofence */ + bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ + + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ + + Mission _mission; /**< class that handles the missions */ + //Loiter _loiter; /**< class that handles loiter */ + RTL _rtl; /**< class that handles RTL */ + + bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ + bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ + uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ + + bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ + + struct { + float acceptance_radius; + float loiter_radius; + int onboard_mission_enabled; + float takeoff_alt; + } _parameters; /**< local copies of parameters */ + + struct { + param_t acceptance_radius; + param_t loiter_radius; + param_t onboard_mission_enabled; + param_t takeoff_alt; + } _parameter_handles; /**< handles for parameters */ + + /** + * Update our local parameter cache. + */ + void parameters_update(); + + /** + * Retrieve global position + */ + void global_position_update(); + + /** + * Retrieve home position + */ + void home_position_update(); + + /** + * Retreive navigation capabilities + */ + void navigation_capabilities_update(); + + /** + * Retrieve vehicle status + */ + void vehicle_status_update(); + + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main task. + */ + void task_main(); + + /** + * Translate mission item to a position setpoint. + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + + /** + * Publish a new position setpoint triplet for position controllers + */ + void publish_position_setpoint_triplet(); +}; +#endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f30cd9a94..b991ffc8c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -62,11 +62,8 @@ #include #include -#include #include -#include #include -#include #include #include #include @@ -74,24 +71,13 @@ #include #include -#include #include #include #include #include #include -#include "mission.h" -#include "rtl.h" -#include "mission_feasibility_checker.h" -#include "geofence.h" - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include "navigator.h" /** * navigator app start / stop handling function @@ -100,174 +86,10 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator -{ -public: - /** - * Constructor - */ - Navigator(); - - /** - * Destructor, also kills the navigators task. - */ - ~Navigator(); - - /** - * Start the navigator task. - * - * @return OK on success. - */ - int start(); - - /** - * Display the navigator status. - */ - void status(); - - /** - * Add point to geofence - */ - void add_fence_point(int argc, char *argv[]); - - /** - * Load fence from file - */ - void load_fence_from_file(const char *filename); - -private: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _navigator_task; /**< task handle for sensor task */ - - int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ - - int _global_pos_sub; /**< global position subscription */ - int _home_pos_sub; /**< home position subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ - int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _control_mode_sub; /**< vehicle control mode subscription */ - - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - - vehicle_status_s _vstatus; /**< vehicle status */ - vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - vehicle_global_position_s _global_pos; /**< global vehicle position */ - home_position_s _home_pos; /**< home position for RTL */ - mission_item_s _mission_item; /**< current mission item */ - navigation_capabilities_s _nav_caps; /**< navigation capabilities */ - position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - - bool _mission_item_valid; /**< flags if the current mission item is valid */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Geofence _geofence; /**< class that handles the geofence */ - bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - - bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ - - - Mission _mission; /**< class that handles the missions */ - - RTL _rtl; /**< class that handles RTL */ - - bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ - bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ - uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ - - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - - bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ - - struct { - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - } _parameters; /**< local copies of parameters */ - - struct { - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - } _parameter_handles; /**< handles for parameters */ - - /** - * Update our local parameter cache. - */ - void parameters_update(); - - /** - * Retrieve global position - */ - void global_position_update(); - - /** - * Retrieve home position - */ - void home_position_update(); - - /** - * Retreive navigation capabilities - */ - void navigation_capabilities_update(); - - /** - * Retrieve offboard mission. - */ - void offboard_mission_update(); - - /** - * Retrieve onboard mission. - */ - void onboard_mission_update(); - - /** - * Retrieve vehicle status - */ - void vehicle_status_update(); - - /** - * Retrieve vehicle control mode - */ - void vehicle_control_mode_update(); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main task. - */ - void task_main(); - - /** - * Translate mission item to a position setpoint. - */ - void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - - /** - * Publish a new position setpoint triplet for position controllers - */ - void publish_position_setpoint_triplet(); -}; namespace navigator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Navigator *g_navigator; } @@ -299,8 +121,9 @@ Navigator::Navigator() : _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission({}), - _rtl({}), + _mission(this), + //_loiter(&_global_pos, &_home_pos, &_vstatus), + _rtl(this), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -351,7 +174,7 @@ Navigator::parameters_update() param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - _mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); + //_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); _geofence.updateParams(); } @@ -366,8 +189,6 @@ void Navigator::home_position_update() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - - _rtl.set_home_position(&_home_pos); } void @@ -376,54 +197,6 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } - -void -Navigator::offboard_mission_update() -{ - struct mission_s offboard_mission; - - if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { - - /* Check mission feasibility, for now do not handle the return value, - * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current; - - if (offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence); - - _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - - _mission.set_offboard_mission_count(offboard_mission.count); - _mission.command_current_offboard_mission_index(offboard_mission.current_index); - - } else { - _mission.set_offboard_mission_count(0); - _mission.command_current_offboard_mission_index(0); - } -} - -void -Navigator::onboard_mission_update() -{ - struct mission_s onboard_mission; - - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { - - _mission.set_onboard_mission_count(onboard_mission.count); - _mission.command_current_onboard_mission_index(onboard_mission.current_index); - - } else { - _mission.set_onboard_mission_count(0); - _mission.command_current_onboard_mission_index(0); - } -} - void Navigator::vehicle_status_update() { @@ -459,8 +232,7 @@ Navigator::task_main() /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file - * else clear geofence data in datamanager - */ + * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { @@ -474,9 +246,7 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* - * do subscriptions - */ + /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); @@ -493,8 +263,6 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(); - onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -503,7 +271,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -514,14 +282,10 @@ Navigator::task_main() fds[2].events = POLLIN; fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; - fds[4].fd = _offboard_mission_sub; + fds[4].fd = _vstatus_sub; fds[4].events = POLLIN; - fds[5].fd = _onboard_mission_sub; + fds[5].fd = _control_mode_sub; fds[5].events = POLLIN; - fds[6].fd = _vstatus_sub; - fds[6].events = POLLIN; - fds[7].fd = _control_mode_sub; - fds[7].events = POLLIN; while (!_task_should_exit) { @@ -547,12 +311,12 @@ Navigator::task_main() } /* vehicle control mode updated */ - if (fds[7].revents & POLLIN) { + if (fds[5].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[6].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_status_update(); } @@ -567,16 +331,6 @@ Navigator::task_main() navigation_capabilities_update(); } - /* offboard mission updated */ - if (fds[4].revents & POLLIN) { - offboard_mission_update(); - } - - /* onboard mission updated */ - if (fds[5].revents & POLLIN) { - onboard_mission_update(); - } - /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); @@ -600,6 +354,33 @@ Navigator::task_main() } } + /* Do stuff according to navigation state set by commander */ + switch (_vstatus.set_nav_state) { + case NAVIGATION_STATE_MANUAL: + case NAVIGATION_STATE_ACRO: + case NAVIGATION_STATE_ALTCTL: + case NAVIGATION_STATE_POSCTL: + break; + case NAVIGATION_STATE_AUTO_MISSION: + _update_triplet = _mission.update(&_pos_sp_triplet); + _rtl.reset(); + break; + case NAVIGATION_STATE_AUTO_LOITER: + //_loiter.update(); + break; + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + _mission.reset(); + _update_triplet = _rtl.update(&_pos_sp_triplet); + break; + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + default: + break; + + } + if (_update_triplet ) { publish_position_setpoint_triplet(); _update_triplet = false; @@ -880,47 +661,6 @@ Navigator::start_land() return true; } #endif -void -Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) -{ - sp->valid = true; - - if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); - - } else { - /* else use current position */ - sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); - } - - } else { - sp->lat = item->lat; - sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.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) { - sp->type = SETPOINT_TYPE_TAKEOFF; - - } else if (item->nav_cmd == NAV_CMD_LAND) { - sp->type = SETPOINT_TYPE_LAND; - - } 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) { - sp->type = SETPOINT_TYPE_LOITER; - - } else { - sp->type = SETPOINT_TYPE_NORMAL; - } -} #if 0 bool Navigator::check_mission_item_reached() diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b8ea06275..9d7886aa6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -51,26 +51,41 @@ #include "rtl.h" - -RTL::RTL() : - SuperBlock(NULL, "RTL"), +RTL::RTL(Navigator *navigator) : + Mission(navigator), _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), _home_position({}), + _loiter_radius(50), + _acceptance_radius(50), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY"), - _loiter_radius(50), - _acceptance_radius(50) + _param_land_delay(this, "LAND_DELAY") { /* load initial params */ updateParams(); + /* initial reset */ + reset(); } RTL::~RTL() { } +bool +RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated = false; + + return updated; +} + +void +RTL::reset() +{ + +} + void RTL::set_home_position(const home_position_s *new_home_position) { @@ -206,7 +221,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss } default: - return false; + return false; } return true; @@ -226,7 +241,7 @@ RTL::move_to_next() case RTL_STATE_CLIMB: _rtl_state = RTL_STATE_RETURN; break; - + case RTL_STATE_RETURN: _rtl_state = RTL_STATE_DESCEND; break; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c761837fc..8d1bfec59 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/*************************************************************************** * * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. * @@ -43,23 +43,33 @@ #include #include +#include #include #include #include -class RTL : public control::SuperBlock +#include "mission.h" + +class Navigator; + +class RTL : public Mission { public: /** * Constructor */ - RTL(); + RTL(Navigator *navigator); /** * Destructor */ - ~RTL(); + virtual ~RTL(); + + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void reset(); + +private: void set_home_position(const home_position_s *home_position); bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); @@ -67,7 +77,6 @@ public: void move_to_next(); -private: int _mavlink_fd; enum RTLState { @@ -77,7 +86,7 @@ private: RTL_STATE_DESCEND, RTL_STATE_LAND, RTL_STATE_FINISHED, - } _rtl_state; + } _rtl_state; home_position_s _home_position; float _loiter_radius; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 66a9e31c6..2b6caf159 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1340,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 4532b329d..d25db3a77 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,9 @@ /** * @file mission.h * Definition of a mission consisting of mission items. + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier */ #ifndef TOPIC_MISSION_H_ @@ -92,9 +92,9 @@ struct mission_item_s { float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ - int do_jump_mission_index; /**< the mission index that we want to jump to */ - int do_jump_repeat_count; /**< how many times the jump should be repeated */ - int do_jump_current_count; /**< how many times the jump has already been repeated */ + int do_jump_mission_index; /**< index where the do jump will go to */ + unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ + unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; struct mission_s diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index c2741a05b..ce42035ba 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file mission_item_triplet.h * Definition of the global WGS84 position setpoint uORB topic. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier */ #ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ @@ -53,11 +54,12 @@ enum SETPOINT_TYPE { - SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ - SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ - SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ + SETPOINT_TYPE_POSITION = 0, /**< position setpoint */ + SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ + SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ }; struct position_setpoint_s @@ -73,16 +75,6 @@ struct position_setpoint_s float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; -typedef enum { - NAV_STATE_MANUAL = 0, - NAV_STATE_POSCTL, - NAV_STATE_AUTO, - NAV_STATE_RC_LOSS, - NAV_STATE_DL_LOSS, - NAV_STATE_TERMINATION, - MAX_NAV_STATE -} nav_state_t; - /** * Global position setpoint triplet in WGS84 coordinates. * @@ -93,8 +85,6 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; - - nav_state_t nav_state; /**< navigation state */ }; /** -- cgit v1.2.3 From 82b7b80f475d12fc31eb63c0d69a7cf8f75ac534 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Jun 2014 21:20:44 +0200 Subject: navigator: bugfixing (WIP: mission topic not copying) --- src/modules/navigator/mission.cpp | 37 ++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 2 ++ src/modules/navigator/navigator.h | 2 -- src/modules/navigator/navigator_main.cpp | 9 +++----- src/modules/navigator/rtl.cpp | 4 +--- 5 files changed, 34 insertions(+), 20 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f3a86666f..7e02f8c15 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,6 +50,7 @@ #include #include +#include #include #include "navigator.h" @@ -61,8 +62,6 @@ Mission::Mission(Navigator *navigator) : _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), - _onboard_mission_sub(-1), - _offboard_mission_sub(-1), _onboard_mission({0}), _offboard_mission({0}), _mission_item({0}), @@ -74,6 +73,9 @@ Mission::Mission(Navigator *navigator) : updateParams(); /* set initial mission items */ reset(); + + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); } Mission::~Mission() @@ -89,9 +91,14 @@ Mission::reset() bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { + /* check if anything has changed */ + bool onboard_updated = false; //is_onboard_mission_updated(); + bool offboard_updated = is_offboard_mission_updated(); + bool updated = false; - /* check if anything has changed, and reset mission items if needed */ - if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) { + + /* reset mission items if needed */ + if (onboard_updated || offboard_updated) { set_mission_items(pos_sp_triplet); updated = true; _first_run = false; @@ -229,7 +236,7 @@ Mission::is_onboard_mission_updated() bool updated; orb_check(_onboard_mission_sub, &updated); - if (!updated) { + if (!updated && !_first_run) { return false; } @@ -244,12 +251,17 @@ bool Mission::is_offboard_mission_updated() { bool updated; + warnx("sub: %d", _offboard_mission_sub); orb_check(_offboard_mission_sub, &updated); - if (!updated) { + if (!updated && !_first_run) { + warnx("not updated"); return false; } - - if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) { + struct mission_s offboard_mission; + int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission); + warnx("ret: %d", ret); + if (ret == OK) { + warnx("copy new offboard mission"); /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -263,8 +275,10 @@ Mission::is_offboard_mission_updated() missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t)_offboard_mission.count, - _navigator->get_geofence()); + _navigator->get_geofence(), + _navigator->get_home_position()->alt); } else { + warnx("no success with orb_copy"); _offboard_mission.count = 0; _offboard_mission.current_index = 0; } @@ -341,7 +355,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { + warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count ); if (_offboard_mission.current_index < (int)_offboard_mission.count) { + warnx("theoretically possible"); dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -356,8 +372,11 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren report_current_offboard_mission_item(); return true; + } else { + warnx("read fail"); } } + warnx("failed with offboard mission"); return false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e86dd25bb..65a0991b5 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -47,9 +47,11 @@ #include +#include #include #include #include +#include #include #include "mission_feasibility_checker.h" diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1838fe32b..927e77391 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -103,8 +103,6 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ - int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f91032196..9dd253127 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -103,8 +103,6 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _offboard_mission_sub(-1), - _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), @@ -248,8 +246,6 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -360,6 +356,8 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + _mission.reset(); + _rtl.reset(); break; case NAVIGATION_STATE_AUTO_MISSION: _update_triplet = _mission.update(&_pos_sp_triplet); @@ -660,8 +658,6 @@ Navigator::start_land() _update_triplet = true; return true; } -#endif -#if 0 bool Navigator::check_mission_item_reached() { @@ -766,6 +762,7 @@ Navigator::reset_reached() _waypoint_yaw_reached = false; } +#endif void Navigator::publish_position_setpoint_triplet() { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 9d7886aa6..dc87b0521 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,9 +57,7 @@ RTL::RTL(Navigator *navigator) : _rtl_state(RTL_STATE_NONE), _home_position({}), _loiter_radius(50), - _acceptance_radius(50), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), + _acceptance_radius(50) _param_land_delay(this, "LAND_DELAY") { /* load initial params */ -- cgit v1.2.3 From 5ed3652c2fdc20ba70df1cde6c28d58fa6be0e04 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Jun 2014 21:29:26 +0200 Subject: navigator: compile fix --- src/modules/navigator/rtl.cpp | 4 +++- src/modules/navigator/rtl.h | 1 - 2 files changed, 3 insertions(+), 2 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index dc87b0521..9d7886aa6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,7 +57,9 @@ RTL::RTL(Navigator *navigator) : _rtl_state(RTL_STATE_NONE), _home_position({}), _loiter_radius(50), - _acceptance_radius(50) + _acceptance_radius(50), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), _param_land_delay(this, "LAND_DELAY") { /* load initial params */ diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 8d1bfec59..60336d5d0 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -92,7 +92,6 @@ private: float _loiter_radius; float _acceptance_radius; - control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; -- cgit v1.2.3 From 93295861c532595e080ae61c32d5e2cb625b4eac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Jun 2014 01:07:50 +0200 Subject: navigator: missions work again, loiter when finished or no mission available or sd card removed works as well --- src/modules/navigator/mission.cpp | 133 +++++++++++++++++++++++-------- src/modules/navigator/mission.h | 16 +++- src/modules/navigator/mission_params.c | 68 ++++++++++++++++ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 7 ++ src/modules/navigator/navigator_main.cpp | 4 + 6 files changed, 191 insertions(+), 38 deletions(-) create mode 100644 src/modules/navigator/mission_params.c (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7e02f8c15..38badcff9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -62,20 +62,22 @@ Mission::Mission(Navigator *navigator) : _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), + _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), + _current_onboard_mission_index(-1), + _current_offboard_mission_index(-1), _mission_item({0}), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _loiter_set(false) { /* load initial params */ updateParams(); /* set initial mission items */ reset(); - _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); - _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); } Mission::~Mission() @@ -86,13 +88,15 @@ void Mission::reset() { _first_run = true; + _loiter_set = false; } bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { + /* check if anything has changed */ - bool onboard_updated = false; //is_onboard_mission_updated(); + bool onboard_updated = is_onboard_mission_updated(); bool offboard_updated = is_offboard_mission_updated(); bool updated = false; @@ -104,11 +108,20 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) _first_run = false; } + /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(pos_sp_triplet); updated = true; } + + /* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */ + if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) { + bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached); + set_loiter_item(use_current_pos_sp, pos_sp_triplet); + updated = true; + _loiter_set = true; + } return updated; } @@ -195,17 +208,17 @@ Mission::is_mission_item_reached() } void -Mission::reset_mission_item_reached() { +Mission::reset_mission_item_reached() +{ _waypoint_position_reached = false; _waypoint_yaw_reached = false; _time_first_inside_orbit = 0; } void -Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) +Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) { sp->valid = true; - sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; @@ -230,19 +243,56 @@ Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_ } } +void +Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) +{ + if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { + /* nothing to be done, just use the current item */ + } else { + 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 */ + } + pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; + pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get(); + 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; +} + + bool Mission::is_onboard_mission_updated() { bool updated; - orb_check(_onboard_mission_sub, &updated); + orb_check(_navigator->get_onboard_mission_sub(), &updated); if (!updated && !_first_run) { return false; } - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) { + if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { + /* accept the current index set by the onboard mission if it is within bounds */ + if (_onboard_mission.current_index >=0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_index; + } else { + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= (int)_onboard_mission.count) { + _current_onboard_mission_index = 0; + /* if not initialized, set it to 0 */ + } else if (_current_onboard_mission_index < 0) { + _current_onboard_mission_index = 0; + } + /* otherwise, just leave it */ + } + } else { _onboard_mission.count = 0; _onboard_mission.current_index = 0; + _current_onboard_mission_index = 0; } return true; } @@ -251,17 +301,27 @@ bool Mission::is_offboard_mission_updated() { bool updated; - warnx("sub: %d", _offboard_mission_sub); - orb_check(_offboard_mission_sub, &updated); + orb_check(_navigator->get_offboard_mission_sub(), &updated); + if (!updated && !_first_run) { - warnx("not updated"); return false; } - struct mission_s offboard_mission; - int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission); - warnx("ret: %d", ret); - if (ret == OK) { - warnx("copy new offboard mission"); + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { + + /* determine current index */ + if (_offboard_mission.current_index >= 0 + && _offboard_mission.current_index < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_index; + } else { + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= (int)_offboard_mission.count) { + _current_offboard_mission_index = 0; + /* if not initialized, set it to 0 */ + } else if (_current_offboard_mission_index < 0) { + _current_offboard_mission_index = 0; + } + /* otherwise, just leave it */ + } /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -278,9 +338,9 @@ Mission::is_offboard_mission_updated() _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { - warnx("no success with orb_copy"); _offboard_mission.count = 0; _offboard_mission.current_index = 0; + _current_offboard_mission_index = 0; } return true; } @@ -291,11 +351,11 @@ Mission::advance_mission() { switch (_mission_type) { case MISSION_TYPE_ONBOARD: - _onboard_mission.current_index++; + _current_onboard_mission_index++; break; case MISSION_TYPE_OFFBOARD: - _offboard_mission.current_index++; + _current_offboard_mission_index++; break; case MISSION_TYPE_NONE: @@ -307,17 +367,17 @@ Mission::advance_mission() void Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { - warnx("set mission items"); - set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting onboard mission item */ _mission_type = MISSION_TYPE_ONBOARD; + _loiter_set = false; } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting offboard mission item */ _mission_type = MISSION_TYPE_OFFBOARD; + _loiter_set = false; } else { _mission_type = MISSION_TYPE_NONE; } @@ -337,15 +397,18 @@ bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { if (_param_onboard_enabled.get() > 0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { + && _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index, + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); current_pos_sp->valid = true; + reset_mission_item_reached(); + /* TODO: report this somehow */ + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; } } @@ -355,9 +418,7 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count ); - if (_offboard_mission.current_index < (int)_offboard_mission.count) { - warnx("theoretically possible"); + if (_current_offboard_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -365,18 +426,20 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } struct mission_item_s new_mission_item; - if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) { + if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); current_pos_sp->valid = true; + reset_mission_item_reached(); + report_current_offboard_mission_item(); + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; } else { - warnx("read fail"); + warnx("ERROR: WP read fail"); } } - warnx("failed with offboard mission"); return false; } @@ -443,6 +506,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + /* TODO: do this check more gracefully since it is not a serious error */ if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { return false; } @@ -458,6 +522,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* not supposed to happen unless the datamanager can't access the dataman */ return false; } + /* TODO: report about DO JUMP count */ } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -479,7 +544,7 @@ Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _offboard_mission.current_index; + _mission_result.mission_index_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -487,7 +552,7 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _offboard_mission.current_index; + _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 65a0991b5..f75c8a882 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -58,7 +58,7 @@ class Navigator; -class Mission : public control::SuperBlock +class __EXPORT Mission : public control::SuperBlock { public: /** @@ -97,6 +97,11 @@ protected: */ void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp); + /** + * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position + */ + void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); + class Navigator *_navigator; bool _waypoint_position_reached; @@ -179,13 +184,14 @@ private: void publish_mission_result(); control::BlockParamFloat _param_onboard_enabled; - - int _onboard_mission_sub; - int _offboard_mission_sub; + control::BlockParamFloat _param_loiter_radius; struct mission_s _onboard_mission; struct mission_s _offboard_mission; + int _current_onboard_mission_index; + int _current_offboard_mission_index; + struct mission_item_s _mission_item; orb_advert_t _mission_result_pub; @@ -197,6 +203,8 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _loiter_set; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c new file mode 100644 index 000000000..9696b3e53 --- /dev/null +++ b/src/modules/navigator/mission_params.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mission_params.c + * + * Parameters for mission. + * + * @author Julian Oes + */ + +#include + +#include + +/* + * Mission parameters, accessible via MAVLink + */ + +/** + * Loiter radius (fixed wing only) + * + * Default value of loiter radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f); + +/** + * Enable onboard mission + * + * @min 0 + * @max 1 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 2d07bc49c..e2cc475da 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ mission.cpp \ + mission_params.c \ rtl.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 927e77391..a6ab85519 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -87,9 +87,14 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Getters + */ struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } + int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } private: @@ -105,6 +110,8 @@ private: int _params_sub; /**< notification of parameter updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ + int _onboard_mission_sub; /**< onboard mission subscription */ + int _offboard_mission_sub; /**< offboard mission subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9dd253127..b7dfec047 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -106,6 +106,8 @@ Navigator::Navigator() : _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), _vstatus({}), _control_mode({}), _global_pos({}), @@ -251,6 +253,8 @@ Navigator::task_main() _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); /* copy all topics first time */ vehicle_status_update(); -- cgit v1.2.3 From 09e4cc605ba898c8fc2d92c105ec511ffcb6781d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Jun 2014 15:16:20 +0200 Subject: navigator: use different param names for mission and RTL --- src/modules/navigator/mission.cpp | 4 ++-- src/modules/navigator/mission.h | 2 +- src/modules/navigator/navigator_main.cpp | 4 ++-- src/modules/navigator/rtl.cpp | 4 ++-- src/modules/navigator/rtl.h | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 38badcff9..879e5b618 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -57,8 +57,8 @@ #include "mission.h" -Mission::Mission(Navigator *navigator) : - SuperBlock(NULL, "MIS"), +Mission::Mission(Navigator *navigator, const char *name) : + SuperBlock(NULL, name), _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index f75c8a882..a9ef71606 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -64,7 +64,7 @@ public: /** * Constructor */ - Mission(Navigator *navigator); + Mission(Navigator *navigator, const char *name); /** * Destructor diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b7dfec047..e166058b9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -121,9 +121,9 @@ Navigator::Navigator() : _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission(this), + _mission(this, "MIS"), //_loiter(&_global_pos, &_home_pos, &_vstatus), - _rtl(this), + _rtl(this, "RTL"), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 9d7886aa6..10499085b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -51,8 +51,8 @@ #include "rtl.h" -RTL::RTL(Navigator *navigator) : - Mission(navigator), +RTL::RTL(Navigator *navigator, const char *name) : + Mission(navigator, name), _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), _home_position({}), diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 60336d5d0..a3a819403 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -58,7 +58,7 @@ public: /** * Constructor */ - RTL(Navigator *navigator); + RTL(Navigator *navigator, const char *name); /** * Destructor -- cgit v1.2.3 From 425b454a87f0eb4dd0300154cdeffa5723c1b3b8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Jun 2014 15:33:09 +0200 Subject: navigator: parameter cleanup --- src/modules/navigator/mission_params.c | 15 ++++- src/modules/navigator/module.mk | 2 +- src/modules/navigator/navigator.h | 20 ------- src/modules/navigator/navigator_main.cpp | 60 ++++---------------- src/modules/navigator/navigator_params.c | 96 -------------------------------- src/modules/navigator/rtl.cpp | 1 + src/modules/navigator/rtl.h | 1 + src/modules/navigator/rtl_params.c | 11 ++++ 8 files changed, 39 insertions(+), 167 deletions(-) delete mode 100644 src/modules/navigator/navigator_params.c (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 9696b3e53..f5067a70b 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -48,9 +48,20 @@ */ /** - * Loiter radius (fixed wing only) + * Take-off altitude * - * Default value of loiter radius (if not specified in mission item). + * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to + * MIS_TAKEOFF_ALT on takeoff, then go to waypoint. + * + * @unit meters + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); + +/** + * Loiter radius after/during mission (FW only) + * + * Default value of loiter radius (fixedwing only). * * @unit meters * @min 0.0 diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index e2cc475da..88d8aac5d 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,10 +38,10 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ - navigator_params.c \ mission.cpp \ mission_params.c \ rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a6ab85519..9f877cd76 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -107,7 +107,6 @@ private: int _global_pos_sub; /**< global position subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ @@ -143,25 +142,6 @@ private: bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ - struct { - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - } _parameters; /**< local copies of parameters */ - - struct { - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - } _parameter_handles; /**< handles for parameters */ - - /** - * Update our local parameter cache. - */ - void parameters_update(); - /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e166058b9..3c8875a74 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -64,12 +64,10 @@ #include #include #include -#include #include #include #include -#include #include #include #include @@ -102,7 +100,6 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _params_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), _pos_sp_triplet_pub(-1), @@ -127,14 +124,8 @@ Navigator::Navigator() : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _update_triplet(false), - _parameters({}), - _parameter_handles({}) + _update_triplet(false) { - _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); - _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); - _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); - _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); } Navigator::~Navigator() @@ -162,23 +153,6 @@ Navigator::~Navigator() navigator::g_navigator = nullptr; } -void -Navigator::parameters_update() -{ - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); - param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); - param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - - //_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); - - _geofence.updateParams(); -} - void Navigator::global_position_update() { @@ -251,7 +225,6 @@ Navigator::task_main() _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -259,7 +232,6 @@ Navigator::task_main() /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); - parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); @@ -271,21 +243,19 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[5]; /* Setup of loop */ - fds[0].fd = _params_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _global_pos_sub; + fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; - fds[2].fd = _home_pos_sub; + fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; - fds[3].fd = _capabilities_sub; + fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; - fds[4].fd = _vstatus_sub; + fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; - fds[5].fd = _control_mode_sub; - fds[5].events = POLLIN; while (!_task_should_exit) { @@ -311,33 +281,27 @@ Navigator::task_main() } /* vehicle control mode updated */ - if (fds[5].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[4].revents & POLLIN) { + if (fds[3].revents & POLLIN) { vehicle_status_update(); } - /* 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 */ - } - /* navigation capabilities updated */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } /* home position updated */ - if (fds[2].revents & POLLIN) { + if (fds[1].revents & POLLIN) { home_position_update(); } /* global position updated */ - if (fds[1].revents & POLLIN) { + if (fds[0].revents & POLLIN) { global_position_update(); /* Check geofence violation */ diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c deleted file mode 100644 index ad079a250..000000000 --- a/src/modules/navigator/navigator_params.c +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file navigator_params.c - * - * Parameters defined by the navigator task. - * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin - */ - -#include - -#include - -/* - * Navigator parameters, accessible via MAVLink - */ - -/** - * Waypoint acceptance radius - * - * Default value of acceptance radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); - -/** - * Loiter radius (fixed wing only) - * - * Default value of loiter radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); - -/** - * Enable onboard mission - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); - -/** - * Take-off altitude - * - * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); - -/** - * Enable parachute deployment - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 10499085b..d4e609584 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) : _home_position({}), _loiter_radius(50), _acceptance_radius(50), + _param_loiter_rad(this, "LOITER_RAD"), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), _param_land_delay(this, "LAND_DELAY") diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index a3a819403..d84fd1a6f 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -92,6 +92,7 @@ private: float _loiter_radius; float _acceptance_radius; + control::BlockParamFloat _param_loiter_rad; control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7a8b1d745..bfe6ce7e1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -47,6 +47,17 @@ * RTL parameters, accessible via MAVLink */ +/** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + /** * RTL altitude * -- cgit v1.2.3 From 7af1103bf3d4936c259e4ee44454d7e34100a7d0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 00:55:18 +0200 Subject: navigator: mission and loiter working now --- src/modules/navigator/loiter.cpp | 79 ++++++++++++++++++++++++++++++++ src/modules/navigator/loiter.h | 69 ++++++++++++++++++++++++++++ src/modules/navigator/mission.cpp | 33 +++++++------ src/modules/navigator/mission.h | 20 ++++---- src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 28 ++++++----- src/modules/navigator/navigator_main.cpp | 19 ++++---- 7 files changed, 202 insertions(+), 47 deletions(-) create mode 100644 src/modules/navigator/loiter.cpp create mode 100644 src/modules/navigator/loiter.h (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp new file mode 100644 index 000000000..6da08f072 --- /dev/null +++ b/src/modules/navigator/loiter.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file loiter.cpp + * + * Helper class to loiter + * + * @author Julian Oes + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "loiter.h" + +Loiter::Loiter(Navigator *navigator, const char *name) : + Mission(navigator, name) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + reset(); +} + +Loiter::~Loiter() +{ +} + +bool +Loiter::update(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);; +} + +void +Loiter::reset() +{ +} + diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h new file mode 100644 index 000000000..4ae265e44 --- /dev/null +++ b/src/modules/navigator/loiter.h @@ -0,0 +1,69 @@ +/*************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file loiter.h + * + * Helper class to loiter + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_LOITER_H +#define NAVIGATOR_LOITER_H + +#include +#include + +#include "mission.h" + +class Navigator; + +class Loiter : public Mission +{ +public: + /** + * Constructor + */ + Loiter(Navigator *navigator, const char *name); + + /** + * Destructor + */ + virtual ~Loiter(); + + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + + virtual void reset(); +}; + +#endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 879e5b618..839c4c960 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -70,8 +70,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_item({0}), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE), - _loiter_set(false) + _mission_type(MISSION_TYPE_NONE) { /* load initial params */ updateParams(); @@ -88,13 +87,11 @@ void Mission::reset() { _first_run = true; - _loiter_set = false; } bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - /* check if anything has changed */ bool onboard_updated = is_onboard_mission_updated(); bool offboard_updated = is_offboard_mission_updated(); @@ -115,13 +112,6 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) updated = true; } - /* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */ - if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) { - bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached); - set_loiter_item(use_current_pos_sp, pos_sp_triplet); - updated = true; - _loiter_set = true; - } return updated; } @@ -243,12 +233,18 @@ Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, st } } -void +bool Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) { + if (_navigator->get_is_in_loiter()) { + /* already loitering, bail out */ + return false; + } + if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { - /* nothing to be done, just use the current item */ + /* leave position setpoint as is */ } else { + /* use current position */ 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; @@ -261,6 +257,9 @@ Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_tri pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; + + _navigator->set_is_in_loiter(true); + return true; } @@ -372,14 +371,18 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting onboard mission item */ _mission_type = MISSION_TYPE_ONBOARD; - _loiter_set = false; + _navigator->set_is_in_loiter(false); } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting offboard mission item */ _mission_type = MISSION_TYPE_OFFBOARD; - _loiter_set = false; + _navigator->set_is_in_loiter(false); } else { _mission_type = MISSION_TYPE_NONE; + + bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; + reset_mission_item_reached(); + set_loiter_item(use_current_pos_sp, pos_sp_triplet); } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a9ef71606..4d0083d85 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -32,6 +32,7 @@ ****************************************************************************/ /** * @file mission.h + * * Helper class to access missions * * @author Julian Oes @@ -58,7 +59,7 @@ class Navigator; -class __EXPORT Mission : public control::SuperBlock +class Mission : public control::SuperBlock { public: /** @@ -99,17 +100,12 @@ protected: /** * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position + * @return true if setpoint has changed */ - void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); + bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); class Navigator *_navigator; - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - hrt_abstime _time_first_inside_orbit; - - bool _first_run; - private: /** * Update onboard mission topic @@ -183,6 +179,12 @@ private: */ void publish_mission_result(); + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; + + bool _first_run; + control::BlockParamFloat _param_onboard_enabled; control::BlockParamFloat _param_loiter_radius; @@ -203,8 +205,6 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; - bool _loiter_set; - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 88d8aac5d..f4243c5b8 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ mission.cpp \ mission_params.c \ + loiter.cpp \ rtl.cpp \ rtl_params.c \ mission_feasibility_checker.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9f877cd76..0c551bb41 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -49,6 +49,7 @@ #include #include "mission.h" +#include "loiter.h" #include "rtl.h" #include "geofence.h" @@ -87,15 +88,21 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Setters + */ + void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; } + /** * Getters */ - struct vehicle_status_s* get_vstatus() { return &_vstatus; } - struct vehicle_global_position_s* get_global_position() { return &_global_pos; } - struct home_position_s* get_home_position() { return &_home_pos; } - int get_onboard_mission_sub() { return _onboard_mission_sub; } - int get_offboard_mission_sub() { return _offboard_mission_sub; } - Geofence& get_geofence() { return _geofence; } + struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct home_position_s* get_home_position() { return &_home_pos; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } + int get_offboard_mission_sub() { return _offboard_mission_sub; } + Geofence& get_geofence() { return _geofence; } + bool get_is_in_loiter() { return _is_in_loiter; } private: @@ -133,14 +140,11 @@ private: bool _inside_fence; /**< vehicle is inside fence */ Mission _mission; /**< class that handles the missions */ - //Loiter _loiter; /**< class that handles loiter */ + Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ - bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */ - bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */ - uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */ - - bool _update_triplet; /**< flags if position setpoint triplet needs to be published */ + bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ + bool _update_triplet; /**< flags if position SP triplet needs to be published */ /** * Retrieve global position diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3c8875a74..44c1075c1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -102,9 +102,9 @@ Navigator::Navigator() : _vstatus_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - _pos_sp_triplet_pub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), + _pos_sp_triplet_pub(-1), _vstatus({}), _control_mode({}), _global_pos({}), @@ -119,11 +119,8 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(this, "MIS"), - //_loiter(&_global_pos, &_home_pos, &_vstatus), + _loiter(this, "LOI"), _rtl(this, "RTL"), - _waypoint_position_reached(false), - _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), _update_triplet(false) { } @@ -325,26 +322,29 @@ Navigator::task_main() case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: _mission.reset(); + _loiter.reset(); _rtl.reset(); + _is_in_loiter = false; break; case NAVIGATION_STATE_AUTO_MISSION: _update_triplet = _mission.update(&_pos_sp_triplet); - _rtl.reset(); break; case NAVIGATION_STATE_AUTO_LOITER: - //_loiter.update(); + _update_triplet = _loiter.update(&_pos_sp_triplet); break; case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL_RC: case NAVIGATION_STATE_AUTO_RTL_DL: - _mission.reset(); _update_triplet = _rtl.update(&_pos_sp_triplet); break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: + _mission.reset(); + _loiter.reset(); + _rtl.reset(); + _is_in_loiter = false; break; - } if (_update_triplet ) { @@ -354,7 +354,6 @@ Navigator::task_main() perf_end(_loop_perf); } - warnx("exiting."); _navigator_task = -1; -- cgit v1.2.3 From 034856a24c4ffd41c991797e75517ed456f77a54 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 17:16:49 +0200 Subject: mission_feasability_checker: fixed warnings --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index e1a6854b2..dd7f4c801 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size // float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; + return false; } void MissionFeasibilityChecker::updateNavigationCapabilities() { - int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } void MissionFeasibilityChecker::init() -- cgit v1.2.3 From 9bfae10b73406ca4f6600a0441c6edf5077f1446 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 17:17:06 +0200 Subject: geofence: fixed warnings --- src/modules/navigator/geofence.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index bc8dbca50..266215308 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { double lat = vehicle->lat / 1e7d; double lon = vehicle->lon / 1e7d; - float alt = vehicle->alt; + //float alt = vehicle->alt; return inside(lat, lon, vehicle->alt); } @@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude) } // skip vertex 0 (return point) - if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) && - (lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) / - (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) { + if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { c = !c; } @@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename) int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); + return OK; } -- cgit v1.2.3 From d78c3a224267f4dbd1fac72e893c81b83b43df9b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 17:17:41 +0200 Subject: navigator: new class structure, loiter and mission working --- src/modules/navigator/loiter.cpp | 3 +- src/modules/navigator/loiter.h | 19 ++- src/modules/navigator/mission.cpp | 194 +++------------------------- src/modules/navigator/mission.h | 46 +------ src/modules/navigator/mission_block.cpp | 215 +++++++++++++++++++++++++++++++ src/modules/navigator/mission_block.h | 105 +++++++++++++++ src/modules/navigator/module.mk | 2 + src/modules/navigator/navigator.h | 4 + src/modules/navigator/navigator_main.cpp | 34 +++-- src/modules/navigator/navigator_mode.cpp | 70 ++++++++++ src/modules/navigator/navigator_mode.h | 86 +++++++++++++ src/modules/navigator/rtl.cpp | 4 +- src/modules/navigator/rtl.h | 24 ++-- 13 files changed, 564 insertions(+), 242 deletions(-) create mode 100644 src/modules/navigator/mission_block.cpp create mode 100644 src/modules/navigator/mission_block.h create mode 100644 src/modules/navigator/navigator_mode.cpp create mode 100644 src/modules/navigator/navigator_mode.h (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 6da08f072..035d035e1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -53,7 +53,8 @@ #include "loiter.h" Loiter::Loiter(Navigator *navigator, const char *name) : - Mission(navigator, name) + NavigatorMode(navigator, name), + MissionBlock(navigator) { /* load initial params */ updateParams(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 4ae265e44..a83b53f43 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -44,11 +44,10 @@ #include #include -#include "mission.h" +#include "navigator_mode.h" +#include "mission_block.h" -class Navigator; - -class Loiter : public Mission +class Loiter : public NavigatorMode, MissionBlock { public: /** @@ -59,11 +58,17 @@ public: /** * Destructor */ - virtual ~Loiter(); + ~Loiter(); - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + /** + * This function is called while the mode is inactive + */ + bool update(struct position_setpoint_triplet_s *pos_sp_triplet); - virtual void reset(); + /** + * This function is called while the mode is active + */ + void reset(); }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 839c4c960..167f24ca6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -56,18 +56,15 @@ #include "navigator.h" #include "mission.h" - Mission::Mission(Navigator *navigator, const char *name) : - SuperBlock(NULL, name), - _navigator(navigator), - _first_run(true), + NavigatorMode(navigator, name), + MissionBlock(navigator), _param_onboard_enabled(this, "ONBOARD_EN"), _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), - _mission_item({0}), _mission_result_pub(-1), _mission_result({0}), _mission_type(MISSION_TYPE_NONE) @@ -92,14 +89,23 @@ Mission::reset() bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { + bool updated; + /* check if anything has changed */ - bool onboard_updated = is_onboard_mission_updated(); - bool offboard_updated = is_offboard_mission_updated(); + bool onboard_updated; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } - bool updated = false; + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } /* reset mission items if needed */ - if (onboard_updated || offboard_updated) { + if (onboard_updated || offboard_updated || _first_run) { set_mission_items(pos_sp_triplet); updated = true; _first_run = false; @@ -115,164 +121,9 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) return updated; } -bool -Mission::is_mission_item_reached() -{ - /* 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) { - - return false; - } -#endif - - hrt_abstime now = hrt_absolute_time(); - - if (!_waypoint_position_reached) { - - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - float altitude_amsl = _mission_item.altitude_is_relative - ? _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->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) { - - /* require only altitude for takeoff */ - if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { - _waypoint_position_reached = true; - } - } else { - if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { - _waypoint_position_reached = true; - } - } - } - - if (_waypoint_position_reached && !_waypoint_yaw_reached) { - - /* TODO: removed takeoff, why? */ - 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->get_global_position()->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 >= (hrt_abstime)_mission_item.time_inside * 1e6f) { - return true; - } - } - return false; -} - void -Mission::reset_mission_item_reached() -{ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; -} - -void -Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) -{ - sp->valid = true; - sp->lat = item->lat; - sp->lon = item->lon; - 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) { - sp->type = SETPOINT_TYPE_TAKEOFF; - - } else if (item->nav_cmd == NAV_CMD_LAND) { - sp->type = SETPOINT_TYPE_LAND; - - } 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) { - sp->type = SETPOINT_TYPE_LOITER; - - } else { - sp->type = SETPOINT_TYPE_POSITION; - } -} - -bool -Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::update_onboard_mission() { - if (_navigator->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 { - /* use current position */ - 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 */ - } - pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; - pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get(); - 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; - - _navigator->set_is_in_loiter(true); - return true; -} - - -bool -Mission::is_onboard_mission_updated() -{ - bool updated; - orb_check(_navigator->get_onboard_mission_sub(), &updated); - - if (!updated && !_first_run) { - return false; - } - if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ if (_onboard_mission.current_index >=0 @@ -293,18 +144,11 @@ Mission::is_onboard_mission_updated() _onboard_mission.current_index = 0; _current_onboard_mission_index = 0; } - return true; } -bool -Mission::is_offboard_mission_updated() +void +Mission::update_offboard_mission() { - bool updated; - orb_check(_navigator->get_offboard_mission_sub(), &updated); - - if (!updated && !_first_run) { - return false; - } if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { /* determine current index */ @@ -341,7 +185,6 @@ Mission::is_offboard_mission_updated() _offboard_mission.current_index = 0; _current_offboard_mission_index = 0; } - return true; } @@ -381,7 +224,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) _mission_type = MISSION_TYPE_NONE; bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; - reset_mission_item_reached(); set_loiter_item(use_current_pos_sp, pos_sp_triplet); } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4d0083d85..ad8cb467c 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -33,7 +33,7 @@ /** * @file mission.h * - * Helper class to access missions + * Navigator mode to access missions * * @author Julian Oes */ @@ -50,16 +50,19 @@ #include #include +#include #include #include #include #include +#include "navigator_mode.h" +#include "mission_block.h" #include "mission_feasibility_checker.h" class Navigator; -class Mission : public control::SuperBlock +class Mission : public NavigatorMode, MissionBlock { public: /** @@ -82,42 +85,16 @@ public: */ virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); -protected: - /** - * Check if mission item has been reached - * @return true if successfully reached - */ - bool is_mission_item_reached(); - /** - * Reset all reached flags - */ - void reset_mission_item_reached(); - - /** - * Convert a mission item to a position setpoint - */ - void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp); - - /** - * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position - * @return true if setpoint has changed - */ - bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet); - - class Navigator *_navigator; - private: /** * Update onboard mission topic - * @return true if onboard mission has been updated */ - bool is_onboard_mission_updated(); + void update_onboard_mission(); /** * Update offboard mission topic - * @return true if offboard mission has been updated */ - bool is_offboard_mission_updated(); + void update_offboard_mission(); /** * Move on to next mission item or switch to loiter @@ -179,12 +156,6 @@ private: */ void publish_mission_result(); - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - hrt_abstime _time_first_inside_orbit; - - bool _first_run; - control::BlockParamFloat _param_onboard_enabled; control::BlockParamFloat _param_loiter_radius; @@ -194,8 +165,6 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; - struct mission_item_s _mission_item; - orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; @@ -206,7 +175,6 @@ private: } _mission_type; MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ - }; #endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp new file mode 100644 index 000000000..92090d995 --- /dev/null +++ b/src/modules/navigator/mission_block.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mission_block.cpp + * + * Helper class to use mission items + * + * @author Julian Oes + */ + +#include +#include +#include +#include + +#include +#include + +#include + +#include "navigator.h" +#include "mission_block.h" + + +MissionBlock::MissionBlock(Navigator *navigator) : + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), + _mission_item({0}), + _mission_item_valid(false), + _navigator_priv(navigator) +{ +} + +MissionBlock::~MissionBlock() +{ +} + +bool +MissionBlock::is_mission_item_reached() +{ + /* 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) { + + return false; + } +#endif + + hrt_abstime now = hrt_absolute_time(); + + if (!_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + 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; + + 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, + &dist_xy, &dist_z); + + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } + } + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + /* TODO: removed takeoff, why? */ + if (_navigator_priv->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); + + 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 >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + return true; + } + } + return false; +} + +void +MissionBlock::reset_mission_item_reached() +{ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; +} + +void +MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) +{ + 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->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) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } 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) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_POSITION; + } +} + +bool +MissionBlock::set_loiter_item(bool reuse_current_pos_sp, 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 { + /* 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.yaw = NAN; /* NAN means to use current yaw */ + } + 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; + + _navigator_priv->set_is_in_loiter(true); + return true; +} + diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h new file mode 100644 index 000000000..47e6fe81f --- /dev/null +++ b/src/modules/navigator/mission_block.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file mission_block.h + * + * Helper class to use mission items + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MISSION_BLOCK_H +#define NAVIGATOR_MISSION_BLOCK_H + +#include + +#include +#include +#include + +class Navigator; + +class MissionBlock +{ +public: + /** + * Constructor + * + * @param pointer to parent class + */ + MissionBlock(Navigator *navigator); + + /** + * Destructor + */ + virtual ~MissionBlock(); + + /** + * Check if mission item has been reached + * @return true if successfully reached + */ + bool is_mission_item_reached(); + /** + * Reset all reached flags + */ + void reset_mission_item_reached(); + + /** + * Convert a mission item to a position setpoint + * + * @param the mission item to convert + * @param the position setpoint that needs to be set + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + + /** + * 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 _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; + + mission_item_s _mission_item; + bool _mission_item_valid; + +private: + Navigator *_navigator_priv; +}; + +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index f4243c5b8..c7cba64cc 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,8 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ + navigator_mode.cpp \ + mission_block.cpp \ mission.cpp \ mission_params.c \ loiter.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 0c551bb41..476e9a36c 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -48,6 +48,7 @@ #include #include +#include "navigator_mode.h" #include "mission.h" #include "loiter.h" #include "rtl.h" @@ -104,6 +105,8 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } + float get_loiter_radius() { return 50.0f; }; /* TODO: make param*/ + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -139,6 +142,7 @@ private: bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ + NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 44c1075c1..fe1f4893e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -118,6 +118,7 @@ Navigator::Navigator() : _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), + _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), @@ -321,32 +322,47 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: - _mission.reset(); - _loiter.reset(); - _rtl.reset(); + _navigation_mode = nullptr; _is_in_loiter = false; break; case NAVIGATION_STATE_AUTO_MISSION: - _update_triplet = _mission.update(&_pos_sp_triplet); + _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: - _update_triplet = _loiter.update(&_pos_sp_triplet); + _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTL_RC: case NAVIGATION_STATE_AUTO_RTL_DL: - _update_triplet = _rtl.update(&_pos_sp_triplet); + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: - _mission.reset(); - _loiter.reset(); - _rtl.reset(); + _navigation_mode = nullptr; _is_in_loiter = false; break; } + /* TODO: make list of modes and loop through it */ + if (_navigation_mode == &_mission) { + _update_triplet = _mission.update(&_pos_sp_triplet); + } else { + _mission.reset(); + } + + if (_navigation_mode == &_rtl) { + _update_triplet = _rtl.update(&_pos_sp_triplet); + } else { + _rtl.reset(); + } + + if (_navigation_mode == &_loiter) { + _update_triplet = _loiter.update(&_pos_sp_triplet); + } else { + _loiter.reset(); + } + if (_update_triplet ) { publish_position_setpoint_triplet(); _update_triplet = false; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp new file mode 100644 index 000000000..c96829190 --- /dev/null +++ b/src/modules/navigator/navigator_mode.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mode.cpp + * + * Helper class for different modes in navigator + * + * @author Julian Oes + */ + +#include "navigator_mode.h" + +NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : + SuperBlock(NULL, name), + _navigator(navigator), + _first_run(true) +{ + /* load initial params */ + updateParams(); + /* set initial mission items */ + reset(); +} + +NavigatorMode::~NavigatorMode() +{ +} + +void +NavigatorMode::reset() +{ + _first_run = true; +} + +bool +NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + pos_sp_triplet->current.valid = false; + _first_run = false; + return false; +} diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h new file mode 100644 index 000000000..0844bb94d --- /dev/null +++ b/src/modules/navigator/navigator_mode.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mode.h + * + * Helper class for different modes in navigator + * + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MODE_H +#define NAVIGATOR_MODE_H + +#include + +#include +#include + +#include + +#include + +class Navigator; + +class NavigatorMode : public control::SuperBlock +{ +public: + /** + * Constructor + */ + NavigatorMode(Navigator *navigator, const char *name); + + /** + * Destructor + */ + virtual ~NavigatorMode(); + + /** + * This function is called while the mode is inactive + */ + virtual void reset(); + + /** + * This function is called while the mode is active + * + * @param position setpoint triplet to set + * @return true if position setpoint triplet has been changed + */ + virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + +protected: + Navigator *_navigator; + bool _first_run; +}; + +#endif diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d4e609584..774aa802d 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -52,13 +52,13 @@ #include "rtl.h" RTL::RTL(Navigator *navigator, const char *name) : - Mission(navigator, name), + NavigatorMode(navigator, name), + MissionBlock(navigator), _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), _home_position({}), _loiter_radius(50), _acceptance_radius(50), - _param_loiter_rad(this, "LOITER_RAD"), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), _param_land_delay(this, "LAND_DELAY") diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index d84fd1a6f..9b32fbb0c 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -31,8 +31,9 @@ * ****************************************************************************/ /** - * @file navigator_mission.h - * Helper class to access missions + * @file navigator_rtl.h + * Helper class for RTL + * * @author Julian Oes * @author Anton Babushkin */ @@ -48,11 +49,12 @@ #include #include -#include "mission.h" +#include "navigator_mode.h" +#include "mission_block.h" class Navigator; -class RTL : public Mission +class RTL : public NavigatorMode, MissionBlock { public: /** @@ -63,11 +65,18 @@ public: /** * Destructor */ - virtual ~RTL(); + ~RTL(); + + /** + * This function is called while the mode is inactive + */ + void reset(); - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + /** + * This function is called while the mode is active + */ + bool update(struct position_setpoint_triplet_s *pos_sp_triplet); - virtual void reset(); private: void set_home_position(const home_position_s *home_position); @@ -92,7 +101,6 @@ private: float _loiter_radius; float _acceptance_radius; - control::BlockParamFloat _param_loiter_rad; control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; -- cgit v1.2.3 From 85d7fdc741a39184d251e2d35d914a6506d6ecd1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 18:13:45 +0200 Subject: navigator: param enhancements --- src/modules/navigator/mission.cpp | 3 +- src/modules/navigator/mission.h | 1 - src/modules/navigator/mission_params.c | 10 ------ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 15 +++++++-- src/modules/navigator/navigator_main.cpp | 26 ++++++++++++--- src/modules/navigator/navigator_params.c | 55 ++++++++++++++++++++++++++++++++ 7 files changed, 93 insertions(+), 18 deletions(-) create mode 100644 src/modules/navigator/navigator_params.c (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 167f24ca6..93007d939 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -60,7 +60,6 @@ Mission::Mission(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), _param_onboard_enabled(this, "ONBOARD_EN"), - _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -241,6 +240,8 @@ Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { + /* make sure param is up to date */ + updateParams(); if (_param_onboard_enabled.get() > 0 && _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ad8cb467c..cb3242c87 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -157,7 +157,6 @@ private: void publish_mission_result(); control::BlockParamFloat _param_onboard_enabled; - control::BlockParamFloat _param_loiter_radius; struct mission_s _onboard_mission; struct mission_s _offboard_mission; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index f5067a70b..8692328db 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -58,16 +58,6 @@ */ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); -/** - * Loiter radius after/during mission (FW only) - * - * Default value of loiter radius (fixedwing only). - * - * @unit meters - * @min 0.0 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f); /** * Enable onboard mission diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c7cba64cc..a1e109030 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ + navigator_params.c \ navigator_mode.cpp \ mission_block.cpp \ mission.cpp \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 476e9a36c..b22da7117 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -42,11 +42,15 @@ #include +#include +#include + #include #include #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -54,7 +58,7 @@ #include "rtl.h" #include "geofence.h" -class Navigator +class Navigator : public control::SuperBlock { public: /** @@ -105,7 +109,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } - float get_loiter_radius() { return 50.0f; }; /* TODO: make param*/ + float get_loiter_radius() { return _param_loiter_radius.get(); }; private: @@ -121,6 +125,7 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ + int _param_update_sub; /**< param update subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ @@ -150,6 +155,7 @@ private: bool _is_in_loiter; /**< 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 */ /** * Retrieve global position */ @@ -175,6 +181,11 @@ private: */ void vehicle_control_mode_update(); + /** + * Update parameters + */ + void params_update(); + /** * Shim for calling task_main from task_create. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fe1f4893e..b1658a6b8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -92,8 +92,7 @@ Navigator *g_navigator; } Navigator::Navigator() : - - /* state machine transition table */ + SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -122,8 +121,10 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _update_triplet(false) + _update_triplet(false), + _param_loiter_radius(this, "LOITER_RAD") { + updateParams(); } Navigator::~Navigator() @@ -188,6 +189,13 @@ Navigator::vehicle_control_mode_update() } } +void +Navigator::params_update() +{ + parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); +} + void Navigator::task_main_trampoline(int argc, char *argv[]) { @@ -226,6 +234,7 @@ Navigator::task_main() _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _param_update_sub - orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); @@ -233,6 +242,7 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); + params_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -241,7 +251,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[5]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -254,6 +264,8 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; + fds[5].fd = _param_update_sub; + fds[5].events = POLLIN; while (!_task_should_exit) { @@ -278,6 +290,12 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* parameters updated */ + if (fds[5].revents & POLLIN) { + params_update(); + updateParams(); + } + /* vehicle control mode updated */ if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c new file mode 100644 index 000000000..9235f3046 --- /dev/null +++ b/src/modules/navigator/navigator_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file navigator_params.c + * + * Parameters for navigator in general + * + * @author Julian Oes + */ + +#include + +#include + +/** + * Loiter radius (FW only) + * + * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); -- cgit v1.2.3 From b97b74491bfa7bddf8b7a80525f031e4c83fe5d4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 18:27:59 +0200 Subject: navigator: typo --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b1658a6b8..241d1d8a4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -234,7 +234,7 @@ Navigator::task_main() _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - _param_update_sub - orb_subscribe(ORB_ID(parameter_update)); + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); -- cgit v1.2.3 From 1bec9dfe2bab4200a2e2859d1efee0a1bb66e6d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 20:02:38 +0200 Subject: navigator: RTL working again --- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/rtl.cpp | 222 ++++++++++++++++++++----------------- src/modules/navigator/rtl.h | 25 +++-- src/modules/navigator/rtl_params.c | 12 ++ 5 files changed, 148 insertions(+), 117 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 93007d939..cd694be9a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -88,7 +88,7 @@ Mission::reset() bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - bool updated; + bool updated = false; /* check if anything has changed */ bool onboard_updated; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b22da7117..296294a91 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -108,8 +108,8 @@ public: int get_offboard_mission_sub() { return _offboard_mission_sub; } Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } - - float get_loiter_radius() { return _param_loiter_radius.get(); }; + float get_loiter_radius() { return _param_loiter_radius.get(); } + int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 774aa802d..c4c5f2aab 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -49,19 +49,17 @@ #include #include +#include "navigator.h" #include "rtl.h" RTL::RTL(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), - _mavlink_fd(-1), _rtl_state(RTL_STATE_NONE), - _home_position({}), - _loiter_radius(50), - _acceptance_radius(50), _param_return_alt(this, "RETURN_ALT"), _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _param_land_delay(this, "LAND_DELAY"), + _param_acceptance_radius(this, "ACCEPT_RAD") { /* load initial params */ updateParams(); @@ -73,40 +71,52 @@ RTL::~RTL() { } +void +RTL::reset() +{ + _first_run = true; + _rtl_state = RTL_STATE_NONE; +} + bool RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; - return updated; -} + if (_first_run) { + set_rtl_item(pos_sp_triplet); + updated = true; + _first_run = false; + } -void -RTL::reset() -{ + if ((_rtl_state == RTL_STATE_CLIMB + || _rtl_state == RTL_STATE_RETURN + || _rtl_state == RTL_STATE_DESCEND) + && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(pos_sp_triplet); + updated = true; + } + return updated; } void -RTL::set_home_position(const home_position_s *new_home_position) -{ - memcpy(&_home_position, new_home_position, sizeof(home_position_s)); -} - -bool -RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item) +RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) { - /* Open mavlink fd */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } + /* make sure we have the latest params */ + updateParams(); - /* decide if we need climb */ + /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { - if (global_position->alt < _home_position.alt + _param_return_alt.get()) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_FINISHED; + /* 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; } @@ -114,129 +124,137 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss /* if switching directly to return state, set altitude setpoint to current altitude */ if (_rtl_state == RTL_STATE_RETURN) { - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = global_position->alt; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; } switch (_rtl_state) { case RTL_STATE_CLIMB: { - float climb_alt = _home_position.alt + _param_return_alt.get(); + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); /* TODO understand and fix this */ // if (_vstatus.condition_landed) { // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); // } - new_mission_item->lat = global_position->lat; - new_mission_item->lon = global_position->lon; - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = climb_alt; - new_mission_item->yaw = NAN; - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = 0.0f; - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = true; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home", - (int)(climb_alt - _home_position.alt)); + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _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.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: { - new_mission_item->lat = _home_position.lat; - new_mission_item->lon = _home_position.lon; + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; /* 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 */ - // new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon); + // _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 */ - // new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon); + // _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); // } - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = 0.0f; - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = true; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home", - (int)(new_mission_item->altitude - _home_position.alt)); + _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.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: { - new_mission_item->lat = _home_position.lat; - new_mission_item->lon = _home_position.lon; - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = _home_position.alt + _param_descend_alt.get(); - new_mission_item->yaw = NAN; - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = _param_land_delay.get() > -0.001f; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home", - (int)(new_mission_item->altitude - _home_position.alt)); + _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 = 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.pitch_min = 0.0f; + _mission_item.autocontinue = _param_land_delay.get() > -0.001f; + _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: { - new_mission_item->lat = _home_position.lat; - new_mission_item->lon = _home_position.lon; - new_mission_item->altitude_is_relative = false; - new_mission_item->altitude = _home_position.alt; - new_mission_item->yaw = NAN; - new_mission_item->loiter_radius = _loiter_radius; - new_mission_item->loiter_direction = 1; - new_mission_item->nav_cmd = NAV_CMD_LAND; - new_mission_item->acceptance_radius = _acceptance_radius; - new_mission_item->time_inside = 0.0f; - new_mission_item->pitch_min = 0.0f; - new_mission_item->autocontinue = true; - new_mission_item->origin = ORIGIN_ONBOARD; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home"); + _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_LAND; + _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _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 */ - return false; } default: - return false; + break; } - return true; -} - -bool -RTL::get_next_rtl_item(mission_item_s *new_mission_item) -{ - /* TODO implement */ - return false; + 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; + } } void -RTL::move_to_next() +RTL::advance_rtl() { switch (_rtl_state) { case RTL_STATE_CLIMB: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9b32fbb0c..9836f5064 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -74,19 +74,23 @@ public: /** * This function is called while the mode is active + * + * @param position setpoint triplet that needs to be set + * @return true if updated */ - bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + bool update(position_setpoint_triplet_s *pos_sp_triplet); private: - void set_home_position(const home_position_s *home_position); - - bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); - bool get_next_rtl_item(mission_item_s *mission_item); - - void move_to_next(); + /** + * Set the RTL item + */ + void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); - int _mavlink_fd; + /** + * Move to next RTL item + */ + void advance_rtl(); enum RTLState { RTL_STATE_NONE = 0, @@ -97,13 +101,10 @@ private: RTL_STATE_FINISHED, } _rtl_state; - home_position_s _home_position; - float _loiter_radius; - float _acceptance_radius; - control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; + control::BlockParamFloat _param_acceptance_radius; }; #endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index bfe6ce7e1..63724f461 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -96,3 +96,15 @@ 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); -- cgit v1.2.3 From 94c4fc56aa229bcb909e5437804e7475adfbcdba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 23:08:11 +0200 Subject: navigator: audio messages about what is happening, RTL during mission not triggered but after mission --- src/modules/commander/commander.cpp | 17 ++++++++++- src/modules/navigator/mission.cpp | 60 +++++++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 5 ++++ src/modules/navigator/rtl.cpp | 6 +--- 4 files changed, 70 insertions(+), 18 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d7a95b0d6..bb75b2af0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1296,7 +1296,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ @@ -1306,10 +1305,26 @@ int commander_thread_main(int argc, char *argv[]) status.failsafe_state = FAILSAFE_STATE_LAND; } failsafe_state_changed = true; + } else { + mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); } } } + /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ + if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; + mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); + } else { + /* this probably doesn't make sense since we are in mission and have global position */ + status.failsafe_state = FAILSAFE_STATE_LAND; + } + failsafe_state_changed = true; + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cd694be9a..db606e6fa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -46,6 +46,7 @@ #include #include +#include #include #include @@ -210,20 +211,39 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); + /* try setting onboard mission item */ if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting onboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_ONBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: onboard mission running"); + } _mission_type = MISSION_TYPE_ONBOARD; _navigator->set_is_in_loiter(false); + /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting offboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_OFFBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: offboard mission running"); + } _mission_type = MISSION_TYPE_OFFBOARD; _navigator->set_is_in_loiter(false); } else { + if (_mission_type != MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: mission finished"); + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: no mission available"); + } _mission_type = MISSION_TYPE_NONE; bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; set_loiter_item(use_current_pos_sp, pos_sp_triplet); + reset_mission_item_reached(); + report_mission_finished(); } } @@ -242,8 +262,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current { /* make sure param is up to date */ updateParams(); - if (_param_onboard_enabled.get() > 0 - && _current_onboard_mission_index < (int)_onboard_mission.count) { + if (_param_onboard_enabled.get() > 0 && + _current_onboard_mission_index >= 0&& + _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, &new_mission_item)) { @@ -264,7 +285,8 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - if (_current_offboard_mission_index < (int)_offboard_mission.count) { + if (_current_offboard_mission_index >= 0 && + _current_offboard_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -282,8 +304,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren report_current_offboard_mission_item(); memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; - } else { - warnx("ERROR: WP read fail"); } } return false; @@ -295,7 +315,8 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) int next_temp_mission_index = _onboard_mission.current_index + 1; /* try if there is a next onboard mission */ - if (next_temp_mission_index < (int)_onboard_mission.count) { + if (_onboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ @@ -315,7 +336,9 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ int next_temp_mission_index = _offboard_mission.current_index + 1; - if (next_temp_mission_index < (int)_offboard_mission.count) { + warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); + if (_offboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -346,14 +369,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR waypoint could not be read"); return false; } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* TODO: do this check more gracefully since it is not a serious error */ if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: DO JUMP repetitions completed"); return false; } @@ -366,9 +392,10 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); return false; } - /* TODO: report about DO JUMP count */ } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -381,7 +408,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP is cycling, giving up"); return false; } @@ -402,6 +430,13 @@ Mission::report_current_offboard_mission_item() publish_mission_result(); } +void +Mission::report_mission_finished() +{ + _mission_result.mission_finished = true; + publish_mission_result(); +} + void Mission::publish_mission_result() { @@ -416,4 +451,5 @@ Mission::publish_mission_result() } /* reset reached bool */ _mission_result.mission_reached = false; + _mission_result.mission_finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index cb3242c87..a3dd09ecd 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -151,6 +151,11 @@ private: */ void report_current_offboard_mission_item(); + /** + * Report that the mission is finished if one exists or that none exists + */ + void report_mission_finished(); + /** * Publish the mission result so commander and mavlink know what is going on */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c4c5f2aab..dde35d5b6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -112,6 +112,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* 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()) { @@ -133,11 +134,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - /* TODO understand and fix this */ - // if (_vstatus.condition_landed) { - // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - // } - _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; -- cgit v1.2.3 From 13b6dffb2e719219ffccccd6681d36e695165aa5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 7 Jun 2014 17:18:58 +0200 Subject: navigator: don't reset descend WP in RTL --- src/modules/navigator/rtl.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index dde35d5b6..8888c6b62 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -90,8 +90,7 @@ RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) } if ((_rtl_state == RTL_STATE_CLIMB - || _rtl_state == RTL_STATE_RETURN - || _rtl_state == RTL_STATE_DESCEND) + || _rtl_state == RTL_STATE_RETURN) && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); -- cgit v1.2.3 From d3f182d433551c72a2fd8105481938129b8332ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 9 Jun 2014 11:37:33 +0200 Subject: navigator: don't check reached for land waypoints --- src/modules/navigator/mission_block.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 92090d995..861aed813 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -69,6 +69,10 @@ MissionBlock::~MissionBlock() bool MissionBlock::is_mission_item_reached() { + /* don't check landed WPs */ + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + return false; + } /* TODO: count turns */ #if 0 if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || -- cgit v1.2.3 From fab1b4e3669ce083c124b2ef04b38e7bdea663ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 14:25:23 +0200 Subject: navigator: don't say triplet is valid in ALTCTL --- src/modules/navigator/navigator_main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 241d1d8a4..da06f826a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -381,6 +381,14 @@ Navigator::task_main() _loiter.reset(); } + /* if nothing is running, set position setpoint triplet invalid */ + if (_navigation_mode == nullptr) { + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _update_triplet = true; + } + if (_update_triplet ) { publish_position_setpoint_triplet(); _update_triplet = false; -- cgit v1.2.3 From fd1f1c81efb384e46a5767555a58061082612c9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 17:24:04 +0200 Subject: navigator: added parameter for acceptance radius for take-off mission items --- src/modules/navigator/mission_block.cpp | 14 ++++++++++---- src/modules/navigator/navigator.h | 2 ++ src/modules/navigator/navigator_main.cpp | 3 ++- src/modules/navigator/navigator_params.c | 11 +++++++++++ 4 files changed, 25 insertions(+), 5 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 861aed813..08576750c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -102,13 +102,19 @@ MissionBlock::is_mission_item_reached() _navigator_priv->get_global_position()->alt, &dist_xy, &dist_z); - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - - /* require only altitude for takeoff */ - if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->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()) { + _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()) { _waypoint_position_reached = true; } } else { + /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { _waypoint_position_reached = true; } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 296294a91..fe7485f56 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -109,6 +109,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } float get_loiter_radius() { return _param_loiter_radius.get(); } + float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); } int get_mavlink_fd() { return _mavlink_fd; } private: @@ -156,6 +157,7 @@ private: 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 */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index da06f826a..dc8573813 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -122,7 +122,8 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _update_triplet(false), - _param_loiter_radius(this, "LOITER_RAD") + _param_loiter_radius(this, "LOITER_RAD"), + _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { updateParams(); } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9235f3046..ca31adecc 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,3 +53,14 @@ * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * Takeoff Acceptance Radius (FW only) + * + * Acceptance radius for fixedwing. + * + * @unit meters + * @min 1.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f); -- cgit v1.2.3 From 6f24afd68a840698a9a77b46bc8f1ca81cc1f5b9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 17:33:12 +0200 Subject: navigator: always listen to new current mission index and to new missions --- src/modules/navigator/mission.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index db606e6fa..33a1399b1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -84,6 +84,19 @@ void Mission::reset() { _first_run = true; + + /* check anyway if missions have changed so that feedback to groundstation is given */ + bool onboard_updated; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } + + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } } bool @@ -185,6 +198,7 @@ Mission::update_offboard_mission() _offboard_mission.current_index = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } -- cgit v1.2.3 From d9a64bb58720300417f190b8a8b610ab2966a11f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Jun 2014 23:16:22 +0200 Subject: navigator: don't give up after DO_JUMPS --- src/modules/navigator/mission.cpp | 43 +++++++++++++++++++++------------------ 1 file changed, 23 insertions(+), 20 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 33a1399b1..a551df9a2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -391,29 +391,32 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + /* do DO_JUMP as many times as requested */ + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the + * dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: DO JUMP repetitions completed"); - return false; - } - - /* only raise the repeat count if this is for the current mission item - * but not for the next mission item */ - if (is_current) { - (new_mission_item->do_jump_current_count)++; - - /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, - new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the dataman */ - mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); - return false; - } + /* no more DO_JUMPS, therefore just try to continue with next mission item */ + (*mission_index)++; } - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; } else { /* if it's not a DO_JUMP, then we were successful */ -- cgit v1.2.3 From d48a8bc073a3aa5f515c582a1c2c3cae58a8d783 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:09:18 +0200 Subject: navigator: renamed the different RTL states --- src/modules/commander/commander.cpp | 10 +++---- src/modules/commander/state_machine_helper.cpp | 8 +++--- src/modules/mavlink/mavlink_messages.cpp | 4 +-- src/modules/navigator/navigator_main.cpp | 4 +-- src/modules/uORB/topics/vehicle_status.h | 37 +++++++++++++------------- 5 files changed, 32 insertions(+), 31 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb75b2af0..e992706ac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1300,7 +1300,7 @@ int commander_thread_main(int argc, char *argv[]) /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; } else { status.failsafe_state = FAILSAFE_STATE_LAND; } @@ -1313,10 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); } else { /* this probably doesn't make sense since we are in mission and have global position */ @@ -1719,8 +1719,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 214480079..c52e618ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -406,12 +406,12 @@ void set_nav_state(struct vehicle_status_s *status) } break; - case FAILSAFE_STATE_RTL_RC: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + case FAILSAFE_STATE_RC_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; break; - case FAILSAFE_STATE_RTL_DL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + case FAILSAFE_STATE_DL_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; break; case FAILSAFE_STATE_LAND: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e7d2ba515..b58fb4cb8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -161,8 +161,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dc8573813..20cff5c28 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,8 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 259d7329e..9390ff717 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes + * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +41,11 @@ * All apps should write to subsystem_info: * * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + * + * @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes */ #ifndef VEHICLE_STATUS_H_ @@ -90,25 +91,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_MANUAL = 0, - NAVIGATION_STATE_ACRO, - NAVIGATION_STATE_ALTCTL, - NAVIGATION_STATE_POSCTL, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_RTL_RC, - NAVIGATION_STATE_AUTO_RTL_DL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TERMINATION, + NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ + NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ + NAVIGATION_STATE_POSCTL, /**< Position control mode */ + NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ + NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ + NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; enum VEHICLE_MODE_FLAG { -- cgit v1.2.3 From 3b39a8a789e347e318375ebe18c583eabec0501c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:37:25 +0200 Subject: navigator: rename update and reset calls to on_active and on_inactive --- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/mission.h | 4 ++-- src/modules/navigator/navigator_main.cpp | 12 ++++++------ src/modules/navigator/navigator_mode.cpp | 6 +++--- src/modules/navigator/navigator_mode.h | 4 ++-- src/modules/navigator/rtl.cpp | 6 +++--- src/modules/navigator/rtl.h | 4 ++-- 7 files changed, 21 insertions(+), 21 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a551df9a2..9244063b1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -72,7 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* set initial mission items */ - reset(); + on_inactive(); } @@ -81,7 +81,7 @@ Mission::~Mission() } void -Mission::reset() +Mission::on_inactive() { _first_run = true; @@ -100,7 +100,7 @@ Mission::reset() } bool -Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) +Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a3dd09ecd..322aaf96a 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -78,12 +78,12 @@ public: /** * This function is called while the mode is inactive */ - virtual void reset(); + virtual void on_inactive(); /** * This function is called while the mode is active */ - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); private: /** diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 20cff5c28..a3c190c7f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -365,21 +365,21 @@ Navigator::task_main() /* TODO: make list of modes and loop through it */ if (_navigation_mode == &_mission) { - _update_triplet = _mission.update(&_pos_sp_triplet); + _update_triplet = _mission.on_active(&_pos_sp_triplet); } else { - _mission.reset(); + _mission.on_inactive(); } if (_navigation_mode == &_rtl) { - _update_triplet = _rtl.update(&_pos_sp_triplet); + _update_triplet = _rtl.on_active(&_pos_sp_triplet); } else { - _rtl.reset(); + _rtl.on_inactive(); } if (_navigation_mode == &_loiter) { - _update_triplet = _loiter.update(&_pos_sp_triplet); + _update_triplet = _loiter.on_active(&_pos_sp_triplet); } else { - _loiter.reset(); + _loiter.on_inactive(); } /* if nothing is running, set position setpoint triplet invalid */ diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index c96829190..25e767c2b 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -48,7 +48,7 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* set initial mission items */ - reset(); + on_inactive(); } NavigatorMode::~NavigatorMode() @@ -56,13 +56,13 @@ NavigatorMode::~NavigatorMode() } void -NavigatorMode::reset() +NavigatorMode::on_inactive() { _first_run = true; } bool -NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet) +NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { pos_sp_triplet->current.valid = false; _first_run = false; diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 0844bb94d..cbb53d91b 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -68,7 +68,7 @@ public: /** * This function is called while the mode is inactive */ - virtual void reset(); + virtual void on_inactive(); /** * This function is called while the mode is active @@ -76,7 +76,7 @@ public: * @param position setpoint triplet to set * @return true if position setpoint triplet has been changed */ - virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); protected: Navigator *_navigator; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8888c6b62..c1b1d3f09 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -64,7 +64,7 @@ RTL::RTL(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* initial reset */ - reset(); + on_inactive(); } RTL::~RTL() @@ -72,14 +72,14 @@ RTL::~RTL() } void -RTL::reset() +RTL::on_inactive() { _first_run = true; _rtl_state = RTL_STATE_NONE; } bool -RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet) +RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) { bool updated = false; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9836f5064..bcccf7454 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -70,7 +70,7 @@ public: /** * This function is called while the mode is inactive */ - void reset(); + void on_inactive(); /** * This function is called while the mode is active @@ -78,7 +78,7 @@ public: * @param position setpoint triplet that needs to be set * @return true if updated */ - bool update(position_setpoint_triplet_s *pos_sp_triplet); + bool on_active(position_setpoint_triplet_s *pos_sp_triplet); private: -- cgit v1.2.3 From 9bb8b12f43de3aa4e7f24c516e7e8e7e9e6c196d Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 12 Jun 2014 15:50:06 -0700 Subject: Using a vector to store navigation modes --- src/modules/navigator/navigator.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 29 ++++++++++++----------------- 2 files changed, 16 insertions(+), 17 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fe7485f56..dfa939c38 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -40,6 +40,8 @@ #ifndef NAVIGATOR_H #define NAVIGATOR_H +#include + #include #include @@ -153,6 +155,8 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + std::vector _navigation_mode_vector; + bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..ac32d4b22 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -125,6 +125,11 @@ Navigator::Navigator() : _param_loiter_radius(this, "LOITER_RAD"), _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { + /* Create a list of our possible navigation types */ + _navigation_mode_vector.push_back(&_mission); + _navigation_mode_vector.push_back(&_loiter); + _navigation_mode_vector.push_back(&_rtl); + updateParams(); } @@ -363,23 +368,13 @@ Navigator::task_main() break; } - /* TODO: make list of modes and loop through it */ - if (_navigation_mode == &_mission) { - _update_triplet = _mission.on_active(&_pos_sp_triplet); - } else { - _mission.on_inactive(); - } - - if (_navigation_mode == &_rtl) { - _update_triplet = _rtl.on_active(&_pos_sp_triplet); - } else { - _rtl.on_inactive(); - } - - if (_navigation_mode == &_loiter) { - _update_triplet = _loiter.on_active(&_pos_sp_triplet); - } else { - _loiter.on_inactive(); + /* iterate through navigation modes and set active/inactive for each */ + for(unsigned int i = 0; i < _navigation_mode_vector.size(); i++) { + if (_navigation_mode == _navigation_mode_vector[i]) { + _update_triplet = _navigation_mode_vector[i]->on_active(&_pos_sp_triplet); + } else { + _navigation_mode_vector[i]->on_inactive(); + } } /* if nothing is running, set position setpoint triplet invalid */ -- cgit v1.2.3 From e53c2ab98504cf398a48f5051383796a1ad4b85e Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 12 Jun 2014 16:22:04 -0700 Subject: Switched to using c-type arrays --- src/modules/navigator/navigator.h | 6 +++--- src/modules/navigator/navigator_main.cpp | 14 +++++++------- 2 files changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dfa939c38..929de0259 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -40,8 +40,6 @@ #ifndef NAVIGATOR_H #define NAVIGATOR_H -#include - #include #include @@ -60,6 +58,8 @@ #include "rtl.h" #include "geofence.h" +#define NAVIGATOR_MODE_ARRAY_SIZE 3 + class Navigator : public control::SuperBlock { public: @@ -155,7 +155,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ - std::vector _navigation_mode_vector; + 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 _update_triplet; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ac32d4b22..b91af8e4c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -126,9 +126,9 @@ Navigator::Navigator() : _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { /* Create a list of our possible navigation types */ - _navigation_mode_vector.push_back(&_mission); - _navigation_mode_vector.push_back(&_loiter); - _navigation_mode_vector.push_back(&_rtl); + _navigation_mode_array[0] = &_mission; + _navigation_mode_array[1] = &_loiter; + _navigation_mode_array[2] = &_rtl; updateParams(); } @@ -369,11 +369,11 @@ Navigator::task_main() } /* iterate through navigation modes and set active/inactive for each */ - for(unsigned int i = 0; i < _navigation_mode_vector.size(); i++) { - if (_navigation_mode == _navigation_mode_vector[i]) { - _update_triplet = _navigation_mode_vector[i]->on_active(&_pos_sp_triplet); + for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + if (_navigation_mode == _navigation_mode_array[i]) { + _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); } else { - _navigation_mode_vector[i]->on_inactive(); + _navigation_mode_array[i]->on_inactive(); } } -- cgit v1.2.3 From 51f05612ebf056a46d0838fed0dc562d3e5289ad Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 12 Jun 2014 16:33:15 -0700 Subject: Added some comments --- src/modules/navigator/navigator.h | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 929de0259..dadd15527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -58,6 +58,10 @@ #include "rtl.h" #include "geofence.h" +/** + * Number of navigation modes that need on_active/on_inactive calls + * Currently: mission, loiter, and rtl + */ #define NAVIGATOR_MODE_ARRAY_SIZE 3 class Navigator : public control::SuperBlock -- cgit v1.2.3 From e14d4751423e21a9ce052e1f6760f929d3694515 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Jun 2014 14:31:19 +0200 Subject: navigator: update Loiter navigator mode to use new method names --- src/modules/navigator/loiter.cpp | 6 +++--- src/modules/navigator/loiter.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 035d035e1..8bbb79d5e 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -59,7 +59,7 @@ Loiter::Loiter(Navigator *navigator, const char *name) : /* load initial params */ updateParams(); /* initial reset */ - reset(); + on_inactive(); } Loiter::~Loiter() @@ -67,14 +67,14 @@ Loiter::~Loiter() } bool -Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet) +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);; } void -Loiter::reset() +Loiter::on_inactive() { } diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index a83b53f43..685ae6141 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -63,12 +63,12 @@ public: /** * This function is called while the mode is inactive */ - bool update(struct position_setpoint_triplet_s *pos_sp_triplet); + virtual void on_inactive(); /** * This function is called while the mode is active */ - void reset(); + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); }; #endif -- cgit v1.2.3 From e0ed0625f81841194b4bd9b26c7e047a1f68a1fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Jun 2014 17:34:21 +0200 Subject: commander: failsafe_state removed, replaced with bool failsafe, navigation state and failsafe determined directly from main state and conditions --- src/modules/commander/commander.cpp | 90 +++++++++----------- src/modules/commander/state_machine_helper.cpp | 112 +++++++++++++++---------- src/modules/commander/state_machine_helper.h | 2 - src/modules/gpio_led/gpio_led.c | 2 +- src/modules/mavlink/mavlink_messages.cpp | 4 +- src/modules/mavlink/mavlink_receiver.h | 2 + src/modules/navigator/navigator_main.cpp | 4 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 25 +++--- 9 files changed, 125 insertions(+), 118 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..43683f833 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 +#define DL_TIMEOUT 5 * 1000* 1000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -495,12 +497,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -639,13 +641,6 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[5] = "REBOOT"; arming_states_str[6] = "IN_AIR_RESTORE"; - char *failsafe_states_str[FAILSAFE_STATE_MAX]; - failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL_RC"; - failsafe_states_str[2] = "RTL_DL"; - failsafe_states_str[3] = "LAND"; - failsafe_states_str[4] = "TERMINATION"; - /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -666,10 +661,10 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_MANUAL; + status.nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; - status.failsafe_state = FAILSAFE_STATE_NORMAL; + status.failsafe = false; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -678,6 +673,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status.offboard_control_signal_lost = true; + status.data_link_lost = true; /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -771,6 +767,11 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to telemetry status */ + int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -908,6 +909,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + orb_check(telemetry_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -1186,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); } - /* start RC input check */ + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -1198,9 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - status.failsafe_state = FAILSAFE_STATE_NORMAL; - failsafe_state_changed = true; } } @@ -1271,12 +1275,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - - // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - // /* recover from failsafe */ - // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - // } - /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1295,34 +1293,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; - } - failsafe_state_changed = true; - } else { - mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); - } } } - /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ - if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); - } else { - /* this probably doesn't make sense since we are in mission and have global position */ - status.failsafe_state = FAILSAFE_STATE_LAND; + /* data link check */ + if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = false; + status_changed = true; } - failsafe_state_changed = true; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1377,7 +1365,7 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; - /* now set nav state according to failsafe and main state */ + /* now set navigation state according to failsafe and main state */ set_nav_state(&status); if (main_state_changed) { @@ -1388,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[]) if (failsafe_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); failsafe_state_changed = false; } @@ -1415,7 +1403,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1519,7 +1507,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ @@ -1667,7 +1655,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - switch (status.set_nav_state) { + switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1719,8 +1707,6 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c52e618ef..df718ff99 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } } - if (ret = TRANSITION_CHANGED) { + if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here @@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, return ret; } - /** * Check failsafe and main status and set navigation status for navigator accordingly */ void set_nav_state(struct vehicle_status_s *status) { - switch (status->failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case MAIN_STATE_MANUAL: - status->set_nav_state = NAVIGATION_STATE_MANUAL; - break; - - case MAIN_STATE_ALTCTL: - status->set_nav_state = NAVIGATION_STATE_ALTCTL; - break; + status->failsafe = false; - case MAIN_STATE_POSCTL: - status->set_nav_state = NAVIGATION_STATE_POSCTL; - break; - - case MAIN_STATE_AUTO_MISSION: - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; - break; - - case MAIN_STATE_AUTO_LOITER: - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; - break; + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_ACRO: + case MAIN_STATE_MANUAL: + case MAIN_STATE_ALTCTL: + case MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if (status->rc_signal_lost) { + status->failsafe = true; - case MAIN_STATE_AUTO_RTL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; - break; + } else { + switch (status->main_state) { + case MAIN_STATE_ACRO: + status->nav_state = NAVIGATION_STATE_ACRO; + break; + + case MAIN_STATE_MANUAL: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + + case MAIN_STATE_ALTCTL: + status->nav_state = NAVIGATION_STATE_ALTCTL; + break; + + case MAIN_STATE_POSCTL: + status->nav_state = NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + } + } + break; - case MAIN_STATE_ACRO: - status->set_nav_state = NAVIGATION_STATE_ACRO; - break; + case MAIN_STATE_AUTO_MISSION: + /* require data link and global position */ + if (status->data_link_lost || !status->condition_global_position_valid) { + status->failsafe = true; - default: - break; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; - case FAILSAFE_STATE_RC_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; - break; + case MAIN_STATE_AUTO_LOITER: + /* require data link and local position */ + if (status->data_link_lost || !status->condition_local_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_DL_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } break; - case FAILSAFE_STATE_LAND: - status->set_nav_state = NAVIGATION_STATE_LAND; - break; + case MAIN_STATE_AUTO_RTL: + /* require global position and home */ + if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_TERMINATION: - status->set_nav_state = NAVIGATION_STATE_TERMINATION; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } break; default: break; } + + if (status->failsafe) { + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 594bf23a1..062271764 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,8 +63,6 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); - transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); void set_nav_state(struct vehicle_status_s *status); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 6dfd22fdf..839a7890b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg) pattern = 0x2A; // *_*_*_ fast blink (armed, error) } else if (priv->status.arming_state == ARMING_STATE_ARMED) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) { pattern = 0x3f; // ****** solid (armed) } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b58fb4cb8..d7f56b63d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,7 +118,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->set_nav_state) { + switch (status->nav_state) { case NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED @@ -161,8 +161,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..ab3dc81c6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -138,6 +139,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + hrt_abstime _telemetry_heartbeat_time; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..70da5393f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -336,7 +336,7 @@ Navigator::task_main() } /* Do stuff according to navigation state set by commander */ - switch (_vstatus.set_nav_state) { + switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: @@ -351,8 +351,6 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 53cd6a797..19872bbd0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9390ff717..898c176fb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -59,7 +59,9 @@ * @addtogroup topics @{ */ -/* main state machine */ +/** + * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. + */ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, @@ -89,15 +91,9 @@ typedef enum { HIL_STATE_ON } hil_state_t; -typedef enum { - FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ - FAILSAFE_STATE_LAND, /**< Land as safe as possible */ - FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX, -} failsafe_state_t; - +/** + * Navigation state, i.e. "what should vehicle do". + */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ NAVIGATION_STATE_ACRO, /**< Acro mode */ @@ -106,9 +102,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; @@ -171,10 +166,10 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ + navigation_state_t nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ - failsafe_state_t failsafe_state; /**< current failsafe state */ + bool failsafe; /**< true if system is in failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -199,6 +194,8 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ + bool data_link_lost; /**< datalink to GCS lost */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; -- cgit v1.2.3 From e24925c743330bc3c6c0a24ba913a9c5fab5e07d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Jun 2014 19:01:41 +0200 Subject: commander: added some failsafe logic --- src/modules/commander/commander.cpp | 39 +++++--- src/modules/commander/commander_params.c | 13 ++- src/modules/commander/state_machine_helper.cpp | 132 ++++++++++++++++++------- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/navigator_main.cpp | 4 + src/modules/uORB/topics/vehicle_status.h | 2 + 6 files changed, 142 insertions(+), 50 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f8dcec8cb..5954dcbb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,6 +619,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); + param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); /* welcome user */ warnx("starting"); @@ -627,9 +628,9 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -637,21 +638,23 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -853,6 +856,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; + int32_t datalink_loss_enabled = false; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); + param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } orb_check(sp_man_sub, &updated); @@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where RC signal was regained */ + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ if (status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: data link regained"); status.data_link_lost = false; @@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = false; + status.data_link_lost = true; status_changed = true; } } @@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ - bool nav_state_changed = set_nav_state(&status); + bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, + mission_result.mission_finished); // TODO handle mode changes by commands if (main_state_changed) { @@ -1727,6 +1734,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 80ca68f21..4750f9d5c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -39,7 +39,7 @@ * * @author Lorenz Meier * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes */ #include @@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * Datalink loss mode enabled. + * + * Set to 1 to enable actions triggered when the datalink is lost. + * + * @group commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c0d730949..f0fe13921 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -366,7 +366,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) { navigation_state_t nav_state_old = status->nav_state; @@ -383,6 +383,16 @@ bool set_nav_state(struct vehicle_status_s *status) if (status->rc_signal_lost && armed) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { switch (status->main_state) { case MAIN_STATE_ACRO: @@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status) break; case MAIN_STATE_AUTO_MISSION: - /* require data link and global position */ - if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { + /* go into failsafe + * - if either the datalink is enabled and lost as well as RC is lost + * - if there is no datalink and the mission is finished */ + if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + + /* don't bother if RC is lost and mission is not yet finished */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for missions */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; case MAIN_STATE_AUTO_LOITER: - /* require data link and local position */ - if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { + /* go into failsafe if datalink and RC is lost */ + if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } else { - // TODO which mode should we set when disarmed? + /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } break; case MAIN_STATE_AUTO_RTL: /* require global position and home */ - if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) { + if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } break; @@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status) break; } - if (status->failsafe) { - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - } - return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index cffbc9b8f..2e076cdae 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 70da5393f..50bcfd58d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,12 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + _navigation_mode = &_rtl; /* TODO: change this to something else */ + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 851938795..16652fddf 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -101,6 +101,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ + NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From 62faa2ee5155253779d3b93c5a280e8918585f6e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Jun 2014 10:26:56 +0200 Subject: commander/navigator: renamed FS modes to RTL and RTGS (return to ground station) --- src/modules/commander/commander.cpp | 6 ++---- src/modules/commander/px4_custom_mode.h | 1 + src/modules/commander/state_machine_helper.cpp | 12 ++++++------ src/modules/mavlink/mavlink_messages.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 3 +-- src/modules/uORB/topics/vehicle_status.h | 5 ++--- 6 files changed, 20 insertions(+), 15 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5954dcbb1..b957ad9b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -649,8 +649,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -1734,8 +1733,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e0f8dc95d..7f5f93801 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, + PX4_CUSTOM_SUB_MODE_AUTO_RTGS }; union px4_custom_mode { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f0fe13921..74885abf6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,7 +384,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -427,7 +427,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -441,7 +441,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -467,7 +467,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -481,7 +481,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -495,7 +495,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d7f56b63d..1fd8b1a27 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -176,6 +176,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; + case NAVIGATION_STATE_AUTO_RTGS: + *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.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; + case NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 50bcfd58d..661d3d744 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,10 +351,9 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 16652fddf..5dc46dacb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -100,9 +100,8 @@ typedef enum { NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ - NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ - NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From 9d3d5a30af0781b975d17c5d73796f593c78b6e4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Jun 2014 17:13:47 +0200 Subject: navigator: move set_previous_pos_sp to MissionBlock class --- src/modules/navigator/mission.cpp | 12 +----------- src/modules/navigator/mission.h | 6 ------ src/modules/navigator/mission_block.cpp | 9 +++++++++ src/modules/navigator/mission_block.h | 5 +++++ 4 files changed, 15 insertions(+), 17 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b1..becaa09e8 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -223,7 +223,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)) { @@ -261,16 +261,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) } } -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) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 322aaf96a..842c8c9ee 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -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 diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 08576750c..a8eec92ca 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -194,6 +194,15 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite } } +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) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 47e6fe81f..a740deea4 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -82,6 +82,11 @@ 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 * -- cgit v1.2.3 From 39454ca99d984698983404b2c82bbd61f93be3fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Jun 2014 17:56:30 +0200 Subject: navigator: RTL return altitude fixed --- src/modules/navigator/rtl.cpp | 63 ++++++++++++++++++++++--------------------- 1 file changed, 33 insertions(+), 30 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c1b1d3f09..46194ed52 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -112,21 +113,29 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) 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 { + /* 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; } } - /* 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; - } + if (_rtl_state == RTL_STATE_FINISHED) { + /* RTL finished, nothing to do */ + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + return; + } + + set_previous_pos_setpoint(pos_sp_triplet); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -157,17 +166,20 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.lat = _navigator->get_home_position()->lat; _mission_item.lon = _navigator->get_home_position()->lon; - - /* 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); - - // } 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); - // } + // 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); + + } 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; @@ -228,24 +240,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } - case RTL_STATE_FINISHED: { - /* nothing to do, report fail */ - } - 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 -- cgit v1.2.3 From c5a5604ae975294347f1571258a633c11ef61261 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 00:17:25 +0200 Subject: navigator: loiter fixes --- src/modules/navigator/loiter.cpp | 3 +-- src/modules/navigator/mission.cpp | 8 +++---- src/modules/navigator/mission_block.cpp | 40 ++++++++++++++++++-------------- src/modules/navigator/mission_block.h | 3 +-- src/modules/navigator/navigator.h | 6 ++--- src/modules/navigator/navigator_main.cpp | 4 ++-- src/modules/navigator/rtl.cpp | 9 +------ 7 files changed, 34 insertions(+), 39 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 8bbb79d5e..f5eb1e7a5 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -70,11 +70,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() { } - diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index becaa09e8..c09f61744 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -233,7 +233,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 +243,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,9 +253,9 @@ 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(); } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a8eec92ca..41d81ad9b 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -204,31 +204,35 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_ } 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_priv->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.yaw = NAN; /* NAN means to use current yaw */ + + _navigator_priv->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_priv->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_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; + return true; + } - _navigator_priv->set_is_in_loiter(true); - return true; + return false; } - diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index a740deea4..8726964fa 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -90,11 +90,10 @@ public: /** * 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; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dadd15527..4709f7196 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -102,7 +102,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 @@ -113,7 +113,7 @@ public: int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_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(); } int get_mavlink_fd() { return _mavlink_fd; } @@ -161,7 +161,7 @@ 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 */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d762b8a9d..f39300590 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -347,7 +347,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; @@ -365,7 +365,7 @@ Navigator::task_main() case NAVIGATION_STATE_TERMINATION: default: _navigation_mode = nullptr; - _is_in_loiter = false; + _can_loiter_at_sp = false; break; } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 46194ed52..411f8c527 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -136,6 +136,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) } set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -156,8 +157,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _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; @@ -189,8 +188,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _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; @@ -211,8 +208,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _mission_item.autocontinue = _param_land_delay.get() > -0.001f; _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; @@ -234,8 +229,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _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; } -- cgit v1.2.3 From 63e14c73bab8f5b14b5259ba249f84a8edb0ee05 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 12:18:19 +0200 Subject: navigator: don't reset RTL state on loiter --- src/modules/navigator/rtl.cpp | 58 +++++++++++++++++++++++-------------------- 1 file changed, 31 insertions(+), 27 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 411f8c527..a2eec8589 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -76,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 @@ -85,14 +89,35 @@ 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_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 { + /* 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()) { + if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) { advance_rtl(); set_rtl_item(pos_sp_triplet); updated = true; @@ -107,31 +132,10 @@ 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 { - /* 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; - } - } - if (_rtl_state == RTL_STATE_FINISHED) { /* RTL finished, nothing to do */ - pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); return; } -- cgit v1.2.3 From 6e5aafb3a753f83b1db936e94de09144ee115c0d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 13:44:41 +0200 Subject: navigator: minor formatting fix --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f39300590..493e86d4a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -386,7 +386,7 @@ Navigator::task_main() _update_triplet = true; } - if (_update_triplet ) { + if (_update_triplet) { publish_position_setpoint_triplet(); _update_triplet = false; } -- cgit v1.2.3 From 8e8798a5225d333596c9018ec703da0c6787493d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 23:39:17 +0200 Subject: navigator: spaces/tabs fixed, old commented code removed --- src/modules/navigator/navigator_main.cpp | 322 ------------------------------- src/modules/navigator/rtl.cpp | 72 +++---- 2 files changed, 36 insertions(+), 358 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 493e86d4a..e22c4d72d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -446,329 +446,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() { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index a2eec8589..c5bb06c3b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -79,7 +79,7 @@ RTL::on_inactive() /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { - _rtl_state = RTL_STATE_NONE; + _rtl_state = RTL_STATE_NONE; } } @@ -89,28 +89,28 @@ 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_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 { - /* 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; - } - } + _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_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 { + /* 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); @@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* make sure we have the latest params */ updateParams(); - if (_rtl_state == RTL_STATE_FINISHED) { - /* RTL finished, nothing to do */ - pos_sp_triplet->next.valid = false; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); - return; - } + if (_rtl_state == RTL_STATE_FINISHED) { + /* RTL finished, nothing to do */ + pos_sp_triplet->next.valid = false; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); + return; + } - set_previous_pos_setpoint(pos_sp_triplet); - _navigator->set_can_loiter_at_sp(false); + set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -241,11 +241,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } - /* 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 -- cgit v1.2.3 From 9ae44291b169a90945f090487e3757db9fc8c075 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 00:27:08 +0200 Subject: navigator: added NAV_CMD_IDLE, added RTL_STATE_LOITER and RTL_STATE_LANDED instead of FINISHED --- src/modules/navigator/mission_block.cpp | 25 ++++++---- src/modules/navigator/rtl.cpp | 84 ++++++++++++++++++++++++--------- src/modules/navigator/rtl.h | 3 +- src/modules/uORB/topics/mission.h | 1 + 4 files changed, 82 insertions(+), 31 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 41d81ad9b..5e545706d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -69,10 +69,10 @@ 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_priv->get_vstatus()->condition_landed; } + /* TODO: count turns */ #if 0 if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || @@ -178,19 +178,28 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite 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; } } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c5bb06c3b..b7961a260 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -53,6 +53,8 @@ #include "navigator.h" #include "rtl.h" +#define DELAY_SIGMA 0.01f + RTL::RTL(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), MissionBlock(navigator), @@ -95,7 +97,7 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) 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; + _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 */ @@ -113,11 +115,9 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) } set_rtl_item(pos_sp_triplet); - updated = true; - } - if ((_rtl_state != RTL_STATE_FINISHED) && 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; @@ -132,19 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* make sure we have the latest params */ updateParams(); - if (_rtl_state == RTL_STATE_FINISHED) { - /* RTL finished, nothing to do */ - pos_sp_triplet->next.valid = false; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); - return; - } - 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; @@ -165,8 +157,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) (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 @@ -196,8 +188,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) (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; @@ -207,9 +199,9 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _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.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; mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", @@ -217,8 +209,35 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) 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 = _param_acceptance_radius.get(); + _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; @@ -237,6 +256,25 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } + 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 = _param_acceptance_radius.get(); + _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; } @@ -262,18 +300,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: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index bcccf7454..c4286b391 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -97,8 +97,9 @@ 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; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index d25db3a77..d9dd61df1 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -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, -- cgit v1.2.3 From 1bae18377a444279f91b75281ffde5da70cc6086 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 00:29:00 +0200 Subject: navigator: is_mission_item_reached() for LOITER items fixed --- src/modules/navigator/mission_block.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 5e545706d..8ef7be449 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -74,15 +74,10 @@ MissionBlock::is_mission_item_reached() } /* 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) { - + 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(); -- cgit v1.2.3 From 52eb49ba0bd1ea5a05845350f1b3c46f0b059a39 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 11:09:49 +0200 Subject: navigator: use common "acceptance radius" parameter for all modes --- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/navigator.h | 4 ++-- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/navigator_params.c | 6 +++--- src/modules/navigator/rtl.cpp | 15 +++++++-------- src/modules/navigator/rtl.h | 1 - src/modules/navigator/rtl_params.c | 12 ------------ 7 files changed, 15 insertions(+), 29 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8ef7be449..4f177d76e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -100,12 +100,12 @@ MissionBlock::is_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->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()) { + altitude_amsl - _navigator_priv->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_priv->get_acceptance_radius()) { _waypoint_position_reached = true; } } else { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 4709f7196..184ecd365 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -115,7 +115,7 @@ public: Geofence& get_geofence() { return _geofence; } 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: @@ -165,7 +165,7 @@ private: 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 */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e22c4d72d..546602741 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -123,7 +123,7 @@ Navigator::Navigator() : _rtl(this, "RTL"), _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; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ca31adecc..084afe340 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -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); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b7961a260..af2ff308c 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -61,8 +61,7 @@ RTL::RTL(Navigator *navigator, const char *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(); @@ -147,7 +146,7 @@ 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; @@ -178,7 +177,7 @@ 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; @@ -198,7 +197,7 @@ 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.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = false; @@ -220,7 +219,7 @@ 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 = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _param_acceptance_radius.get(); + _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; @@ -246,7 +245,7 @@ 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; @@ -265,7 +264,7 @@ 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_IDLE; - _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; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index c4286b391..90299c3fa 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -105,7 +105,6 @@ private: control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; - control::BlockParamFloat _param_acceptance_radius; }; #endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 63724f461..bfe6ce7e1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -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); -- cgit v1.2.3 From affc312411b7634fa13bab6da8889de90f964ce8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Jun 2014 11:34:19 +0200 Subject: navigator: make MissionBlock subclass of NavigatorMode --- src/modules/navigator/loiter.cpp | 3 +-- src/modules/navigator/loiter.h | 2 +- src/modules/navigator/mission.cpp | 3 +-- src/modules/navigator/mission.h | 2 +- src/modules/navigator/mission_block.cpp | 44 ++++++++++++++++----------------- src/modules/navigator/mission_block.h | 11 +++------ src/modules/navigator/rtl.cpp | 3 +-- src/modules/navigator/rtl.h | 2 +- 8 files changed, 32 insertions(+), 38 deletions(-) (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f5eb1e7a5..542483fb1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -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(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 685ae6141..65ff5c31e 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -47,7 +47,7 @@ #include "navigator_mode.h" #include "mission_block.h" -class Loiter : public NavigatorMode, MissionBlock +class Loiter : public MissionBlock { public: /** diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c09f61744..72255103b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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}), diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 842c8c9ee..6e4761946 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -62,7 +62,7 @@ class Navigator; -class Mission : public NavigatorMode, MissionBlock +class Mission : public MissionBlock { public: /** diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4f177d76e..9b8d3d9c7 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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) { } @@ -70,7 +70,7 @@ bool MissionBlock::is_mission_item_reached() { if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _navigator_priv->get_vstatus()->condition_landed; + return _navigator->get_vstatus()->condition_landed; } /* TODO: count turns */ @@ -88,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_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_acceptance_radius()) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else { @@ -119,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; @@ -167,7 +167,7 @@ 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; @@ -211,25 +211,25 @@ bool MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) { /* don't change setpoint if 'can_loiter_at_sp' flag set */ - if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { + 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_priv->set_can_loiter_at_sp(true); + _navigator->set_can_loiter_at_sp(true); } if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius() + || 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_priv->get_loiter_radius(); + pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); pos_sp_triplet->current.loiter_direction = 1; pos_sp_triplet->previous.valid = false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 8726964fa..f99002752 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -47,17 +47,17 @@ #include #include +#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 @@ -101,9 +101,6 @@ public: mission_item_s _mission_item; bool _mission_item_valid; - -private: - Navigator *_navigator_priv; }; #endif diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index af2ff308c..043f773a4 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -56,8 +56,7 @@ #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"), diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 90299c3fa..b4b729e89 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -54,7 +54,7 @@ class Navigator; -class RTL : public NavigatorMode, MissionBlock +class RTL : public MissionBlock { public: /** -- cgit v1.2.3