diff options
Diffstat (limited to 'src/modules/commander')
| -rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 2 | ||||
| -rw-r--r-- | src/modules/commander/airspeed_calibration.cpp | 2 | ||||
| -rw-r--r-- | src/modules/commander/commander.cpp | 83 | ||||
| -rw-r--r-- | src/modules/commander/commander_helper.cpp | 91 | ||||
| -rw-r--r-- | src/modules/commander/commander_helper.h | 14 | ||||
| -rw-r--r-- | src/modules/commander/px4_custom_mode.h | 2 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 53 |
7 files changed, 144 insertions, 103 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 36b75dd58..1cbdf9bf8 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (double)accel_ref[orient][2]); data_collected[orient] = true; - tune_neutral(); + tune_neutral(true); } close(sensor_combined_sub); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 1809f9688..6039d92a7 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd) } mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(); + tune_neutral(true); close(diff_pres_sub); return OK; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 801ef8d01..b5570f424 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define RC_TIMEOUT_HIL 500000 #define OFFBOARD_TIMEOUT 200000 #define DIFFPRESS_TIMEOUT 2000000 @@ -616,7 +617,6 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; - bool battery_tune_played = false; bool arm_tune_played = false; /* set parameters */ @@ -909,7 +909,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); @@ -969,7 +969,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; @@ -1032,14 +1032,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - battery_tune_played = false; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; if (armed.armed) { arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); @@ -1113,12 +1111,20 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; - tune_positive(); + tune_positive(true); } } + + /* + * XXX workaround: + * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) + * which can trigger RC loss if the computer/simulator lags. + */ + uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; + /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1208,8 +1214,9 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine according to mode switches */ res = set_main_state_rc(&status); + /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { - tune_positive(); + tune_positive(armed.armed); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1309,7 +1316,7 @@ int commander_thread_main(int argc, char *argv[]) /* flight termination in manual mode if assisted switch is on easy position */ if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(); + tune_positive(armed.armed); } } @@ -1364,26 +1371,23 @@ int commander_thread_main(int argc, char *argv[]) /* play arming and battery warning tunes */ if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ - if (tune_arm() == OK) - arm_tune_played = true; - - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* play tune on battery warning */ - if (tune_low_bat() == OK) - battery_tune_played = true; + set_tune(TONE_ARMING_WARNING_TUNE); + arm_tune_played = true; } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ - if (tune_critical_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_FAST_TUNE); + + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* play tune on battery warning or failsafe */ + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (battery_tune_played) { - tune_stop(); - battery_tune_played = false; + } else { + set_tune(TONE_STOP_TUNE); } /* reset arm_tune_played when disarmed */ - if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { + if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } @@ -1478,11 +1482,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - rgbled_set_color(RGBLED_COLOR_AMBER); - } - + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { @@ -1802,15 +1803,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg) sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - // only buzz if armed, because else we're driving people nuts indoors - // they really need to look at the leds as well. - if (status->arming_state == ARMING_STATE_ARMED) { - tune_negative(); - } else { - - // Always show the led indication - led_negative(); - } + /* only buzz if armed, because else we're driving people nuts indoors + they really need to look at the leds as well. */ + tune_negative(armed.armed); } } @@ -1824,7 +1819,7 @@ print_reject_arm(const char *msg) char s[80]; sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + tune_negative(true); } } @@ -1832,27 +1827,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); - tune_negative(); + tune_negative(true); break; default: @@ -1992,9 +1987,9 @@ void *commander_low_prio_loop(void *arg) } if (calib_ret == OK) - tune_positive(); + tune_positive(true); else - tune_negative(); + tune_negative(true); arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 033e7dc88..fe6c9bfaa 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -45,6 +45,7 @@ #include <stdbool.h> #include <fcntl.h> #include <math.h> +#include <string.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -81,11 +82,22 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } -static int buzzer; -static hrt_abstime blink_msg_end; +static int buzzer = -1; +static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message +static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence +static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end +static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { + tune_end = 0; + tune_current = 0; + memset(tune_durations, 0, sizeof(tune_durations)); + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; + tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { @@ -101,58 +113,60 @@ void buzzer_deinit() close(buzzer); } -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +void set_tune(int tune) { + unsigned int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ + if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { + /* allow interrupting current non-repeating tune by the same tune */ + if (tune != tune_current || new_tune_duration != 0) { + ioctl(buzzer, TONE_SET_ALARM, tune); + } + tune_current = tune; + if (new_tune_duration != 0) { + tune_end = hrt_absolute_time() + new_tune_duration; + } else { + tune_end = 0; + } + } } -void tune_positive() +/** + * Blink green LED and play positive tune (if use_buzzer == true). + */ +void tune_positive(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } } -void tune_neutral() +/** + * Blink white LED and play neutral tune (if use_buzzer == true). + */ +void tune_neutral(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); -} - -void tune_negative() -{ - led_negative(); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } } -void led_negative() +/** + * Blink red LED and play negative tune (if use_buzzer == true). + */ +void tune_negative(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); -} - -int tune_arm() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); -} - -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); -} - -int tune_critical_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); -} - -void tune_stop() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + } } int blink_msg_state() @@ -161,6 +175,7 @@ int blink_msg_state() return 0; } else if (hrt_absolute_time() > blink_msg_end) { + blink_msg_end = 0; return 2; } else { @@ -168,8 +183,8 @@ int blink_msg_state() } } -static int leds; -static int rgbleds; +static int leds = -1; +static int rgbleds = -1; int led_init() { diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index af25a5e97..e75f2592f 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); -int tune_arm(void); -int tune_low_bat(void); -int tune_critical_bat(void); -void tune_stop(void); - -void led_negative(); +void set_tune(int tune); +void tune_positive(bool use_buzzer); +void tune_neutral(bool use_buzzer); +void tune_negative(bool use_buzzer); int blink_msg_state(); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 4843e72c3..65c8e8947 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,6 +8,8 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ +#include <stdint.h> + enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_SEATBELT, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7ea025e8a..9cecf5371 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,6 +44,7 @@ #include <stdbool.h> #include <dirent.h> #include <fcntl.h> +#include <string.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -321,10 +322,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s bool valid_transition = false; int ret = ERROR; - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); valid_transition = true; } else { @@ -352,23 +350,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; - struct dirent *direntry; d = opendir("/dev"); if (d) { + struct dirent *direntry; + char devname[24]; + while ((direntry = readdir(d)) != NULL) { - int sensfd = ::open(direntry->d_name, 0); - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + /* skip serial ports */ + if (!strncmp("tty", direntry->d_name, 3)) { + continue; + } + /* skip mtd devices */ + if (!strncmp("mtd", direntry->d_name, 3)) { + continue; + } + /* skip ram devices */ + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { + continue; + } + /* skip mavlink */ + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); - warnx("directory listing ok (FS mounted and readable)"); - } else { /* failed opening dir */ warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); |