fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-15 10:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Checkpoint: Added HIL state, arming/disarming works now, also from GQC

Browse Source
This commit is contained in:
Julian Oes
2013-02-19 12:32:47 -08:00
parent b7faaca435
commit aab6214cdc
3 changed files with 413 additions and 352 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+278 -282
View File
@@ -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
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;
//
// } else {
result = VEHICLE_CMD_RESULT_DENIED;
// }
}
break;
/* request to activate HIL */
if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) {
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 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, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
//
// mavlink_log_info(mavlink_fd, "starting gyro cal");
// tune_confirm();
// do_gyro_calibration(status_pub, &current_status);
// mavlink_log_info(mavlink_fd, "finished gyro cal");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_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;
}
/* magnetometer calibration */
if ((int)(cmd->param2) == 1) {
/* transition to init state */
// XXX add this again
// if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
// mavlink_log_info(mavlink_fd, "starting mag cal");
// tune_confirm();
// do_mag_calibration(status_pub, &current_status);
// mavlink_log_info(mavlink_fd, "finished mag cal");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_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;
}
/* 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, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_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, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_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, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
// mavlink_log_info(mavlink_fd, "starting trim cal");
// tune_confirm();
// do_rc_calibration(status_pub, &current_status);
// mavlink_log_info(mavlink_fd, "finished trim cal");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_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) {
// XXX add this again
// if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
//
// mavlink_log_info(mavlink_fd, "CMD starting accel cal");
// tune_confirm();
// do_accel_calibration(status_pub, &current_status);
// tune_confirm();
// mavlink_log_info(mavlink_fd, "CMD finished accel cal");
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_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;
}
/* 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 {
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;
}
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 {
mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
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;
}
}
break;
case VEHICLE_CMD_COMPONENT_ARM_DISARM:
/* 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;
}
}
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;
} 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) {
if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
mavlink_log_info(mavlink_fd, "starting gyro cal");
tune_confirm();
do_gyro_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished gyro cal");
tune_confirm();
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
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)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
mavlink_log_info(mavlink_fd, "starting mag cal");
tune_confirm();
do_mag_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished mag cal");
tune_confirm();
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
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, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_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, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_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, &current_status, mavlink_fd, ARMING_STATE_INIT)
// && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
// mavlink_log_info(mavlink_fd, "starting trim cal");
// tune_confirm();
// do_rc_calibration(status_pub, &current_status);
// mavlink_log_info(mavlink_fd, "finished trim cal");
// tune_confirm();
//
// /* back to standby state */
// do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
// do_navigation_state_update(status_pub, &current_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) {
if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
mavlink_log_info(mavlink_fd, "starting acc cal");
tune_confirm();
do_accel_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished acc cal");
tune_confirm();
do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else {
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;
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 {
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;
}
} else {
mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
result = VEHICLE_CMD_RESULT_UNSUPPORTED;
}
}
}
break;
default: {
mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
@@ -1326,6 +1321,7 @@ int commander_thread_main(int argc, char *argv[])
memset(&current_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 */
apps/commander/state_machine_helper.c
+129 -70
View File
@@ -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
{
apps/uORB/topics/vehicle_status.h
+6
View File
@@ -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 */