diff options
-rw-r--r-- | apps/commander/commander.c | 498 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 199 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 6 |
3 files changed, 382 insertions, 321 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index ac535dd9a..6b026f287 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -797,302 +797,297 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* announce command handling */ tune_confirm(); - - /* supported command handling start */ - /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - // XXX implement this - -// 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_DO_SET_MODE: - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - /* request to disarm */ - // XXX this arms it instad of disarming - } else if ((int)cmd->param1 == 0) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - } - } - break; + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; } } - } - break; -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* transition to init state */ - // XXX add this again -// 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(); -// -// /* 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 { -// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; + if ((int)cmd->param1 & 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 { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* transition to init state */ - // XXX add this again -// 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(); -// -// /* 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 { -// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } + break; - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - // XXX add this again -// 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)) { -// -// // 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; -// } - - handled = true; - } + case VEHICLE_CMD_COMPONENT_ARM_DISARM: - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - // XXX add this again -// 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(); -// -// /* 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 { -// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - // XXX add this again -// 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, "CMD starting accel cal"); -// tune_confirm(); -// do_accel_calibration(status_pub, ¤t_status); -// tune_confirm(); -// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); -// -// /* 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 { -// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + result = VEHICLE_CMD_RESULT_ACCEPTED; - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + } + } } - } - break; + break; - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { - } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ + 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(); - if (((int)(cmd->param1)) == 0) { + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; + handled = true; + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + 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(); - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_FAILED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; } - } else if (((int)(cmd->param1)) == 1) { + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + // XXX add this again + // 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)) { + // + // // 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; + // } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + // XXX add this again + // 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(); + // + // /* 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 { + // mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + // result = VEHICLE_CMD_RESULT_DENIED; + // } + + handled = true; + } + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); result = VEHICLE_CMD_RESULT_ACCEPTED; + mavlink_log_info(mavlink_fd, "starting acc cal"); + tune_confirm(); + do_accel_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished acc cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + + handled = true; + } + + /* none found */ + if (!handled) { + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + + if (read_ret == OK) { + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "OK no changes in"); mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "OK saved param file"); mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; } - result = VEHICLE_CMD_RESULT_FAILED; + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } - } - break; + break; default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); @@ -1326,6 +1321,7 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; /* neither manual nor offboard control commands have been received */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index f1de99e4d..aae119d35 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -413,6 +413,63 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat return ret; } +/** + * Transition from one hil state to another + */ +int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) @@ -684,7 +741,78 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_system_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} #if 0 @@ -821,76 +949,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur } -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - return ret; -} 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) //TODO: add more checks to avoid state switching in critical situations { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f5ab91bad..27a471f13 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -82,6 +82,11 @@ typedef enum { ARMING_STATE_IN_AIR_RESTORE } arming_state_t; +typedef enum { + HIL_STATE_OFF = 0, + HIL_STATE_ON +} hil_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -177,6 +182,7 @@ struct vehicle_status_s navigation_state_t navigation_state; /**< current navigation state */ arming_state_t arming_state; /**< current arming state */ + hil_state_t hil_state; /**< current hil 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 */ |