diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-16 13:40:09 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-16 13:40:09 -0800 |
commit | 0e2db0beb9228720a40bd19a7bd8891e5a8fdaba (patch) | |
tree | ff49a98efd4bf9540b287820fb6812c6adac3fe1 /apps/commander | |
parent | 2d1009a89727582bc38093c67b930015cdbcc353 (diff) | |
download | px4-firmware-0e2db0beb9228720a40bd19a7bd8891e5a8fdaba.tar.gz px4-firmware-0e2db0beb9228720a40bd19a7bd8891e5a8fdaba.tar.bz2 px4-firmware-0e2db0beb9228720a40bd19a7bd8891e5a8fdaba.zip |
Checkpoint: implement new state machine, compiling, WIP
Diffstat (limited to 'apps/commander')
-rw-r--r-- | apps/commander/commander.c | 410 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 472 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 157 |
3 files changed, 517 insertions, 522 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b1bc0f9b..8716caef7 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -124,18 +124,24 @@ extern struct system_load_s system_load; static int leds; static int buzzer; static int mavlink_fd; + +/* flags */ static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* Main state machine */ +static struct vehicle_status_s current_status; static orb_advert_t stat_pub; +/* XXX ? */ // static uint16_t nofix_counter = 0; // static uint16_t gotfix_counter = 0; +/* XXX ? */ static unsigned int failsafe_lowlevel_timeout_ms; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ /* pthread loops */ static void *orb_receive_loop(void *arg); @@ -252,18 +258,19 @@ enum AUDIO_PATTERN { int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) { +#warning add alarms for state transitions /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } +// if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || +// ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); +// } /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } +// if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); +// } /* Trigger Tetris on being bored */ @@ -277,6 +284,7 @@ void tune_confirm(void) void tune_error(void) { + /* XXX change alarm to 2 tones*/ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -795,19 +803,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + // XXX implement this - } else { +// if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { result = VEHICLE_CMD_RESULT_DENIED; - } +// } } break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -815,9 +825,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* request to disarm */ - + // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -830,7 +840,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { + if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -871,16 +881,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + mavlink_log_info(mavlink_fd, "starting gyro cal"); tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -893,16 +908,20 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting mag cal"); tune_confirm(); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -916,12 +935,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + // XXX implement this mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); tune_confirm(); + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + } else { mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); result = VEHICLE_CMD_RESULT_DENIED; @@ -933,15 +957,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting trim cal"); tune_confirm(); do_rc_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished trim cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -954,16 +981,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "CMD starting accel cal"); tune_confirm(); do_accel_calibration(status_pub, ¤t_status); tune_confirm(); mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1262,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); /* welcome user */ - warnx("I am in command now!\n"); + warnx("[commander] starting"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; @@ -1270,33 +1300,39 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); + warnx("ERROR: Failed to initialize leds"); } if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); + warnx("ERROR: Failed to initialize buzzer"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -1315,6 +1351,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } + // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); /* create pthreads */ @@ -1325,7 +1362,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ uint16_t counter = 0; - uint8_t flight_env; /* Initialize to 0.0V */ float battery_voltage = 0.0f; @@ -1350,11 +1386,13 @@ int commander_thread_main(int argc, char *argv[]) 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)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); uint64_t last_global_position_time = 0; + /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); @@ -1365,10 +1403,13 @@ int commander_thread_main(int argc, char *argv[]) * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. */ + + /* Subscribe to GPS topic */ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1502,45 +1543,36 @@ int commander_thread_main(int argc, char *argv[]) /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed */ - led_toggle(LED_BLUE); - - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_BLUE); - } - - /* toggle error led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_AMBER); +// if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || +// current_status.state_machine == SYSTEM_STATE_AUTO || +// current_status.state_machine == SYSTEM_STATE_MANUAL)) { +// /* armed */ +// led_toggle(LED_BLUE); + + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_BLUE); + } - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle error (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); + /* toggle error led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_AMBER); - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle error (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); - // } else { - // led_off(LED_AMBER); - // } - } + } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - last_idle_time = system_load.tasks[0].total_runtime; - } + last_idle_time = system_load.tasks[0].total_runtime; } // // XXX Export patterns and threshold to parameters @@ -1583,13 +1615,14 @@ int commander_thread_main(int argc, char *argv[]) low_voltage_counter++; } - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + /* Critical, this is rather an emergency, change state machine */ else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // XXX implement this +// state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1698,7 +1731,7 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { @@ -1734,85 +1767,112 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); - - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); + + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { + + /* assuming a fixed wing, fall back to direct pass-through */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, fall back to m */ + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { + + /* assuming a fixed wing, set to direct pass-through as requested */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.flag_control_position_enabled = true; + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + + } else { + /* center stick position, set seatbelt/simple control */ + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if manual stability control modes have to be switched + * Check if land/RTL is requested */ - if (!isfinite(sp_man.manual_sas_switch)) { + if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + current_status.flag_land_requested = false; - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + current_status.flag_land_requested = true; } else { /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.flag_mission_activated = false; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.flag_mission_activated = true; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.flag_mission_activated = false; + + } else { + + /* center switch position, set default */ + current_status.flag_mission_activated = false; // XXX default? + } + + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } /* @@ -1826,7 +1886,7 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = false; stick_on_counter = 0; } else { @@ -1838,7 +1898,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = true; stick_on_counter = 0; } else { @@ -1847,37 +1907,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; @@ -1965,12 +1994,13 @@ int commander_thread_main(int argc, char *argv[]) /* arm / disarm on request */ if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); +#warning fix this +// update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } } else { @@ -2006,18 +2036,20 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); + // XXX what is this? /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } +// if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && +// current_status.flag_preflight_gyro_calibration == false && +// current_status.flag_preflight_mag_calibration == false && +// current_status.flag_preflight_accel_calibration == false) { +// /* All ok, no calibration going on, go to standby */ +// do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); +// } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); +#warning fix this +// publish_armed_status(¤t_status); orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } @@ -2043,7 +2075,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(cmd_sub); - warnx("exiting..\n"); + warnx("exiting"); fflush(stdout); thread_running = false; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index bea388a10..e1ec73110 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,182 +40,297 @@ #include <stdio.h> #include <unistd.h> +#include <stdbool.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; /** - * Transition from one state to another + * Transition from one navigation state to another */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { - int invalid_state = false; + bool valid_transition = false; int ret = ERROR; - commander_state_machine_t old_state = current_status->state_machine; + if (current_status->navigation_state == new_state) { + warnx("Navigation state not changed"); - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + } else { - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } + switch (new_state) { + case NAVIGATION_STATE_INIT: + + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_STANDBY: + + if (current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MANUAL: + + if ( + ( current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) + && current_status->arming_state == ARMING_STATE_ARMED) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_SEATBELT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LOITER: + + if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) + && current_status->flag_global_position_valid) + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + if ( + (current_status->navigation_state == NAVIGATION_STATE_STANDBY + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + || current_status->navigation_state == NAVIGATION_STATE_LAND) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MISSION: + + if ( + current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_RTL: + + if ( + current_status->navigation_state == NAVIGATION_STATE_MISSION + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_TAKEOFF: + + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LAND: + if (current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_REBOOT: + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->flag_hil_enabled) { + valid_transition = true; + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + } + + break; + + default: + warnx("Unknown navigation state"); + break; } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; + } - case SYSTEM_STATE_GROUND_ERROR: + if (valid_transition) { + current_status->navigation_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); +// publish_armed_status(current_status); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); + } - /* set system flags according to state */ + return ret; +} - /* prevent actuators from arming */ - current_status->flag_system_armed = false; +/** + * Transition from one arming state to another + */ +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; + if (current_status->arming_state == new_state) { + warnx("Arming state not changed"); - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + } else { - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } + switch (new_state) { - break; + case ARMING_STATE_INIT: - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + if (current_status->arming_state == ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + valid_transition = true; + } + break; - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } + case ARMING_STATE_STANDBY: - break; + // TODO check for sensors + // XXX check if coming from reboot? + if (current_status->arming_state == ARMING_STATE_INIT) { - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); + valid_transition = true; + } + break; - /* standby enforces disarmed */ - current_status->flag_system_armed = false; + case ARMING_STATE_ARMED: - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - case SYSTEM_STATE_GROUND_READY: + mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + valid_transition = true; + } + break; - /* set system flags according to state */ + case ARMING_STATE_MISSION_ABORT: - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; + if (current_status->arming_state == ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + valid_transition = true; + } + break; - case SYSTEM_STATE_AUTO: + case ARMING_STATE_ERROR: - /* set system flags according to state */ + if (current_status->arming_state == ARMING_STATE_ARMED + || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_INIT) { - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + valid_transition = true; + } + break; - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; + case ARMING_STATE_REBOOT: - case SYSTEM_STATE_STABILIZED: + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_ERROR) { - /* set system flags according to state */ - current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); + valid_transition = true; + // XXX reboot here? + } + break; - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; + case ARMING_STATE_IN_AIR_RESTORE: - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; + if (current_status->arming_state == ARMING_STATE_INIT) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE"); + valid_transition = true; + } + break; + default: + warnx("Unknown arming state"); + break; + } } - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; + if (valid_transition) { + current_status->arming_state = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); +// publish_armed_status(current_status); ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); } return ret; } + + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { /* publish the new state */ @@ -223,70 +338,69 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->timestamp = hrt_absolute_time(); /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// if (current_status->flag_control_rates_enabled) { +// current_status->onboard_control_sensors_present |= 0x400; +// +// } else { +// current_status->onboard_control_sensors_present &= ~0x400; +// } + +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} +//void publish_armed_status(const struct vehicle_status_s *current_status) +//{ +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// /* lock down actuators if required, only in HIL */ +// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; +// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); +// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +//} /* * Private functions, update the state machine */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} +//void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// warnx("EMERGENCY HANDLER\n"); +// /* Depending on the current state go to one of the error states */ +// +// if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); +// +// } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { +// +// // DO NOT abort mission +// //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); +// +// } else { +// warnx("Unknown system state: #%d\n", current_status->state_machine); +// } +//} + +//void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +//{ +// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself +// state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); +// +// } else { +// //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); +// } +// +//} @@ -488,6 +602,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st /* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +#if 0 void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { @@ -750,3 +865,4 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ return ret; } +#endif diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 2f2ccc729..bf9296caf 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,163 +47,10 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); - -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); - - - #endif /* STATE_MACHINE_HELPER_H_ */ |