diff options
author | Julian Oes <julian@oes.ch> | 2014-04-21 17:36:59 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-04-21 17:36:59 +0200 |
commit | c3c0328e8bb9211580dbe5a52ecb23e0452cb402 (patch) | |
tree | 0c449cb2733f3c6cbfc5593e38929fea34152c11 | |
parent | 488785250f4f1fa3c2f6d1e3283fd8eabb6b3144 (diff) | |
download | px4-firmware-c3c0328e8bb9211580dbe5a52ecb23e0452cb402.tar.gz px4-firmware-c3c0328e8bb9211580dbe5a52ecb23e0452cb402.tar.bz2 px4-firmware-c3c0328e8bb9211580dbe5a52ecb23e0452cb402.zip |
navigator: lot's of cleanup (WIP)
21 files changed, 957 insertions, 930 deletions
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 <lm@inf.ethz.ch> * * 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/navigator_mission.cpp b/src/modules/navigator/mission.cpp index ac7e604ef..e95bcfd43 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes <joes@student.ethz.ch> + * 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 @@ -33,22 +32,22 @@ ****************************************************************************/ /** * @file navigator_mission.cpp + * * Helper class to access missions + * + * @author Julian Oes <julian@oes.ch> */ #include <string.h> #include <stdlib.h> + #include <dataman/dataman.h> #include <systemlib/err.h> + #include <uORB/uORB.h> #include <uORB/topics/mission_result.h> -#include "navigator_mission.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include "mission.h" Mission::Mission() : @@ -60,27 +59,43 @@ Mission::Mission() : _onboard_mission_item_count(0), _onboard_mission_allowed(false), _current_mission_type(MISSION_TYPE_NONE), - _mission_result_pub(-1) + _mission_result_pub(-1), + _mission_result({}) { - memset(&_mission_result, 0, sizeof(struct mission_result_s)); } Mission::~Mission() { - } void -Mission::set_offboard_dataman_id(int new_id) +Mission::set_offboard_dataman_id(const int new_id) { _offboard_dataman_id = new_id; } void -Mission::set_current_offboard_mission_index(int new_index) +Mission::set_offboard_mission_count(int new_count) { - if (new_index != -1) { - warnx("specifically set to %d", new_index); + _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 { @@ -92,8 +107,8 @@ Mission::set_current_offboard_mission_index(int new_index) report_current_offboard_mission_item(); } -void -Mission::set_current_onboard_mission_index(int new_index) +bool +Mission::command_current_onboard_mission_index(int new_index) { if (new_index != -1) { _current_onboard_mission_index = (unsigned)new_index; @@ -108,153 +123,130 @@ Mission::set_current_onboard_mission_index(int new_index) // 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() +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) { - 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; + *onboard = false; + *index = -1; /* 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) { + 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 */ - } else if (current_offboard_mission_available()) { + 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)) { - 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 true; + } } - return ret; + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + + return false; } -int +bool Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { - int ret = ERROR; + int next_temp_mission_index = _current_onboard_mission_index + 1; /* 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; + 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; } + } - unsigned next_offboard_mission_index = _current_offboard_mission_index + 1; - ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item); - + /* then try offboard mission */ + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { - /* happens when no more mission items can be added as a next item */ - ret = ERROR; + 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; + } } - 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; + /* both failed, bail out */ + return false; } bool -Mission::next_onboard_mission_available() +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) { - unsigned next = 0; + /* 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); - if (_current_mission_type != MISSION_TYPE_ONBOARD) { - next = 1; - } + /* 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; + } - return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; -} + /* 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) { -bool -Mission::next_offboard_mission_available() -{ - unsigned next = 0; + 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; + } - if (_current_mission_type != MISSION_TYPE_OFFBOARD) { - next = 1; + } else { + /* if it's not a DO_JUMP, then we were successful */ + return true; + } } - return _offboard_mission_item_count > (_current_offboard_mission_index + next); + /* 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++; @@ -277,12 +269,14 @@ Mission::report_mission_item_reached() _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 @@ -301,32 +295,3 @@ Mission::publish_mission_result() _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/mission.h index fef145410..0fa2ff3fa 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/mission.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes <joes@student.ethz.ch> + * 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 @@ -34,6 +33,7 @@ /** * @file navigator_mission.h * Helper class to access missions + * @author Julian Oes <julian@oes.ch> */ #ifndef NAVIGATOR_MISSION_H @@ -41,6 +41,7 @@ #include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> +#include <dataman/dataman.h> class __EXPORT Mission @@ -52,44 +53,36 @@ public: Mission(); /** - * Destructor, also kills the sensors task. + * Destructor */ ~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_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); - void set_onboard_mission_allowed(bool allowed); + bool command_current_offboard_mission_index(const int new_index); + bool command_current_onboard_mission_index(const int new_index); - 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); + 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(); -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(); + void publish_mission_result(); 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 */ - + 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 { @@ -98,9 +91,8 @@ private: MISSION_TYPE_OFFBOARD, } _current_mission_type; - int _mission_result_pub; - - struct mission_result_s _mission_result; + 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 <lm@inf.ethz.ch> - * @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> + * 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 <lm@inf.ethz.ch> * @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> + #include <stdio.h> #include <stdlib.h> #include <string.h> @@ -58,9 +55,13 @@ #include <poll.h> #include <time.h> #include <sys/ioctl.h> +#include <sys/types.h> +#include <sys/stat.h> + #include <drivers/device/device.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> + #include <uORB/uORB.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/home_position.h> @@ -71,20 +72,19 @@ #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> + #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/state_table.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <geo/geo.h> -#include <mathlib/mathlib.h> #include <dataman/dataman.h> +#include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include <sys/types.h> -#include <sys/stat.h> -#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 @@ -1544,62 +1282,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() { /* update navigation state */ @@ -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_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 @@ -53,16 +53,6 @@ */ /** - * 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 * * Default value of acceptance radius (if not specified in mission item). @@ -102,37 +92,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 * * @group Navigation 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 <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> + +#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 <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef NAVIGATOR_RTL_H +#define NAVIGATOR_RTL_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_global_position.h> + +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 <julian@oes.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * 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 <julian@oes.ch> */ #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<StateTable::Action>(_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 <stdint.h> #include <stdbool.h> #include "../uORB.h" -#include <navigator/navigator_state.h> /** * @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 <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> * @author Lorenz Meier <lm@inf.ethz.ch> */ @@ -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 <stdbool.h> #include "../uORB.h" -#include <navigator/navigator_state.h> - /** * @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 */ |