fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 14:23:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Massive improvements in state machine, still tracing wrong throttle scaling in manual input path

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-13 18:53:37 +02:00
parent 56b3b46f75
commit 9014577aff
10 changed files with 268 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+47 -47
View File
@@ -230,11 +230,11 @@ void cal_bsort(int16_t a[], int n)
}
}
void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
{
/* set to mag calibration mode */
current_status->preflight_mag_calibration = true;
state_machine_publish(status_pub, current_status);
status->preflight_mag_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw;
@@ -312,8 +312,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
}
/* disable calibration mode */
current_status->preflight_mag_calibration = false;
state_machine_publish(status_pub, current_status);
status->preflight_mag_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
/* sort values */
cal_bsort(mag_minima[0], peak_samples);
@@ -393,7 +393,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status)
free(mag_minima[2]);
char offset_output[50];
sprintf(offset_output, "[commander] mag calibration finished, offsets: x:%d, y:%d, z:%d", mag_offset[0], mag_offset[1], mag_offset[2]);
sprintf(offset_output, "[commander] mag cal: x:%8.4f y:%8.4f z:%8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
mavlink_log_info(mavlink_fd, offset_output);
close(sub_sensor_combined);
@@ -436,7 +436,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2];
char offset_output[50];
sprintf(offset_output, "[commander] gyro calibration finished, offsets: x:%d, y:%d, z:%d", (int)gyro_offset[0], (int)gyro_offset[1], (int)gyro_offset[2]);
sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
mavlink_log_info(mavlink_fd, offset_output);
close(sub_sensor_combined);
@@ -458,7 +458,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
switch (cmd->command) {
case MAV_CMD_DO_SET_MODE:
{
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1)) {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_DENIED;
@@ -466,18 +466,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
{
case MAV_CMD_COMPONENT_ARM_DISARM: {
/* request to arm */
if ((int)cmd->param1 == 1) {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_DENIED;
}
/* request to disarm */
} else if ((int)cmd->param1 == 0) {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_DENIED;
@@ -487,19 +486,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
break;
/* request for an autopilot reboot */
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
{
// if ((int)cmd->param1 == 1) {
// if (OK == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) {
// result = MAV_RESULT_ACCEPTED;//TODO: this has no effect
// } else {
// result = MAV_RESULT_DENIED;
// }
// }
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
if ((int)cmd->param1 == 1) {
if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
result = MAV_RESULT_ACCEPTED;
} else {
/* system may return here */
result = MAV_RESULT_DENIED;
}
}
}
break;
//
// /* request to land */
// case MAV_CMD_NAV_LAND:
// {
@@ -524,18 +523,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
// break;
//
/* preflight calibration */
case MAV_CMD_PREFLIGHT_CALIBRATION: {
case MAV_CMD_PREFLIGHT_CALIBRATION: {
bool handled = false;
/* gyro calibration */
if ((int)(cmd->param1) == 1) {
/* transition to calibration state */
do_state_update(status_pub, &current_status, SYSTEM_STATE_PREFLIGHT);
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
do_gyro_calibration(status_pub, &current_status);
do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration");
@@ -547,12 +546,12 @@ 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, &current_status, SYSTEM_STATE_PREFLIGHT);
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
do_mag_calibration(status_pub, &current_status);
do_state_update(status_pub, &current_status, SYSTEM_STATE_STANDBY);
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration");
@@ -571,7 +570,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
break;
/* preflight parameter load / store */
case MAV_CMD_PREFLIGHT_STORAGE: {
case MAV_CMD_PREFLIGHT_STORAGE: {
/* Read all parameters from EEPROM to RAM */
if (((int)cmd->param1) == 0) {
@@ -610,7 +609,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
default: {
default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED;
}
@@ -625,8 +624,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* send acknowledge command */
mavlink_message_t msg;
mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result);
//global_data_send_mavlink_message_out(&msg);
}
}
@@ -736,6 +733,8 @@ enum BAT_CHEM {
*
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
{
float ret = 0;
@@ -783,17 +782,19 @@ int commander_main(int argc, char *argv[])
fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
}
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
state_machine_publish(stat_pub, &current_status, mavlink_fd);
if (stat_pub < 0) {
printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n");
exit(ERROR);
}
/* make sure we are in preflight state */
//do_state_update(stat_pub, &current_status, (commander_state_machine_t)SYSTEM_STATE_PREFLIGHT);
mavlink_log_info(mavlink_fd, "[commander] system is running");
/* load EEPROM parameters */
@@ -854,11 +855,10 @@ int commander_main(int argc, char *argv[])
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
uint8_t vehicle_state_previous = current_status.state_machine;
// uint8_t vehicle_state_previous = current_status.state_machine;
float voltage_previous = 0.0f;
uint64_t last_idle_time = 0;
unsigned int signal_loss_counter = 0;
/* now initialized */
commander_initialized = true;
@@ -891,7 +891,7 @@ int commander_main(int argc, char *argv[])
}
/* toggle error led at 5 Hz in HIL mode */
if ((current_status.mode & MAV_MODE_FLAG_HIL_ENABLED)) {
if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
/* armed */
led_toggle(LED_AMBER);
@@ -994,7 +994,7 @@ int commander_main(int argc, char *argv[])
//
//
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
update_state_machine_got_position_fix(stat_pub, &current_status);
update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
/* end: check gps */
/* Check battery voltage */
@@ -1017,7 +1017,7 @@ int commander_main(int argc, char *argv[])
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
state_machine_emergency(stat_pub, &current_status);
state_machine_emergency(stat_pub, &current_status, mavlink_fd);
}
critical_voltage_counter++;
@@ -1044,7 +1044,7 @@ int commander_main(int argc, char *argv[])
/* check if left stick is in lower left position --> switch to standby state */
if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_disarm(stat_pub, &current_status);
update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;
} else {
@@ -1056,7 +1056,7 @@ int commander_main(int argc, char *argv[])
/* check if left stick is in lower right position --> arm */
if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_arm(stat_pub, &current_status);
update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
stick_on_counter = 0;
} else {
@@ -1067,13 +1067,13 @@ int commander_main(int argc, char *argv[])
//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
update_state_machine_mode_manual(stat_pub, &current_status);
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status);
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status);
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
}
/* Publish RC signal */
@@ -1112,11 +1112,11 @@ int commander_main(int argc, char *argv[])
current_status.preflight_gyro_calibration == false &&
current_status.preflight_mag_calibration == false) {
/* All ok, no calibration going on, go to standby */
do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY);
do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY, mavlink_fd);
}
/* Store old modes to detect and act on state transitions */
vehicle_state_previous = current_status.state_machine;
// vehicle_state_previous = current_status.state_machine;
voltage_previous = current_status.voltage_battery;
fflush(stdout);
apps/commander/state_machine_helper.c
+83 -64
View File
@@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_status.h>
#include <systemlib/systemlib.h>
#include <arch/board/up_hrt.h>
#include <mavlink/mavlink_log.h>
static const char* system_state_txt[] = {
"SYSTEM_STATE_PREFLIGHT",
@@ -61,10 +62,13 @@ static const char* system_state_txt[] = {
};
void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state)
/**
* Transition from one 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 invalid_state = false;
int ret = ERROR;
commander_state_machine_t old_state = current_status->state_machine;
@@ -74,10 +78,10 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
} else {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
}
return;
@@ -89,63 +93,63 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
//TODO: add emcy landing code here
fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
//global_data_send_mavlink_statustext_message_out("Commander: state: emergency landing", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
break;
case SYSTEM_STATE_EMCY_CUTOFF:
/* Tell the controller to cutoff the motors (thrust = 0), make sure that this is not overwritten by another app and stays at 0 */
//TODO: add emcy cutoff code here
/* Tell the controller to cutoff the motors (thrust = 0) */
fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
//global_data_send_mavlink_statustext_message_out("Commander: state: emergency cutoff", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
break;
case SYSTEM_STATE_GROUND_ERROR:
fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
//global_data_send_mavlink_statustext_message_out("Commander: state: ground error", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
break;
case SYSTEM_STATE_PREFLIGHT:
//global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO);
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false;
mavlink_log_info(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
}
break;
case SYSTEM_STATE_REBOOT:
usleep(500000);
reboot();
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
usleep(500000);
reboot();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
}
break;
case SYSTEM_STATE_STANDBY:
//global_data_send_mavlink_statustext_message_out("Commander: state: standby", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
break;
case SYSTEM_STATE_GROUND_READY:
//global_data_send_mavlink_statustext_message_out("Commander: state: armed", MAV_SEVERITY_INFO);
//if in manual mode switch to manual state
// if (current_status->remote_manual) {
// printf("[commander] manual mode\n");
// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
// return;
// }
mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
break;
case SYSTEM_STATE_AUTO:
//global_data_send_mavlink_statustext_message_out("Commander: state: auto", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
break;
case SYSTEM_STATE_STABILIZED:
//global_data_send_mavlink_statustext_message_out("Commander: state: stabilized", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
break;
case SYSTEM_STATE_MANUAL:
//global_data_send_mavlink_statustext_message_out("Commander: state: manual", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
break;
default:
@@ -155,11 +159,11 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
if (invalid_state == false || old_state != new_state) {
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status);
state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status) {
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
/* publish the new state */
current_status->counter++;
current_status->timestamp = hrt_absolute_time();
@@ -171,26 +175,26 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
/*
* Private functions, update the state machine
*/
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status)
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
fprintf(stderr, "[commander] 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, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
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_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
} else {
fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
}
}
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
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);
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
} else {
//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
@@ -399,28 +403,28 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status)
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
/* Depending on the current state switch state */
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
}
}
void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status)
void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
/* Depending on the current state switch state */
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
state_machine_emergency(status_pub, current_status);
state_machine_emergency(status_pub, current_status, mavlink_fd);
}
}
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status)
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
// XXX CHANGE BACK
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
printf("[commander] arming\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
} /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] landing\n");
@@ -428,52 +432,67 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
}*/
}
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status)
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
printf("[commander] going standby\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] MISSION ABORT!\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
}
}
void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status)
void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->control_manual_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] manual mode\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
}
}
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status)
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->control_manual_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] stabilized mode\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
}
}
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status)
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->control_manual_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
printf("[commander] auto mode\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
}
}
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode)
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
{
printf("in update state request\n");
uint8_t ret = 1;
current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
/* Set manual input enabled flag */
current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
/* vehicle is disarmed, mode requests arming */
if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
@@ -481,10 +500,10 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
// XXX REMOVE
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* Set armed flag */
current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
/* Set manual input enabled flag */
current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
ret = OK;
printf("[commander] arming due to command request\n");
}
@@ -495,7 +514,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
/* only disarm in ground ready */
//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
/* Clear armed flag, leave manual input enabled */
// current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
// current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED;
// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK;
// printf("[commander] disarming due to command request\n");
@@ -503,16 +522,16 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
//}
/* Switch on HIL if in standby */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) {
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
/* Enable HIL on request */
current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED;
current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED;
ret = OK;
state_machine_publish(status_pub, current_status);
state_machine_publish(status_pub, current_status, mavlink_fd);
printf("[commander] Enabling HIL\n");
}
/* NEVER actually switch off HIL without reboot */
if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) {
if ((current_status->mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
ret = ERROR;
}
@@ -520,7 +539,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
return ret;
}
uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
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
{
commander_state_machine_t current_system_state = current_status->state_machine;
@@ -539,7 +558,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
printf("system will reboot\n");
//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
ret = 0;
}
@@ -549,7 +568,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
printf("try to switch to auto/takeoff\n");
if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
printf("state: auto\n");
ret = 0;
}
@@ -560,7 +579,7 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
printf("try to switch to manual\n");
if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
printf("state: manual\n");
ret = 0;
}
apps/commander/state_machine_helper.h
+72 -15
View File
@@ -46,8 +46,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <v1.0/common/mavlink.h>
/**
* Switch to new state with no checking.
@@ -56,8 +54,13 @@
* 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)
*/
void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state);
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);
@@ -72,43 +75,75 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co
/**
* 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);
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* 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);
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);
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);
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);
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);
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 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);
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);
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/*
@@ -119,17 +154,39 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
/**
* 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, uint8_t mode);
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, uint8_t custom_mode);
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);
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status);
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status);
/**
* 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);