Added part of RGBLED stuff to commander

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) {
+#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) {
 		/* armed, solid */
-			led_on(LED_AMBER);
+		led_on(LED_BLUE);
 
 	} 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 {
+		/* 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 */
+			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 {
-				led_toggle(LED_AMBER);
+				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 {
-			/* not ready to arm, blink at 10Hz */
+					pattern.color[i*2] = RGBLED_COLOR_GREEN;
-			led_toggle(LED_AMBER);
 				}
+				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) {
-			/* 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 {
-			/* 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++;

src/modules/commander/commander_helper.cpp

@@ -122,16 +122,18 @@ int tune_arm()
 	return ioctl(buzzer, TONE_SET_ALARM, 12);
 }
 
-int tune_critical_bat()
+int tune_low_bat()
 {
-	return ioctl(buzzer, TONE_SET_ALARM, 14);
+	return ioctl(buzzer, TONE_SET_ALARM, 13);
 }
 
-int tune_low_bat()
+int tune_critical_bat()
 {
-	return ioctl(buzzer, TONE_SET_ALARM, 13);
+	return ioctl(buzzer, TONE_SET_ALARM, 14);
 }
 
+
+
 void tune_stop()
 {
 	ioctl(buzzer, TONE_SET_ALARM, 0);
@@ -201,9 +203,22 @@ int led_off(int led)
 	return ioctl(leds, LED_OFF, led);
 }
 
-int 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) {
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+}
+
+void rgbled_set_pattern(rgbled_pattern_t *pattern) {
 
-	return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color);
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
 }
 
 float battery_remaining_estimate_voltage(float voltage)

src/modules/commander/commander_helper.h

@@ -45,6 +45,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <drivers/drv_rgbled.h>
 
 
 bool is_multirotor(const struct vehicle_status_s *current_status);
@@ -58,8 +59,8 @@ void tune_positive(void);
 void tune_neutral(void);
 void tune_negative(void);
 int tune_arm(void);
-int tune_critical_bat(void);
 int tune_low_bat(void);
+int tune_critical_bat(void);
 void tune_stop(void);
 
 int led_init(void);
@@ -68,6 +69,12 @@ int led_toggle(int led);
 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);
+
 /**
  * Provides a coarse estimate of remaining battery power.
  *