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

Checkpoint: implement new state machine, compiling, WIP

Browse Source
This commit is contained in:
Julian Oes
2013-02-16 13:40:09 -08:00
parent 2d1009a897
commit 0e2db0beb9
14 changed files with 920 additions and 936 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+214 -182
View File
@@ -124,19 +124,25 @@ extern struct system_load_s system_load;
static int leds; static int leds;
static int buzzer; static int buzzer;
static int mavlink_fd; static int mavlink_fd;
/* flags */
static bool commander_initialized = false; static bool commander_initialized = false;
static struct vehicle_status_s current_status; /**< Main state machine */
static orb_advert_t stat_pub;
// static uint16_t nofix_counter = 0;
// static uint16_t gotfix_counter = 0;
static unsigned int failsafe_lowlevel_timeout_ms;
static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */ static bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */ 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;
/* pthread loops */ /* pthread loops */
static void *orb_receive_loop(void *arg); 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) 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 */ /* Trigger alarm if going into any error state */
if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_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))) { // ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
ioctl(buzzer, TONE_SET_ALARM, 0); // ioctl(buzzer, TONE_SET_ALARM, 0);
ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); // ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
} // }
/* Trigger neutral on arming / disarming */ /* Trigger neutral on arming / disarming */
if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { // if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
ioctl(buzzer, TONE_SET_ALARM, 0); // ioctl(buzzer, TONE_SET_ALARM, 0);
ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); // ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
} // }
/* Trigger Tetris on being bored */ /* Trigger Tetris on being bored */
@@ -277,6 +284,7 @@ void tune_confirm(void)
void tune_error(void) void tune_error(void)
{ {
/* XXX change alarm to 2 tones*/
ioctl(buzzer, TONE_SET_ALARM, 4); 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 */ /* request to set different system mode */
switch (cmd->command) { switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: { case VEHICLE_CMD_DO_SET_MODE: {
if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { // XXX implement this
result = VEHICLE_CMD_RESULT_ACCEPTED;
} 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; result = VEHICLE_CMD_RESULT_DENIED;
} // }
} }
break; break;
case VEHICLE_CMD_COMPONENT_ARM_DISARM: { case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
/* request to arm */ /* request to arm */
if ((int)cmd->param1 == 1) { 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -815,9 +825,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
} }
/* request to disarm */ /* request to disarm */
// XXX this arms it instad of disarming
} else if ((int)cmd->param1 == 0) { } 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -830,7 +840,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* request for an autopilot reboot */ /* request for an autopilot reboot */
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
if ((int)cmd->param1 == 1) { 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 */ /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
result = VEHICLE_CMD_RESULT_ACCEPTED; result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -871,16 +881,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* gyro calibration */ /* gyro calibration */
if ((int)(cmd->param1) == 1) { if ((int)(cmd->param1) == 1) {
/* transition to calibration state */
do_state_update(status_pub, &current_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, &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"); mavlink_log_info(mavlink_fd, "starting gyro cal");
tune_confirm(); tune_confirm();
do_gyro_calibration(status_pub, &current_status); do_gyro_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished gyro cal"); mavlink_log_info(mavlink_fd, "finished gyro cal");
tune_confirm(); tune_confirm();
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
/* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -893,16 +908,20 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* magnetometer calibration */ /* magnetometer calibration */
if ((int)(cmd->param2) == 1) { if ((int)(cmd->param2) == 1) {
/* transition to calibration state */
do_state_update(status_pub, &current_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, &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"); mavlink_log_info(mavlink_fd, "starting mag cal");
tune_confirm(); tune_confirm();
do_mag_calibration(status_pub, &current_status); do_mag_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished mag cal"); mavlink_log_info(mavlink_fd, "finished mag cal");
tune_confirm(); tune_confirm();
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
/* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -916,12 +935,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* zero-altitude pressure calibration */ /* zero-altitude pressure calibration */
if ((int)(cmd->param3) == 1) { if ((int)(cmd->param3) == 1) {
/* transition to calibration state */ /* transition to calibration state */
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); 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)) {
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { // XXX implement this
mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
tune_confirm(); 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 { } else {
mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
result = VEHICLE_CMD_RESULT_DENIED; result = VEHICLE_CMD_RESULT_DENIED;
@@ -933,15 +957,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* trim calibration */ /* trim calibration */
if ((int)(cmd->param4) == 1) { if ((int)(cmd->param4) == 1) {
/* transition to calibration state */ /* transition to calibration state */
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); 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)) {
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "starting trim cal"); mavlink_log_info(mavlink_fd, "starting trim cal");
tune_confirm(); tune_confirm();
do_rc_calibration(status_pub, &current_status); do_rc_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished trim cal"); mavlink_log_info(mavlink_fd, "finished trim cal");
tune_confirm(); tune_confirm();
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
/* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -954,16 +981,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* accel calibration */ /* accel calibration */
if ((int)(cmd->param5) == 1) { if ((int)(cmd->param5) == 1) {
/* transition to calibration state */ if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "CMD starting accel cal"); mavlink_log_info(mavlink_fd, "CMD starting accel cal");
tune_confirm(); tune_confirm();
do_accel_calibration(status_pub, &current_status); do_accel_calibration(status_pub, &current_status);
tune_confirm(); tune_confirm();
mavlink_log_info(mavlink_fd, "CMD finished accel cal"); mavlink_log_info(mavlink_fd, "CMD finished accel cal");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
/* 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; result = VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -1262,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_sys_type = param_find("MAV_TYPE");
/* welcome user */ /* welcome user */
warnx("I am in command now!\n"); warnx("[commander] starting");
/* pthreads for command and subsystem info handling */ /* pthreads for command and subsystem info handling */
// pthread_t command_handling_thread; // pthread_t command_handling_thread;
@@ -1270,33 +1300,39 @@ int commander_thread_main(int argc, char *argv[])
/* initialize */ /* initialize */
if (led_init() != 0) { if (led_init() != 0) {
warnx("ERROR: Failed to initialize leds\n"); warnx("ERROR: Failed to initialize leds");
} }
if (buzzer_init() != 0) { if (buzzer_init() != 0) {
warnx("ERROR: Failed to initialize buzzer\n"); warnx("ERROR: Failed to initialize buzzer");
} }
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 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 */ /* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status)); memset(&current_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; current_status.flag_system_armed = false;
/* neither manual nor offboard control commands have been received */ /* neither manual nor offboard control commands have been received */
current_status.offboard_control_signal_found_once = false; current_status.offboard_control_signal_found_once = false;
current_status.rc_signal_found_once = false; current_status.rc_signal_found_once = false;
/* mark all signals lost as long as they haven't been found */ /* mark all signals lost as long as they haven't been found */
current_status.rc_signal_lost = true; current_status.rc_signal_lost = true;
current_status.offboard_control_signal_lost = true; current_status.offboard_control_signal_lost = true;
/* allow manual override initially */ /* allow manual override initially */
current_status.flag_external_manual_override_ok = true; current_status.flag_external_manual_override_ok = true;
/* flag position info as bad, do not allow auto mode */ /* flag position info as bad, do not allow auto mode */
current_status.flag_vector_flight_mode_ok = false; current_status.flag_vector_flight_mode_ok = false;
/* set battery warning flag */ /* set battery warning flag */
current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
@@ -1315,6 +1351,7 @@ int commander_thread_main(int argc, char *argv[])
exit(ERROR); exit(ERROR);
} }
// XXX needed?
mavlink_log_info(mavlink_fd, "system is running"); mavlink_log_info(mavlink_fd, "system is running");
/* create pthreads */ /* create pthreads */
@@ -1325,7 +1362,6 @@ int commander_thread_main(int argc, char *argv[])
/* Start monitoring loop */ /* Start monitoring loop */
uint16_t counter = 0; uint16_t counter = 0;
uint8_t flight_env;
/* Initialize to 0.0V */ /* Initialize to 0.0V */
float battery_voltage = 0.0f; float battery_voltage = 0.0f;
@@ -1350,11 +1386,13 @@ int commander_thread_main(int argc, char *argv[])
struct offboard_control_setpoint_s sp_offboard; struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard)); memset(&sp_offboard, 0, sizeof(sp_offboard));
/* Subscribe to global position */
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_global_position_s global_position; struct vehicle_global_position_s global_position;
memset(&global_position, 0, sizeof(global_position)); memset(&global_position, 0, sizeof(global_position));
uint64_t last_global_position_time = 0; uint64_t last_global_position_time = 0;
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
struct vehicle_local_position_s local_position; struct vehicle_local_position_s local_position;
memset(&local_position, 0, sizeof(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 * position estimator and commander. RAW GPS is more than good enough for a
* non-flying vehicle. * non-flying vehicle.
*/ */
/* Subscribe to GPS topic */
int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
struct vehicle_gps_position_s gps_position; struct vehicle_gps_position_s gps_position;
memset(&gps_position, 0, sizeof(gps_position)); memset(&gps_position, 0, sizeof(gps_position));
/* Subscribe to sensor topic */
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s sensors; struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors)); memset(&sensors, 0, sizeof(sensors));
@@ -1502,45 +1543,36 @@ int commander_thread_main(int argc, char *argv[])
/* Slow but important 8 Hz checks */ /* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || // if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
current_status.state_machine == SYSTEM_STATE_AUTO || // current_status.state_machine == SYSTEM_STATE_AUTO ||
current_status.state_machine == SYSTEM_STATE_MANUAL)) { // current_status.state_machine == SYSTEM_STATE_MANUAL)) {
/* armed */ // /* armed */
led_toggle(LED_BLUE); // led_toggle(LED_BLUE);
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* not armed */ /* not armed */
led_toggle(LED_BLUE); led_toggle(LED_BLUE);
} }
/* toggle error led at 5 Hz in HIL mode */ /* toggle error led at 5 Hz in HIL mode */
if (current_status.flag_hil_enabled) { if (current_status.flag_hil_enabled) {
/* hil enabled */ /* hil enabled */
led_toggle(LED_AMBER); led_toggle(LED_AMBER);
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { } 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 */ /* toggle error (red) at 5 Hz on low battery or error */
led_toggle(LED_AMBER); led_toggle(LED_AMBER);
} else { }
// /* Constant error indication in standby mode without GPS */
// if (!current_status.gps_valid) {
// led_on(LED_AMBER);
// } else { if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
// led_off(LED_AMBER); /* compute system load */
// } uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { if (last_idle_time > 0)
/* compute system load */ current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
if (last_idle_time > 0) last_idle_time = system_load.tasks[0].total_runtime;
current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle
last_idle_time = system_load.tasks[0].total_runtime;
}
} }
// // XXX Export patterns and threshold to parameters // // XXX Export patterns and threshold to parameters
@@ -1583,13 +1615,14 @@ int commander_thread_main(int argc, char *argv[])
low_voltage_counter++; 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)) { 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) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true; critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
state_machine_emergency(stat_pub, &current_status, mavlink_fd); // XXX implement this
// state_machine_emergency(stat_pub, &current_status, mavlink_fd);
} }
critical_voltage_counter++; critical_voltage_counter++;
@@ -1698,7 +1731,7 @@ int commander_thread_main(int argc, char *argv[])
if (gps_position.fix_type == GPS_FIX_TYPE_3D if (gps_position.fix_type == GPS_FIX_TYPE_3D
&& (hdop_m < hdop_threshold_m) && (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 && !home_position_set
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
&& !current_status.flag_system_armed) { && !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) { if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
// /* /*
// * Check if manual control modes have to be switched * Check if manual control modes have to be switched
// */ */
// if (!isfinite(sp_man.manual_mode_switch)) { if (!isfinite(sp_man.mode_switch)) {
// warnx("man mode sw not finite\n"); warnx("mode sw not finite");
// /* this switch is not properly mapped, set default */ /* this switch is not properly mapped, set attitude stabilized */
// if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
// /* assuming a rotary wing, fall back to SAS */ current_status.flag_control_attitude_enabled = true;
// current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; current_status.flag_control_rates_enabled = true;
// current_status.flag_control_attitude_enabled = true; } else {
// current_status.flag_control_rates_enabled = true;
// } else {
// /* assuming a fixed wing, fall back to direct pass-through */ /* 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_attitude_enabled = false; current_status.flag_control_rates_enabled = false;
// current_status.flag_control_rates_enabled = false; }
// }
// } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
// /* bottom stick position, set direct mode for vehicles supporting it */ /* this switch is not properly mapped, set attitude stabilized */
// if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
// /* assuming a rotary wing, fall back to SAS */ /* assuming a rotary wing, fall back to m */
// current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; current_status.flag_control_attitude_enabled = true;
// current_status.flag_control_attitude_enabled = true; current_status.flag_control_rates_enabled = true;
// current_status.flag_control_rates_enabled = true; } else {
// } else {
// /* assuming a fixed wing, set to direct pass-through as requested */ /* 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_attitude_enabled = false; current_status.flag_control_rates_enabled = false;
// current_status.flag_control_rates_enabled = false; }
// }
// } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
// /* top stick position, set SAS for all vehicle types */ /* top stick position, set auto/mission for all vehicle types */
// current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; current_status.flag_control_position_enabled = true;
// current_status.flag_control_attitude_enabled = true; current_status.flag_control_velocity_enabled = true;
// current_status.flag_control_rates_enabled = true; current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
// } else { } else {
// /* center stick position, set rate control */ /* center stick position, set seatbelt/simple control */
// current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; current_status.flag_control_velocity_enabled = true;
// current_status.flag_control_attitude_enabled = false; current_status.flag_control_attitude_enabled = true;
// current_status.flag_control_rates_enabled = true; current_status.flag_control_rates_enabled = true;
// } }
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); // 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 */ /* 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 */ /* 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 */ /* top stick position */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; current_status.flag_land_requested = true;
} else { } else {
/* center stick position, set default */ /* 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.yaw < -STICK_ON_OFF_LIMIT)) &&
(sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_disarm(stat_pub, &current_status, mavlink_fd); current_status.flag_system_armed = false;
stick_on_counter = 0; stick_on_counter = 0;
} else { } else {
@@ -1838,7 +1898,7 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position --> arm */ /* 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 (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
update_state_machine_arm(stat_pub, &current_status, mavlink_fd); current_status.flag_system_armed = true;
stick_on_counter = 0; stick_on_counter = 0;
} else { } 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, &current_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, &current_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, &current_status, mavlink_fd);
// }
} else {
/* center stick position, set SAS for all vehicle types */
update_state_machine_mode_stabilized(stat_pub, &current_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_cutting_off = false;
current_status.rc_signal_lost = false; current_status.rc_signal_lost = false;
current_status.rc_signal_lost_interval = 0; current_status.rc_signal_lost_interval = 0;
@@ -1965,12 +1994,13 @@ int commander_thread_main(int argc, char *argv[])
/* arm / disarm on request */ /* arm / disarm on request */
if (sp_offboard.armed && !current_status.flag_system_armed) { if (sp_offboard.armed && !current_status.flag_system_armed) {
update_state_machine_arm(stat_pub, &current_status, mavlink_fd); #warning fix this
// update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
/* switch to stabilized mode = takeoff */ /* switch to stabilized mode = takeoff */
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd); // update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} else if (!sp_offboard.armed && current_status.flag_system_armed) { } else if (!sp_offboard.armed && current_status.flag_system_armed) {
update_state_machine_disarm(stat_pub, &current_status, mavlink_fd); // update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
} }
} else { } else {
@@ -2006,18 +2036,20 @@ int commander_thread_main(int argc, char *argv[])
current_status.timestamp = hrt_absolute_time(); current_status.timestamp = hrt_absolute_time();
// XXX what is this?
/* If full run came back clean, transition to standby */ /* If full run came back clean, transition to standby */
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
current_status.flag_preflight_gyro_calibration == false && // current_status.flag_preflight_gyro_calibration == false &&
current_status.flag_preflight_mag_calibration == false && // current_status.flag_preflight_mag_calibration == false &&
current_status.flag_preflight_accel_calibration == false) { // current_status.flag_preflight_accel_calibration == false) {
/* All ok, no calibration going on, go to standby */ // /* All ok, no calibration going on, go to standby */
do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY); // do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
} // }
/* publish at least with 1 Hz */ /* publish at least with 1 Hz */
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
publish_armed_status(&current_status); #warning fix this
// publish_armed_status(&current_status);
orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status); orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
state_changed = false; state_changed = false;
} }
@@ -2043,7 +2075,7 @@ int commander_thread_main(int argc, char *argv[])
close(sensor_sub); close(sensor_sub);
close(cmd_sub); close(cmd_sub);
warnx("exiting..\n"); warnx("exiting");
fflush(stdout); fflush(stdout);
thread_running = false; thread_running = false;
apps/commander/state_machine_helper.c
+317 -201
View File
@@ -40,182 +40,297 @@
#include <stdio.h> #include <stdio.h>
#include <unistd.h> #include <unistd.h>
#include <stdbool.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include "state_machine_helper.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; 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) { } else {
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 { switch (new_state) {
// ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); 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:
/* set system flags according to state */
/* prevent actuators from arming */
current_status->flag_system_armed = false;
warnx("GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
break;
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 {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
}
break;
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 */
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
}
break;
case SYSTEM_STATE_STANDBY:
/* set system flags according to state */
/* standby enforces disarmed */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
break;
case SYSTEM_STATE_GROUND_READY:
/* set system flags according to state */
/* ground ready has motors / actuators armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
break;
case SYSTEM_STATE_AUTO:
/* set system flags according to state */
/* auto is airborne and in auto mode, motors armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
break;
case SYSTEM_STATE_STABILIZED:
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
break;
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 (invalid_state == false || old_state != new_state) { if (valid_transition) {
current_status->state_machine = new_state; current_status->navigation_state = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd); state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status); // publish_armed_status(current_status);
ret = OK; ret = OK;
} } else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition");
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
ret = ERROR;
} }
return ret; return ret;
} }
/**
* 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;
if (current_status->arming_state == new_state) {
warnx("Arming state not changed");
} else {
switch (new_state) {
case ARMING_STATE_INIT:
if (current_status->arming_state == ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE");
valid_transition = true;
}
break;
case ARMING_STATE_STANDBY:
// TODO check for sensors
// XXX check if coming from reboot?
if (current_status->arming_state == ARMING_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE");
valid_transition = true;
}
break;
case ARMING_STATE_ARMED:
if (current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE");
valid_transition = true;
}
break;
case ARMING_STATE_MISSION_ABORT:
if (current_status->arming_state == ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE");
valid_transition = true;
}
break;
case ARMING_STATE_ERROR:
if (current_status->arming_state == ARMING_STATE_ARMED
|| current_status->arming_state == ARMING_STATE_MISSION_ABORT
|| current_status->arming_state == ARMING_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE");
valid_transition = true;
}
break;
case ARMING_STATE_REBOOT:
if (current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_ERROR) {
mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE");
valid_transition = true;
// XXX reboot here?
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
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 (valid_transition) {
current_status->arming_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 arming state transition");
}
return ret;
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
/* publish the new state */ /* 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(); current_status->timestamp = hrt_absolute_time();
/* assemble state vector based on flag values */ /* assemble state vector based on flag values */
if (current_status->flag_control_rates_enabled) { // if (current_status->flag_control_rates_enabled) {
current_status->onboard_control_sensors_present |= 0x400; // current_status->onboard_control_sensors_present |= 0x400;
//
// } else {
// current_status->onboard_control_sensors_present &= ~0x400;
// }
} else { // current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
current_status->onboard_control_sensors_present &= ~0x400; // 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_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_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 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_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 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_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_rates_enabled) ? 0x400 : 0; // current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 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); 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) //void publish_armed_status(const struct vehicle_status_s *current_status)
{ //{
struct actuator_armed_s armed; // struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed; // armed.armed = current_status->flag_system_armed;
/* lock down actuators if required, only in HIL */ // /* lock down actuators if required, only in HIL */
armed.lockdown = (current_status->flag_hil_enabled) ? true : false; // armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); // orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); // orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
} //}
/* /*
* Private functions, update the state machine * 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) //void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ //{
warnx("EMERGENCY HANDLER\n"); // warnx("EMERGENCY HANDLER\n");
/* Depending on the current state go to one of the error states */ // /* 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);
// }
//}
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { //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
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); //{
// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { // state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
//
// DO NOT abort mission // } else {
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); // //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);
// }
} 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*/ /* 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) 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; return ret;
} }
#endif
apps/commander/state_machine_helper.h
+2 -155
View File
@@ -47,163 +47,10 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
/** int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
* 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 */ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
// 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);
/**
* 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);
/**
* 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); 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_ */ #endif /* STATE_MACHINE_HELPER_H_ */
apps/controllib/fixedwing.cpp
+17 -16
View File
@@ -39,6 +39,7 @@
#include "fixedwing.hpp" #include "fixedwing.hpp"
namespace control namespace control
{ {
@@ -294,7 +295,7 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[i] = 0.0f; _actuators.control[i] = 0.0f;
// only update guidance in auto mode // only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) { if (_status.navigation_state == NAVIGATION_STATE_MISSION) {
// update guidance // update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd); _guide.update(_pos, _att, _posCmd, _lastPosCmd);
} }
@@ -304,8 +305,8 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position // the setpoint should update to loitering around this position
// handle autopilot modes // handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO || if (_status.navigation_state == NAVIGATION_STATE_MISSION ||
_status.state_machine == SYSTEM_STATE_STABILIZED) { _status.navigation_state == NAVIGATION_STATE_MANUAL) {
// update guidance // update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd); _guide.update(_pos, _att, _posCmd, _lastPosCmd);
@@ -340,25 +341,25 @@ void BlockMultiModeBacksideAutopilot::update()
} }
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) { } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
#warning fix the different manual modes
_actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle; _actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { // } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
//
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, // _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed); // _att.rollspeed, _att.pitchspeed, _att.yawspeed);
//
_actuators.control[CH_AIL] = _stabilization.getAileron(); // _actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = - _stabilization.getElevator(); // _actuators.control[CH_ELV] = - _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder(); // _actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle; // _actuators.control[CH_THR] = _manual.throttle;
} // }
} }
// update all publications // update all publications
apps/drivers/blinkm/blinkm.cpp
+19 -18
View File
@@ -554,26 +554,27 @@ BlinkM::led()
led_blink = LED_BLINK; led_blink = LED_BLINK;
/* handle 4th led - flightmode indicator */ /* handle 4th led - flightmode indicator */
switch((int)vehicle_status_raw.flight_mode) { #warning fix this
case VEHICLE_FLIGHT_MODE_MANUAL: // switch((int)vehicle_status_raw.flight_mode) {
led_color_4 = LED_AMBER; // case VEHICLE_FLIGHT_MODE_MANUAL:
break; // led_color_4 = LED_AMBER;
// break;
case VEHICLE_FLIGHT_MODE_STAB: //
led_color_4 = LED_YELLOW; // case VEHICLE_FLIGHT_MODE_STAB:
break; // led_color_4 = LED_YELLOW;
// break;
case VEHICLE_FLIGHT_MODE_HOLD: //
led_color_4 = LED_BLUE; // case VEHICLE_FLIGHT_MODE_HOLD:
break; // led_color_4 = LED_BLUE;
// break;
case VEHICLE_FLIGHT_MODE_AUTO: //
led_color_4 = LED_GREEN; // case VEHICLE_FLIGHT_MODE_AUTO:
break; // led_color_4 = LED_GREEN;
} // break;
// }
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used sat´s */ /* handling used sat�s */
if(num_of_used_sats >= 7) { if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF; led_color_1 = LED_OFF;
led_color_2 = LED_OFF; led_color_2 = LED_OFF;
apps/fixedwing_att_control/fixedwing_att_control_main.c
+82 -81
View File
@@ -185,87 +185,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* control */ /* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO || #warning fix this
vstatus.state_machine == SYSTEM_STATE_STABILIZED) { // if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
/* attitude control */ // vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); // /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */ //
fixedwing_att_control_rates(&rates_sp, gyro, &actuators); // /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */ //
actuators.control[3] = att_sp.thrust; // /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
/* set flaps to zero */ //
actuators.control[4] = 0.0f; // /* set flaps to zero */
// actuators.control[4] = 0.0f;
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { //
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { // } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */ //
if (vstatus.rc_signal_lost) { // /* if the RC signal is lost, try to stay level and go slowly back down to ground */
// if (vstatus.rc_signal_lost) {
/* put plane into loiter */ //
att_sp.roll_body = 0.3f; // /* put plane into loiter */
att_sp.pitch_body = 0.0f; // att_sp.roll_body = 0.3f;
// att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */ //
if (isfinite(manual_sp.throttle) && // /* limit throttle to 60 % of last value if sane */
(manual_sp.throttle >= 0.0f) && // if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle <= 1.0f)) { // (manual_sp.throttle >= 0.0f) &&
att_sp.thrust = 0.6f * manual_sp.throttle; // (manual_sp.throttle <= 1.0f)) {
// att_sp.thrust = 0.6f * manual_sp.throttle;
} else { //
att_sp.thrust = 0.0f; // } else {
} // att_sp.thrust = 0.0f;
// }
att_sp.yaw_body = 0; //
// att_sp.yaw_body = 0;
// XXX disable yaw control, loiter //
// // XXX disable yaw control, loiter
} else { //
// } else {
att_sp.roll_body = manual_sp.roll; //
att_sp.pitch_body = manual_sp.pitch; // att_sp.roll_body = manual_sp.roll;
att_sp.yaw_body = 0; // att_sp.pitch_body = manual_sp.pitch;
att_sp.thrust = manual_sp.throttle; // att_sp.yaw_body = 0;
} // att_sp.thrust = manual_sp.throttle;
// }
att_sp.timestamp = hrt_absolute_time(); //
// att_sp.timestamp = hrt_absolute_time();
/* attitude control */ //
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); // /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */ //
fixedwing_att_control_rates(&rates_sp, gyro, &actuators); // /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */ //
actuators.control[3] = att_sp.thrust; // /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
/* pass through flaps */ //
if (isfinite(manual_sp.flaps)) { // /* pass through flaps */
actuators.control[4] = manual_sp.flaps; // if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
} else { //
actuators.control[4] = 0.0f; // } else {
} // actuators.control[4] = 0.0f;
// }
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { //
/* directly pass through values */ // } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
actuators.control[0] = manual_sp.roll; // /* directly pass through values */
/* positive pitch means negative actuator -> pull up */ // actuators.control[0] = manual_sp.roll;
actuators.control[1] = manual_sp.pitch; // /* positive pitch means negative actuator -> pull up */
actuators.control[2] = manual_sp.yaw; // actuators.control[1] = manual_sp.pitch;
actuators.control[3] = manual_sp.throttle; // actuators.control[2] = manual_sp.yaw;
// actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) { //
actuators.control[4] = manual_sp.flaps; // if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
} else { //
actuators.control[4] = 0.0f; // } else {
} // actuators.control[4] = 0.0f;
} // }
} // }
// }
/* publish rates */ /* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
apps/mavlink/mavlink.c
+56 -66
View File
@@ -195,95 +195,85 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
/* reset MAVLink mode bitfield */ /* reset MAVLink mode bitfield */
*mavlink_mode = 0; *mavlink_mode = 0;
/* set mode flags independent of system state */ /**
* Set mode flags
**/
/* HIL */ /* HIL */
if (v_status.flag_hil_enabled) { if (v_status.flag_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
} }
/* manual input */
if (v_status.flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
/* attitude or rate control */
if (v_status.flag_control_attitude_enabled ||
v_status.flag_control_rates_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
/* vector control */
if (v_status.flag_control_velocity_enabled ||
v_status.flag_control_position_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
/* autonomous mode */ /* autonomous mode */
if (v_status.state_machine == SYSTEM_STATE_AUTO) { if (v_status.navigation_state == NAVIGATION_STATE_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_RTL
|| v_status.navigation_state == NAVIGATION_STATE_LAND
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
} }
/* set arming state */ /* set arming state */
if (armed.armed) { if (v_status.flag_system_armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else { } else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
} }
switch (v_status.state_machine) { if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
case SYSTEM_STATE_PREFLIGHT:
if (v_status.flag_preflight_gyro_calibration ||
v_status.flag_preflight_mag_calibration ||
v_status.flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
*mavlink_state = MAV_STATE_UNINIT; }
}
break; if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
}
if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
*mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
}
/**
* Set mavlink state
**/
/* set calibration state */
if (v_status.flag_preflight_gyro_calibration ||
v_status.flag_preflight_mag_calibration ||
v_status.flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.flag_system_emergency) {
*mavlink_state = MAV_STATE_EMERGENCY;
} else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_RTL
|| v_status.navigation_state == NAVIGATION_STATE_LAND
|| v_status.navigation_state == NAVIGATION_STATE_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) {
case SYSTEM_STATE_STANDBY:
*mavlink_state = MAV_STATE_STANDBY; *mavlink_state = MAV_STATE_STANDBY;
break;
case SYSTEM_STATE_GROUND_READY: } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_MANUAL: *mavlink_state = MAV_STATE_UNINIT;
*mavlink_state = MAV_STATE_ACTIVE; } else {
break;
case SYSTEM_STATE_STABILIZED: warnx("Unknown mavlink state");
*mavlink_state = MAV_STATE_ACTIVE; *mavlink_state = MAV_STATE_CRITICAL;
break;
case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_MISSION_ABORT:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_LANDING:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_CUTOFF:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_GROUND_ERROR:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_REBOOT:
*mavlink_state = MAV_STATE_POWEROFF;
break;
} }
} }
@@ -688,7 +678,7 @@ int mavlink_thread_main(int argc, char *argv[])
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* send heartbeat */ /* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
/* switch HIL mode if required */ /* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled); set_hil_on_off(v_status.flag_hil_enabled);
apps/mavlink/orb_listener.c
+1 -1
View File
@@ -275,7 +275,7 @@ l_vehicle_status(struct listener *l)
mavlink_system.type, mavlink_system.type,
MAV_AUTOPILOT_PX4, MAV_AUTOPILOT_PX4,
mavlink_mode, mavlink_mode,
v_status.state_machine, v_status.navigation_state,
mavlink_state); mavlink_state);
} }
apps/mavlink_onboard/mavlink.c
+55 -55
View File
@@ -296,60 +296,60 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
} }
switch (v_status->state_machine) { // switch (v_status->state_machine) {
case SYSTEM_STATE_PREFLIGHT: // case SYSTEM_STATE_PREFLIGHT:
if (v_status->flag_preflight_gyro_calibration || // if (v_status->flag_preflight_gyro_calibration ||
v_status->flag_preflight_mag_calibration || // v_status->flag_preflight_mag_calibration ||
v_status->flag_preflight_accel_calibration) { // v_status->flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING; // *mavlink_state = MAV_STATE_CALIBRATING;
} else { // } else {
*mavlink_state = MAV_STATE_UNINIT; // *mavlink_state = MAV_STATE_UNINIT;
} // }
break; // break;
//
case SYSTEM_STATE_STANDBY: // case SYSTEM_STATE_STANDBY:
*mavlink_state = MAV_STATE_STANDBY; // *mavlink_state = MAV_STATE_STANDBY;
break; // break;
//
case SYSTEM_STATE_GROUND_READY: // case SYSTEM_STATE_GROUND_READY:
*mavlink_state = MAV_STATE_ACTIVE; // *mavlink_state = MAV_STATE_ACTIVE;
break; // break;
//
case SYSTEM_STATE_MANUAL: // case SYSTEM_STATE_MANUAL:
*mavlink_state = MAV_STATE_ACTIVE; // *mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; // *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break; // break;
//
case SYSTEM_STATE_STABILIZED: // case SYSTEM_STATE_STABILIZED:
*mavlink_state = MAV_STATE_ACTIVE; // *mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; // *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break; // break;
//
case SYSTEM_STATE_AUTO: // case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE; // *mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; // *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break; // break;
//
case SYSTEM_STATE_MISSION_ABORT: // case SYSTEM_STATE_MISSION_ABORT:
*mavlink_state = MAV_STATE_EMERGENCY; // *mavlink_state = MAV_STATE_EMERGENCY;
break; // break;
//
case SYSTEM_STATE_EMCY_LANDING: // case SYSTEM_STATE_EMCY_LANDING:
*mavlink_state = MAV_STATE_EMERGENCY; // *mavlink_state = MAV_STATE_EMERGENCY;
break; // break;
//
case SYSTEM_STATE_EMCY_CUTOFF: // case SYSTEM_STATE_EMCY_CUTOFF:
*mavlink_state = MAV_STATE_EMERGENCY; // *mavlink_state = MAV_STATE_EMERGENCY;
break; // break;
//
case SYSTEM_STATE_GROUND_ERROR: // case SYSTEM_STATE_GROUND_ERROR:
*mavlink_state = MAV_STATE_EMERGENCY; // *mavlink_state = MAV_STATE_EMERGENCY;
break; // break;
//
case SYSTEM_STATE_REBOOT: // case SYSTEM_STATE_REBOOT:
*mavlink_state = MAV_STATE_POWEROFF; // *mavlink_state = MAV_STATE_POWEROFF;
break; // break;
} // }
} }
@@ -434,7 +434,7 @@ int mavlink_thread_main(int argc, char *argv[])
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */ /* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
/* send status (values already copied in the section above) */ /* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan, mavlink_msg_sys_status_send(chan,
apps/multirotor_att_control/multirotor_att_control_main.c
+43 -41
View File
@@ -296,38 +296,39 @@ mc_thread_main(int argc, char *argv[])
} }
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */ /* act if stabilization is active or if the (nonsense) direct pass through mode is set */
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || #warning fix this
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { // if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { //
rates_sp.yaw = manual.yaw; // if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
control_yaw_position = false; // rates_sp.yaw = manual.yaw;
// control_yaw_position = false;
} else { //
/* // } else {
* This mode SHOULD be the default mode, which is: // /*
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS // * This mode SHOULD be the default mode, which is:
* // * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
* However, we fall back to this setting for all other (nonsense) // *
* settings as well. // * However, we fall back to this setting for all other (nonsense)
*/ // * settings as well.
// */
/* only move setpoint if manual input is != 0 */ //
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // /* only move setpoint if manual input is != 0 */
rates_sp.yaw = manual.yaw; // if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
control_yaw_position = false; // rates_sp.yaw = manual.yaw;
first_time_after_yaw_speed_control = true; // control_yaw_position = false;
// first_time_after_yaw_speed_control = true;
} else { //
if (first_time_after_yaw_speed_control) { // } else {
att_sp.yaw_body = att.yaw; // if (first_time_after_yaw_speed_control) {
first_time_after_yaw_speed_control = false; // att_sp.yaw_body = att.yaw;
} // first_time_after_yaw_speed_control = false;
// }
control_yaw_position = true; //
} // control_yaw_position = true;
} // }
} // }
// }
att_sp.thrust = manual.throttle; att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
@@ -348,16 +349,17 @@ mc_thread_main(int argc, char *argv[])
} }
} else { } else {
#warning fix this
/* manual rate inputs, from RC control or joystick */ /* manual rate inputs, from RC control or joystick */
if (state.flag_control_rates_enabled && // if (state.flag_control_rates_enabled &&
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { // state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
rates_sp.roll = manual.roll; // rates_sp.roll = manual.roll;
//
rates_sp.pitch = manual.pitch; // rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw; // rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle; // rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time(); // rates_sp.timestamp = hrt_absolute_time();
} // }
} }
} }
apps/sensors/sensors.cpp
+46 -62
View File
@@ -195,13 +195,11 @@ private:
int rc_map_yaw; int rc_map_yaw;
int rc_map_throttle; int rc_map_throttle;
int rc_map_manual_override_sw; int rc_map_mode_sw;
int rc_map_auto_mode_sw; int rc_map_return_sw;
int rc_map_mission_sw;
int rc_map_manual_mode_sw; // int rc_map_offboard_ctrl_mode_sw;
int rc_map_sas_mode_sw;
int rc_map_rtl_sw;
int rc_map_offboard_ctrl_mode_sw;
int rc_map_flaps; int rc_map_flaps;
@@ -241,13 +239,11 @@ private:
param_t rc_map_yaw; param_t rc_map_yaw;
param_t rc_map_throttle; param_t rc_map_throttle;
param_t rc_map_manual_override_sw; param_t rc_map_mode_sw;
param_t rc_map_auto_mode_sw; param_t rc_map_return_sw;
param_t rc_map_mission_sw;
param_t rc_map_manual_mode_sw; // param_t rc_map_offboard_ctrl_mode_sw;
param_t rc_map_sas_mode_sw;
param_t rc_map_rtl_sw;
param_t rc_map_offboard_ctrl_mode_sw;
param_t rc_map_flaps; param_t rc_map_flaps;
@@ -436,16 +432,15 @@ Sensors::Sensors() :
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
/* mandatory mode switches, mapped to channel 5 and 6 per default */ /* mandatory mode switches, mapped to channel 5 and 6 per default */
_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW");
_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */ /* optional mode switches, not mapped per default */
_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW");
_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -579,33 +574,25 @@ Sensors::parameters_update()
warnx("Failed getting throttle chan index"); warnx("Failed getting throttle chan index");
} }
if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx("Failed getting override sw chan index"); warnx("Failed getting mode sw chan index");
} }
if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
warnx("Failed getting auto mode sw chan index"); warnx("Failed getting return sw chan index");
}
if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
warnx("Failed getting mission sw chan index");
} }
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("Failed getting flaps chan index"); warnx("Failed getting flaps chan index");
} }
if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
warnx("Failed getting manual mode sw chan index"); // warnx("Failed getting offboard control mode sw chan index");
} // }
if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
warnx("Failed getting rtl sw chan index");
}
if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
warnx("Failed getting sas mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
warnx("Failed getting offboard control mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
warnx("Failed getting mode aux 1 index"); warnx("Failed getting mode aux 1 index");
@@ -649,15 +636,13 @@ Sensors::parameters_update()
_rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1;
_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; _rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; // _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
@@ -1122,10 +1107,10 @@ Sensors::ppm_poll()
manual_control.yaw = NAN; manual_control.yaw = NAN;
manual_control.throttle = NAN; manual_control.throttle = NAN;
manual_control.manual_mode_switch = NAN; manual_control.mode_switch = NAN;
manual_control.manual_sas_switch = NAN; manual_control.return_switch = NAN;
manual_control.return_to_launch_switch = NAN; manual_control.mission_switch = NAN;
manual_control.auto_offboard_input_switch = NAN; // manual_control.auto_offboard_input_switch = NAN;
manual_control.flaps = NAN; manual_control.flaps = NAN;
manual_control.aux1 = NAN; manual_control.aux1 = NAN;
@@ -1211,11 +1196,14 @@ Sensors::ppm_poll()
manual_control.yaw *= _parameters.rc_scale_yaw; manual_control.yaw *= _parameters.rc_scale_yaw;
} }
/* override switch input */
manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
/* mode switch input */ /* mode switch input */
manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
/* land switch input */
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
/* land switch input */
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
/* flaps */ /* flaps */
if (_rc.function[FLAPS] >= 0) { if (_rc.function[FLAPS] >= 0) {
@@ -1227,21 +1215,17 @@ Sensors::ppm_poll()
} }
} }
if (_rc.function[MANUAL_MODE] >= 0) { if (_rc.function[MODE] >= 0) {
manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
} }
if (_rc.function[SAS_MODE] >= 0) { if (_rc.function[MISSION] >= 0) {
manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
} }
if (_rc.function[RTL] >= 0) { // if (_rc.function[OFFBOARD_MODE] >= 0) {
manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
} // }
if (_rc.function[OFFBOARD_MODE] >= 0) {
manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
}
/* aux functions, only assign if valid mapping is present */ /* aux functions, only assign if valid mapping is present */
if (_rc.function[AUX_1] >= 0) { if (_rc.function[AUX_1] >= 0) {
apps/uORB/topics/manual_control_setpoint.h
+6 -6
View File
@@ -56,17 +56,17 @@ struct manual_control_setpoint_s {
float yaw; /**< rudder / yaw rate / yaw */ float yaw; /**< rudder / yaw rate / yaw */
float throttle; /**< throttle / collective thrust / altitude */ float throttle; /**< throttle / collective thrust / altitude */
float manual_override_switch; /**< manual override mode (mandatory) */ float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
float auto_mode_switch; /**< auto mode switch (mandatory) */ float return_switch; /**< land 2 position switch (mandatory): land, no effect */
float mission_switch; /**< mission 2 position switch (optional): mission, loiter */
/** /**
* Any of the channels below may not be available and be set to NaN * Any of the channels below may not be available and be set to NaN
* to indicate that it does not contain valid data. * to indicate that it does not contain valid data.
*/ */
float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */
float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ // XXX needed or parameter?
float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
float flaps; /**< flap position */ float flaps; /**< flap position */
apps/uORB/topics/rc_channels.h
+10 -12
View File
@@ -68,18 +68,16 @@ enum RC_CHANNELS_FUNCTION
ROLL = 1, ROLL = 1,
PITCH = 2, PITCH = 2,
YAW = 3, YAW = 3,
OVERRIDE = 4, MODE = 4,
AUTO_MODE = 5, RETURN = 5,
MANUAL_MODE = 6, MISSION = 6,
SAS_MODE = 7, OFFBOARD_MODE = 7,
RTL = 8, FLAPS = 8,
OFFBOARD_MODE = 9, AUX_1 = 9,
FLAPS = 10, AUX_2 = 10,
AUX_1 = 11, AUX_3 = 11,
AUX_2 = 12, AUX_4 = 12,
AUX_3 = 13, AUX_5 = 13,
AUX_4 = 14,
AUX_5 = 15,
RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
}; };
apps/uORB/topics/vehicle_status.h
+52 -40
View File
@@ -59,21 +59,30 @@
*/ */
/* State Machine */ /* State Machine */
typedef enum typedef enum {
{ NAVIGATION_STATE_INIT = 0,
SYSTEM_STATE_PREFLIGHT = 0, NAVIGATION_STATE_STANDBY,
SYSTEM_STATE_STANDBY = 1, NAVIGATION_STATE_MANUAL,
SYSTEM_STATE_GROUND_READY = 2, NAVIGATION_STATE_SEATBELT,
SYSTEM_STATE_MANUAL = 3, NAVIGATION_STATE_LOITER,
SYSTEM_STATE_STABILIZED = 4, NAVIGATION_STATE_AUTO_READY,
SYSTEM_STATE_AUTO = 5, NAVIGATION_STATE_MISSION,
SYSTEM_STATE_MISSION_ABORT = 6, NAVIGATION_STATE_RTL,
SYSTEM_STATE_EMCY_LANDING = 7, NAVIGATION_STATE_TAKEOFF,
SYSTEM_STATE_EMCY_CUTOFF = 8, NAVIGATION_STATE_LAND,
SYSTEM_STATE_GROUND_ERROR = 9, NAVIGATION_STATE_GROUND_ERROR,
SYSTEM_STATE_REBOOT= 10, NAVIGATION_STATE_REBOOT
} navigation_state_t;
} commander_state_machine_t; typedef enum {
ARMING_STATE_INIT = 0,
ARMING_STATE_STANDBY,
ARMING_STATE_ARMED,
ARMING_STATE_MISSION_ABORT,
ARMING_STATE_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE
} arming_state_t;
enum VEHICLE_MODE_FLAG { enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -86,25 +95,25 @@ enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
enum VEHICLE_FLIGHT_MODE { //enum VEHICLE_FLIGHT_MODE {
VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ // VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ // VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */
VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ // VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */
VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ // VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */
}; //};
//
enum VEHICLE_MANUAL_CONTROL_MODE { //enum VEHICLE_MANUAL_CONTROL_MODE {
VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ // VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */
VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ // VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */
VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ // VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */
}; //};
//
enum VEHICLE_MANUAL_SAS_MODE { //enum VEHICLE_MANUAL_SAS_MODE {
VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ // VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */
VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ // VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */
VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ // VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */
VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ // VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */
}; //};
/** /**
* Should match 1:1 MAVLink's MAV_TYPE ENUM * Should match 1:1 MAVLink's MAV_TYPE ENUM
@@ -134,7 +143,7 @@ enum VEHICLE_TYPE {
enum VEHICLE_BATTERY_WARNING { enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */
VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */
}; };
@@ -150,17 +159,17 @@ struct vehicle_status_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
//uint64_t failsave_highlevel_begin; TO BE COMPLETED // uint64_t failsave_highlevel_begin; TO BE COMPLETED
navigation_state_t navigation_state; /**< current navigation state */
arming_state_t arming_state; /**< current arming state */
commander_state_machine_t state_machine; /**< current flight state, main state machine */
enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */
int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */
/* system flags - these represent the state predicates */ /* system flags - these represent the state predicates */
bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_armed; /**< true is motors / actuators are armed */
bool flag_system_emergency;
bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */
bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */
@@ -170,6 +179,9 @@ struct vehicle_status_s
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_land_requested; /**< true if land requested */
bool flag_mission_activated; /**< true if in mission mode */
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration; bool flag_preflight_accel_calibration;
@@ -185,7 +197,7 @@ struct vehicle_status_s
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
//bool failsave_highlevel; bool failsave_highlevel;
/* see SYS_STATUS mavlink message for the following */ /* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_present;