From 2362041dc127620ac1fb823b8aab8416ba17f0c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 19:58:41 +0200 Subject: commander: state indication changed, 3 green blinks = succsess, 3 white blinks = neutral, 3 red blinks = reject --- src/modules/commander/commander.cpp | 119 ++++++++++++++--------------- src/modules/commander/commander_helper.cpp | 50 +++++++++--- src/modules/commander/commander_helper.h | 3 +- 3 files changed, 100 insertions(+), 72 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a2443b0f6..1e86e8e24 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -5,6 +5,7 @@ * Lorenz Meier * Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; - uint64_t start_time = 0; bool status_changed = true; @@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - toggle_status_leds(&status, &armed, true); + control_status_leds(&status, &armed, true); /* now initialized */ commander_initialized = true; @@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - // XXX not sure what should happen when voltage is low in flight - //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - // XXX should we still allow to arm with critical battery? - //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -1166,9 +1164,6 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; - - } else { - status_changed = false; } hrt_abstime t1 = hrt_absolute_time(); @@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; - toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + int blink_state = blink_msg_state(); + if (blink_state > 0) { + /* blinking LED message, don't touch LEDs */ + if (blink_state == 2) { + /* blinking LED message completed, restore normal state */ + control_status_leds(&status, &armed, true); + } + } else { + /* normal state */ + control_status_leds(&status, &armed, status_changed); + } + + status_changed = false; usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { + /* driving rgbled */ + if (changed) { + bool set_normal_color = false; + /* set mode */ + if (status->arming_state == ARMING_STATE_ARMED) { + rgbled_set_mode(RGBLED_MODE_ON); + set_normal_color = true; + + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + rgbled_set_color(RGBLED_COLOR_RED); + + } else if (status->arming_state == ARMING_STATE_STANDBY) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; + + } else { // STANDBY_ERROR and other states + rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); + rgbled_set_color(RGBLED_COLOR_RED); + } + + 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); + } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + + } else { + if (status->condition_local_position_valid) { + rgbled_set_color(RGBLED_COLOR_GREEN); + + } else { + rgbled_set_color(RGBLED_COLOR_BLUE); + } + } + } + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ @@ -1298,54 +1345,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif - if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ - - if (armed->armed) { - rgbled_set_mode(RGBLED_MODE_ON); - - } else if (armed->ready_to_arm) { - rgbled_set_mode(RGBLED_MODE_BREATHE); - - } else { - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - } - } - - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - - default: - break; - } - - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - - default: - break; - } - } - /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { if (leds_counter % 2 == 0) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 017499cb5..565b4b66a 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,7 +56,6 @@ #include #include - #include "commander_helper.h" /* oddly, ERROR is not defined for c++ */ @@ -64,21 +64,24 @@ #endif static const int ERROR = -1; +#define BLINK_MSG_TIME 700000 // 3 fast blinks + bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } static int buzzer; +static hrt_abstime blink_msg_end; int buzzer_init() { @@ -104,16 +107,25 @@ void tune_error() void tune_positive() { + 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); } void tune_neutral() { + 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() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_RED); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } @@ -132,18 +144,31 @@ 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); } +int blink_msg_state() +{ + if (blink_msg_end == 0) { + return 0; + + } else if (hrt_absolute_time() > blink_msg_end) { + return 2; + + } else { + return 1; + } +} + static int leds; static int rgbleds; int led_init() { + blink_msg_end = 0; + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); @@ -159,6 +184,7 @@ int led_init() warnx("Blue LED: ioctl fail\n"); return ERROR; } + #endif if (ioctl(leds, LED_ON, LED_AMBER)) { @@ -168,6 +194,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED_DEVICE_PATH, 0); + if (rgbleds == -1) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 errx(1, "Unable to open " RGBLED_DEVICE_PATH); @@ -203,19 +230,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -void rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } -void rgbled_set_mode(rgbled_mode_t mode) { +void rgbled_set_mode(rgbled_mode_t mode) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } -void rgbled_set_pattern(rgbled_pattern_t *pattern) { +void rgbled_set_pattern(rgbled_pattern_t *pattern) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 027d0535f..e9514446c 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,7 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); +int blink_msg_state(); int led_init(void); void led_deinit(void); @@ -70,9 +71,7 @@ int led_on(int led); int led_off(int led); void rgbled_set_color(rgbled_color_t color); - void rgbled_set_mode(rgbled_mode_t mode); - void rgbled_set_pattern(rgbled_pattern_t *pattern); /** -- cgit v1.2.3 From d542735b2a945ed874fab8b004f9c31e87bc1346 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 19 Sep 2013 11:10:37 +0200 Subject: re-enable state hil --- ROMFS/px4fmu_common/init.d/1000_rc.hil | 89 ------------------------- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 89 +++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil | 84 +++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 13 +++- src/modules/mavlink/mavlink_receiver.cpp | 1 + 5 files changed, 184 insertions(+), 92 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1000_rc.hil create mode 100644 ROMFS/px4fmu_common/init.d/1000_rc_fw.hil create mode 100644 ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/1000_rc.hil b/ROMFS/px4fmu_common/init.d/1000_rc.hil deleted file mode 100644 index 11318023c..000000000 --- a/ROMFS/px4fmu_common/init.d/1000_rc.hil +++ /dev/null @@ -1,89 +0,0 @@ -#!nsh -# -# USB HIL start -# - -echo "[HIL] HILStar starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" - diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil new file mode 100644 index 000000000..11318023c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil @@ -0,0 +1,89 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil new file mode 100644 index 000000000..d5782a89b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -0,0 +1,84 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting in state-HIL mode.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c2a8c0c6a..9ec346465 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,15 +108,22 @@ then if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/1000_rc.hil + sh /etc/init.d/1000_rc_fw.hil set MODE custom else if param compare SYS_AUTOSTART 1001 then sh /etc/init.d/1001_rc_quad.hil + set MODE custom else - # Try to get an USB console - nshterm /dev/ttyACM0 & + if param compare SYS_AUTOSTART 1002 + then + sh /etc/init.d/1002_rc_fw_state.hil + set MODE custom + else + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi fi fi diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 222d1f45f..a3ef1d63b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -574,6 +574,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + hil_global_pos.valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; -- cgit v1.2.3