diff options
Diffstat (limited to 'src/modules/commander')
| -rw-r--r-- | src/modules/commander/accelerometer_calibration.c | 26 | ||||
| -rw-r--r-- | src/modules/commander/accelerometer_calibration.h | 2 | ||||
| -rw-r--r-- | src/modules/commander/commander.c | 1295 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.c | 1085 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.h | 160 |
5 files changed, 1301 insertions, 1267 deletions
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index d79dd93dd..231739962 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -78,24 +78,21 @@ #include <systemlib/conversions.h> #include <mavlink/mavlink_log.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +void do_accel_calibration(int mavlink_fd); +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { +void do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); - /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_scale[3]; - int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { /* measurements complete successfully, set parameters */ @@ -131,24 +128,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_error(); - sleep(2); + tune_negative(); } /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); } -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -198,7 +188,7 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; - tune_confirm(); + tune_neutral(); } close(sensor_combined_sub); diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index a11cf93d3..6a920c4ef 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -11,6 +11,6 @@ #include <stdint.h> #include <uORB/topics/vehicle_status.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +void do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index aab8f3e04..a34e526a8 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -60,13 +60,9 @@ #include <debug.h> #include <sys/prctl.h> #include <string.h> -#include <drivers/drv_led.h> -#include <drivers/drv_hrt.h> -#include <drivers/drv_tone_alarm.h> -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" #include <math.h> #include <poll.h> + #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/battery_status.h> @@ -83,11 +79,18 @@ #include <uORB/topics/differential_pressure.h> #include <mavlink/mavlink_log.h> +#include <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> + +#include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> +#include <systemlib/cpuload.h> + +#include "state_machine_helper.h" -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include <drivers/drv_accel.h> #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> @@ -102,7 +105,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -#include <systemlib/cpuload.h> + extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ @@ -121,25 +124,43 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + /* File descriptors */ static int leds; static int buzzer; static int mavlink_fd; + +/* flags */ static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ -static orb_advert_t stat_pub; +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; +/* Main state machine */ +static struct vehicle_status_s current_status; +static orb_advert_t stat_pub; +/* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + /* pthread loops */ -static void *orb_receive_loop(void *arg); +void *orb_receive_loop(void *arg); +void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -148,17 +169,25 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +int buzzer_init(void); +void buzzer_deinit(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +void do_reboot(void); +void do_gyro_calibration(void); +void do_mag_calibration(void); +void do_rc_calibration(void); +void do_airspeed_calibration(void); + + +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -167,7 +196,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u /** * Print the correct usage. */ -static void usage(const char *reason); +void usage(const char *reason); /** * Sort calibration values. @@ -179,7 +208,7 @@ static void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -static int buzzer_init() +int buzzer_init() { buzzer = open("/dev/tone_alarm", O_WRONLY); @@ -191,13 +220,13 @@ static int buzzer_init() return 0; } -static void buzzer_deinit() +void buzzer_deinit() { close(buzzer); } -static int led_init() +int led_init() { leds = open(LED_DEVICE_PATH, 0); @@ -214,12 +243,12 @@ static int led_init() return 0; } -static void led_deinit() +void led_deinit() { close(leds); } -static int led_toggle(int led) +int led_toggle(int led) { static int last_blue = LED_ON; static int last_amber = LED_ON; @@ -231,57 +260,50 @@ static int led_toggle(int led) return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); } -static int led_on(int led) +int led_on(int led) { return ioctl(leds, LED_ON, led); } -static int led_off(int led) +int led_off(int led) { return ioctl(leds, LED_OFF, led); } -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +void tune_error() { - - /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } - - /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } - - /* Trigger Tetris on being bored */ - - return 0; + ioctl(buzzer, TONE_SET_ALARM, 2); } -void tune_confirm(void) +void tune_positive() { ioctl(buzzer, TONE_SET_ALARM, 3); } -void tune_error(void) +void tune_neutral() { ioctl(buzzer, TONE_SET_ALARM, 4); } -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +void do_reboot() +{ + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +} + + +void do_rc_calibration() { + mavlink_log_info(mavlink_fd, "trim calibration starting"); + if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); return; @@ -292,7 +314,6 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; param_set(param_find("TRIM_ROLL"), &p); p = sp.pitch; @@ -301,22 +322,21 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ - /* auto-save to EEPROM */ + /* auto-save */ int save_ret = param_save_default(); if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); } + tune_positive(); + mavlink_log_info(mavlink_fd, "trim calibration done"); } -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +void do_mag_calibration() { - - /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -331,15 +351,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* limit update rate to get equally spaced measurements over time (in ms) */ orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); /* erase old calibration */ @@ -385,10 +396,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) return; } - tune_confirm(); - sleep(2); - tune_confirm(); - while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -402,9 +409,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); - tune_confirm(); + tune_neutral(); axis_deadline += calibration_interval / 3; } @@ -540,28 +547,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); } - /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - close(sub_mag); } -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +void do_gyro_calibration() { - /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); const int calibration_count = 5000; @@ -612,10 +610,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; - /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) @@ -652,10 +646,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { @@ -665,14 +656,11 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - mavlink_log_info(mavlink_fd, "keep it still"); - /* set to accel calibration mode */ - status->flag_preflight_airspeed_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); +void do_airspeed_calibration() +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); const int calibration_count = 2500; @@ -721,340 +709,277 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); } - /* exit airspeed calibration mode */ - status->flag_preflight_airspeed_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - close(diff_pres_sub); } - - void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command handling */ - tune_confirm(); - - - /* supported command handling start */ - /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + case VEHICLE_CMD_DO_SET_MODE: - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - break; + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { result = VEHICLE_CMD_RESULT_DENIED; } + } - /* request to disarm */ - - } 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 ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { result = VEHICLE_CMD_RESULT_DENIED; } - } - } - break; - - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + } else { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; } } - } - break; -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; + break; - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); result = VEHICLE_CMD_RESULT_DENIED; } - - handled = true; - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); result = VEHICLE_CMD_RESULT_DENIED; } - - handled = true; } - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + break; - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { + /* reboot is executed later, after positive tune is sent */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + do_reboot(); - handled = true; + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); + } + } } + break; - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status, mavlink_fd); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - break; - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { - } else { + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - if (((int)(cmd->param1)) == 0) { - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - result = VEHICLE_CMD_RESULT_FAILED; + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } - } else if (((int)(cmd->param1)) == 1) { + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } + case VEHICLE_CMD_PREFLIGHT_STORAGE: - result = VEHICLE_CMD_RESULT_FAILED; - } + if (((int)(cmd->param1)) == 0) { + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + + } else if (((int)(cmd->param1)) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } - } break; - default: { - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); - } + default: + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; break; } /* supported command handling stop */ if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + + tune_negative(); } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); + + tune_positive(); } /* send any requested ACKs */ @@ -1065,7 +990,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal { /* Set thread name */ prctl(PR_SET_NAME, "commander orb rcv", getpid()); @@ -1168,8 +1093,7 @@ float battery_remaining_estimate_voltage(float voltage) return ret; } -static void -usage(const char *reason) +void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -1244,44 +1168,59 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); /* welcome user */ - warnx("I am in command now!\n"); + warnx("[commander] starting"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; pthread_t subsystem_info_thread; + pthread_t commander_low_prio_thread; /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); + warnx("ERROR: Failed to initialize leds"); } if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); + warnx("ERROR: Failed to initialize buzzer"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; - current_status.flag_system_armed = false; + + + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; + current_status.flag_hil_enabled = false; + current_status.flag_fmu_armed = false; + current_status.flag_io_armed = false; // XXX read this from somewhere + /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; + // current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + // XXX for now just set sensors as initialized + current_status.condition_system_sensors_initialized = true; + /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1298,6 +1237,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } + // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); /* create pthreads */ @@ -1306,9 +1246,18 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&subsystem_info_attr, 2048); pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + /* Start monitoring loop */ uint16_t counter = 0; - uint8_t flight_env; /* Initialize to 0.0V */ float battery_voltage = 0.0f; @@ -1317,12 +1266,14 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint8_t low_voltage_counter = 0; uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; float bat_remain = 1.0f; uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; + /* XXX what exactly is this for? */ + uint64_t last_print_time = 0; + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1333,11 +1284,13 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); uint64_t last_global_position_time = 0; + /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); @@ -1348,10 +1301,13 @@ int commander_thread_main(int argc, char *argv[]) * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. */ + + /* Subscribe to GPS topic */ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1387,7 +1343,9 @@ int commander_thread_main(int argc, char *argv[]) thread_running = true; uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; bool state_changed = true; bool param_init_forced = true; @@ -1441,7 +1399,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!current_status.flag_system_armed) { + if (!current_status.flag_fmu_armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1481,6 +1439,13 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + /* update battery status */ orb_check(battery_sub, &new_data); if (new_data) { @@ -1499,10 +1464,11 @@ int commander_thread_main(int argc, char *argv[]) /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || + + /* XXX if armed */ + if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { + current_status.state_machine == SYSTEM_STATE_MANUAL*/)) { /* armed, solid */ led_on(LED_AMBER); @@ -1537,26 +1503,18 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { /* toggle arming (red) at 5 Hz on low battery or error */ led_toggle(LED_AMBER); - - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); - - // } else { - // led_off(LED_AMBER); - // } } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - last_idle_time = system_load.tasks[0].total_runtime; - } + last_idle_time = system_load.tasks[0].total_runtime; } // // XXX Export patterns and threshold to parameters @@ -1572,7 +1530,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { /* For less than 20%, start be slightly annoying at 1 Hz */ ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); + tune_positive(); } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -1599,13 +1557,14 @@ int commander_thread_main(int argc, char *argv[]) low_voltage_counter++; } - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + /* Critical, this is rather an emergency, change state machine */ else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // XXX implement this + // state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1617,6 +1576,14 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + // XXX check for sensors + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + } else { + // XXX: Add emergency stuff if sensors are lost + } + /* * Check for valid position information. @@ -1628,53 +1595,53 @@ int commander_thread_main(int argc, char *argv[]) */ /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; - bool airspeed_valid = current_status.flag_airspeed_valid; + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; + current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_global_position_valid = false; + current_status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; + current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_local_position_valid = false; + current_status.condition_local_position_valid = false; } /* Check for valid airspeed/differential pressure measurements */ if (hrt_absolute_time() - last_diff_pres_time < 2000000) { - current_status.flag_airspeed_valid = true; + current_status.condition_airspeed_valid = true; } else { - current_status.flag_airspeed_valid = false; + current_status.condition_airspeed_valid = false; } /* * Consolidate global position and local position valid flags * for vector flight mode. */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; - } else { - current_status.flag_vector_flight_mode_ok = false; - } + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid || - airspeed_valid != current_status.flag_airspeed_valid) { + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { state_changed = true; } @@ -1688,18 +1655,18 @@ int commander_thread_main(int argc, char *argv[]) * 2) The system has not aquired position lock before * 3) The system is not armed (on the ground) */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ - - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it + + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + if (orb_check(gps_sub, &new_data)) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); @@ -1724,10 +1691,10 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { + && !current_status.flag_fmu_armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1750,7 +1717,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ home_position_set = true; - tune_confirm(); + tune_positive(); } } @@ -1760,100 +1727,262 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); - - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + + warnx("mode sw not finite"); + /* no valid stick position, go to default */ + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + printf("mode switch: manual\n"); + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; + printf("mode switch: auto\n"); + + } else { + + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + printf("mode switch: seatbelt\n"); + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if manual stability control modes have to be switched - */ - if (!isfinite(sp_man.manual_sas_switch)) { + * Check if land/RTL is requested + */ + if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + current_status.return_switch = RETURN_SWITCH_RETURN; } else { /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.return_switch = RETURN_SWITCH_NONE; + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else { + + /* center switch position, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + /* 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!"); + } } /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && - (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + (sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + stick_off_counter = 0; } else { stick_off_counter++; @@ -1864,7 +1993,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; } else { @@ -1873,49 +2002,24 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + last_print_time = hrt_absolute_time(); } @@ -1949,8 +2053,8 @@ int commander_thread_main(int argc, char *argv[]) /* decide about attitude control flag, enable in att/pos/vel */ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); /* decide about rate control flag, enable it always XXX (for now) */ bool rates_ctrl_enabled = true; @@ -1973,7 +2077,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_control_manual_enabled = false; current_status.flag_control_offboard_enabled = true; state_changed = true; - tune_confirm(); + tune_positive(); mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); @@ -1981,7 +2085,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); state_changed = true; - tune_confirm(); + tune_positive(); } } @@ -1989,18 +2093,18 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = false; current_status.offboard_control_signal_lost_interval = 0; + // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + if (sp_offboard.armed && !current_status.flag_fmu_armed) { - } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + + } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { @@ -2016,7 +2120,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost_interval > 100000) { current_status.offboard_control_signal_lost = true; current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); + tune_positive(); /* kill motors after timeout */ if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { @@ -2032,25 +2136,28 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); + // XXX this is missing /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } + // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + // current_status.flag_preflight_gyro_calibration == false && + // current_status.flag_preflight_mag_calibration == false && + // current_status.flag_preflight_accel_calibration == false) { + // /* All ok, no calibration going on, go to standby */ + // do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } + + /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; - + /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -2059,6 +2166,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ // pthread_join(command_handling_thread, NULL); pthread_join(subsystem_info_thread, NULL); + pthread_join(commander_low_prio_thread, NULL); /* close fds */ led_deinit(); @@ -2069,10 +2177,93 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(cmd_sub); - warnx("exiting..\n"); + warnx("exiting"); fflush(stdout); thread_running = false; return 0; } + + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + + // do_baro_calibration(); + + case LOW_PRIO_TASK_RC_CALIBRATION: + + // do_rc_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bb..daed81553 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -40,182 +40,495 @@ #include <stdio.h> #include <unistd.h> +#include <stdint.h> +#include <stdbool.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; - -/** - * Transition from one state to another - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) -{ - int invalid_state = false; +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { + int ret = ERROR; - commander_state_machine_t old_state = current_status->state_machine; - - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_state->arming_state == ARMING_STATE_STANDBY) { + ret = OK; + current_state->flag_fmu_armed = false; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->condition_system_sensors_initialized) { + ret = OK; + current_state->flag_fmu_armed = false; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ + ret = OK; + current_state->flag_fmu_armed = true; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_ARMED) { + + /* XXX conditions for an error state? */ + ret = OK; + current_state->flag_fmu_armed = true; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + current_state->flag_fmu_armed = false; + } + break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + current_state->flag_fmu_armed = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + default: + 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; + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + } - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; + return ret; +} - case SYSTEM_STATE_GROUND_ERROR: - /* set system flags according to state */ - /* prevent actuators from arming */ - current_status->flag_system_armed = false; +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; + int ret = ERROR; - 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"); + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: + + /* transitions back to INIT are possible for calibration */ + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + + ret = OK; + current_state->flag_control_rates_enabled = false; + current_state->flag_control_attitude_enabled = false; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; + } + } + break; + + case NAVIGATION_STATE_MANUAL: + + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; + } + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + } else if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state-> condition_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + } else { + ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; + } + } + break; + + default: + break; } - 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"); + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); } + } - 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 */ + return ret; +} - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; +/** +* Transition from one hil state to another +*/ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; - case SYSTEM_STATE_AUTO: + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - /* set system flags according to state */ + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; + } else { - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; + switch (new_state) { - case SYSTEM_STATE_STABILIZED: + case HIL_STATE_OFF: - /* set system flags according to state */ - current_status->flag_system_armed = true; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; - case SYSTEM_STATE_MANUAL: + case HIL_STATE_ON: - /* set system flags according to state */ - current_status->flag_system_armed = true; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; - default: - invalid_state = true; - break; + default: + warnx("Unknown hil state"); + break; + } } - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; + if (valid_transition) { + current_status->hil_state = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); } return ret; } + + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { /* publish the new state */ @@ -223,76 +536,41 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->timestamp = hrt_absolute_time(); /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// if (current_status->flag_control_rates_enabled) { +// current_status->onboard_control_sensors_present |= 0x400; +// +// } else { +// current_status->onboard_control_sensors_present &= ~0x400; +// } + +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); -} - -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - - /* XXX allow arming by external components on multicopters only if not yet armed by RC */ - /* XXX allow arming only if core sensors are ok */ - armed.ready_to_arm = true; - - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} - - -/* - * Private functions, update the state machine - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - } +// void publish_armed_status(const struct vehicle_status_s *current_status) +// { +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// +// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ +// /* XXX allow arming only if core sensors are ok */ +// armed.ready_to_arm = true; +// +// /* lock down actuators if required, only in HIL */ +// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; +// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); +// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +// } // /* @@ -405,353 +683,76 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st // } -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - - -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ - - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_fmu_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2f2ccc729..5b57cffb7 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,163 +47,15 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); - -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - - -/** - * 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); +void navigation_state_update(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); +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); -/** - * Handle state machine if 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); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, 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); - - +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); -#endif /* STATE_MACHINE_HELPER_H_ */ +#endif /* STATE_MACHINE_HELPER_H_ */
\ No newline at end of file |