From 451adf2aa0d9795f69f5675b00ff3fb245312eb0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:05:59 +0200 Subject: Added part of RGBLED stuff to commander --- src/modules/commander/commander.cpp | 114 +++++++++++++++++++++++++++--------- 1 file changed, 87 insertions(+), 27 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fc7670ee5..2e5d080b9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -197,7 +197,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, vehicle_gps_position_s *gps_position); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -698,6 +698,8 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { hrt_abstime t = hrt_absolute_time(); + status_changed = false; + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -855,8 +857,6 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - toggle_status_leds(&status, &armed, &gps_position); - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -867,14 +867,17 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } + warnx("bat remaining: %2.2f", status.battery_remaining); + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; status_changed = true; + battery_tune_played = false; } low_voltage_counter++; @@ -885,12 +888,14 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + 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); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + // XXX should we still allow to arm with critical battery? + //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; } @@ -1259,7 +1264,6 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - status_changed = false; } /* play arming and battery warning tunes */ @@ -1273,7 +1277,7 @@ int commander_thread_main(int argc, char *argv[]) if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1294,6 +1298,9 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; + + toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1325,40 +1332,93 @@ int commander_thread_main(int argc, char *argv[]) } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { - if (leds_counter % 2 == 0) { - /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_BLUE); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 1Hz */ + if (leds_counter % 20 == 0) + led_toggle(LED_BLUE); + } else { + /* not ready to arm, blink at 10Hz */ + if (leds_counter % 2 == 0) + led_toggle(LED_BLUE); + } +#endif + + if (changed) { + + warnx("changed"); + + int i; + rgbled_pattern_t pattern; + memset(&pattern, 0, sizeof(pattern)); + if (armed->armed) { /* armed, solid */ - led_on(LED_AMBER); + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[0] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[0] = RGBLED_COLOR_RED; + } else { + pattern.color[0] = RGBLED_COLOR_GREEN; + } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + for (i=0; i<3; i++) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[i*2] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[i*2] = RGBLED_COLOR_RED; + } else { + pattern.color[i*2] = RGBLED_COLOR_GREEN; + } + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; + } + if (status->condition_global_position_valid) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 1000; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; } else { - led_toggle(LED_AMBER); + for (i=3; i<6; i++) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 100; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 100; + } + pattern.color[6*2] = RGBLED_COLOR_OFF; + pattern.duration[6*2] = 700; } } else { + for (i=0; i<3; i++) { + pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 200; + } /* not ready to arm, blink at 10Hz */ - led_toggle(LED_AMBER); } - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 0) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); + rgbled_set_pattern(&pattern); + } - } else { - /* no position estimator available, off */ - led_off(LED_BLUE); - } + /* give system warnings on error LED, XXX maybe add memory usage warning too */ + if (status->load > 0.95f) { + if (leds_counter % 2 == 0) + led_toggle(LED_AMBER); + } else { + led_off(LED_AMBER); } leds_counter++; -- cgit v1.2.3