diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 21:21:16 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 21:27:44 +0100 |
commit | cf6d89301b774ae019d1d2c089a30a8ed0d7308f (patch) | |
tree | 7e56739c95dedf53cfb0bae88e6ba65c88b661c8 /src/modules/commander/commander.cpp | |
parent | 01c9092213449c761759bcda11ef9613226be713 (diff) | |
parent | 6f559b279e3d03dbf28eff436b41f3b022c5fa82 (diff) | |
download | px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.tar.gz px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.tar.bz2 px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.zip |
Merge branch 'beta' into offboard2
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 517 |
1 files changed, 348 insertions, 169 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bec6884e2..e1cca8bfb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -200,9 +200,11 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); + +void set_control_mode(); void print_reject_mode(const char *msg); @@ -414,51 +416,45 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (status->rc_signal_lost) { - /* allow mode switching by command only if no RC signal */ - 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_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + 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_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } 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_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + } 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_OFFBOARD) { - /* OFFBOARD */ - main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); - } + } 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 (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); } } else { - mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); + /* 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); + } + } } if (main_res == TRANSITION_CHANGED) @@ -527,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -566,17 +562,14 @@ 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[]) { /* not yet initialized */ commander_initialized = false; - bool home_position_set = false; bool battery_tune_played = false; bool arm_tune_played = false; @@ -590,6 +583,28 @@ 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"; + main_states_str[4] = "OFFBOARD"; + + 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] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -604,21 +619,15 @@ 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; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -635,14 +644,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); @@ -703,10 +718,15 @@ int commander_thread_main(int argc, char *argv[]) struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); + /* Subscribe to offboard control data */ + int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); + /* Subscribe to offboard control data */ - int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -820,12 +840,16 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(offboard_sp_sub, &updated); - if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); } + orb_check(sp_offboard_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -862,7 +886,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1018,19 +1042,18 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + 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.valid) { - /* copy position data to uORB home message, store it locally as well */ - + && global_position.global_valid) { - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; + /* 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; - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + 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); /* announce new home position */ if (home_pub > 0) { @@ -1041,13 +1064,13 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (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) { status.rc_signal_found_once = true; @@ -1122,24 +1145,27 @@ 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) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill current_status according to mode switches */ + /* fill status according to mode switches */ check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ - res = check_main_state_machine(&status); + /* evaluate the main state machine according to mode switches */ + res = set_main_state_rc(&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 { @@ -1149,26 +1175,66 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + /* switch to OFFBOARD mode if offboard signal available */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + if (res == TRANSITION_DENIED) { + /* can't switch to OFFBOARD, do normal failsafe if needed */ + 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); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); + 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, switching to SEATBELT mode"); + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + + } else if (status.main_state == MAIN_STATE_OFFBOARD) { + /* check if OFFBOARD mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); + + 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_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + } + + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } } /* check offboard signal */ - if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { if (!status.offboard_control_signal_found_once) { status.offboard_control_signal_found_once = true; mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); @@ -1189,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); + mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } @@ -1211,19 +1277,14 @@ int commander_thread_main(int argc, char *argv[]) } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1239,19 +1300,35 @@ 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 flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_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 */ 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); } @@ -1317,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(offboard_sp_sub); + close(sp_offboard_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); @@ -1424,133 +1501,235 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } - /* offboard switch */ + /* offboard switch */ if (!isfinite(sp_man->offboard_switch)) { - current_status->offboard_switch = OFFBOARD_SWITCH_NONE; + status->offboard_switch = OFFBOARD_SWITCH_NONE; } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { - current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; + status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +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; - if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { - /* offboard switch overrides main switch */ - res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); + /* offboard switch overrides main switch */ + if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + res = main_state_transition(status, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode("OFFBOARD"); - } else { - switch (current_status->mode_switch) { - case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + } else { + return res; + } + } - case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + /* offboard switched off or denied, check mode switch */ + switch (status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_ASSISTED: + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); - // else fallback to SEATBELT - print_reject_mode("EASY"); - } + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT + print_reject_mode("EASY"); + } - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this mode + res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here - break; + if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); - case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + // else fallback to MANUAL + res = main_state_transition(status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_AUTO: + res = main_state_transition(status, MAIN_STATE_AUTO); - // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + 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 fallback to SEATBELT (EASY likely will not work too) + print_reject_mode("AUTO"); + res = main_state_transition(status, MAIN_STATE_SEATBELT); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } - // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); - // TRANSITION_DENIED is not possible here + 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; } - return res; + /* 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 |