From ebc7cb03b726ebfb864e770a82b92bb67b6bfd4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:24:12 +0100 Subject: «flighttermination state» replaced by more general «failsafe state» MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/uORB/topics/vehicle_status.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_status.h') diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..a3a862d85 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -82,9 +82,11 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, + FAILSAFE_STATE_RTL, + FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +175,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming 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 */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +226,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** -- cgit v1.2.3 From 92ddf7903b4a540215905f01acd1819eac1f176d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:37:26 +0100 Subject: commander: more user-friendly states indication --- src/modules/commander/commander.cpp | 43 ++++++++++++++++++++++++++------ src/modules/navigator/navigator_main.cpp | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 ++- 3 files changed, 40 insertions(+), 9 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_status.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7715f73e0..722230eff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -575,6 +575,26 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + 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"; + + 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"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1108,8 +1128,8 @@ int commander_thread_main(int argc, char *argv[]) } } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { @@ -1127,13 +1147,11 @@ int commander_thread_main(int argc, char *argv[]) res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } } else { @@ -1179,9 +1197,20 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed || failsafe_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); + /* print new state */ + if (arming_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 89a62e166..8ecc28b11 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -802,7 +802,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; pub_control_mode = true; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a3a862d85..73102090f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -73,7 +74,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { -- cgit v1.2.3 From c7f05539382a48d7ecaad3bfdf71261cde2ee8c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:50:34 +0100 Subject: cammander: state machine can now deny current state (e.g. when position lock lost during EASY mode), added FAILSAFE_STATE_LAND --- src/modules/commander/commander.cpp | 35 +++++++++- src/modules/commander/state_machine_helper.cpp | 91 ++++++++++++++------------ src/modules/navigator/navigator_main.cpp | 58 +++++++++++++++- src/modules/uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 7 +- 5 files changed, 144 insertions(+), 48 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_status.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 722230eff..bca0569d5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ + /* evaluate the main state machine according to mode switches */ res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { @@ -1160,12 +1160,41 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && 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); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } + } + + } else if (armed.armed) { + /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } } - // TODO add other failsafe modes if position estimate not available } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 90ca2a6d2..77edea546 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -223,51 +223,50 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; + + case MAIN_STATE_SEATBELT: + + /* need at minimum altitude estimate */ + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } + } - break; + break; - case MAIN_STATE_EASY: + case MAIN_STATE_EASY: - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - case MAIN_STATE_AUTO: + break; - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO: - break; + /* need global position estimate */ + if (current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; } - if (ret == TRANSITION_CHANGED) { + break; + } + + if (ret == TRANSITION_CHANGED) { + if (current_state->main_state != new_main_state) { current_state->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -367,17 +366,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_failsafe_state == status->failsafe_state) { - ret = TRANSITION_NOT_CHANGED; + /* 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 if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - ret = TRANSITION_CHANGED; + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } break; case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; @@ -388,8 +392,13 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } if (ret == TRANSITION_CHANGED) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d72ed7058..388fefd02 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -216,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -292,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -733,6 +734,12 @@ Navigator::task_main() dispatch(EVENT_RTL_REQUESTED); } + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + } else { /* shouldn't act */ dispatch(EVENT_NONE_REQUESTED); @@ -892,6 +899,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* 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}, }, @@ -902,6 +910,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* 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_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, @@ -912,6 +921,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* 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_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, @@ -922,6 +932,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, @@ -932,9 +943,21 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* 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), NAV_STATE_LAND}, /* 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 }, + { + /* STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* 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_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + }, }; void @@ -1142,6 +1165,27 @@ Navigator::start_rtl() set_rtl_item(); } +void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; + + mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); +} + void Navigator::set_rtl_item() { @@ -1508,9 +1552,21 @@ Navigator::publish_control_mode() navigator_enabled = true; break; + case FAILSAFE_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + case FAILSAFE_STATE_TERMINATION: navigator_enabled = true; /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index f9f8414df..5aecac898 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 73102090f..a5988d3ba 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -84,9 +84,10 @@ typedef enum { } hil_state_t; typedef enum { - FAILSAFE_STATE_NORMAL = 0, - FAILSAFE_STATE_RTL, - FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX } failsafe_state_t; -- cgit v1.2.3 From d1508a7813ad09a173fe314608c25dc8c3cd7a1f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 20:49:17 +0100 Subject: vehicle_control_mode publication moved to commander, WIP --- src/modules/commander/commander.cpp | 136 ++++++++-- src/modules/commander/state_machine_helper.cpp | 4 + src/modules/mavlink/mavlink.c | 4 + src/modules/navigator/navigator_main.cpp | 342 ++++++++----------------- src/modules/navigator/navigator_state.h | 21 ++ src/modules/sdlog2/sdlog2.c | 4 +- src/modules/uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 255 insertions(+), 277 deletions(-) create mode 100644 src/modules/navigator/navigator_state.h (limited to 'src/modules/uORB/topics/vehicle_status.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52a..60fb4f486 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -203,6 +203,8 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t set_main_state_rc(struct vehicle_status_s *status); +void set_control_mode(); + void print_reject_mode(const char *msg); void print_reject_arm(const char *msg); @@ -555,10 +557,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } static struct vehicle_status_s status; - -/* armed topic */ +static struct vehicle_control_mode_s control_mode; static struct actuator_armed_s armed; - static struct safety_s safety; int commander_thread_main(int argc, char *argv[]) @@ -613,16 +613,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; @@ -645,14 +638,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -1244,8 +1243,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1472,7 +1476,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta transition_result_t set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; switch (status->mode_switch) { @@ -1530,6 +1534,102 @@ set_main_state_rc(struct vehicle_status_s *status) return res; } +void +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + 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_SEATBELT: + 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_EASY: + 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; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + 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; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } +} + void print_reject_mode(const char *msg) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7256583a..43d0e023e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,6 +384,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f 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 = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } @@ -392,6 +394,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_LAND: /* 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(); ret = TRANSITION_CHANGED; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f8091716..5406c35c2 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,6 +220,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO get nav state + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + /* if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_READY) { @@ -231,6 +234,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e..2117755ee 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -84,6 +84,7 @@ #include #include +#include "navigator_state.h" #include "navigator_mission.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -151,10 +152,10 @@ private: int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ - orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -274,6 +275,10 @@ private: */ void vehicle_status_update(); + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); /** * Shim for calling task_main from task_create. @@ -341,11 +346,6 @@ private: * Publish a new mission item triplet for position controller */ void publish_position_setpoint_triplet(); - - /** - * Publish vehicle_control_mode topic for controllers - */ - void publish_control_mode(); }; namespace navigator @@ -373,6 +373,7 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), @@ -381,7 +382,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), _mission_result_pub(-1), - _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -541,9 +541,19 @@ Navigator::onboard_mission_update() void Navigator::vehicle_status_update() { - /* try to load initial states */ if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ + /* in case the commander is not be running */ + _vstatus.arming_state = ARMING_STATE_STANDBY; + } +} + +void +Navigator::vehicle_control_mode_update() +{ + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { + /* in case the commander is not be running */ + _control_mode.flag_control_auto_enabled = false; + _control_mode.flag_armed = false; } } @@ -589,11 +599,13 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); /* copy all topics first time */ vehicle_status_update(); + vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); @@ -605,12 +617,11 @@ Navigator::task_main() orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; - bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -627,6 +638,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _control_mode_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -652,127 +665,113 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* vehicle control mode updated */ + if (fds[7].revents & POLLIN) { + vehicle_control_mode_update(); + } + /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - pub_control_mode = true; /* evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { - if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { - if (_vstatus.main_state == MAIN_STATE_AUTO) { - bool stick_mode = false; + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + bool stick_mode = false; + + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + stick_mode = true; + + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; - stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - - stick_mode = true; - } - - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } + dispatch(EVENT_LOITER_REQUESTED); } + + stick_mode = true; } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } + } + } - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - break; + } else { + dispatch(EVENT_LOITER_REQUESTED); + } - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + break; - break; + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + break; - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + case NAV_STATE_LAND: + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); } - } - } else { - /* not in AUTO mode */ - dispatch(EVENT_NONE_REQUESTED); - } + break; - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { - /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - dispatch(EVENT_RTL_REQUESTED); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { - /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - - } else { - /* shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); } } else { - /* not armed */ + /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } @@ -840,12 +839,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - pub_control_mode = true; - } - - /* publish control mode if updated */ - if (pub_control_mode) { - publish_control_mode(); } perf_end(_loop_perf); @@ -1556,139 +1549,6 @@ Navigator::publish_position_setpoint_triplet() } } -void -Navigator::publish_control_mode() -{ - /* update vehicle_control_mode topic*/ - _control_mode.main_state = _vstatus.main_state; - _control_mode.nav_state = static_cast(myState); - _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; - _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; - _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator has control */ - bool navigator_enabled = false; - - switch (_vstatus.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.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_SEATBELT: - _control_mode.flag_control_manual_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 = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_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; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - - default: - break; - } - - break; - - case FAILSAFE_STATE_RTL: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_LAND: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_TERMINATION: - navigator_enabled = true; - /* disable all controllers on termination */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_termination_enabled = true; - break; - - default: - break; - } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - _control_mode.flag_control_manual_enabled = false; - - switch (myState) { - case NAV_STATE_READY: - /* disable all controllers, armed but idle */ - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - break; - - case NAV_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - - default: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - } - } - - _control_mode.timestamp = hrt_absolute_time(); - - /* lazily publish the mission triplet only once available */ - if (_control_mode_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); - - } else { - /* advertise and publish */ - _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h new file mode 100644 index 000000000..6a1475c9b --- /dev/null +++ b/src/modules/navigator/navigator_state.h @@ -0,0 +1,21 @@ +/* + * 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/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bac2958e..1e032d1c8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1067,8 +1067,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac898..7cbb37cd8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,23 +61,10 @@ * Encodes the complete system state and is set by the commander app. */ -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; - struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; - nav_state_t nav_state; - bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -86,14 +73,14 @@ struct vehicle_control_mode_s bool flag_system_hil_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a5988d3ba..1b3639e30 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,6 +54,8 @@ #include #include "../uORB.h" +#include + /** * @addtogroup topics @{ */ -- cgit v1.2.3