From 7f4f9a5f5f6c4921453f3e0c98531bcb4e44d981 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 22:44:05 +0100 Subject: ACRO mode implemented --- src/modules/commander/commander.cpp | 35 +++++++++++++++++++++++++++++++---- 1 file changed, 31 insertions(+), 4 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713..89294fc76 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,9 +619,10 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; - main_states_str[3] = "AUTO"; + main_states_str[1] = "ACRO"; + main_states_str[2] = "SEATBELT"; + main_states_str[3] = "EASY"; + main_states_str[4] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { status->mission_switch = MISSION_SWITCH_MISSION; } + + /* acro switch */ + if (!isfinite(sp_man->acro_switch)) { + status->acro_switch = ACRO_SWITCH_NONE; + + } else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) { + status->acro_switch = ACRO_SWITCH_ACRO; + + } else { + status->acro_switch = ACRO_SWITCH_NORMAL; + } } transition_result_t @@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status) switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (status->acro_switch == ACRO_SWITCH_ACRO) { + res = main_state_transition(status, MAIN_STATE_ACRO); + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1600,6 +1616,17 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case MAIN_STATE_SEATBELT: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; -- cgit v1.2.3 From 2923bdf39fd6e424523f0b6b47bef3cabcdc0645 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:25:29 +0100 Subject: commander: allow disarming in ACRO without landing (as in MANUAL) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89294fc76..30f75b9fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1120,7 +1120,7 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { -- cgit v1.2.3 From 3fa82675e79013fedf9a787ca21e06e5bd15e5ca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:13:03 +0200 Subject: commander: don't beep if message is not understood --- src/modules/commander/commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d4567c4f1..27d5e4260 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -591,11 +591,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); - } + /* silently ignore unsupported commands, maybe they are passed on over mavlink */ +// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { +// /* already warned about unsupported commands in "default" case */ +// answer_command(*cmd, result); +// } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From 3dd64086e49192aabc020b50f48d68233bad392f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:19 +0200 Subject: commander: put unsupported warning back in place --- src/modules/commander/commander.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 27d5e4260..662b98b35 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -587,15 +587,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* silently ignore unsupported commands, maybe they are passed on over mavlink */ -// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -// /* already warned about unsupported commands in "default" case */ -// answer_command(*cmd, result); -// } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); + } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From 03a3b1d67127af8681275a50e5604ea1b60814ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:06:52 +0200 Subject: commander: handle_command: filter commands by sysid and componentid --- src/modules/commander/commander.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 662b98b35..e6de58563 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ -- cgit v1.2.3 From 93227f9200ee3369f0d2a3aa2fa7bb2a738e18c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:29:11 +0200 Subject: commander: handle_command: do not filter command if componentid == 0 --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e6de58563..e7cf2b3fa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe bool ret = false; /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } -- cgit v1.2.3 From 46a796fb86986c5172ab4d85d1902e7648afd651 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef..7ac7aff0f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); -- cgit v1.2.3 From 37b133e231e945c978dd66fd5daed0f12caa8073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef..7ac7aff0f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); -- cgit v1.2.3 From c3c0328e8bb9211580dbe5a52ecb23e0452cb402 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Apr 2014 17:36:59 +0200 Subject: navigator: lot's of cleanup (WIP) --- src/modules/commander/commander.cpp | 57 +- src/modules/commander/state_machine_helper.cpp | 6 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +- src/modules/mavlink/mavlink_main.cpp | 7 +- src/modules/mavlink/mavlink_messages.cpp | 5 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/navigator/geofence_params.c | 1 - src/modules/navigator/mission.cpp | 297 +++++++ src/modules/navigator/mission.h | 98 +++ src/modules/navigator/module.mk | 6 +- src/modules/navigator/navigator_main.cpp | 934 +++++++-------------- src/modules/navigator/navigator_mission.cpp | 332 -------- src/modules/navigator/navigator_mission.h | 106 --- src/modules/navigator/navigator_params.c | 41 - src/modules/navigator/navigator_state.h | 21 - src/modules/navigator/rtl.cpp | 253 ++++++ src/modules/navigator/rtl.h | 92 ++ src/modules/navigator/rtl_params.c | 87 ++ src/modules/systemlib/state_table.h | 23 +- src/modules/uORB/topics/mission.h | 2 + .../uORB/topics/position_setpoint_triplet.h | 12 +- src/modules/uORB/topics/vehicle_global_position.h | 12 +- src/modules/uORB/topics/vehicle_status.h | 13 +- 23 files changed, 1221 insertions(+), 1194 deletions(-) create mode 100644 src/modules/navigator/mission.cpp create mode 100644 src/modules/navigator/mission.h delete mode 100644 src/modules/navigator/navigator_mission.cpp delete mode 100644 src/modules/navigator/navigator_mission.h delete mode 100644 src/modules/navigator/navigator_state.h create mode 100644 src/modules/navigator/rtl.cpp create mode 100644 src/modules/navigator/rtl.h create mode 100644 src/modules/navigator/rtl_params.c (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fa..c2d24a883 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -547,15 +547,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAV_STATE_LOITER; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAV_STATE_MISSION; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -668,8 +666,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; - status.set_nav_state_timestamp = 0; + status.set_nav_state = NAVIGATION_STATE_NONE; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -944,7 +941,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; - if (status.is_rotary_wing && status.condition_local_altitude_valid) { + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1222,30 +1219,30 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + if (sp_man.mode_switch == SWITCH_POS_ON) { - } else { - /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAV_STATE_LOITER; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAVIGATION_STATE_RTL; + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAVIGATION_STATE_LOITER; + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAVIGATION_STATE_MISSION; + } + /* XXX: I don't understand this */ + //else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && + // pos_sp_triplet.nav_state == NAVIGATION_STATE_RTL) { + // /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + // status.set_nav_state = NAV_STATE_MISSION; + // } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e6f245cf9..54f0f13f4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -450,8 +450,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAV_STATE_RTL; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_RTL; ret = TRANSITION_CHANGED; } @@ -461,8 +460,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAV_STATE_LAND; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LAND; ret = TRANSITION_CHANGED; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df785..a00a388a8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -150,8 +150,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ double _loiter_hold_lat; @@ -393,7 +391,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), /* states */ - _setpoint_valid(false), _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), @@ -663,7 +660,6 @@ FixedwingPositionControl::vehicle_setpoint_poll() if (pos_sp_triplet_updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - _setpoint_valid = true; } } @@ -708,7 +704,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -781,7 +777,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. - if (_control_mode.flag_control_position_enabled) { + if (pos_sp_triplet.current.valid) { /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 36d47b7ee..c60c85495 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -872,7 +872,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item break; case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; - /* TODO: also save param2 (repeat count) */ + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; @@ -889,6 +889,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + return OK; } @@ -908,7 +911,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - /* TODO: also save param2 (repeat count) */ + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 648228e3b..3cf69bf7c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -117,7 +117,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND + || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { /* use main state when navigator is not active */ if (status->main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); @@ -142,7 +143,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2bd2d356a..7d3d39d18 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -645,7 +645,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = true; - _pos_sp_triplet.nav_state = NAV_STATE_NONE; + // _pos_sp_triplet.nav_state = NAV_STATE_NONE; _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; _pos_sp_triplet.current.lat = _lat_sp; _pos_sp_triplet.current.lon = _lon_sp; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 5831a0ca9..653b1ad84 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp new file mode 100644 index 000000000..e95bcfd43 --- /dev/null +++ b/src/modules/navigator/mission.cpp @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.cpp + * + * Helper class to access missions + * + * @author Julian Oes + */ + +#include +#include + +#include +#include + +#include +#include + +#include "mission.h" + + +Mission::Mission() : + + _offboard_dataman_id(-1), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE), + _mission_result_pub(-1), + _mission_result({}) +{ +} + +Mission::~Mission() +{ +} + +void +Mission::set_offboard_dataman_id(const int new_id) +{ + _offboard_dataman_id = new_id; +} + +void +Mission::set_offboard_mission_count(int new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(int new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::command_current_offboard_mission_index(const int new_index) +{ + if (new_index >= 0) { + _current_offboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= _offboard_mission_item_count) { + _current_offboard_mission_index = 0; + } + } + report_current_offboard_mission_item(); +} + +bool +Mission::command_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= _onboard_mission_item_count) { + _current_onboard_mission_index = 0; + } + } + // TODO: implement this for onboard missions as well + // report_current_mission_item(); +} + +bool +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +{ + *onboard = false; + *index = -1; + + /* try onboard mission first */ + if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + return true; + } + } + + /* otherwise fallback to offboard */ + if (_current_offboard_mission_index < _offboard_mission_item_count) { + + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { + + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + return true; + } + } + + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + + return false; +} + +bool +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + int next_temp_mission_index = _current_onboard_mission_index + 1; + + /* try onboard mission first */ + if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* then try offboard mission */ + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + next_temp_mission_index = _current_offboard_mission_index + 1; + if (next_temp_mission_index < _offboard_mission_item_count) { + if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* both failed, bail out */ + return false; +} + +bool +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ + if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { + return false; + } + + } else { + /* if it's not a DO_JUMP, then we were successful */ + return true; + } + } + + /* we have given up, we don't want to cycle forever */ + warnx("ERROR: cycling through mission items without success"); + return false; +} + +void +Mission::move_to_next() +{ + report_mission_item_reached(); + + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } +} + +void +Mission::report_mission_item_reached() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } + publish_mission_result(); +} + +void +Mission::report_current_offboard_mission_item() +{ + _mission_result.index_current_mission = _current_offboard_mission_index; + publish_mission_result(); +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.mission_reached = false; +} + diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h new file mode 100644 index 000000000..0fa2ff3fa --- /dev/null +++ b/src/modules/navigator/mission.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include +#include +#include + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor + */ + ~Mission(); + + void set_offboard_dataman_id(const int new_id); + void set_offboard_mission_count(const int new_count); + void set_onboard_mission_count(const int new_count); + void set_onboard_mission_allowed(const bool allowed); + + bool command_current_offboard_mission_index(const int new_index); + bool command_current_onboard_mission_index(const int new_index); + + bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index); + bool get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + +private: + bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + + void report_mission_item_reached(); + void report_current_offboard_mission_item(); + + void publish_mission_result(); + + int _offboard_dataman_id; + int _current_offboard_mission_index; + int _current_onboard_mission_index; + int _offboard_mission_item_count; /** number of offboard mission items available */ + int _onboard_mission_item_count; /** number of onboard mission items available */ + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; + + orb_advert_t _mission_result_pub; /**< publish mission result topic */ + mission_result_s _mission_result; /**< mission result for commander/mavlink */ +}; + +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 55f8a4caa..5f113f686 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -39,7 +39,9 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp \ + mission.cpp \ + rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 494266dd3..34a28aec3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Jean Cyr - * @author Julian Oes - * @author Anton Babushkin + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,19 +31,20 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * - * Handles missions, geo fencing and failsafe navigation behavior. - * Published the mission item triplet for the position controller. + * Handles mission items, geo fencing and failsafe navigation behavior. + * Published the position setpoint triplet for the position controller. * * @author Lorenz Meier * @author Jean Cyr - * @author Julian Oes + * @author Julian Oes * @author Anton Babushkin */ #include + #include #include #include @@ -58,9 +55,13 @@ #include #include #include +#include +#include + #include #include #include + #include #include #include @@ -71,20 +72,19 @@ #include #include #include + #include #include #include #include #include #include -#include #include +#include #include -#include -#include -#include "navigator_state.h" -#include "navigator_mission.h" +#include "mission.h" +#include "rtl.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -130,12 +130,12 @@ public: /** * Add point to geofence */ - void add_fence_point(int argc, char *argv[]); + void add_fence_point(int argc, char *argv[]); /** * Load fence from file */ - void load_fence_from_file(const char *filename); + void load_fence_from_file(const char *filename); private: @@ -154,15 +154,15 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_item_s _mission_item; /**< current mission item */ + + bool _mission_item_valid; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -174,56 +174,41 @@ private: struct navigation_capabilities_s _nav_caps; - class Mission _mission; + Mission _mission; + + RTL _rtl; - bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ - MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - - bool _pos_sp_triplet_updated; - - const char *nav_states_str[NAV_STATE_MAX]; + bool _update_triplet; struct { - float min_altitude; float acceptance_radius; float loiter_radius; int onboard_mission_enabled; float takeoff_alt; - float land_alt; - float rtl_alt; - float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { - param_t min_altitude; param_t acceptance_radius; param_t loiter_radius; param_t onboard_mission_enabled; param_t takeoff_alt; - param_t land_alt; - param_t rtl_alt; - param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { EVENT_NONE_REQUESTED, - EVENT_READY_REQUESTED, EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, - EVENT_LAND_REQUESTED, + EVENT_TAKEN_OFF, + EVENT_LANDED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, + EVENT_MISSION_ITEM_REACHED, MAX_EVENT }; @@ -232,15 +217,6 @@ private: */ static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND - }; - - enum RTLState _rtl_state; - /** * Update our local parameter cache. */ @@ -264,7 +240,7 @@ private: /** * Retrieve offboard mission. */ - void offboard_mission_update(bool isrotaryWing); + void offboard_mission_update(); /** * Retrieve onboard mission. @@ -296,19 +272,20 @@ private: /** * Functions that are triggered when a new state is entered. */ - void start_none(); - void start_ready(); - void start_loiter(); - void start_mission(); - void start_rtl(); - void start_land(); - void start_land_home(); + bool start_none_on_ground(); + bool start_none_in_air(); + bool start_auto_on_ground(); + bool start_loiter(); + bool start_mission(); + bool advance_mission(); + bool start_rtl(); + bool advance_rtl(); + bool start_land(); /** * Fork for state transitions */ - void request_loiter_or_ready(); - void request_mission_if_available(); + // void request_loiter_or_ready(); /** * Guards offboard mission @@ -333,12 +310,12 @@ private: /** * Perform actions when current mission item reached. */ - void on_mission_item_reached(); + // void on_mission_item_reached(); /** * Move to next waypoint */ - void set_mission_item(); + bool set_mission_items(); /** * Switch to next RTL state @@ -348,7 +325,7 @@ private: /** * Set position setpoint for mission item */ - void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); /** * Helper function to get a takeoff item @@ -377,12 +354,9 @@ Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), - _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), - -/* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), @@ -391,53 +365,29 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - -/* publications */ _pos_sp_triplet_pub(-1), - -/* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _pos_sp_triplet({}), + _mission_item({}), _mission_item_valid(false), - _global_pos_valid(false), - _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _need_takeoff(true), - _do_takeoff(false), - _set_nav_state_timestamp(0), - _pos_sp_triplet_updated(false), -/* states */ - _rtl_state(RTL_STATE_NONE) + _update_triplet(false) { - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); - _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - - memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_item, 0, sizeof(struct mission_item_s)); - - memset(&nav_states_str, 0, sizeof(nav_states_str)); - nav_states_str[0] = "NONE"; - nav_states_str[1] = "READY"; - nav_states_str[2] = "LOITER"; - nav_states_str[3] = "MISSION"; - nav_states_str[4] = "RTL"; - nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = NAV_STATE_NONE; - start_none(); + myState = NAV_STATE_NONE_ON_GROUND; + + start_none_on_ground(); } Navigator::~Navigator() @@ -472,16 +422,12 @@ Navigator::parameters_update() struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); - param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); - param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); - param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); + _mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); _geofence.updateParams(); } @@ -496,6 +442,8 @@ void Navigator::home_position_update() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + + _rtl.set_home_position(&_home_pos); } void @@ -506,7 +454,7 @@ Navigator::navigation_capabilities_update() void -Navigator::offboard_mission_update(bool isrotaryWing) +Navigator::offboard_mission_update() { struct mission_s offboard_mission; @@ -523,19 +471,17 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_offboard_mission_count(offboard_mission.count); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.command_current_offboard_mission_index(offboard_mission.current_index); } else { _mission.set_offboard_mission_count(0); - _mission.set_current_offboard_mission_index(0); + _mission.command_current_offboard_mission_index(0); } - - _mission.publish_mission_result(); } void @@ -546,11 +492,11 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); - _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.command_current_onboard_mission_index(onboard_mission.current_index); } else { _mission.set_onboard_mission_count(0); - _mission.set_current_onboard_mission_index(0); + _mission.command_current_onboard_mission_index(0); } } @@ -626,13 +572,13 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(_vstatus.is_rotary_wing); + offboard_mission_update(); onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; + unsigned prevState = NAV_STATE_NONE_ON_GROUND; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -690,55 +636,45 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state requested by commander */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } - } - - } else { - /* navigator shouldn't act */ + /* commander requested new navigation mode, forward it to state machine */ + switch (_vstatus.set_nav_state) { + case NAVIGATION_STATE_NONE: dispatch(EVENT_NONE_REQUESTED); + break; + + case NAVIGATION_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAVIGATION_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; + + case NAVIGATION_STATE_RTL: + /* TODO: what is this for? */ + // if (!(_rtl_state == RTL_STATE_DESCEND && + // (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + // _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); + // } + break; + + case NAVIGATION_STATE_LAND: + /* TODO: add this */ + // dispatch(EVENT_LAND_REQUESTED); + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + /* commander sets in-air/on-ground flag as well */ + if (_vstatus.condition_landed) { + dispatch(EVENT_LANDED); + } else { + dispatch(EVENT_TAKEN_OFF); + } } /* parameters updated */ @@ -754,74 +690,49 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { - offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + offboard_mission_update(); + /* TODO: check if mission really changed */ dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); - // XXX check if mission really changed + /* TODO: check if mission really changed */ dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); - // XXX check if home position really changed + /* TODO check if home position really changed */ dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - - /* publish position setpoint triplet on each position update if navigator active */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - _pos_sp_triplet_updated = true; - - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { - /* got global position when landing, update setpoint */ - start_land(); - } - - _global_pos_valid = _global_pos.global_valid; - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } + if (check_mission_item_reached()) { + dispatch(EVENT_MISSION_ITEM_REACHED); } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { - //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } - } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } - /* publish position setpoint triplet if updated */ - if (_pos_sp_triplet_updated) { - _pos_sp_triplet_updated = false; + if (_update_triplet ) { publish_position_setpoint_triplet(); - } - - /* notify user about state changes */ - if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); - prevState = myState; + _update_triplet = false; } perf_end(_loop_perf); @@ -879,8 +790,12 @@ Navigator::status() } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); + case NAV_STATE_NONE_ON_GROUND: + warnx("State: None on ground"); + break; + + case NAV_STATE_NONE_IN_AIR: + warnx("State: None in air"); break; case NAV_STATE_LOITER: @@ -895,6 +810,10 @@ Navigator::status() warnx("State: RTL"); break; + case NAV_STATE_LAND: + warnx("State: LAND"); + break; + default: warnx("State: Unknown"); break; @@ -903,92 +822,119 @@ Navigator::status() StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { - /* NAV_STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* NAV_STATE_NONE_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + }, + { + /* NAV_STATE_NONE_IN_AIR */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, }, { - /* NAV_STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* NAV_STATE_AUTO_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, }, { /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, }, { /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, }, { /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, }, }; -void -Navigator::start_none() +bool +Navigator::start_none_on_ground() { reset_reached(); _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - _reset_loiter_pos = true; - _do_takeoff = false; - _rtl_state = RTL_STATE_NONE; + _update_triplet = true; + return true; +} + +bool +Navigator::start_none_in_air() +{ + reset_reached(); + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void -Navigator::start_ready() +bool +Navigator::start_auto_on_ground() { reset_reached(); @@ -998,46 +944,26 @@ Navigator::start_ready() _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - if (_rtl_state != RTL_STATE_DESCEND) { - /* reset RTL state if landed not at home */ - _rtl_state = RTL_STATE_NONE; - } + // if (_rtl_state != RTL_STATE_DESCEND) { + // reset RTL state if landed not at home + // _rtl_state = RTL_STATE_NONE; + // } - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void +bool Navigator::start_loiter() { - reset_reached(); - - _do_takeoff = false; - - /* set loiter position if needed */ - if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { - _reset_loiter_pos = false; + /* if no existing item available, use current position */ + if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) { _pos_sp_triplet.current.lat = _global_pos.lat; _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - - float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; - - /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { - _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - - } else { - _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - } - + _pos_sp_triplet.current.alt = _global_pos.alt; } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; @@ -1045,167 +971,146 @@ Navigator::start_loiter() _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - _pos_sp_triplet_updated = true; + mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); + + _update_triplet = true; + return true; } -void +bool Navigator::start_mission() { - _need_takeoff = true; + /* start fresh */ + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; - set_mission_item(); + return set_mission_items(); } -void -Navigator::set_mission_item() +bool +Navigator::advance_mission() { - reset_reached(); + /* tell mission to move by one */ + _mission.move_to_next(); - /* copy current mission to previous item */ - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + /* now try to set the new mission items, if it fails, it will dispatch loiter */ + return set_mission_items(); +} - _reset_loiter_pos = true; - _do_takeoff = false; +bool +Navigator::set_mission_items() +{ + if (_pos_sp_triplet.current.valid) { + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + _pos_sp_triplet.previous.valid = true; + } - int ret; bool onboard; - unsigned index; + int index; - ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); + /* if we fail to set the current mission, continue to loiter */ + if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - if (ret == OK) { - _mission.report_current_offboard_mission_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; + } - _mission_item_valid = true; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { - /* don't reset RTL state on RTL or LOITER items */ - _rtl_state = RTL_STATE_NONE; - } + /* if we got an RTL mission item, switch to RTL mode and give up */ + if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + dispatch(EVENT_RTL_REQUESTED); + return false; + } - if (_vstatus.is_rotary_wing) { - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { - /* do special TAKEOFF handling for VTOL */ - _need_takeoff = false; - - /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _pos_sp_triplet.current.alt; - - if (_vstatus.condition_landed) { - /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ - takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); - } + _mission_item_valid = true; - /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { - /* force TAKEOFF if landed or waypoint altitude is more than current */ - _do_takeoff = true; + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; + - /* move current position setpoint to next */ - memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + mission_item_s next_mission_item; - /* set current setpoint to takeoff */ + bool last_wp = false; + /* now try to set the next mission item as well, if there is no more next + * this means we're heading to the last waypoint */ + if (_mission.get_next_mission_item(&next_mission_item)) { + /* convert the next mission item and set it valid */ + mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next); + _pos_sp_triplet.next.valid = true; + } else { + last_wp = true; + } - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = takeoff_alt_amsl; - _pos_sp_triplet.current.yaw = NAN; - _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; - } + /* notify user about what happened */ + mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d", + (last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index); - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - /* will need takeoff after landing */ - _need_takeoff = true; - } - } + _update_triplet = true; - if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); + reset_reached(); - } else { - if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); + return true; +} - } else { - mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); - } - } +bool +Navigator::start_rtl() +{ + /* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */ + if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - } else { - /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_valid = false; - _pos_sp_triplet.current.valid = false; - warnx("ERROR: current WP can't be set"); - } + _mission_item_valid = true; - if (!_do_takeoff) { - mission_item_s item_next; - ret = _mission.get_next_mission_item(&item_next); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; - if (ret == OK) { - position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + reset_reached(); - } else { - /* this will fail for the last WP */ - _pos_sp_triplet.next.valid = false; - } + _update_triplet = true; + return true; } + dispatch(EVENT_LOITER_REQUESTED); - _pos_sp_triplet_updated = true; + return false; } -void -Navigator::start_rtl() +bool +Navigator::advance_rtl() { - _do_takeoff = false; + /* tell mission to move by one */ + _rtl.move_to_next(); - /* decide if we need climb */ - if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { - _rtl_state = RTL_STATE_CLIMB; + /* now try to set the new mission items, if it fails, it will dispatch loiter */ + if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - } else { - _rtl_state = RTL_STATE_RETURN; - } - } + _mission_item_valid = true; - /* if switching directly to return state, reset altitude setpoint */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; + + reset_reached(); + + _update_triplet = true; + return true; } - _reset_loiter_pos = true; - set_rtl_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; } -void +bool Navigator::start_land() { + /* TODO: verify/test */ + reset_reached(); /* this state can be requested by commander even if no global position available, * in his case controller must perform landing without position control */ - _do_takeoff = false; - _reset_loiter_pos = true; memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _global_pos.lat; _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; @@ -1220,185 +1125,22 @@ Navigator::start_land() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::start_land_home() -{ - reset_reached(); - - /* land to home position, should be called when hovering above home, from RTL state */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); _pos_sp_triplet.next.valid = false; -} - -void -Navigator::set_rtl_item() -{ - reset_reached(); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - } - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); - break; - } - - case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // don't change altitude - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - default: { - mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::request_loiter_or_ready() -{ - /* XXX workaround: no landing detector for fixedwing yet */ - if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } -} - -void -Navigator::request_mission_if_available() -{ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - request_loiter_or_ready(); - } + _update_triplet = true; + return true; } void -Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { sp->valid = true; if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* set home position for RTL item */ - sp->lat = _home_pos.lat; - sp->lon = _home_pos.lon; - sp->alt = _home_pos.alt + _parameters.rtl_alt; if (_pos_sp_triplet.previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ @@ -1408,9 +1150,6 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ /* else use current position */ sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); } - sp->loiter_radius = _parameters.loiter_radius; - sp->loiter_direction = 1; - sp->pitch_min = 0.0f; } else { sp->lat = item->lat; @@ -1450,14 +1189,14 @@ Navigator::check_mission_item_reached() return _vstatus.condition_landed; } - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { + /* TODO count turns */ + // if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + // _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + // _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + // _mission_item.loiter_radius > 0.01f) { - return false; - } + // return false; + // } uint64_t now = hrt_absolute_time(); @@ -1475,22 +1214,19 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _home_pos.alt : _mission_item.altitude; - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _global_pos.lat, _global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_global_pos.alt > altitude_amsl - acceptance_radius) { _waypoint_position_reached = true; } - } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1499,7 +1235,10 @@ Navigator::check_mission_item_reached() } if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { + + /* TODO: removed takeoff, why? */ + if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) { + /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); @@ -1514,24 +1253,23 @@ Navigator::check_mission_item_reached() /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { + if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + (double)_mission_item.time_inside); } } /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - reset_reached(); return true; } } - return false; - } void @@ -1543,62 +1281,6 @@ Navigator::reset_reached() } -void -Navigator::on_mission_item_reached() -{ - if (myState == NAV_STATE_MISSION) { - - _mission.report_mission_item_reached(); - - if (_do_takeoff) { - /* takeoff completed */ - _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); - - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - if (_mission.current_mission_available()) { - set_mission_item(); - - } else { - /* if no more mission items available then finish mission */ - /* loiter at last waypoint */ - _reset_loiter_pos = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - request_loiter_or_ready(); - } - - } else if (myState == NAV_STATE_RTL) { - /* RTL completed */ - if (_rtl_state == RTL_STATE_DESCEND) { - /* hovering above home position, land if needed or loiter */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - - if (_mission_item.autocontinue) { - dispatch(EVENT_LAND_REQUESTED); - - } else { - _reset_loiter_pos = false; - dispatch(EVENT_LOITER_REQUESTED); - } - - } else { - /* next RTL step */ - _rtl_state = (RTLState)(_rtl_state + 1); - set_rtl_item(); - } - - } else if (myState == NAV_STATE_LAND) { - /* landing completed */ - mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); - dispatch(EVENT_READY_REQUESTED); - } - _mission.publish_mission_result(); -} - void Navigator::publish_position_setpoint_triplet() { @@ -1607,11 +1289,9 @@ Navigator::publish_position_setpoint_triplet() /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { - /* advertise and publish */ _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp deleted file mode 100644 index ac7e604ef..000000000 --- a/src/modules/navigator/navigator_mission.cpp +++ /dev/null @@ -1,332 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file navigator_mission.cpp - * Helper class to access missions - */ - -#include -#include -#include -#include -#include -#include -#include "navigator_mission.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - - -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), - _mission_result_pub(-1) -{ - memset(&_mission_result, 0, sizeof(struct mission_result_s)); -} - -Mission::~Mission() -{ - -} - -void -Mission::set_offboard_dataman_id(int new_id) -{ - _offboard_dataman_id = new_id; -} - -void -Mission::set_current_offboard_mission_index(int new_index) -{ - if (new_index != -1) { - warnx("specifically set to %d", new_index); - _current_offboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } - } - report_current_offboard_mission_item(); -} - -void -Mission::set_current_onboard_mission_index(int new_index) -{ - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } - } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); -} - -void -Mission::set_offboard_mission_count(unsigned new_count) -{ - _offboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_count(unsigned new_count) -{ - _onboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; -} - -bool -Mission::current_mission_available() -{ - return (current_onboard_mission_available() || current_offboard_mission_available()); - -} - -bool -Mission::next_mission_available() -{ - return (next_onboard_mission_available() || next_offboard_mission_available()); -} - -int -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (current_onboard_mission_available()) { - - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - } - - /* otherwise fallback to offboard */ - } else if (current_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - } - - } else { - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; - ret == ERROR; - } - - return ret; -} - -int -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (next_onboard_mission_available()) { - - unsigned next_onboard_mission_index = _current_onboard_mission_index + 1; - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item); - - /* otherwise fallback to offboard */ - } else if (next_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - unsigned next_offboard_mission_index = _current_offboard_mission_index + 1; - ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item); - - } else { - /* happens when no more mission items can be added as a next item */ - ret = ERROR; - } - return ret; -} - - -bool -Mission::current_onboard_mission_available() -{ - return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; -} - -bool -Mission::current_offboard_mission_available() -{ - return _offboard_mission_item_count > _current_offboard_mission_index; -} - -bool -Mission::next_onboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_ONBOARD) { - next = 1; - } - - return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; -} - -bool -Mission::next_offboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_OFFBOARD) { - next = 1; - } - - return _offboard_mission_item_count > (_current_offboard_mission_index + next); -} - -void -Mission::move_to_next() -{ - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::report_mission_item_reached() -{ - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; - } -} - -void -Mission::report_current_offboard_mission_item() -{ - _mission_result.index_current_mission = _current_offboard_mission_index; -} - -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.mission_reached = false; -} - -int -Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item) -{ - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ - for (int i=0; i<10; i++) { - - const ssize_t len = sizeof(struct mission_item_s); - - /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* check for DO_JUMP item */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; - } else { - /* if it's not a DO_JUMP, then we were successful */ - return OK; - } - } - - /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); - return ERROR; -} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h deleted file mode 100644 index fef145410..000000000 --- a/src/modules/navigator/navigator_mission.h +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file navigator_mission.h - * Helper class to access missions - */ - -#ifndef NAVIGATOR_MISSION_H -#define NAVIGATOR_MISSION_H - -#include -#include - - -class __EXPORT Mission -{ -public: - /** - * Constructor - */ - Mission(); - - /** - * Destructor, also kills the sensors task. - */ - ~Mission(); - - void set_offboard_dataman_id(int new_id); - void set_current_offboard_mission_index(int new_index); - void set_current_onboard_mission_index(int new_index); - void set_offboard_mission_count(unsigned new_count); - void set_onboard_mission_count(unsigned new_count); - - void set_onboard_mission_allowed(bool allowed); - - bool current_mission_available(); - bool next_mission_available(); - - int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); - int get_next_mission_item(struct mission_item_s *mission_item); - - void move_to_next(); - - void report_mission_item_reached(); - void report_current_offboard_mission_item(); - void publish_mission_result(); - -private: - int get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item); - - bool current_onboard_mission_available(); - bool current_offboard_mission_available(); - bool next_onboard_mission_available(); - bool next_offboard_mission_available(); - - int _offboard_dataman_id; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items available */ - unsigned _onboard_mission_item_count; /** number of onboard mission items available */ - - bool _onboard_mission_allowed; - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; - - int _mission_result_pub; - - struct mission_result_s _mission_result; -}; - -#endif diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9ef359c6d..49969a382 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,16 +52,6 @@ * Navigator parameters, accessible via MAVLink */ -/** - * Minimum altitude (fixed wing only) - * - * Minimum altitude above home for LOITER. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); - /** * Waypoint acceptance radius * @@ -101,37 +91,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); -/** - * Landing altitude - * - * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); - -/** - * Return-To-Launch altitude - * - * Minimum altitude above home position for going home in RTL mode. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); - -/** - * Return-To-Launch delay - * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. - * - * @unit seconds - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); - /** * Enable parachute deployment * diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h deleted file mode 100644 index 6a1475c9b..000000000 --- a/src/modules/navigator/navigator_state.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * navigator_state.h - * - * Created on: 27.01.2014 - * Author: ton - */ - -#ifndef NAVIGATOR_STATE_H_ -#define NAVIGATOR_STATE_H_ - -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - -#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp new file mode 100644 index 000000000..6e621e39d --- /dev/null +++ b/src/modules/navigator/rtl.cpp @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_rtl.cpp + * Helper class to access RTL + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "rtl.h" + + +RTL::RTL() : + SuperBlock(NULL, "RTL"), + _mavlink_fd(-1), + _rtl_state(RTL_STATE_NONE), + _home_position({}), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY"), + _loiter_radius(50), + _acceptance_radius(50) +{ + /* load initial params */ + updateParams(); +} + +RTL::~RTL() +{ +} + +void +RTL::set_home_position(const home_position_s *new_home_position) +{ + memcpy(&_home_position, new_home_position, sizeof(home_position_s)); +} + +bool +RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item) +{ + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* decide if we need climb */ + if (_rtl_state == RTL_STATE_NONE) { + if (global_position->alt < _home_position.alt + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + } else { + _rtl_state = RTL_STATE_RETURN; + } + } + + /* if switching directly to return state, set altitude setpoint to current altitude */ + if (_rtl_state == RTL_STATE_RETURN) { + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = global_position->alt; + } + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + float climb_alt = _home_position.alt + _param_return_alt.get(); + + /* TODO understand and fix this */ + // if (_vstatus.condition_landed) { + // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + // } + + new_mission_item->lat = global_position->lat; + new_mission_item->lon = global_position->lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = climb_alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_TAKEOFF; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _home_position.alt)); + break; + } + case RTL_STATE_RETURN: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + + /* TODO: add this again */ + // don't change altitude + // if (_pos_sp_triplet.previous.valid) { + // /* if previous setpoint is valid then use it to calculate heading to home */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon); + + // } else { + // /* else use current position */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon); + // } + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_DESCEND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt + _param_descend_alt.get(); + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = _param_land_delay.get() > -0.001f; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_LAND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_LAND; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_FINISHED: { + /* nothing to do, report fail */ + return false; + } + + default: + return false; + } + + return true; +} + +bool +RTL::get_next_rtl_item(mission_item_s *new_mission_item) +{ + /* TODO implement */ + return false; +} + +void +RTL::move_to_next() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < 0) { + _rtl_state = RTL_STATE_FINISHED; + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_FINISHED; + break; + + case RTL_STATE_FINISHED: + break; + + default: + break; + } +} \ No newline at end of file diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h new file mode 100644 index 000000000..c761837fc --- /dev/null +++ b/src/modules/navigator/rtl.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + * @author Julian Oes + * @author Anton Babushkin + */ + +#ifndef NAVIGATOR_RTL_H +#define NAVIGATOR_RTL_H + +#include +#include + +#include +#include +#include + +class RTL : public control::SuperBlock +{ +public: + /** + * Constructor + */ + RTL(); + + /** + * Destructor + */ + ~RTL(); + + void set_home_position(const home_position_s *home_position); + + bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); + bool get_next_rtl_item(mission_item_s *mission_item); + + void move_to_next(); + +private: + int _mavlink_fd; + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LAND, + RTL_STATE_FINISHED, + } _rtl_state; + + home_position_s _home_position; + float _loiter_radius; + float _acceptance_radius; + + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c new file mode 100644 index 000000000..7a8b1d745 --- /dev/null +++ b/src/modules/navigator/rtl_params.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes + */ + +#include + +#include + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index f2709d29f..38378651b 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,7 @@ * @file state_table.h * * Finite-State-Machine helper class for state table + * @author: Julian Oes */ #ifndef __SYSTEMLIB_STATE_TABLE_H @@ -43,7 +44,7 @@ class StateTable { public: - typedef void (StateTable::*Action)(); + typedef bool (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; @@ -53,17 +54,23 @@ public: : myTable(table), myNsignals(nSignals), myNstates(nStates) {} #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) static_cast(_target) + #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} void dispatch(unsigned const sig) { - register Tran const *t = myTable + myState*myNsignals + sig; - (this->*(t->action))(); - - myState = t->nextState; + /* get transition using state table */ + Tran const *t = myTable + myState*myNsignals + sig; + /* first up change state, this allows to do further dispatchs in the state functions */ + + /* now execute state function, if it runs with success, accept new state */ + if ((this->*(t->action))()) { + myState = t->nextState; + } + } + bool doNothing() { + return true; } - void doNothing() {} protected: unsigned myState; private: diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index dfc461ae4..4532b329d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -93,6 +93,8 @@ struct mission_item_s { bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ int do_jump_mission_index; /**< the mission index that we want to jump to */ + int do_jump_repeat_count; /**< how many times the jump should be repeated */ + int do_jump_current_count; /**< how many times the jump has already been repeated */ }; struct mission_s diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 34aaa30dd..85e8ef8a5 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,7 +45,6 @@ #include #include #include "../uORB.h" -#include /** * @addtogroup topics @@ -74,6 +73,17 @@ struct position_setpoint_s float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; +typedef enum { + NAV_STATE_NONE_ON_GROUND = 0, + NAV_STATE_NONE_IN_AIR, + NAV_STATE_AUTO_ON_GROUND, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + /** * Global position setpoint triplet in WGS84 coordinates. * diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index adcd028f0..cfab695f8 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -36,7 +36,7 @@ * Definition of the global fused WGS84 position uORB topic. * * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes * @author Lorenz Meier */ @@ -66,16 +66,16 @@ struct vehicle_global_position_s { bool global_valid; /**< true if position satisfies validity criteria of estimator */ bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 48af4c9e2..c1bd828d8 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,8 +54,6 @@ #include #include "../uORB.h" -#include - /** * @addtogroup topics @{ */ @@ -93,6 +91,14 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; +typedef enum { + NAVIGATION_STATE_NONE = 0, + NAVIGATION_STATE_MISSION, + NAVIGATION_STATE_LOITER, + NAVIGATION_STATE_RTL, + NAVIGATION_STATE_LAND +} navigation_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -152,8 +158,7 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - unsigned int set_nav_state; /**< set navigation state machine to specified value */ - uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ + navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ -- cgit v1.2.3 From db15e2811ea01dd023ae930e6e7a73c1a370cecf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:36:32 +0200 Subject: commander: Fix altitude initialisation, do not depend on global pos valid flag. --- src/modules/commander/commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ac7aff0f..077184bfb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1088,13 +1088,12 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ home.lat = gps_position.lat / (double)1e7; home.lon = gps_position.lon / (double)1e7; - home.alt = gps_position.alt; + home.alt = gps_position.alt / (float)1e3; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); -- cgit v1.2.3 From 7e621070ca0f002e2e1ccd863c31a24166ece0c2 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 18:23:27 -0700 Subject: renamed mission_switch to loiter_switch and assisted_switch to easy_switch --- src/modules/commander/commander.cpp | 10 +++++----- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 24 +++++++++++------------ src/modules/uORB/topics/manual_control_setpoint.h | 4 ++-- src/modules/uORB/topics/rc_channels.h | 4 ++-- 5 files changed, 23 insertions(+), 23 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fa..ee818d0f5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + if (sp_man.loiter_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) { + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { /* stick is in MISSION position */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); @@ -1296,7 +1296,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1528,7 +1528,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->assisted_switch == SWITCH_POS_ON) { + if (sp_man->easy_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) { @@ -1545,7 +1545,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this mode } - if (sp_man->assisted_switch != SWITCH_POS_ON) { + if (sp_man->easy_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c04e176a1..a34f81923 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -620,7 +620,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); /** * Mission switch channel mapping. @@ -629,7 +629,7 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 04b74a6f5..469fc5ca0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -253,8 +253,8 @@ private: int rc_map_mode_sw; int rc_map_return_sw; - int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_easy_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,8 +296,8 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; - param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_easy_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -507,8 +507,8 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -650,11 +650,11 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -681,8 +681,8 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1415,8 +1415,8 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.easy_switch = get_rc_switch_position(EASY); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b2..b6257e22a 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,8 +78,8 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd..d99203ff6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,8 +64,8 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, - ASSISTED = 6, - MISSION = 7, + EASY = 6, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, -- cgit v1.2.3 From 831a7c4a833c68b1d418344e2f3aae2c80894b1a Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 20:53:07 -0700 Subject: Added RC_MAP_FAILSAFE parameter for customizing failsafe channel. Default to THROTTLE --- src/modules/commander/commander.cpp | 2 +- src/modules/sensors/sensors.cpp | 51 +++++++++++++++++++++++++++---------- 2 files changed, 39 insertions(+), 14 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee818d0f5..43a4ed8ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1230,7 +1230,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { - /* MISSION switch */ + /* LOITER switch */ if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 469fc5ca0..8b9aee795 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -175,7 +175,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func); /** * Gather and publish RC input data. @@ -250,6 +251,7 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; @@ -293,6 +295,7 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; @@ -499,6 +502,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); @@ -642,6 +646,10 @@ Sensors::parameters_update() warnx(paramerr); } + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx(paramerr); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx(paramerr); } @@ -1275,7 +1283,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } switch_pos_t -Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func) { if (_rc.function[func] >= 0) { float value = _rc.chan[_rc.function[func]].scaled; @@ -1294,6 +1302,23 @@ Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) } } +switch_pos_t +Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else { + return SWITCH_POS_OFF; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1318,13 +1343,13 @@ Sensors::rc_poll() /* signal looks good */ signal_lost = false; - /* check throttle failsafe */ - int8_t thr_ch = _rc.function[THROTTLE]; - if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { - /* throttle failsafe configured */ - if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { - /* throttle failsafe triggered, signal is lost by receiver */ + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; + if (_parameters.rc_fs_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fs_thr)) { + /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } } @@ -1414,10 +1439,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_switch_position(MODE); - manual.easy_switch = get_rc_switch_position(EASY); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); + manual.mode_switch = get_rc_sw3pos_position(MODE); + manual.easy_switch = get_rc_sw2pos_position(EASY); + manual.loiter_switch = get_rc_sw2pos_position(LOITER); + manual.return_switch = get_rc_sw2pos_position(RETURN); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { -- cgit v1.2.3 From 13dfe0447ccfa4f75b551d02b5c979a6ade4c81a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:07:29 +0200 Subject: Send current local position estimate as well --- src/modules/commander/commander.cpp | 24 ++++++++++++++++-------- src/modules/uORB/topics/home_position.h | 5 ++++- 2 files changed, 20 insertions(+), 9 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 352711734..dd2614c1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -927,6 +927,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* update local position estimate */ + orb_check(local_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + } + /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; @@ -952,6 +960,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -968,14 +980,6 @@ int commander_thread_main(int argc, char *argv[]) tune_positive(true); } - /* update local position estimate */ - orb_check(local_position_sub, &updated); - - if (updated) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); @@ -1338,6 +1342,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 08d11abae..70071130d 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -59,10 +59,13 @@ struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ + + float x; /**< X coordinate in meters */ + float y; /**< Y coordinate in meters */ + float z; /**< Z coordinate in meters */ }; /** -- cgit v1.2.3 From 6612cdab856674c4cbec53863c470cb4e96a9f7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:36:09 +0200 Subject: Let commander be less pedantic about positioning data --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 352711734..148a0aafa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 50000 /**< consider the local or global position estimate invalid after 50ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 -- cgit v1.2.3 From 721c90291c12405b4f4a6cf5ddc9ca8cec9f0077 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 27 Apr 2014 02:26:09 +0200 Subject: commander: some HIL commands and land detector cleanup --- src/modules/commander/commander.cpp | 130 +++++++++++++++--------------------- 1 file changed, 53 insertions(+), 77 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6f86e0c2f..13336736d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. * Author: Petri Tanskanen * Lorenz Meier * Thomas Gubler @@ -383,7 +383,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - bool ret = false; /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components @@ -395,89 +394,73 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t) cmd->param1; - uint8_t custom_main_mode = (uint8_t) cmd->param2; - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + uint8_t base_mode = (uint8_t) cmd->param1; + uint8_t custom_main_mode = (uint8_t) cmd->param2; - /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); - - if (arming_res != TRANSITION_DENIED) { - mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { + /* reset the arming mode to disarmed */ + arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); - } - } + // Transition the arming state + transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - if (hil_ret == OK) - ret = true; + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - if (arming_res == TRANSITION_CHANGED) - ret = true; + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - /* use autopilot-specific mode */ - if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } - - } else { - /* use base mode */ - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); - - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); } } + } - if (main_res == TRANSITION_CHANGED) - ret = true; - - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; - break; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + break; + } + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. @@ -503,13 +486,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe 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 = NAVIGATION_STATE_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -525,7 +506,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else { /* reject parachute depoyment not armed */ @@ -981,12 +961,10 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); 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.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; - published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -995,12 +973,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } - } else { - if (!published_condition_landed_fw) { - status.condition_landed = false; // Fixedwing does not have a landing detector currently - published_condition_landed_fw = true; - status_changed = true; - } } /* update battery status */ @@ -1265,6 +1237,8 @@ int commander_thread_main(int argc, char *argv[]) /* LAND not allowed, set TERMINATION state */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } + } else { + status.set_nav_state = NAVIGATION_STATE_MISSION; } } else { @@ -1284,7 +1258,9 @@ int commander_thread_main(int argc, char *argv[]) /* LAND not allowed, set TERMINATION state */ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } - } + } else { + status.set_nav_state = NAVIGATION_STATE_RTL; + } } } else { -- cgit v1.2.3 From 6a7b104c2baad7527d35736979ddd1352bf4119d Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 13:12:28 -0700 Subject: compiles --- src/modules/commander/commander.cpp | 5 +--- src/modules/sensors/sensors.cpp | 34 +---------------------- src/modules/uORB/topics/manual_control_setpoint.h | 4 --- src/modules/uORB/topics/rc_channels.h | 4 --- 4 files changed, 2 insertions(+), 45 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d5f05c991..50380107d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1228,11 +1228,8 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { -<<<<<<< HEAD + /* LOITER switch */ -======= - /* MISSION switch */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a750db12..6449c5283 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -255,11 +255,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; -<<<<<<< HEAD int rc_map_easy_sw; -======= - int rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -313,11 +309,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; -<<<<<<< HEAD param_t rc_map_easy_sw; -======= - param_t rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -534,11 +526,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ -<<<<<<< HEAD _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); -======= - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -679,7 +667,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { @@ -690,21 +678,12 @@ Sensors::parameters_update() warnx("%s", paramerr); } -<<<<<<< HEAD if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { - warnx(paramerr); - } - - if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); -======= - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx("%s", paramerr); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { @@ -745,11 +724,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; -<<<<<<< HEAD _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; -======= - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1502,17 +1477,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ -<<<<<<< HEAD manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv); manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv); manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); -======= - manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c61987df6..b6257e22a 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,11 +78,7 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ -<<<<<<< HEAD switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ -======= - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index df651e78d..d99203ff6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,11 +64,7 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, -<<<<<<< HEAD EASY = 6, -======= - ASSISTED = 6, ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, -- cgit v1.2.3 From 269800b48c31d78fec900b4beaf3f655a8c18730 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 14:06:00 -0700 Subject: renamed EASY to POSHOLD and SEATBELT to ALTHOLD --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 15 files changed, 96 insertions(+), 96 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b75c2297f..aa9c64502 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_EASY) + if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50380107d..a99456370 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { + /* ALTHOLD */ + main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTHOLD"; + main_states_str[2] = "POSHOLD"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on poshold position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->easy_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_EASY); + if (sp_man->poshold_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTHOLD + print_reject_mode(status, "POSHOLD"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->easy_switch != SWITCH_POS_ON) { - print_reject_mode(status, "SEATBELT"); + if (sp_man->poshold_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTHOLD"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTHOLD (POSHOLD likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a946543d..b440e561b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - ut_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTHOLD; + ut_assert("tranisition: manual to althold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTHOLD, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; + new_main_state = MAIN_STATE_ALTHOLD; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - ut_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSHOLD; + ut_assert("transition: manual to poshold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSHOLD, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; + new_main_state = MAIN_STATE_POSHOLD; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 2144d3460..962d2804c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_SEATBELT, - PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_ALTHOLD, + PX4_CUSTOM_MAIN_MODE_POSHOLD, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f09d586c7..21e36a87d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae07275..fafab9bfe 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between 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..d5a68e21f 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 @@ -153,7 +153,7 @@ private: 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 */ + float _althold_hold_heading; /**< heading the system should hold in althold mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* poshold mode enabled */) { - /** EASY FLIGHT **/ + /** POSHOLD FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to poshold */) { + _althold_hold_heading = _att.yaw; } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* poshold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* althold mode enabled */) { - /** SEATBELT FLIGHT **/ + /** ALTHOLD FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to althold */) { + _althold_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* althold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce1645..108ef8ad4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_STATE_POSHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 104df4e59..015d8ad16 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes during flight. + * Limits maximum tilt in AUTO and POSHOLD modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6e..2cb4fc900 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd6e21bdd..59bd99db7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting easy mode + * Threshold for selecting poshold mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 21:47:45 -0700 Subject: Replaces poshold/althold with posctrl/altctrl --- mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 4 +- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 16 files changed, 97 insertions(+), 97 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index 93e868dc3..bd3fc66e7 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -40,8 +40,8 @@ enum AUTOQUAD_NAV_STATUS AQ_NAV_STATUS_INIT=0, /* System is initializing | */ AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ - AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ - AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_ALTCTRL=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSCTRL=8, /* Position hold engaged | */ AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index aa9c64502..974e20ca2 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a99456370..be3e6d269 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { - /* ALTHOLD */ - main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { + /* ALTCTRL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTHOLD"; - main_states_str[2] = "POSHOLD"; + main_states_str[1] = "ALTCTRL"; + main_states_str[2] = "POSCTRL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on poshold position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on posctrl position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->poshold_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSHOLD); + if (sp_man->posctrl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTHOLD - print_reject_mode(status, "POSHOLD"); + // else fallback to ALTCTRL + print_reject_mode(status, "POSCTRL"); } - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->poshold_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTHOLD"); + if (sp_man->posctrl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTRL"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTHOLD (POSHOLD likely will not work too) + // else fallback to ALTCTRL (POSCTRL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b440e561b..18586053b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to ALTHOLD. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTHOLD; - ut_assert("tranisition: manual to althold", + new_main_state = MAIN_STATE_ALTCTRL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); - // MANUAL to ALTHOLD, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTHOLD; + new_main_state = MAIN_STATE_ALTCTRL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to POSHOLD. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSHOLD; - ut_assert("transition: manual to poshold", + new_main_state = MAIN_STATE_POSCTRL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); - // MANUAL to POSHOLD, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSHOLD; + new_main_state = MAIN_STATE_POSCTRL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 962d2804c..e6274fb89 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTHOLD, - PX4_CUSTOM_MAIN_MODE_POSHOLD, + PX4_CUSTOM_MAIN_MODE_ALTCTRL, + PX4_CUSTOM_MAIN_MODE_POSCTRL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21e36a87d..3b6d95135 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index fafab9bfe..dc82ee475 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between 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 d5a68e21f..024dd98ec 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 @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _althold_hold_heading; /**< heading the system should hold in althold mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* poshold mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** POSHOLD FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to poshold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* poshold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* althold mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** ALTHOLD FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to althold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* althold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 108ef8ad4..38a5433ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTHOLD) { + } else if (status->main_state == MAIN_STATE_ALTCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; - } else if (status->main_state == MAIN_STATE_POSHOLD) { + } else if (status->main_state == MAIN_STATE_POSCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 015d8ad16..dacdd46f0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and POSHOLD modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 2cb4fc900..6d46db9bd 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD) { + _status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 59bd99db7..02890b452 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting poshold mode + * Threshold for selecting posctrl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Tue, 29 Apr 2014 20:51:04 -0700 Subject: shortened rc_assisted_th to rc_assist_th in case we add a dedicated switch mapping later --- src/modules/commander/commander.cpp | 6 +++--- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 16 ++++++++-------- 3 files changed, 13 insertions(+), 13 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index be3e6d269..ad15750c0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1145,7 +1145,7 @@ int commander_thread_main(int argc, char *argv[]) /* arm/disarm by RC */ res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && @@ -1299,7 +1299,7 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on posctrl position */ + /* flight termination in manual mode if assist switch is on posctrl position */ if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); @@ -1556,7 +1556,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_MIDDLE: // ASSISTED + case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctrl_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_POSCTRL); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 02890b452..873fff872 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -671,7 +671,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); PARAM_DEFINE_INT32(RC_FAILS_THR, 0); /** - * Threshold for selecting assisted mode + * Threshold for selecting assist mode * * min:-1 * max:+1 @@ -684,7 +684,7 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); * negative : true when channel Date: Sun, 11 May 2014 13:35:05 +0200 Subject: ALTCTRL/POSCTRL renamed to ALTCTL/POSCTL --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 12 +++--- src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 47 insertions(+), 47 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 974e20ca2..c41d8f242 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5124a31f..8c7f6270d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -435,13 +435,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { - /* ALTCTRL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { + /* ALTCTL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -456,8 +456,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -634,8 +634,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTRL"; - main_states_str[2] = "POSCTRL"; + main_states_str[1] = "ALTCTL"; + main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1335,8 +1335,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assist switch is on posctrl position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assist switch is on POSCTL position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1593,25 +1593,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctrl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTRL); + if (sp_man->posctl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTCTRL - print_reject_mode(status, "POSCTRL"); + // else fallback to ALTCTL + print_reject_mode(status, "POSCTL"); } - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctrl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTRL"); + if (sp_man->posctl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTL"); } // else fallback to MANUAL @@ -1626,9 +1626,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTRL (POSCTRL likely will not work too) + // else fallback to ALTCTL (POSCTL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1674,7 +1674,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1685,7 +1685,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 18586053b..ee0d08391 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -320,15 +320,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); @@ -336,15 +336,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e6274fb89..a83c81850 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTCTRL, - PX4_CUSTOM_MAIN_MODE_POSCTRL, + PX4_CUSTOM_MAIN_MODE_ALTCTL, + PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3b6d95135..97a214c33 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 596e286a4..bbb39f20f 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement pos control */) { + } else if (_status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9a7e636c3..b8879157e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTCTRL) { + } else if (status->main_state == MAIN_STATE_ALTCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - } else if (status->main_state == MAIN_STATE_POSCTRL) { + } else if (status->main_state == MAIN_STATE_POSCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 6d46db9bd..91230a37c 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL) { + _status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 727be9492..ac394786d 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -77,8 +77,8 @@ struct manual_control_setpoint_s { float aux5; /**< default function: payload drop */ switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): no effect, return */ - switch_pos_t posctrl_switch; /**< posctrl 2 position switch (optional): altctrl, posctrl */ + switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): normal, return */ + switch_pos_t posctl_switch; /**< posctrl 2 position switch (optional): altctl, posctl */ switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index f401140ae..f56355246 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,8 +63,8 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, - MAIN_STATE_ALTCTRL, - MAIN_STATE_POSCTRL, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, MAIN_STATE_AUTO, MAIN_STATE_MAX } main_state_t; -- cgit v1.2.3 From 349809f5353d70eb9d569a267165e0f1b2054e02 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 11 May 2014 13:36:51 +0200 Subject: sensors, commander: code style fixed --- .../commander/accelerometer_calibration.cpp | 66 +++++---- src/modules/commander/airspeed_calibration.cpp | 6 +- src/modules/commander/commander.cpp | 157 +++++++++++++-------- src/modules/commander/commander_helper.cpp | 20 ++- src/modules/commander/gyro_calibration.cpp | 18 +-- src/modules/commander/mag_calibration.cpp | 31 ++-- src/modules/commander/state_machine_helper.cpp | 156 ++++++++++---------- src/modules/sensors/sensors.cpp | 102 ++++++++----- 8 files changed, 345 insertions(+), 211 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1cbdf9bf8..7180048ff 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix<3,3> board_rotation; + math::Matrix<3, 3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); math::Vector<3> accel_offs_vec(&accel_offs[0]); - math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); - math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec; + math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); @@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } - if (old_done_count != done_count) + if (old_done_count != done_count) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); + } - if (done) + if (done) { break; + } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", @@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); - if (d > still_thr2 * 8.0f) + if (d > still_thr2 * 8.0f) { d = still_thr2 * 8.0f; + } - if (d > accel_disp[i]) + if (d > accel_disp[i]) { accel_disp[i] = d; + } } /* still detector with hysteresis */ @@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 0; // [ g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 0; // [ g, 0, 0 ] + } if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 1; // [ -g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 1; // [ -g, 0, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 2; // [ 0, g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 2; // [ 0, g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 3; // [ 0, -g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 3; // [ 0, -g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) - return 4; // [ 0, 0, g ] + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { + return 4; // [ 0, 0, g ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) - return 5; // [ 0, 0, -g ] + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { + return 5; // [ 0, 0, -g ] + } mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); @@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { accel_sum[i] += sensor.accelerometer_m_s2[i]; + } count++; @@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp continue; } - if (errcount > samples_num / 10) + if (errcount > samples_num / 10) { return ERROR; + } } for (int i = 0; i < 3; i++) { @@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) - return ERROR; // Singular matrix + if (det == 0.0f) { + return ERROR; // Singular matrix + } dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; @@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; - if (mat_invert3(mat_A, mat_A_inv) != OK) + if (mat_invert3(mat_A, mat_A_inv) != OK) { return ERROR; + } /* copy results to accel_T */ for (int i = 0; i < 3; i++) { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index c8c7a42e7..5d21d89d0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd) bool paramreset_successful = false; int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } + close(fd); } @@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8c7f6270d..612393bc1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -221,7 +221,7 @@ void print_status(); transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy); +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -233,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul int commander_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -261,8 +262,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { - if (!thread_running) + if (!thread_running) { errx(0, "commander already stopped"); + } thread_should_exit = true; @@ -304,8 +306,9 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); exit(1); @@ -364,20 +367,22 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - - // Transition the armed state. By passing mavlink_fd to arming_state_transition it will - // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); - if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); - } - - return arming_res; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will + // output appropriate error messages if the state cannot transition. + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + + if (arming_res == TRANSITION_CHANGED && mavlink_fd) { + mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; } bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) @@ -417,14 +422,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (hil_ret == OK) + if (hil_ret == OK) { ret = true; + } - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + // Transition the arming state + arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - if (arming_res == TRANSITION_CHANGED) + if (arming_res == TRANSITION_CHANGED) { ret = true; + } /* set main state */ transition_result_t main_res = TRANSITION_DENIED; @@ -466,8 +473,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (main_res == TRANSITION_CHANGED) + if (main_res == TRANSITION_CHANGED) { ret = true; + } if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -480,24 +488,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { + // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. + // We use an float epsilon delta to test float equality. + if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); - } else { - // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; - } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); - if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } - } + } else { + + // Flick to inair restore first if this comes from an onboard system + if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { + status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + } + + transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + } } break; @@ -525,7 +537,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command //XXX: to enable the parachute, a param needs to be set @@ -545,6 +557,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; + if (use_current) { /* use current position */ if (status->condition_global_position_valid) { @@ -588,6 +601,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -601,6 +615,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ answer_command(*cmd, result); @@ -883,6 +898,7 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); } + /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); @@ -923,6 +939,7 @@ int commander_thread_main(int argc, char *argv[]) /* disarm if safety is now on and still armed */ if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); } @@ -940,9 +957,11 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; + if (status.condition_global_position_valid) { if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { eph_epv_good = false; + } else { eph_epv_good = true; } @@ -950,17 +969,19 @@ int commander_thread_main(int argc, char *argv[]) } else { if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { eph_epv_good = true; + } else { eph_epv_good = false; } } + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -995,6 +1016,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_landed != local_position.landed) { status.condition_landed = local_position.landed; @@ -1008,6 +1030,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { if (!published_condition_landed_fw) { status.condition_landed = false; // Fixedwing does not have a landing detector currently @@ -1077,8 +1100,9 @@ int commander_thread_main(int argc, char *argv[]) /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (last_idle_time > 0) - status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + if (last_idle_time > 0) { + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + } last_idle_time = system_load.tasks[0].total_runtime; @@ -1255,7 +1279,7 @@ int commander_thread_main(int argc, char *argv[]) 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) { + 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(); @@ -1320,6 +1344,7 @@ int commander_thread_main(int argc, char *argv[]) if (res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); } @@ -1350,8 +1375,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) + if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { status_changed = true; + } } /* check which state machines for changes, clear "changed" flag */ @@ -1368,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; @@ -1390,6 +1416,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_home_position_valid = true; } } + was_armed = armed.armed; if (main_state_changed) { @@ -1553,21 +1580,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ - if (leds_counter % 20 == 0) + if (leds_counter % 20 == 0) { led_toggle(LED_BLUE); + } } else { /* not ready to arm, blink at 10Hz */ - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_BLUE); + } } #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); + } } else { led_off(LED_AMBER); @@ -1781,7 +1811,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(true); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: @@ -1831,8 +1861,9 @@ void *commander_low_prio_loop(void *arg) int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); /* timed out - periodic check for thread_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1847,8 +1878,9 @@ void *commander_low_prio_loop(void *arg) if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || cmd.command == VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == VEHICLE_CMD_DO_SET_SERVO) + cmd.command == VEHICLE_CMD_DO_SET_SERVO) { continue; + } /* only handle low-priority commands here */ switch (cmd.command) { @@ -1926,6 +1958,7 @@ void *commander_low_prio_loop(void *arg) /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); + } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { @@ -1940,10 +1973,12 @@ void *commander_low_prio_loop(void *arg) } - if (calib_ret == OK) + if (calib_ret == OK) { tune_positive(true); - else + + } else { tune_negative(true); + } arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); @@ -1963,11 +1998,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1983,11 +2020,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1997,8 +2036,8 @@ void *commander_low_prio_loop(void *arg) } case VEHICLE_CMD_START_RX_PAIR: - /* handled in the IO driver */ - break; + /* handled in the IO driver */ + break; default: /* don't answer on unsupported commands, it will be done in main loop */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0fd3c9e9e..86f77cdb8 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -113,17 +113,22 @@ void buzzer_deinit() close(buzzer); } -void set_tune(int tune) { +void set_tune(int tune) +{ unsigned int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } + tune_current = tune; + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; + } else { tune_end = 0; } @@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); } @@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEUTRAL_TUNE); } @@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEGATIVE_TUNE); } @@ -244,22 +252,25 @@ int led_off(int led) void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + } } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + } } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + } } float battery_remaining_estimate_voltage(float voltage, float discharged) @@ -299,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { /* else use voltage */ ret = remaining_voltage; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 30cd0d48d..cbc2844c1 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd) gyro_scale.z_offset += gyro_report.z; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else { poll_errcount++; @@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd) /* apply new offsets */ fd = open(GYRO_DEVICE_PATH, 0); - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) { warn("WARNING: failed to apply new offsets for gyro"); + } close(fd); @@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd) float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; } - if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; + if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; } uint64_t last_time = hrt_absolute_time(); @@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd) //float mag = -atan2f(magNav(1),magNav(0)); float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2 * M_PI_F; + if (mag > M_PI_F) { mag -= 2 * M_PI_F; } - if (mag < -M_PI_F) mag += 2 * M_PI_F; + if (mag < -M_PI_F) { mag += 2 * M_PI_F; } float diff = mag - mag_last; - if (diff > M_PI_F) diff -= 2 * M_PI_F; + if (diff > M_PI_F) { diff -= 2 * M_PI_F; } - if (diff < -M_PI_F) diff += 2 * M_PI_F; + if (diff < -M_PI_F) { diff += 2 * M_PI_F; } baseline_integral += diff; mag_last = mag; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4ebf266f4..9296db6ed 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -124,6 +124,7 @@ int do_mag_calibration(int mavlink_fd) res = ERROR; return res; } + } else { /* exit */ return ERROR; @@ -163,8 +164,9 @@ int do_mag_calibration(int mavlink_fd) calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) + if (calibration_counter % (calibration_maxcount / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } } else { poll_errcount++; @@ -198,14 +200,17 @@ int do_mag_calibration(int mavlink_fd) } } - if (x != NULL) + if (x != NULL) { free(x); + } - if (y != NULL) + if (y != NULL) { free(y); + } - if (z != NULL) + if (z != NULL) { free(z); + } if (res == OK) { /* apply calibration and set parameters */ @@ -234,23 +239,29 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { res = ERROR; + } if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 97a214c33..abcf72717 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -75,38 +75,38 @@ static bool failsafe_state_changed = true; // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char* state_names[ARMING_STATE_MAX] = { - "ARMING_STATE_INIT", - "ARMING_STATE_STANDBY", - "ARMING_STATE_ARMED", - "ARMING_STATE_ARMED_ERROR", - "ARMING_STATE_STANDBY_ERROR", - "ARMING_STATE_REBOOT", - "ARMING_STATE_IN_AIR_RESTORE", +static const char *state_names[ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", }; transition_result_t arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none + const struct safety_s *safety, /// current safety settings + arming_state_t new_arming_state, /// arming state requested + struct actuator_armed_s *armed, /// current armed status + const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { - // Double check that our static arrays are still valid - ASSERT(ARMING_STATE_INIT == 0); - ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); - + // Double check that our static arrays are still valid + ASSERT(ARMING_STATE_INIT == 0); + ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + /* * Perform an atomic state update */ @@ -117,63 +117,70 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* only check transition if the new state is actually different from the current one */ if (new_arming_state == status->arming_state) { ret = TRANSITION_NOT_CHANGED; + } else { /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } - - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; - if (valid_transition) { - // We have a good transition. Now perform any secondary validation. - if (new_arming_state == ARMING_STATE_ARMED) { - // Fail transition if we need safety switch press - // Allow if coming from in air restore - // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); - } - valid_transition = false; - } - } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { - new_arming_state = ARMING_STATE_STANDBY_ERROR; - } - } - - // HIL can always go to standby - if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { - valid_transition = true; - } - - /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - valid_transition = false; - } - - // Finish up the state transition - if (valid_transition) { - armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; - ret = TRANSITION_CHANGED; - status->arming_state = new_arming_state; - arming_state_changed = true; - } - } - + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == ARMING_STATE_ARMED) { + // Fail transition if we need safety switch press + // Allow if coming from in air restore + // Allow if HIL_STATE_ON + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } + + valid_transition = false; + } + + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; + } + } + + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; + } + + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) { - static const char* errMsg = "Invalid arming transition from %s to %s"; - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } - warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } + if (ret == TRANSITION_DENIED) { + static const char *errMsg = "Invalid arming transition from %s to %s"; + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } + + warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } return ret; } @@ -320,6 +327,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; d = opendir("/dev"); + if (d) { struct dirent *direntry; @@ -331,26 +339,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (!strncmp("tty", direntry->d_name, 3)) { continue; } + /* skip mtd devices */ if (!strncmp("mtd", direntry->d_name, 3)) { continue; } + /* skip ram devices */ if (!strncmp("ram", direntry->d_name, 3)) { continue; } + /* skip MMC devices */ if (!strncmp("mmc", direntry->d_name, 3)) { continue; } + /* skip mavlink */ if (!strcmp("mavlink", direntry->d_name)) { continue; } + /* skip console */ if (!strcmp("console", direntry->d_name)) { continue; } + /* skip null */ if (!strcmp("null", direntry->d_name)) { continue; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a02e8187d..c561d1c0a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -220,8 +220,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ @@ -644,8 +644,9 @@ Sensors::parameters_update() } /* handle wrong values */ - if (!rc_valid) + if (!rc_valid) { warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + } const char *paramerr = "FAIL PARM LOAD"; @@ -701,19 +702,19 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); - _parameters.rc_assist_inv = (_parameters.rc_assist_th<0); + _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); _parameters.rc_assist_th = fabs(_parameters.rc_assist_th); param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); - _parameters.rc_auto_inv = (_parameters.rc_auto_th<0); + _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); - _parameters.rc_posctl_inv = (_parameters.rc_posctl_th<0); + _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th)); - _parameters.rc_return_inv = (_parameters.rc_return_th<0); + _parameters.rc_return_inv = (_parameters.rc_return_th < 0); _parameters.rc_return_th = fabs(_parameters.rc_return_th); param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); - _parameters.rc_loiter_inv = (_parameters.rc_loiter_th<0); + _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); /* update RC function mappings */ @@ -843,12 +844,14 @@ Sensors::gyro_init() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ - if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) + if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) { ioctl(fd, GYROIOCSSAMPLERATE, 800); + } /* set the driver to poll at 1000Hz */ - if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) + if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) { ioctl(fd, SENSORIOCSPOLLRATE, 800); + } #else @@ -903,12 +906,15 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCGEXTERNAL, 0); - if (ret < 0) + if (ret < 0) { errx(1, "FATAL: unknown if magnetometer is external or onboard"); - else if (ret == 1) + + } else if (ret == 1) { _mag_is_external = true; - else + + } else { _mag_is_external = false; + } close(fd); } @@ -1008,10 +1014,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - if (_mag_is_external) + if (_mag_is_external) { vect = _external_mag_rotation * vect; - else + + } else { vect = _board_rotation * vect; + } raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1129,8 +1137,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.gyro_scale[2], }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) { warn("WARNING: failed to set scale / offsets for gyro"); + } close(fd); @@ -1144,8 +1153,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.accel_scale[2], }; - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) { warn("WARNING: failed to set scale / offsets for accel"); + } close(fd); @@ -1159,8 +1169,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.mag_scale[2], }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) { warn("WARNING: failed to set scale / offsets for mag"); + } close(fd); @@ -1174,8 +1185,10 @@ Sensors::parameter_update_poll(bool forced) 1.0f, }; - if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) + if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + close(fd); } @@ -1193,10 +1206,12 @@ void Sensors::adc_poll(struct sensor_combined_s &raw) { /* only read if publishing */ - if (!_publishing) + if (!_publishing) { return; + } hrt_abstime t = hrt_absolute_time(); + /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ @@ -1221,6 +1236,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_v = voltage; + /* one-time initialization of low-pass value to avoid long init delays */ if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_filtered_v = voltage; @@ -1239,19 +1255,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* handle current only if voltage is valid */ if (_battery_status.voltage_v > 0.0f) { float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + /* check measured current value */ if (current >= 0.0f) { _battery_status.timestamp = t; _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { /* initialize discharged value */ - if (_battery_status.discharged_mah < 0.0f) + if (_battery_status.discharged_mah < 0.0f) { _battery_status.discharged_mah = 0.0f; + } + _battery_discharged += current * (t - _battery_current_timestamp); _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; } } } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1284,7 +1305,9 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } + _last_adc = t; + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { /* announce the battery status if needed, just publish else */ if (_battery_pub > 0) { @@ -1303,6 +1326,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max { if (_rc.function[func] >= 0) { float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { return min_value; @@ -1312,6 +1336,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } else { return value; } + } else { return 0.0f; } @@ -1322,6 +1347,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1342,6 +1368,7 @@ Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1380,13 +1407,15 @@ Sensors::rc_poll() /* check failsafe */ int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle - if (_parameters.rc_map_failsafe>0){ // if not 0, use channel number instead of rc.function mapping + + if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping fs_ch = _parameters.rc_map_failsafe - 1; } + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { /* failsafe configured */ if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || - (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } @@ -1395,8 +1424,9 @@ Sensors::rc_poll() unsigned channel_limit = rc_input.channel_count; - if (channel_limit > _rc_max_chan_count) + if (channel_limit > _rc_max_chan_count) { channel_limit = _rc_max_chan_count; + } /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1404,11 +1434,13 @@ Sensors::rc_poll() /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (rc_input.values[i] < _parameters.min[i]) + if (rc_input.values[i] < _parameters.min[i]) { rc_input.values[i] = _parameters.min[i]; + } - if (rc_input.values[i] > _parameters.max[i]) + if (rc_input.values[i] > _parameters.max[i]) { rc_input.values[i] = _parameters.max[i]; + } /* * 2) Scale around the mid point differently for lower and upper range. @@ -1440,8 +1472,9 @@ Sensors::rc_poll() _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (!isfinite(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) { _rc.chan[i].scaled = 0.0f; + } } _rc.chan_count = rc_input.channel_count; @@ -1623,8 +1656,9 @@ Sensors::task_main() diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ - if (_publishing) + if (_publishing) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + } /* Look for new r/c input data */ rc_poll(); @@ -1661,18 +1695,21 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: sensors {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (sensors::g_sensors != nullptr) + if (sensors::g_sensors != nullptr) { errx(0, "already running"); + } sensors::g_sensors = new Sensors; - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "alloc failed"); + } if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; @@ -1684,8 +1721,9 @@ int sensors_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "not running"); + } delete sensors::g_sensors; sensors::g_sensors = nullptr; -- cgit v1.2.3 From dd04a70afa94980dc38a82c592dc61e062c3f853 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 18:10:02 +0200 Subject: Reporting cleanup, use different variables for different state switching results to avoid being tripped on local / global name scope --- src/modules/commander/commander.cpp | 64 +++++++++++++------------------------ 1 file changed, 23 insertions(+), 41 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 612393bc1..22504642f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1178,10 +1178,10 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t arming_res; // store all transitions results here /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ @@ -1193,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1215,7 +1215,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1228,7 +1228,7 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (res == TRANSITION_CHANGED) { + if (arming_res == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); @@ -1236,24 +1236,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - } else if (res == TRANSITION_DENIED) { + } else if (arming_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status, &sp_man); + transition_result_t main_res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ - if (res == TRANSITION_CHANGED) { + if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); - } else if (res == TRANSITION_DENIED) { + } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } @@ -1296,57 +1296,39 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed) { if (status.main_state == MAIN_STATE_AUTO) { /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_NOT_CHANGED) { + if (auto_res == TRANSITION_NOT_CHANGED) { last_auto_state_valid = hrt_absolute_time(); } /* still invalid state after the timeout interval, execute failsafe */ - if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (res == TRANSITION_DENIED)) { + if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { + if (auto_res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); - } - - } else if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } else { /* failsafe for manual modes */ - transition_result_t res = TRANSITION_DENIED; + transition_result_t manual_res = TRANSITION_DENIED; if (!status.condition_landed) { /* vehicle is not landed, try to perform RTL */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND"); - } + manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); } - if (res == TRANSITION_DENIED) { + if (manual_res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); - } + manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { + if (manual_res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - } else if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } @@ -1354,7 +1336,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* reset failsafe when disarmed */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } -- cgit v1.2.3 From cde4c9addbe2e8ccd782c53daf519fcf9669626a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:21:27 +0200 Subject: commander: use new manual control setpoint variable names --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/commander/rc_calibration.cpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfab9d4d6..fb644a8db 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -367,7 +367,7 @@ static orb_advert_t status_pub; transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; - + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); @@ -376,7 +376,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } - + return arming_res; } @@ -1164,7 +1164,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1182,7 +1182,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 41f3ca0aa..0776894fb 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; + float p = sp.y; param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; + p = sp.x; param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; + p = sp.r; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ -- cgit v1.2.3 From 531ba79e55e9ccf9396ee30f067c933b4ec9c649 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:34:37 +0200 Subject: Reduce commander stack size mildly --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17d3d3dcd..f92c7fc31 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2800, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); -- cgit v1.2.3 From 8dc0a21a7ee83dea5b3a3ac719c6a9e64aff78f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:14:58 +0200 Subject: mavlink, commander: Get back close to original stack sizes. Although tests came clean, we do not want to take any chances --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f92c7fc31..17d3d3dcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2800, + 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d222f9edc..611a8b804 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2193,7 +2193,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1800, + 2000, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From 1e13b5a02c4aab5b32f43b6bf2b3588674ee84ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:02:31 +0200 Subject: commander: Reduce calibration count, minimally reduce stack sizes after careful inspection --- src/modules/commander/commander.cpp | 4 ++-- src/modules/commander/mag_calibration.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17d3d3dcd..13da27dcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2950, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -743,7 +743,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 2900); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 388820511..0ead22f77 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_interval = 45 * 1000 * 1000; /* maximum 500 values */ - const unsigned int calibration_maxcount = 500; + const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; struct mag_scale mscale_null = { -- cgit v1.2.3 From 692e8f84a93a932986004d896554a70380ea11e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 May 2014 22:12:07 +0200 Subject: commander: don't require good EPH for local_position_valid, position_estimator_inav: estimate EPH/EPV and publish it in local position topic --- src/modules/commander/commander.cpp | 21 ++++++++++++++++++++- .../position_estimator_inav_main.c | 21 +++++++++++++++++---- src/modules/sdlog2/sdlog2.c | 10 ++++++++-- src/modules/sdlog2/sdlog2_messages.h | 7 ++++--- src/modules/uORB/topics/vehicle_local_position.h | 2 ++ 5 files changed, 51 insertions(+), 10 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd..e4ae357d7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1012,7 +1012,26 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_position_valid and condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); + /* hysteresis for EPH */ + bool local_eph_good; + + if (status.condition_global_position_valid) { + if (local_position.eph > eph_epv_threshold * 2.0f) { + local_eph_good = false; + + } else { + local_eph_good = true; + } + + } else { + if (local_position.eph < eph_epv_threshold) { + local_eph_good = true; + + } else { + local_eph_good = false; + } + } + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); 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; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d8d0ff37d..fdc3233e0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -199,6 +199,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float eph = 1.0; + float epv = 1.0; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -535,6 +538,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_valid = true; + eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm + } else { w_flow = 0.0f; flow_valid = false; @@ -673,6 +678,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } + eph = fminf(eph, gps.eph_m); + epv = fminf(epv, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } @@ -712,6 +720,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; + /* increase EPH/EPV on each step */ + eph *= 1.0 + dt; + epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; @@ -723,7 +735,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); + bool can_estimate_xy = eph < max_eph_epv * 1.5; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -922,6 +934,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.landed = landed; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; + local_pos.eph = eph; + local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; @@ -950,9 +964,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.yaw = local_pos.yaw; - // TODO implement dead-reckoning - global_pos.eph = gps.eph_m; - global_pos.epv = gps.epv_m; + global_pos.eph = eph; + global_pos.epv = epv; if (vehicle_global_position_pub < 0) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39f433eb5..70ce76806 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1119,10 +1119,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); - log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) | + (buf.local_pos.z_valid ? 2 : 0) | + (buf.local_pos.v_xy_valid ? 4 : 0) | + (buf.local_pos.v_z_valid ? 8 : 0) | + (buf.local_pos.xy_global ? 16 : 0) | + (buf.local_pos.z_global ? 32 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + log_msg.body.log_LPOS.eph = buf.local_pos.eph; + log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0c6188657..90025b9ff 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,10 +109,11 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; - uint8_t xy_flags; - uint8_t z_flags; + uint8_t pos_flags; uint8_t landed; uint8_t ground_dist_flags; + float eph; + float epv; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -360,7 +361,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index a15303ea4..5d39c897d 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -83,6 +83,8 @@ struct vehicle_local_position_s { float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ + float eph; + float epv; }; /** -- cgit v1.2.3 From 32ae2dd1d094f4554f5acdad8fb76ed9eb3ba1f0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 11:00:14 +0200 Subject: commander: missed 'break' in 'switch' added --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd..504696ff9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1710,6 +1710,7 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; default: break; -- cgit v1.2.3 From 559bfbb11cdac499cda8070c32e2b7e79b3b883e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 16:42:11 +0200 Subject: commander: allow disarm in ACRO mode --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8336bcf32..45ef79d7c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1192,7 +1192,7 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { -- cgit v1.2.3 From 5f752cdb7cf6c74e34b3ac79b9220c70172791e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:41:48 +0200 Subject: commander: Better audio indications on arming reject reasons --- src/modules/commander/commander.cpp | 6 +++--- src/modules/commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c26200048..0463cf8f8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1233,10 +1233,10 @@ int commander_thread_main(int argc, char *argv[]) sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); + print_reject_arm("#audio: NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f1a353d5b..87309b238 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -138,7 +138,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Allow if HIL_STATE_ON if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); } valid_transition = false; @@ -312,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); valid_transition = false; break; -- cgit v1.2.3 From a359a1b9408c7e4827c7dcbf7c27bf76ea071662 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:02:24 +0200 Subject: Fixed audio output to provide decent user feedback --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0463cf8f8..5c0628a16 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1848,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); + /* this needs additional hints to the user - so let other messages pass and be spoken */ + mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command); tune_negative(true); break; -- cgit v1.2.3 From c7bf00562d685b513e1720f558fee5840aca151b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 08:22:54 +0200 Subject: commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail --- src/modules/commander/commander.cpp | 27 ++++++++++++++++++++++++++ src/modules/commander/state_machine_helper.cpp | 22 +++++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 51 insertions(+) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a16..55c74fdff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -717,6 +718,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = hrt_absolute_time(); + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = -1.0f; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -853,6 +857,11 @@ int commander_thread_main(int argc, char *argv[]) struct position_setpoint_triplet_s pos_sp_triplet; memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + /* Subscribe to system power */ + int system_power_sub = orb_subscribe(ORB_ID(system_power)); + struct system_power_s system_power; + memset(&system_power, 0, sizeof(system_power)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -933,6 +942,24 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); } + orb_check(system_power_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), system_power_sub, &system_power); + + if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + if (system_power.servo_valid && + !system_power.brick_valid && + !system_power.usb_connected) { + /* flying only on servo rail, this is unsafe */ + status.condition_power_input_valid = false; + } else { + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = system_power.voltage5V_v; + } + } + } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); /* update safety topic */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..818974648 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,6 +144,28 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } + + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } + + valid_transition = false; + } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { new_arming_state = ARMING_STATE_STANDBY_ERROR; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ea20a317a..2ed0d6ad4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -179,6 +179,8 @@ struct vehicle_status_s { bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ + bool condition_power_input_valid; /**< set if input power is valid */ + float avionics_power_rail_voltage; /**< voltage of the avionics power rail */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ -- cgit v1.2.3 From 064329dd099819b0f0b8a9a7bc0233440fdf6df1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:32 +0200 Subject: commander: put circuit breaker into effect --- src/modules/commander/commander.cpp | 7 ++++++ src/modules/commander/state_machine_helper.cpp | 32 +++++++++++++++----------- 2 files changed, 25 insertions(+), 14 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55c74fdff..71067ac4f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -721,6 +722,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = true; status.avionics_power_rail_voltage = -1.0f; + // CIRCUIT BREAKERS + status.circuit_breaker_engaged_power_check = false; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -907,6 +911,9 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); + + status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status_changed = true; /* re-check RC calibration */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 818974648..84f4d03af 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,26 +144,30 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } - // Fail transition if power is not good - if (!status->condition_power_input_valid) { + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + valid_transition = false; } - valid_transition = false; - } + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { - // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + valid_transition = false; } - - valid_transition = false; } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From ed6c2a5168ca891f20594687acfd3c6bbf7e1cf9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 May 2014 21:56:32 +0200 Subject: commander and navigator: lot's of changes, failsafe handling in commander, navigator only for execution (WIP) --- src/drivers/blinkm/blinkm.cpp | 5 +- src/drivers/gps/mtk.cpp | 4 +- src/modules/commander/commander.cpp | 472 ++++++++++----------- src/modules/commander/state_machine_helper.cpp | 414 ++++-------------- src/modules/commander/state_machine_helper.h | 16 +- src/modules/mavlink/mavlink_messages.cpp | 86 ++-- src/modules/navigator/navigator_main.cpp | 240 +---------- src/modules/systemlib/state_table.h | 28 +- src/modules/uORB/topics/mission_result.h | 1 + .../uORB/topics/position_setpoint_triplet.h | 15 +- src/modules/uORB/topics/vehicle_status.h | 35 +- 11 files changed, 411 insertions(+), 905 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..98c491ce6 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -655,13 +655,14 @@ BlinkM::led() /* indicate main control state */ if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) + /* TODO: add other Auto modes */ + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; - else + else led_color_4 = LED_OFF; led_color_5 = led_color_4; } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index ea4e95fb2..41716cd97 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -251,8 +251,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->lon = 0; // Indicate this data is not usable and bail out - _gps_position->eph_m = 1000.0f; - _gps_position->epv_m = 1000.0f; + _gps_position->eph = 1000.0f; + _gps_position->epv = 1000.0f; _gps_position->fix_type = 0; return; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 77d810a81..908384bb6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,11 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,8 +33,13 @@ /** * @file commander.cpp - * Main system state machine implementation. + * Main fail-safe handling. * + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -87,6 +88,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -387,103 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { - /* result of the command */ - enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } - /* only handle high-priority commands here */ + /* result of the command */ + enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t) cmd->param1; - uint8_t custom_main_mode = (uint8_t) cmd->param2; + uint8_t base_mode = (uint8_t)cmd->param1; + uint8_t custom_main_mode = (uint8_t)cmd->param2; - /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); - } + transition_result_t arming_ret = TRANSITION_NOT_CHANGED; - // Transition the arming state - transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + transition_result_t main_ret = TRANSITION_NOT_CHANGED; - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - if (hil_ret == OK) { - ret = true; - } - - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - - if (arming_res == TRANSITION_CHANGED) { - ret = true; - } + // Transition the arming state + arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_res = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status, MAIN_STATE_ACRO); } - } else { + } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } } } - } - if (main_res == TRANSITION_CHANGED) { - ret = true; - } + if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) { + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } - break; - } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. @@ -502,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } } } @@ -516,14 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_LOITER; + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_MISSION; + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -531,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; +#if 0 /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command @@ -538,15 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { /* reject parachute depoyment not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; +#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; @@ -560,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } else { @@ -574,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) { warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); @@ -609,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); + answer_command(*cmd, cmd_result); } /* send any requested ACKs */ - if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } @@ -682,7 +663,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_NONE; + status.set_nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -772,6 +753,11 @@ int commander_thread_main(int argc, char *argv[]) safety.safety_switch_available = false; safety.safety_off = false; + /* Subscribe to mission result topic */ + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -849,6 +835,13 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); + transition_result_t arming_ret; + + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = false; + bool main_state_changed = false; + bool failsafe_state_changed = false; + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { @@ -856,6 +849,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + arming_ret = TRANSITION_NOT_CHANGED; + + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -935,6 +931,7 @@ int commander_thread_main(int argc, char *argv[]) if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + arming_state_changed = true; } } } @@ -1131,12 +1128,21 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 1"); + arming_state_changed = true; + } } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 2"); + arming_state_changed = true; + } + } status_changed = true; } @@ -1144,11 +1150,18 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { - // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + /* TODO: check for sensors */ + warnx("arming status1: %d", status.arming_state); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + warnx("arming status2: %d", status.arming_state); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed"); + arming_state_changed = true; + } } else { - // XXX: Add emergency stuff if sensors are lost + /* TODO: Add emergency stuff if sensors are lost */ } @@ -1167,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + } + /* start RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1184,11 +1203,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t arming_res; // store all transitions results here - - /* arm/disarm by RC */ - arming_res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && @@ -1199,7 +1213,11 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 1"); + arming_state_changed = true; + } stick_off_counter = 0; } else { @@ -1221,7 +1239,11 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 2"); + arming_state_changed = true; + } } stick_on_counter = 0; @@ -1234,23 +1256,25 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (arming_res == TRANSITION_CHANGED) { + if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + arming_state_changed = true; - } else if (arming_res == TRANSITION_DENIED) { + } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - } + + // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + // /* recover from failsafe */ + // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + // } /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1258,102 +1282,41 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); + main_state_changed = true; } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAVIGATION_STATE_RTL; - - - } else { - - /* LOITER switch */ - if (sp_man.loiter_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAVIGATION_STATE_LOITER; - - } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - } - } - } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } + } - if (armed.armed) { - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - - if (auto_res == TRANSITION_NOT_CHANGED) { - last_auto_state_valid = hrt_absolute_time(); - } - - /* still invalid state after the timeout interval, execute failsafe */ - if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (auto_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_MISSION; - } + if (armed.armed) { + /* if RC is lost, switch to RTL_RC or Termination unless a mission is running + * and not yet finished) */ + if (status.rc_signal_lost + && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; } else { - /* failsafe for manual modes */ - transition_result_t manual_res = TRANSITION_DENIED; - - if (!status.condition_landed) { - /* vehicle is not landed, try to perform RTL */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - } - - if (manual_res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (manual_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_RTL; - } - } - - } else { - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* reset failsafe when disarmed */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + status.failsafe_state = FAILSAFE_STATE_LAND; } } - } - // TODO remove this hack - /* flight termination in manual mode if assist switch is on POSCTL position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { - if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(armed.armed); - } + /* if the data link is lost, switch to RTL_DL or Termination */ + /* TODO: add this */ + + } else { + /* reset failsafe when disarmed */ + status.failsafe_state = FAILSAFE_STATE_NORMAL; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1369,11 +1332,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check which state machines for changes, clear "changed" flag */ - bool arming_state_changed = check_arming_state_changed(); - bool main_state_changed = check_main_state_changed(); - bool failsafe_state_changed = check_failsafe_state_changed(); - hrt_abstime t1 = hrt_absolute_time(); /* print new state */ @@ -1408,18 +1366,24 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } was_armed = armed.armed; + /* now set nav state according to failsafe and main state */ + set_nav_state(&status); + if (main_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + main_state_changed = false; } if (failsafe_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + failsafe_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1608,6 +1572,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; + warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL @@ -1648,14 +1613,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // else fallback to ALTCTL (POSCTL likely will not work too) - print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1675,85 +1639,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } void - set_control_mode() { - /* set vehicle_control_mode according to main state and failsafe state */ + /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; + /* TODO: check this */ control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator should act */ - bool navigator_enabled = false; - - switch (status.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (status.main_state) { - case MAIN_STATE_MANUAL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - break; + switch (status.set_nav_state) { + case NAVIGATION_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - case MAIN_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - default: - break; - } + case NAVIGATION_STATE_ALTCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_POSCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_RTL: - navigator_enabled = true; + case NAVIGATION_STATE_AUTO_MISSION: + case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_LAND: - navigator_enabled = true; + case NAVIGATION_STATE_LAND: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_TERMINATION: + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -1769,21 +1741,6 @@ set_control_mode() default: break; } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - - /* in failsafe LAND mode position may be not available */ - control_mode.flag_control_position_enabled = status.condition_local_position_valid; - control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; - - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - } } void @@ -1927,7 +1884,6 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ - // XXX disable interrupts in arming_state_transition if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6f0e9794a..214480079 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -59,30 +60,20 @@ #include "state_machine_helper.h" #include "commander_helper.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -static bool arming_state_changed = true; -static bool main_state_changed = true; -static bool failsafe_state_changed = true; - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation @@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; - arming_state_changed = true; } } @@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet } } -bool -check_arming_state_changed() -{ - if (arming_state_changed) { - arming_state_changed = false; - return true; - - } else { - return false; - } -} - transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* transition may be denied even if requested the same state because conditions may be changed */ + /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { case MAIN_STATE_MANUAL: - ret = TRANSITION_CHANGED; - break; - case MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; case MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ + /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || (status->condition_local_altitude_valid || status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } - break; case MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } - break; - case MAIN_STATE_AUTO: - + case MAIN_STATE_AUTO_MISSION: + case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case MAIN_STATE_AUTO_RTL: + /* need global position and home position */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } break; - } + case MAIN_STATE_MAX: + default: + break; + } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { status->main_state = new_main_state; - main_state_changed = true; - } else { ret = TRANSITION_NOT_CHANGED; } @@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } -bool -check_main_state_changed() -{ - if (main_state_changed) { - main_state_changed = false; - return true; - - } else { - return false; - } -} - -bool -check_failsafe_state_changed() -{ - if (failsafe_state_changed) { - failsafe_state_changed = false; - return true; - - } else { - return false; - } -} - /** -* Transition from one hil state to another -*/ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_transition = false; - int ret = ERROR; + transition_result_t ret = TRANSITION_DENIED; if (current_status->hil_state == new_state) { - valid_transition = true; + ret = TRANSITION_NOT_CHANGED; } else { - switch (new_state) { - case HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - valid_transition = false; - + ret = TRANSITION_DENIED; break; case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - - // Disable publication of all attached sensors - + /* Disable publication of all attached sensors */ /* list directory */ DIR *d; d = opendir("/dev"); if (d) { - struct dirent *direntry; char devname[24]; @@ -388,288 +332,98 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } - closedir(d); + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + } else { /* failed opening dir */ - warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); - return 1; + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; } + } else { + mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + ret = TRANSITION_DENIED; } - break; default: - warnx("Unknown hil state"); + warnx("Unknown HIL state"); break; } } - if (valid_transition) { + if (ret = TRANSITION_CHANGED) { current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - // XXX also set lockdown here - - ret = OK; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } - return ret; } /** -* Transition from one failsafe state to another -*/ -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) + * Check failsafe and main status and set navigation status for navigator accordingly + */ +void set_nav_state(struct vehicle_status_s *status) { - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if requested the same state because conditions may be changed */ - if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { - /* transitions from TERMINATION to other states not allowed */ - if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { - ret = TRANSITION_NOT_CHANGED; - } - - } else { - switch (new_failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* always allowed (except from TERMINATION state) */ - ret = TRANSITION_CHANGED; + switch (status->failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_MANUAL: + status->set_nav_state = NAVIGATION_STATE_MANUAL; break; - case FAILSAFE_STATE_RTL: - - /* global position and home position required for RTL */ - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAVIGATION_STATE_RTL; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_ALTCTL: + status->set_nav_state = NAVIGATION_STATE_ALTCTL; + break; + case MAIN_STATE_POSCTL: + status->set_nav_state = NAVIGATION_STATE_POSCTL; break; - case FAILSAFE_STATE_LAND: + case MAIN_STATE_AUTO_MISSION: + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + break; - /* at least relative altitude estimate required for landing */ - if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAVIGATION_STATE_LAND; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO_LOITER: + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + break; + case MAIN_STATE_AUTO_RTL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; break; - case FAILSAFE_STATE_TERMINATION: - /* always allowed */ - ret = TRANSITION_CHANGED; + case MAIN_STATE_ACRO: + status->set_nav_state = NAVIGATION_STATE_ACRO; break; default: break; } + break; - if (ret == TRANSITION_CHANGED) { - if (status->failsafe_state != new_failsafe_state) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; - - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - } + case FAILSAFE_STATE_RTL_RC: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + break; - return ret; -} + case FAILSAFE_STATE_RTL_DL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + break; + case FAILSAFE_STATE_LAND: + status->set_nav_state = NAVIGATION_STATE_LAND; + break; + case FAILSAFE_STATE_TERMINATION: + status->set_nav_state = NAVIGATION_STATE_TERMINATION; + break; -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ -// -//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -//{ -// int ret = 1; -// -//// /* Switch on HIL if in standby and not already in HIL mode */ -//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) -//// && !current_status->flag_hil_enabled) { -//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { -//// /* Enable HIL on request */ -//// current_status->flag_hil_enabled = true; -//// ret = OK; -//// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); -//// -//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_fmu_armed) { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") -//// -//// } else { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") -//// } -//// } -// -// /* switch manual / auto */ -// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { -// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { -// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { -// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { -// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); -// } -// -// /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only arm in standby state */ -// // XXX REMOVE -// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); -// ret = OK; -// printf("[cmd] arming due to command request\n"); -// } -// } -// -// /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only disarm in ground ready */ -// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -// ret = OK; -// printf("[cmd] disarming due to command request\n"); -// } -// } -// -// /* NEVER actually switch off HIL without reboot */ -// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { -// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); -// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); -// ret = ERROR; -// } -// -// return ret; -//} + default: + break; + } +} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..594bf23a1 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,25 +56,17 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); - bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -bool check_arming_state_changed(); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -bool check_main_state_changed(); - transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); -bool check_navigation_state_changed(); - -bool check_failsafe_state_changed(); - -void set_navigation_state_changed(); +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); +void set_nav_state(struct vehicle_status_s *status); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 70087cf3e..31c0c8f79 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,51 +118,71 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND - || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - - } else if (status->main_state == MAIN_STATE_ALTCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + switch (status->set_nav_state) { - } else if (status->main_state == MAIN_STATE_POSCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - - } else if (status->main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - } else if (status->main_state == MAIN_STATE_ACRO) { + case NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - } - - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + break; - if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + case NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + case NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + case NAVIGATION_STATE_LAND: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } + break; + + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 74694447a..f30cd9a94 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ /** * @file navigator_main.cpp - * Implementation of the main navigation state machine. * * Handles mission items, geo fencing and failsafe navigation behavior. * Published the position setpoint triplet for the position controller. @@ -75,7 +74,6 @@ #include #include -#include #include #include #include @@ -102,7 +100,7 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator : public StateTable +class Navigator { public: /** @@ -116,7 +114,7 @@ public: ~Navigator(); /** - * Start the navigator task. + * Start the navigator task. * * @return OK on success. */ @@ -200,24 +198,6 @@ private: param_t takeoff_alt; } _parameter_handles; /**< handles for parameters */ - enum Event { - EVENT_NONE_REQUESTED, - EVENT_LOITER_REQUESTED, - EVENT_MISSION_REQUESTED, - EVENT_RTL_REQUESTED, - EVENT_TAKEN_OFF, - EVENT_LANDED, - EVENT_MISSION_CHANGED, - EVENT_HOME_POSITION_CHANGED, - EVENT_MISSION_ITEM_REACHED, - MAX_EVENT - }; /**< possible events that can be thrown at state machine */ - - /** - * State machine transition table - */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - /** * Update our local parameter cache. */ @@ -268,41 +248,6 @@ private: */ void task_main(); - /** - * Functions that are triggered when a new state is entered. - */ - bool start_none_on_ground(); - bool start_none_in_air(); - bool start_auto_on_ground(); - bool start_loiter(); - bool start_mission(); - bool advance_mission(); - bool start_rtl(); - bool advance_rtl(); - bool start_land(); - - /** - * Reset all "mission item reached" flags. - */ - void reset_reached(); - - /** - * Check if current mission item has been reached. - */ - bool check_mission_item_reached(); - - /** - * Set mission items by accessing the mission class. - * If failing, tell the state machine to loiter. - */ - bool set_mission_items(); - - /** - * Set a RTL item by accessing the RTL class. - * If failing, tell the state machine to loiter. - */ - void set_rtl_item(); - /** * Translate mission item to a position setpoint. */ @@ -329,7 +274,6 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -368,11 +312,6 @@ Navigator::Navigator() : _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - - /* Initialize state machine */ - myState = NAV_STATE_NONE_ON_GROUND; - - start_none_on_ground(); } Navigator::~Navigator() @@ -615,41 +554,6 @@ Navigator::task_main() /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - - /* commander requested new navigation mode, forward it to state machine */ - switch (_vstatus.set_nav_state) { - case NAVIGATION_STATE_NONE: - dispatch(EVENT_NONE_REQUESTED); - break; - - case NAVIGATION_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAVIGATION_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); - break; - - case NAVIGATION_STATE_RTL: - dispatch(EVENT_RTL_REQUESTED); - break; - - case NAVIGATION_STATE_LAND: - /* TODO: add and test this */ - // dispatch(EVENT_LAND_REQUESTED); - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - /* commander sets in-air/on-ground flag as well */ - if (_vstatus.condition_landed) { - dispatch(EVENT_LANDED); - } else { - dispatch(EVENT_TAKEN_OFF); - } } /* parameters updated */ @@ -666,30 +570,21 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); - /* TODO check if home position really changed */ - dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - if (check_mission_item_reached()) { - dispatch(EVENT_MISSION_ITEM_REACHED); - } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { @@ -765,125 +660,8 @@ Navigator::status() } else { warnx("Geofence not set"); } - - switch (myState) { - case NAV_STATE_NONE_ON_GROUND: - warnx("State: None on ground"); - break; - - case NAV_STATE_NONE_IN_AIR: - warnx("State: None in air"); - break; - - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - - case NAV_STATE_LAND: - warnx("State: Land"); - break; - - default: - warnx("State: Unknown"); - break; - } } - -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* NAV_STATE_NONE_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - }, - { - /* NAV_STATE_NONE_IN_AIR */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - }, - { - /* NAV_STATE_AUTO_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - }, - { - /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, - }, - { - /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, - }, - { - /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, - }, - { - /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, - }, -}; - +#if 0 bool Navigator::start_none_on_ground() { @@ -984,13 +762,11 @@ Navigator::set_mission_items() /* if we fail to set the current mission, continue to loiter */ if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - dispatch(EVENT_LOITER_REQUESTED); return false; } /* if we got an RTL mission item, switch to RTL mode and give up */ if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - dispatch(EVENT_RTL_REQUESTED); return false; } @@ -1042,7 +818,6 @@ Navigator::start_rtl() } /* if RTL doesn't work, fallback to loiter */ - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1066,7 +841,6 @@ Navigator::advance_rtl() return true; } - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1105,7 +879,7 @@ Navigator::start_land() _update_triplet = true; return true; } - +#endif void Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { @@ -1147,7 +921,7 @@ Navigator::mission_item_to_position_setpoint(const mission_item_s *item, positio sp->type = SETPOINT_TYPE_NORMAL; } } - +#if 0 bool Navigator::check_mission_item_reached() { @@ -1251,12 +1025,12 @@ Navigator::reset_reached() _waypoint_yaw_reached = false; } - +#endif void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - _pos_sp_triplet.nav_state = static_cast(myState); + /* TODO: set nav_state */ /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index 38378651b..e6011fdef 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -33,7 +33,7 @@ /** * @file state_table.h - * + * * Finite-State-Machine helper class for state table * @author: Julian Oes */ @@ -44,32 +44,32 @@ class StateTable { public: - typedef bool (StateTable::*Action)(); + typedef void (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; }; - + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) : myTable(table), myNsignals(nSignals), myNstates(nStates) {} - + #define NO_ACTION &StateTable::doNothing #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - + void dispatch(unsigned const sig) { /* get transition using state table */ Tran const *t = myTable + myState*myNsignals + sig; - /* first up change state, this allows to do further dispatchs in the state functions */ - - /* now execute state function, if it runs with success, accept new state */ - if ((this->*(t->action))()) { - myState = t->nextState; - } + + /* accept new state */ + myState = t->nextState; + + /* */ + (this->*(t->action))(); } - bool doNothing() { - return true; + void doNothing() { + return; } protected: unsigned myState; @@ -79,4 +79,4 @@ private: unsigned myNstates; }; -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 7c3921198..ad654a9ff 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -56,6 +56,7 @@ struct mission_result_s bool mission_reached; /**< true if mission has been reached */ unsigned mission_index_reached; /**< index of the mission which has been reached */ unsigned index_current_mission; /**< index of the current mission */ + bool mission_finished; /**< true if mission has been completed */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 85e8ef8a5..c2741a05b 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -74,14 +74,13 @@ struct position_setpoint_s }; typedef enum { - NAV_STATE_NONE_ON_GROUND = 0, - NAV_STATE_NONE_IN_AIR, - NAV_STATE_AUTO_ON_GROUND, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX + NAV_STATE_MANUAL = 0, + NAV_STATE_POSCTL, + NAV_STATE_AUTO, + NAV_STATE_RC_LOSS, + NAV_STATE_DL_LOSS, + NAV_STATE_TERMINATION, + MAX_NAV_STATE } nav_state_t; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index d902dc49e..259d7329e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,9 +63,11 @@ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, MAIN_STATE_POSCTL, - MAIN_STATE_AUTO, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, MAIN_STATE_ACRO, - MAIN_STATE_MAX + MAIN_STATE_MAX, } main_state_t; // If you change the order, add or remove arming_state_t states make sure to update the arrays @@ -78,7 +80,7 @@ typedef enum { ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE, - ARMING_STATE_MAX + ARMING_STATE_MAX, } arming_state_t; typedef enum { @@ -88,18 +90,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL, /**< Return To Launch */ - FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX + FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_NONE = 0, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND + NAVIGATION_STATE_MANUAL = 0, + NAVIGATION_STATE_ACRO, + NAVIGATION_STATE_ALTCTL, + NAVIGATION_STATE_POSCTL, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_RTL_RC, + NAVIGATION_STATE_AUTO_RTL_DL, + NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TERMINATION, } navigation_state_t; enum VEHICLE_MODE_FLAG { @@ -160,10 +169,10 @@ struct vehicle_status_s { uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; /**< main state machine */ + main_state_t main_state; /**< main state machine */ navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ - hil_state_t hil_state; /**< current hil state */ + hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ -- cgit v1.2.3 From c8903b167230bc2efdb908e78135a0fa0abc745c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 May 2014 23:24:01 +0200 Subject: commander: modes and RC loss working now --- src/modules/commander/commander.cpp | 69 ++++++++++++++++++++----------------- 1 file changed, 37 insertions(+), 32 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 908384bb6..5638bc09f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -625,8 +625,10 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[0] = "MANUAL"; main_states_str[1] = "ALTCTL"; main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO"; - main_states_str[4] = "ACRO"; + main_states_str[3] = "AUTO_MISSION"; + main_states_str[4] = "AUTO_LOITER"; + main_states_str[5] = "AUTO_RTL"; + main_states_str[6] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -639,9 +641,10 @@ int commander_thread_main(int argc, char *argv[]) char *failsafe_states_str[FAILSAFE_STATE_MAX]; failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL"; - failsafe_states_str[2] = "LAND"; - failsafe_states_str[3] = "TERMINATION"; + failsafe_states_str[1] = "RTL_RC"; + failsafe_states_str[2] = "RTL_DL"; + failsafe_states_str[3] = "LAND"; + failsafe_states_str[4] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1151,12 +1154,9 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - warnx("arming status1: %d", status.arming_state); arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - warnx("arming status2: %d", status.arming_state); if (arming_ret == TRANSITION_CHANGED) { - warnx("changed"); arming_state_changed = true; } @@ -1198,6 +1198,9 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; + + status.failsafe_state = FAILSAFE_STATE_NORMAL; + failsafe_state_changed = true; } } @@ -1215,7 +1218,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed); if (arming_ret == TRANSITION_CHANGED) { - warnx("stick 1"); arming_state_changed = true; } stick_off_counter = 0; @@ -1241,7 +1243,6 @@ int commander_thread_main(int argc, char *argv[]) } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (arming_ret == TRANSITION_CHANGED) { - warnx("stick 2"); arming_state_changed = true; } } @@ -1294,29 +1295,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - } - } - if (armed.armed) { - /* if RC is lost, switch to RTL_RC or Termination unless a mission is running - * and not yet finished) */ - if (status.rc_signal_lost - && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; + if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; + } else { + status.failsafe_state = FAILSAFE_STATE_LAND; + } + failsafe_state_changed = true; } } - - /* if the data link is lost, switch to RTL_DL or Termination */ - /* TODO: add this */ - - } else { - /* reset failsafe when disarmed */ - status.failsafe_state = FAILSAFE_STATE_NORMAL; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1613,10 +1604,24 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + if (sp_man->return_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_RTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else { + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } // else fallback to ALTCTL (POSCTL likely will not work too) -- cgit v1.2.3 From 480c41d83591354480a8fd81cfbc314fccaa6a20 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 13:18:56 +0200 Subject: do not read out home position in gcs (home position is still displayed) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a16..65922b2a5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { -- cgit v1.2.3 From 94c4fc56aa229bcb909e5437804e7475adfbcdba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 23:08:11 +0200 Subject: navigator: audio messages about what is happening, RTL during mission not triggered but after mission --- src/modules/commander/commander.cpp | 17 ++++++++++- src/modules/navigator/mission.cpp | 60 +++++++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 5 ++++ src/modules/navigator/rtl.cpp | 6 +--- 4 files changed, 70 insertions(+), 18 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d7a95b0d6..bb75b2af0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1296,7 +1296,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ @@ -1306,10 +1305,26 @@ int commander_thread_main(int argc, char *argv[]) status.failsafe_state = FAILSAFE_STATE_LAND; } failsafe_state_changed = true; + } else { + mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); } } } + /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ + if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; + mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); + } else { + /* this probably doesn't make sense since we are in mission and have global position */ + status.failsafe_state = FAILSAFE_STATE_LAND; + } + failsafe_state_changed = true; + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cd694be9a..db606e6fa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -46,6 +46,7 @@ #include #include +#include #include #include @@ -210,20 +211,39 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); + /* try setting onboard mission item */ if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting onboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_ONBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: onboard mission running"); + } _mission_type = MISSION_TYPE_ONBOARD; _navigator->set_is_in_loiter(false); + /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting offboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_OFFBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: offboard mission running"); + } _mission_type = MISSION_TYPE_OFFBOARD; _navigator->set_is_in_loiter(false); } else { + if (_mission_type != MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: mission finished"); + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: no mission available"); + } _mission_type = MISSION_TYPE_NONE; bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; set_loiter_item(use_current_pos_sp, pos_sp_triplet); + reset_mission_item_reached(); + report_mission_finished(); } } @@ -242,8 +262,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current { /* make sure param is up to date */ updateParams(); - if (_param_onboard_enabled.get() > 0 - && _current_onboard_mission_index < (int)_onboard_mission.count) { + if (_param_onboard_enabled.get() > 0 && + _current_onboard_mission_index >= 0&& + _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, &new_mission_item)) { @@ -264,7 +285,8 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - if (_current_offboard_mission_index < (int)_offboard_mission.count) { + if (_current_offboard_mission_index >= 0 && + _current_offboard_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -282,8 +304,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren report_current_offboard_mission_item(); memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; - } else { - warnx("ERROR: WP read fail"); } } return false; @@ -295,7 +315,8 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) int next_temp_mission_index = _onboard_mission.current_index + 1; /* try if there is a next onboard mission */ - if (next_temp_mission_index < (int)_onboard_mission.count) { + if (_onboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ @@ -315,7 +336,9 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ int next_temp_mission_index = _offboard_mission.current_index + 1; - if (next_temp_mission_index < (int)_offboard_mission.count) { + warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); + if (_offboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -346,14 +369,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR waypoint could not be read"); return false; } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* TODO: do this check more gracefully since it is not a serious error */ if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: DO JUMP repetitions completed"); return false; } @@ -366,9 +392,10 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); return false; } - /* TODO: report about DO JUMP count */ } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -381,7 +408,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP is cycling, giving up"); return false; } @@ -402,6 +430,13 @@ Mission::report_current_offboard_mission_item() publish_mission_result(); } +void +Mission::report_mission_finished() +{ + _mission_result.mission_finished = true; + publish_mission_result(); +} + void Mission::publish_mission_result() { @@ -416,4 +451,5 @@ Mission::publish_mission_result() } /* reset reached bool */ _mission_result.mission_reached = false; + _mission_result.mission_finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index cb3242c87..a3dd09ecd 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -151,6 +151,11 @@ private: */ void report_current_offboard_mission_item(); + /** + * Report that the mission is finished if one exists or that none exists + */ + void report_mission_finished(); + /** * Publish the mission result so commander and mavlink know what is going on */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c4c5f2aab..dde35d5b6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -112,6 +112,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_FINISHED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { @@ -133,11 +134,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - /* TODO understand and fix this */ - // if (_vstatus.condition_landed) { - // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - // } - _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; -- cgit v1.2.3 From d48a8bc073a3aa5f515c582a1c2c3cae58a8d783 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:09:18 +0200 Subject: navigator: renamed the different RTL states --- src/modules/commander/commander.cpp | 10 +++---- src/modules/commander/state_machine_helper.cpp | 8 +++--- src/modules/mavlink/mavlink_messages.cpp | 4 +-- src/modules/navigator/navigator_main.cpp | 4 +-- src/modules/uORB/topics/vehicle_status.h | 37 +++++++++++++------------- 5 files changed, 32 insertions(+), 31 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb75b2af0..e992706ac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1300,7 +1300,7 @@ int commander_thread_main(int argc, char *argv[]) /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; } else { status.failsafe_state = FAILSAFE_STATE_LAND; } @@ -1313,10 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); } else { /* this probably doesn't make sense since we are in mission and have global position */ @@ -1719,8 +1719,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 214480079..c52e618ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -406,12 +406,12 @@ void set_nav_state(struct vehicle_status_s *status) } break; - case FAILSAFE_STATE_RTL_RC: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + case FAILSAFE_STATE_RC_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; break; - case FAILSAFE_STATE_RTL_DL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + case FAILSAFE_STATE_DL_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; break; case FAILSAFE_STATE_LAND: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e7d2ba515..b58fb4cb8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -161,8 +161,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dc8573813..20cff5c28 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,8 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 259d7329e..9390ff717 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes + * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +41,11 @@ * All apps should write to subsystem_info: * * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + * + * @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes */ #ifndef VEHICLE_STATUS_H_ @@ -90,25 +91,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_MANUAL = 0, - NAVIGATION_STATE_ACRO, - NAVIGATION_STATE_ALTCTL, - NAVIGATION_STATE_POSCTL, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_RTL_RC, - NAVIGATION_STATE_AUTO_RTL_DL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TERMINATION, + NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ + NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ + NAVIGATION_STATE_POSCTL, /**< Position control mode */ + NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ + NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ + NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; enum VEHICLE_MODE_FLAG { -- cgit v1.2.3 From e0ed0625f81841194b4bd9b26c7e047a1f68a1fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Jun 2014 17:34:21 +0200 Subject: commander: failsafe_state removed, replaced with bool failsafe, navigation state and failsafe determined directly from main state and conditions --- src/modules/commander/commander.cpp | 90 +++++++++----------- src/modules/commander/state_machine_helper.cpp | 112 +++++++++++++++---------- src/modules/commander/state_machine_helper.h | 2 - src/modules/gpio_led/gpio_led.c | 2 +- src/modules/mavlink/mavlink_messages.cpp | 4 +- src/modules/mavlink/mavlink_receiver.h | 2 + src/modules/navigator/navigator_main.cpp | 4 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 25 +++--- 9 files changed, 125 insertions(+), 118 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..43683f833 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 +#define DL_TIMEOUT 5 * 1000* 1000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -495,12 +497,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -639,13 +641,6 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[5] = "REBOOT"; arming_states_str[6] = "IN_AIR_RESTORE"; - char *failsafe_states_str[FAILSAFE_STATE_MAX]; - failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL_RC"; - failsafe_states_str[2] = "RTL_DL"; - failsafe_states_str[3] = "LAND"; - failsafe_states_str[4] = "TERMINATION"; - /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -666,10 +661,10 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_MANUAL; + status.nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; - status.failsafe_state = FAILSAFE_STATE_NORMAL; + status.failsafe = false; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -678,6 +673,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status.offboard_control_signal_lost = true; + status.data_link_lost = true; /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -771,6 +767,11 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to telemetry status */ + int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -908,6 +909,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + orb_check(telemetry_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -1186,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); } - /* start RC input check */ + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -1198,9 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - status.failsafe_state = FAILSAFE_STATE_NORMAL; - failsafe_state_changed = true; } } @@ -1271,12 +1275,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - - // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - // /* recover from failsafe */ - // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - // } - /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1295,34 +1293,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; - } - failsafe_state_changed = true; - } else { - mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); - } } } - /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ - if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); - } else { - /* this probably doesn't make sense since we are in mission and have global position */ - status.failsafe_state = FAILSAFE_STATE_LAND; + /* data link check */ + if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = false; + status_changed = true; } - failsafe_state_changed = true; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1377,7 +1365,7 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; - /* now set nav state according to failsafe and main state */ + /* now set navigation state according to failsafe and main state */ set_nav_state(&status); if (main_state_changed) { @@ -1388,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[]) if (failsafe_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); failsafe_state_changed = false; } @@ -1415,7 +1403,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1519,7 +1507,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ @@ -1667,7 +1655,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - switch (status.set_nav_state) { + switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1719,8 +1707,6 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c52e618ef..df718ff99 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } } - if (ret = TRANSITION_CHANGED) { + if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here @@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, return ret; } - /** * Check failsafe and main status and set navigation status for navigator accordingly */ void set_nav_state(struct vehicle_status_s *status) { - switch (status->failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case MAIN_STATE_MANUAL: - status->set_nav_state = NAVIGATION_STATE_MANUAL; - break; - - case MAIN_STATE_ALTCTL: - status->set_nav_state = NAVIGATION_STATE_ALTCTL; - break; + status->failsafe = false; - case MAIN_STATE_POSCTL: - status->set_nav_state = NAVIGATION_STATE_POSCTL; - break; - - case MAIN_STATE_AUTO_MISSION: - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; - break; - - case MAIN_STATE_AUTO_LOITER: - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; - break; + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_ACRO: + case MAIN_STATE_MANUAL: + case MAIN_STATE_ALTCTL: + case MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if (status->rc_signal_lost) { + status->failsafe = true; - case MAIN_STATE_AUTO_RTL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; - break; + } else { + switch (status->main_state) { + case MAIN_STATE_ACRO: + status->nav_state = NAVIGATION_STATE_ACRO; + break; + + case MAIN_STATE_MANUAL: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + + case MAIN_STATE_ALTCTL: + status->nav_state = NAVIGATION_STATE_ALTCTL; + break; + + case MAIN_STATE_POSCTL: + status->nav_state = NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + } + } + break; - case MAIN_STATE_ACRO: - status->set_nav_state = NAVIGATION_STATE_ACRO; - break; + case MAIN_STATE_AUTO_MISSION: + /* require data link and global position */ + if (status->data_link_lost || !status->condition_global_position_valid) { + status->failsafe = true; - default: - break; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; - case FAILSAFE_STATE_RC_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; - break; + case MAIN_STATE_AUTO_LOITER: + /* require data link and local position */ + if (status->data_link_lost || !status->condition_local_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_DL_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } break; - case FAILSAFE_STATE_LAND: - status->set_nav_state = NAVIGATION_STATE_LAND; - break; + case MAIN_STATE_AUTO_RTL: + /* require global position and home */ + if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_TERMINATION: - status->set_nav_state = NAVIGATION_STATE_TERMINATION; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } break; default: break; } + + if (status->failsafe) { + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 594bf23a1..062271764 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,8 +63,6 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); - transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); void set_nav_state(struct vehicle_status_s *status); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 6dfd22fdf..839a7890b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg) pattern = 0x2A; // *_*_*_ fast blink (armed, error) } else if (priv->status.arming_state == ARMING_STATE_ARMED) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) { pattern = 0x3f; // ****** solid (armed) } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b58fb4cb8..d7f56b63d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,7 +118,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->set_nav_state) { + switch (status->nav_state) { case NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED @@ -161,8 +161,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..ab3dc81c6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -138,6 +139,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + hrt_abstime _telemetry_heartbeat_time; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..70da5393f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -336,7 +336,7 @@ Navigator::task_main() } /* Do stuff according to navigation state set by commander */ - switch (_vstatus.set_nav_state) { + switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: @@ -351,8 +351,6 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 53cd6a797..19872bbd0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9390ff717..898c176fb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -59,7 +59,9 @@ * @addtogroup topics @{ */ -/* main state machine */ +/** + * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. + */ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, @@ -89,15 +91,9 @@ typedef enum { HIL_STATE_ON } hil_state_t; -typedef enum { - FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ - FAILSAFE_STATE_LAND, /**< Land as safe as possible */ - FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX, -} failsafe_state_t; - +/** + * Navigation state, i.e. "what should vehicle do". + */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ NAVIGATION_STATE_ACRO, /**< Acro mode */ @@ -106,9 +102,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; @@ -171,10 +166,10 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ + navigation_state_t nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ - failsafe_state_t failsafe_state; /**< current failsafe state */ + bool failsafe; /**< true if system is in failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -199,6 +194,8 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ + bool data_link_lost; /**< datalink to GCS lost */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; -- cgit v1.2.3 From 55e5f747de013fb386727b41cc67bd019c129c31 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 17 Jun 2014 13:19:50 +0200 Subject: commander: modes display fixes, don't activate failsafe while disarmed --- src/modules/commander/commander.cpp | 56 +++++++++++++++++--------- src/modules/commander/state_machine_helper.cpp | 32 +++++++++++---- src/modules/commander/state_machine_helper.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 3 +- 4 files changed, 66 insertions(+), 27 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 43683f833..f8dcec8cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -624,22 +624,34 @@ int commander_thread_main(int argc, char *argv[]) warnx("starting"); char *main_states_str[MAIN_STATE_MAX]; - main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTL"; - main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO_MISSION"; - main_states_str[4] = "AUTO_LOITER"; - main_states_str[5] = "AUTO_RTL"; - main_states_str[6] = "ACRO"; + main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[0] = "INIT"; - arming_states_str[1] = "STANDBY"; - arming_states_str[2] = "ARMED"; - arming_states_str[3] = "ARMED_ERROR"; - arming_states_str[4] = "STANDBY_ERROR"; - arming_states_str[5] = "REBOOT"; - arming_states_str[6] = "IN_AIR_RESTORE"; + arming_states_str[ARMING_STATE_INIT] = "INIT"; + arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + char *nav_states_str[NAVIGATION_STATE_MAX]; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -844,7 +856,7 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; - bool failsafe_state_changed = false; + bool failsafe_old = false; while (!thread_should_exit) { @@ -1366,18 +1378,26 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ - set_nav_state(&status); + bool nav_state_changed = set_nav_state(&status); + // TODO handle mode changes by commands if (main_state_changed) { status_changed = true; + warnx("main state: %s", main_states_str[status.main_state]); mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); main_state_changed = false; } - if (failsafe_state_changed) { + if (status.failsafe != failsafe_old) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); - failsafe_state_changed = false; + failsafe_old = status.failsafe; + } + + if (nav_state_changed) { + status_changed = true; + warnx("nav state: %s", nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index df718ff99..c0d730949 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -366,8 +366,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -void set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status) { + navigation_state_t nav_state_old = status->nav_state; + + bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ @@ -377,7 +380,7 @@ void set_nav_state(struct vehicle_status_s *status) case MAIN_STATE_ALTCTL: case MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if (status->rc_signal_lost) { + if (status->rc_signal_lost && armed) { status->failsafe = true; } else { @@ -407,31 +410,44 @@ void set_nav_state(struct vehicle_status_s *status) case MAIN_STATE_AUTO_MISSION: /* require data link and global position */ - if (status->data_link_lost || !status->condition_global_position_valid) { + if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { status->failsafe = true; } else { - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + if (armed) { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + + } else { + // TODO which mode should we set when disarmed? + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } } break; case MAIN_STATE_AUTO_LOITER: /* require data link and local position */ - if (status->data_link_lost || !status->condition_local_position_valid) { + if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { status->failsafe = true; } else { + // TODO which mode should we set when disarmed? status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } break; case MAIN_STATE_AUTO_RTL: /* require global position and home */ - if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) { status->failsafe = true; } else { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + if (armed) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else { + // TODO which mode should we set when disarmed? + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } } break; @@ -453,5 +469,7 @@ void set_nav_state(struct vehicle_status_s *status) status->nav_state = NAVIGATION_STATE_TERMINATION; } } + + return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 062271764..cffbc9b8f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -void set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 898c176fb..851938795 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -96,15 +96,16 @@ typedef enum { */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ - NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ + NAVIGATION_STATE_MAX, } navigation_state_t; enum VEHICLE_MODE_FLAG { -- cgit v1.2.3 From e24925c743330bc3c6c0a24ba913a9c5fab5e07d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Jun 2014 19:01:41 +0200 Subject: commander: added some failsafe logic --- src/modules/commander/commander.cpp | 39 +++++--- src/modules/commander/commander_params.c | 13 ++- src/modules/commander/state_machine_helper.cpp | 132 ++++++++++++++++++------- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/navigator_main.cpp | 4 + src/modules/uORB/topics/vehicle_status.h | 2 + 6 files changed, 142 insertions(+), 50 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f8dcec8cb..5954dcbb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,6 +619,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); + param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); /* welcome user */ warnx("starting"); @@ -627,9 +628,9 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -637,21 +638,23 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -853,6 +856,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; + int32_t datalink_loss_enabled = false; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); + param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } orb_check(sp_man_sub, &updated); @@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where RC signal was regained */ + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ if (status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: data link regained"); status.data_link_lost = false; @@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = false; + status.data_link_lost = true; status_changed = true; } } @@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ - bool nav_state_changed = set_nav_state(&status); + bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, + mission_result.mission_finished); // TODO handle mode changes by commands if (main_state_changed) { @@ -1727,6 +1734,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 80ca68f21..4750f9d5c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -39,7 +39,7 @@ * * @author Lorenz Meier * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes */ #include @@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * Datalink loss mode enabled. + * + * Set to 1 to enable actions triggered when the datalink is lost. + * + * @group commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c0d730949..f0fe13921 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -366,7 +366,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) { navigation_state_t nav_state_old = status->nav_state; @@ -383,6 +383,16 @@ bool set_nav_state(struct vehicle_status_s *status) if (status->rc_signal_lost && armed) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { switch (status->main_state) { case MAIN_STATE_ACRO: @@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status) break; case MAIN_STATE_AUTO_MISSION: - /* require data link and global position */ - if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { + /* go into failsafe + * - if either the datalink is enabled and lost as well as RC is lost + * - if there is no datalink and the mission is finished */ + if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + + /* don't bother if RC is lost and mission is not yet finished */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for missions */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; case MAIN_STATE_AUTO_LOITER: - /* require data link and local position */ - if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { + /* go into failsafe if datalink and RC is lost */ + if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } else { - // TODO which mode should we set when disarmed? + /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } break; case MAIN_STATE_AUTO_RTL: /* require global position and home */ - if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) { + if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } break; @@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status) break; } - if (status->failsafe) { - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - } - return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index cffbc9b8f..2e076cdae 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 70da5393f..50bcfd58d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,12 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + _navigation_mode = &_rtl; /* TODO: change this to something else */ + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 851938795..16652fddf 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -101,6 +101,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ + NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From 62faa2ee5155253779d3b93c5a280e8918585f6e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Jun 2014 10:26:56 +0200 Subject: commander/navigator: renamed FS modes to RTL and RTGS (return to ground station) --- src/modules/commander/commander.cpp | 6 ++---- src/modules/commander/px4_custom_mode.h | 1 + src/modules/commander/state_machine_helper.cpp | 12 ++++++------ src/modules/mavlink/mavlink_messages.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 3 +-- src/modules/uORB/topics/vehicle_status.h | 5 ++--- 6 files changed, 20 insertions(+), 15 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5954dcbb1..b957ad9b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -649,8 +649,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -1734,8 +1733,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e0f8dc95d..7f5f93801 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, + PX4_CUSTOM_SUB_MODE_AUTO_RTGS }; union px4_custom_mode { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f0fe13921..74885abf6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,7 +384,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -427,7 +427,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -441,7 +441,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -467,7 +467,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -481,7 +481,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -495,7 +495,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d7f56b63d..1fd8b1a27 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -176,6 +176,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; + case NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; + case NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 50bcfd58d..661d3d744 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,10 +351,9 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 16652fddf..5dc46dacb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -100,9 +100,8 @@ typedef enum { NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ - NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ - NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From f318ee21943aa6902a7d4d0c092880a4d3c4ce8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 18:15:15 +0200 Subject: Remove debug output in commander spamming console --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..df7a4c886 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,7 +1592,6 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; - warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL -- cgit v1.2.3 From bdf1b9274c369ddfd52bcb92952f2467abd7c26f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Jun 2014 17:56:11 +0200 Subject: commander: modes fallback and reject messages fixed --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..ec4273fc3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1613,10 +1613,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTL print_reject_mode(status, "POSCTL"); } + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1627,7 +1627,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin print_reject_mode(status, "ALTCTL"); } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1639,28 +1639,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_RTL"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_LOITER"); + } else { res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_MISSION"); } - // else fallback to ALTCTL (POSCTL likely will not work too) + // fallback to POSCTL + res = main_state_transition(status, MAIN_STATE_POSCTL); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; -- cgit v1.2.3 From 1cd82fc89c70060947683851af28556bccd675a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5..588f48225 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } -- cgit v1.2.3 From 547f71d791d0a04001580e8371bdf59b808a3d29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5..588f48225 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } -- cgit v1.2.3 From 690edbd6723746699067f69fd3c76d4d3806d9c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:14:49 +0200 Subject: Remove duplicate code in the arming check --- src/modules/commander/commander.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 588f48225..8e9352394 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1232,12 +1232,13 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("#audio: NOT ARMING: Press safety switch first."); - } else if (status.main_state != MAIN_STATE_MANUAL) { + /* we check outside of the transition function here because the requirement + * for being in manual mode only applies to manual arming actions. + * the system can be armed in auto if armed via the GCS. + */ + if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); - } else { arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } -- cgit v1.2.3 From d5b5dcef24a7f5c7b2ed98081dd69a05f8d46959 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 12:01:43 -0700 Subject: Fix bugs found through compiler warnings --- src/drivers/mkblctrl/mkblctrl.cpp | 8 ++++---- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 10 ++++++++-- src/modules/commander/commander.cpp | 1 + src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- src/modules/gpio_led/gpio_led.c | 3 ++- src/modules/px4iofirmware/sbus.c | 10 ++++++++-- src/systemcmds/mtd/mtd.c | 8 ++++++-- src/systemcmds/tests/test_mixer.cpp | 1 + 10 files changed, 33 insertions(+), 14 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5954c40da..493678ae5 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - int set_px4mode(int px4mode); - int set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: @@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate) return OK; } -int +void MK::set_px4mode(int px4mode) { _px4mode = px4mode; } -int +void MK::set_frametype(int frametype) { _frametype = frametype; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a4bfa18c..84ea9a3bc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -741,7 +741,7 @@ PX4FMU::task_main() } for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..b0a8527c2 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor) return _motor1Position; } else if (motor == MOTOR_2) { return _motor2Position; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } float RoboClaw::getMotorSpeed(e_motor motor) @@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor) return _motor1Speed; } else if (motor == MOTOR_2) { return _motor2Speed; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } int RoboClaw::setMotorSpeed(e_motor motor, float value) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1535967b1..3cd250458 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,6 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1320b4668..f8b9e9fbd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1986,7 +1986,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt posNED[2] = -(hgt - hgtRef); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e821089f2..69d8cfce8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -301,7 +301,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 839a7890b..1fc6d1295 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -189,7 +189,8 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; - warnx("stop"); + // FIXME: Is this correct? changed from warnx + errx(0, "stop"); } else { errx(1, "not running"); diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0e7dc621c..8ed69678c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,13 +119,19 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'A', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool sbus2_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'B', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a57eaafe7..8b43e54da 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,8 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) - warnx(1, "failed to set bus speed"); + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index df382e2c6..dc2c20e79 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); + return 0; } static int -- cgit v1.2.3 From 2448e2adbc22af348aa799473c30ac5f1cba9cd0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:55:55 -0700 Subject: More tab fixes --- src/drivers/roboclaw/RoboClaw.cpp | 8 ++++---- src/modules/commander/commander.cpp | 2 +- src/modules/gpio_led/gpio_led.c | 4 ++-- src/systemcmds/mtd/mtd.c | 12 ++++++------ src/systemcmds/tests/test_mixer.cpp | 2 +- 5 files changed, 14 insertions(+), 14 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index b0a8527c2..fdaa7f27b 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -183,8 +183,8 @@ float RoboClaw::getMotorPosition(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Position; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } @@ -195,8 +195,8 @@ float RoboClaw::getMotorSpeed(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Speed; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3cd250458..aa243d79b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,7 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } - return true; + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index def958c95..7758faed7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,13 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); - exit(0); + exit(0); } } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); - exit(0); + exit(0); } else { errx(1, "not running"); } diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 8b43e54da..fcc9b8366 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,12 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) { - // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried - // that but setting the bug speed does fail all the time. Which was then exiting and the board would - // not run correctly. So changed to warnx. - warnx("failed to set bus speed"); - } + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index dc2c20e79..0b826b826 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,7 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); - return 0; + return 0; } static int -- cgit v1.2.3 From 3f6851b9879c2e4d764926a3bc29ff800a17b73d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:19:08 +0200 Subject: Re-send the RC config warnings once the GCS is connected, fixed compile warnings --- .../commander/accelerometer_calibration.cpp | 3 ++- src/modules/commander/calibration_routines.cpp | 7 +++--- src/modules/commander/commander.cpp | 26 +++++++++++++++++----- 3 files changed, 27 insertions(+), 9 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 24da452b1..be465ab76 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -131,6 +131,7 @@ #include #include #include +#include #include #include #include @@ -526,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) { + if (fabsf(det) < FLT_EPSILON) { return ERROR; // Singular matrix } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 9d79124e7..43f341ae7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -40,6 +40,7 @@ */ #include +#include #include "calibration_routines.h" @@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; + aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA; + aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB; + aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC; //Compute next iteration nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb42889ea..ef4c18ea6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -371,16 +371,16 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -507,7 +507,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -752,7 +759,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -862,6 +868,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; + bool system_checked = false; while (!thread_should_exit) { @@ -915,6 +922,15 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } + /* Perform system checks (again) once params are loaded and MAVLink is up. */ + if (!system_checked && mavlink_fd && + (telemetry.heartbeat_time > 0) && + (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { + + (void)rc_calibration_check(mavlink_fd); + system_checked = true; + } + orb_check(sp_man_sub, &updated); if (updated) { -- cgit v1.2.3 From 799f568ab48ca3446b24f41290589f7f3c39ef0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 14:33:56 +0200 Subject: commander: Minor checks / improvements to power enforce patch --- src/modules/commander/commander.cpp | 4 +++- src/modules/commander/state_machine_helper.cpp | 16 +++++----------- 2 files changed, 8 insertions(+), 12 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 507f18e24..a15712a91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -974,8 +974,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = false; } else { status.condition_power_input_valid = true; - status.avionics_power_rail_voltage = system_power.voltage5V_v; } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1997729d4..7a61d481b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -141,22 +141,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); - } - + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { - - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); - } + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f)) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From bbdf2bc07aaaa363ef58f15831511c64c2d856bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 16:58:05 +0200 Subject: commander: Notify negative on manual arming fails --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e4a709ef6..efa26eb97 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1342,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + tune_negative(true); } /* evaluate the main state machine according to mode switches */ -- cgit v1.2.3