forked from Archive/PX4-Autopilot
Added part of RGBLED stuff to commander
This commit is contained in:
parent
af3e0d459a
commit
451adf2aa0
|
@ -197,7 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
|
||||||
*/
|
*/
|
||||||
int commander_thread_main(int argc, char *argv[]);
|
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);
|
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) {
|
while (!thread_should_exit) {
|
||||||
hrt_abstime t = hrt_absolute_time();
|
hrt_abstime t = hrt_absolute_time();
|
||||||
|
|
||||||
|
status_changed = false;
|
||||||
|
|
||||||
/* update parameters */
|
/* update parameters */
|
||||||
orb_check(param_changed_sub, &updated);
|
orb_check(param_changed_sub, &updated);
|
||||||
|
|
||||||
|
@ -855,8 +857,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
toggle_status_leds(&status, &armed, &gps_position);
|
|
||||||
|
|
||||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||||
/* compute system load */
|
/* compute system load */
|
||||||
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
|
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;
|
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 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
|
//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
|
||||||
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
|
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
|
||||||
low_battery_voltage_actions_done = true;
|
low_battery_voltage_actions_done = true;
|
||||||
mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
|
mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
|
||||||
status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
|
status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
|
||||||
status_changed = true;
|
status_changed = true;
|
||||||
|
battery_tune_played = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
low_voltage_counter++;
|
low_voltage_counter++;
|
||||||
|
@ -885,12 +888,14 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
critical_battery_voltage_actions_done = true;
|
critical_battery_voltage_actions_done = true;
|
||||||
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
|
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
|
||||||
status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
|
status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
|
||||||
|
battery_tune_played = false;
|
||||||
|
|
||||||
if (armed.armed) {
|
if (armed.armed) {
|
||||||
// XXX not sure what should happen when voltage is low in flight
|
// 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 {
|
} 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;
|
status_changed = true;
|
||||||
}
|
}
|
||||||
|
@ -1259,7 +1264,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
status.counter++;
|
status.counter++;
|
||||||
status.timestamp = t;
|
status.timestamp = t;
|
||||||
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
|
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
|
||||||
status_changed = false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* play arming and battery warning tunes */
|
/* play arming and battery warning tunes */
|
||||||
|
@ -1273,7 +1277,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
if (tune_low_bat() == OK)
|
if (tune_low_bat() == OK)
|
||||||
battery_tune_played = true;
|
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 */
|
/* play tune on battery critical */
|
||||||
if (tune_critical_bat() == OK)
|
if (tune_critical_bat() == OK)
|
||||||
battery_tune_played = true;
|
battery_tune_played = true;
|
||||||
|
@ -1294,6 +1298,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
counter++;
|
counter++;
|
||||||
|
|
||||||
|
toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
|
||||||
|
|
||||||
usleep(COMMANDER_MONITORING_INTERVAL);
|
usleep(COMMANDER_MONITORING_INTERVAL);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1325,40 +1332,93 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
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) {
|
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||||
/* run at 10Hz, full cycle is 16 ticks = 10/16Hz */
|
/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
|
||||||
if (armed->armed) {
|
if (armed->armed) {
|
||||||
/* armed, solid */
|
/* armed, solid */
|
||||||
led_on(LED_AMBER);
|
led_on(LED_BLUE);
|
||||||
|
|
||||||
} else if (armed->ready_to_arm) {
|
} else if (armed->ready_to_arm) {
|
||||||
/* ready to arm, blink at 2.5Hz */
|
/* ready to arm, blink at 1Hz */
|
||||||
if (leds_counter % 8 == 0) {
|
if (leds_counter % 20 == 0)
|
||||||
led_toggle(LED_AMBER);
|
led_toggle(LED_BLUE);
|
||||||
|
|
||||||
} else {
|
|
||||||
led_toggle(LED_AMBER);
|
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* not ready to arm, blink at 10Hz */
|
/* not ready to arm, blink at 10Hz */
|
||||||
led_toggle(LED_AMBER);
|
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 */
|
||||||
|
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) {
|
||||||
|
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) {
|
if (status->condition_global_position_valid) {
|
||||||
/* position estimated, solid */
|
pattern.color[i*2] = RGBLED_COLOR_BLUE;
|
||||||
led_on(LED_BLUE);
|
pattern.duration[i*2] = 1000;
|
||||||
|
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
|
||||||
} else if (leds_counter == 0) {
|
pattern.duration[i*2+1] = 800;
|
||||||
/* waiting for position estimate, short blink at 1.25Hz */
|
} else {
|
||||||
led_on(LED_BLUE);
|
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 {
|
} else {
|
||||||
/* no position estimator available, off */
|
for (i=0; i<3; i++) {
|
||||||
led_off(LED_BLUE);
|
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 */
|
||||||
|
}
|
||||||
|
|
||||||
|
rgbled_set_pattern(&pattern);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 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++;
|
leds_counter++;
|
||||||
|
|
|
@ -122,15 +122,17 @@ int tune_arm()
|
||||||
return ioctl(buzzer, TONE_SET_ALARM, 12);
|
return ioctl(buzzer, TONE_SET_ALARM, 12);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int tune_low_bat()
|
||||||
|
{
|
||||||
|
return ioctl(buzzer, TONE_SET_ALARM, 13);
|
||||||
|
}
|
||||||
|
|
||||||
int tune_critical_bat()
|
int tune_critical_bat()
|
||||||
{
|
{
|
||||||
return ioctl(buzzer, TONE_SET_ALARM, 14);
|
return ioctl(buzzer, TONE_SET_ALARM, 14);
|
||||||
}
|
}
|
||||||
|
|
||||||
int tune_low_bat()
|
|
||||||
{
|
|
||||||
return ioctl(buzzer, TONE_SET_ALARM, 13);
|
|
||||||
}
|
|
||||||
|
|
||||||
void tune_stop()
|
void tune_stop()
|
||||||
{
|
{
|
||||||
|
@ -201,9 +203,22 @@ int led_off(int led)
|
||||||
return ioctl(leds, LED_OFF, led);
|
return ioctl(leds, LED_OFF, led);
|
||||||
}
|
}
|
||||||
|
|
||||||
int rgbled_set_color(rgbled_color_t color) {
|
void rgbled_set_color(rgbled_color_t color) {
|
||||||
|
|
||||||
return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color);
|
if (rgbleds != -1)
|
||||||
|
ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
|
||||||
|
}
|
||||||
|
|
||||||
|
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) {
|
||||||
|
|
||||||
|
if (rgbleds != -1)
|
||||||
|
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
|
||||||
}
|
}
|
||||||
|
|
||||||
float battery_remaining_estimate_voltage(float voltage)
|
float battery_remaining_estimate_voltage(float voltage)
|
||||||
|
|
|
@ -45,6 +45,7 @@
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_armed.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
#include <drivers/drv_rgbled.h>
|
||||||
|
|
||||||
|
|
||||||
bool is_multirotor(const struct vehicle_status_s *current_status);
|
bool is_multirotor(const struct vehicle_status_s *current_status);
|
||||||
|
@ -58,8 +59,8 @@ void tune_positive(void);
|
||||||
void tune_neutral(void);
|
void tune_neutral(void);
|
||||||
void tune_negative(void);
|
void tune_negative(void);
|
||||||
int tune_arm(void);
|
int tune_arm(void);
|
||||||
int tune_critical_bat(void);
|
|
||||||
int tune_low_bat(void);
|
int tune_low_bat(void);
|
||||||
|
int tune_critical_bat(void);
|
||||||
void tune_stop(void);
|
void tune_stop(void);
|
||||||
|
|
||||||
int led_init(void);
|
int led_init(void);
|
||||||
|
@ -68,6 +69,12 @@ int led_toggle(int led);
|
||||||
int led_on(int led);
|
int led_on(int led);
|
||||||
int led_off(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);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Provides a coarse estimate of remaining battery power.
|
* Provides a coarse estimate of remaining battery power.
|
||||||
*
|
*
|
||||||
|
|
Loading…
Reference in New Issue