rgbled_pwm: switch to new led uorb interface

src/drivers/rgbled_pwm/CMakeLists.txt

@@ -38,5 +38,6 @@ px4_add_module(
 		rgbled_pwm.cpp
 	DEPENDS
 		platforms__common
+		lib__led
 	)
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/rgbled_pwm/rgbled_pwm.cpp

@@ -62,7 +62,8 @@
 
 #include <board_config.h>
 
-#include <drivers/drv_rgbled.h>
+#include <drivers/drv_led.h>
+#include <lib/led/led.h>
 #include <drivers/device/device.h>
 #include <systemlib/err.h>
 
@@ -78,44 +79,24 @@ public:
 
 	virtual int		init();
 	virtual int		probe();
-	virtual int		info();
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+	int		status();
 
 private:
-	struct hrt_call         _call;
-	struct hrt_call		_call_r;
-	struct hrt_call         _call_g;
-	struct hrt_call         _call_b;
-
 	work_s			_work;
 
-	rgbled_mode_t		_mode;
-	rgbled_pattern_t	_pattern;
-
 	uint8_t			_r;
 	uint8_t			_g;
 	uint8_t			_b;
-	bool			_enable;
 	float			_brightness;
 
-	bool			_running;
-	int			_led_interval;
-	bool			_should_run;
-	int			_counter;
+	volatile bool		_running;
+	volatile bool		_should_run;
 
-	static void             pwm_begin(void *arg);
-	static void 		pwm_end_r(void *arg);
-	static void             pwm_end_g(void *arg);
-	static void             pwm_end_b(void *arg);
-
-	void 			set_color(rgbled_color_t ledcolor);
-	void			set_mode(rgbled_mode_t mode);
-	void			set_pattern(rgbled_pattern_t *pattern);
+	LedController		_led_controller;
 
 	static void		led_trampoline(void *arg);
 	void			led();
 
-	int			send_led_enable(bool enable);
 	int			send_led_rgb();
 	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
 };
@@ -137,32 +118,24 @@ void rgbled_usage();
 
 RGBLED_PWM::RGBLED_PWM() :
 	CDev("rgbled", RGBLED0_DEVICE_PATH),
-	_call{},
-	_call_r{},
-	_call_g{},
-	_call_b{},
-	_mode(RGBLED_MODE_OFF),
+	_work{},
 	_r(0),
 	_g(0),
 	_b(0),
-	_enable(false),
 	_brightness(1.0f),
 	_running(false),
-	_led_interval(0),
-	_should_run(false),
-	_counter(0)
+	_should_run(true)
 {
-	memset(&_work, 0, sizeof(_work));
-	memset(&_pattern, 0, sizeof(_pattern));
-
-	memset(&_call, 0, sizeof(_call));
-	memset(&_call_r, 0, sizeof(_call_r));
-	memset(&_call_g, 0, sizeof(_call_g));
-	memset(&_call_b, 0, sizeof(_call_b));
 }
 
 RGBLED_PWM::~RGBLED_PWM()
 {
+	_should_run = false;
+	int counter = 0;
+
+	while (_running && ++counter < 10) {
+		usleep(100000);
+	}
 }
 
 int
@@ -171,32 +144,17 @@ RGBLED_PWM::init()
 	/* switch off LED on start */
 	CDev::init();
 	led_pwm_servo_init();
-	send_led_enable(false);
 	send_led_rgb();
-	return OK;
-}
-void
-RGBLED_PWM::pwm_begin(void *arg)
-{
-}
 
+	_running = true;
+	// kick off work queue
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, 0);
 
-void
-RGBLED_PWM::pwm_end_r(void *arg)
-{
-}
-void
-RGBLED_PWM::pwm_end_g(void *arg)
-{
-}
-void
-RGBLED_PWM::pwm_end_b(void *arg)
-{
+	return OK;
 }
 
-
 int
-RGBLED_PWM::info()
+RGBLED_PWM::status()
 {
 	int ret;
 	bool on, powersave;
@@ -220,45 +178,6 @@ RGBLED_PWM::probe()
 {
 	return (OK);
 }
-int
-RGBLED_PWM::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	int ret = ENOTTY;
-
-	switch (cmd) {
-	case RGBLED_SET_RGB:
-		/* set the specified color */
-		_r = ((rgbled_rgbset_t *) arg)->red;
-		_g = ((rgbled_rgbset_t *) arg)->green;
-		_b = ((rgbled_rgbset_t *) arg)->blue;
-		send_led_rgb();
-		return OK;
-
-	case RGBLED_SET_COLOR:
-		/* set the specified color name */
-		set_color((rgbled_color_t)arg);
-		send_led_rgb();
-		return OK;
-
-	case RGBLED_SET_MODE:
-		/* set the specified mode */
-		set_mode((rgbled_mode_t)arg);
-		return OK;
-
-	case RGBLED_SET_PATTERN:
-		/* set a special pattern */
-		set_pattern((rgbled_pattern_t *)arg);
-		return OK;
-
-	default:
-		/* see if the parent class can make any use of it */
-		ret = CDev::ioctl(filp, cmd, arg);
-		break;
-	}
-
-	return ret;
-}
-
 
 void
 RGBLED_PWM::led_trampoline(void *arg)
@@ -275,257 +194,65 @@ void
 RGBLED_PWM::led()
 {
 	if (!_should_run) {
-		_running = false;
-		return;
-	}
-
-	switch (_mode) {
-	case RGBLED_MODE_BLINK_SLOW:
-	case RGBLED_MODE_BLINK_NORMAL:
-	case RGBLED_MODE_BLINK_FAST:
-		if (_counter >= 2) {
-			_counter = 0;
-		}
-
-		send_led_enable(_counter == 0);
-
-		break;
-
-	case RGBLED_MODE_BREATHE:
+		int led_control_sub = _led_controller.led_control_subscription();
 
-		if (_counter >= 62) {
-			_counter = 0;
+		if (led_control_sub >= 0) {
+			orb_unsubscribe(led_control_sub);
 		}
 
-		int n;
-
-		if (_counter < 32) {
-			n = _counter;
-
-		} else {
-			n = 62 - _counter;
-		}
-
-		_brightness = n * n / (31.0f * 31.0f);
-//		send_led_rgb();
-		break;
-
-	case RGBLED_MODE_PATTERN:
-
-		/* don't run out of the pattern array and stop if the next frame is 0 */
-		if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) {
-			_counter = 0;
-		}
-
-		set_color(_pattern.color[_counter]);
-		send_led_rgb();
-		_led_interval = _pattern.duration[_counter];
-		break;
-
-	default:
-		break;
+		_running = false;
+		return;
 	}
 
-	_counter++;
-
-	/* re-queue ourselves to run again later */
-	work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, _led_interval);
-}
-
-/**
- * Parse color constant and set _r _g _b values
- */
-void
-RGBLED_PWM::set_color(rgbled_color_t color)
-{
-	switch (color) {
-	case RGBLED_COLOR_OFF:
-		_r = 0;
-		_g = 0;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_RED:
-		_r = 255;
-		_g = 0;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_YELLOW:
-		_r = 255;
-		_g = 200;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_PURPLE:
-		_r = 255;
-		_g = 0;
-		_b = 255;
-		break;
-
-	case RGBLED_COLOR_GREEN:
-		_r = 0;
-		_g = 255;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_BLUE:
-		_r = 0;
-		_g = 0;
-		_b = 255;
-		break;
-
-	case RGBLED_COLOR_WHITE:
-		_r = 255;
-		_g = 255;
-		_b = 255;
-		break;
-
-	case RGBLED_COLOR_AMBER:
-		_r = 255;
-		_g = 80;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_DIM_RED:
-		_r = 90;
-		_g = 0;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_DIM_YELLOW:
-		_r = 80;
-		_g = 30;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_DIM_PURPLE:
-		_r = 45;
-		_g = 0;
-		_b = 45;
-		break;
-
-	case RGBLED_COLOR_DIM_GREEN:
-		_r = 0;
-		_g = 90;
-		_b = 0;
-		break;
-
-	case RGBLED_COLOR_DIM_BLUE:
-		_r = 0;
-		_g = 0;
-		_b = 90;
-		break;
-
-	case RGBLED_COLOR_DIM_WHITE:
-		_r = 30;
-		_g = 30;
-		_b = 30;
-		break;
-
-	case RGBLED_COLOR_DIM_AMBER:
-		_r = 80;
-		_g = 20;
-		_b = 0;
-		break;
-
-	default:
-		PX4_WARN("color unknown");
-		break;
+	if (!_led_controller.is_init()) {
+		int led_control_sub = orb_subscribe(ORB_ID(led_control));
+		_led_controller.init(led_control_sub);
 	}
-}
 
-/**
- * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
- */
-void
-RGBLED_PWM::set_mode(rgbled_mode_t mode)
-{
-	if (mode != _mode) {
-		_mode = mode;
+	LedControlData led_control_data;
 
-		switch (mode) {
-		case RGBLED_MODE_OFF:
-			_should_run = false;
-			send_led_enable(false);
+	if (_led_controller.update(led_control_data) == 1) {
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			_r = 255; _g = 0; _b = 0;
 			break;
 
-		case RGBLED_MODE_ON:
-			_brightness = 1.0f;
-			send_led_rgb();
-			send_led_enable(true);
+		case led_control_s::COLOR_GREEN:
+			_r = 0; _g = 255; _b = 0;
 			break;
 
-		case RGBLED_MODE_BLINK_SLOW:
-			_should_run = true;
-			_counter = 0;
-			_led_interval = 2000;
-			_brightness = 1.0f;
-			send_led_rgb();
+		case led_control_s::COLOR_BLUE:
+			_r = 0; _g = 0; _b = 255;
 			break;
 
-		case RGBLED_MODE_BLINK_NORMAL:
-			_should_run = true;
-			_counter = 0;
-			_led_interval = 500;
-			_brightness = 1.0f;
-			send_led_rgb();
+		case led_control_s::COLOR_AMBER: //make it the same as yellow
+		case led_control_s::COLOR_YELLOW:
+			_r = 255; _g = 255; _b = 0;
 			break;
 
-		case RGBLED_MODE_BLINK_FAST:
-			_should_run = true;
-			_counter = 0;
-			_led_interval = 100;
-			_brightness = 1.0f;
-			send_led_rgb();
+		case led_control_s::COLOR_PURPLE:
+			_r = 255; _g = 0; _b = 255;
 			break;
 
-		case RGBLED_MODE_BREATHE:
-			_should_run = true;
-			_counter = 0;
-			_led_interval = 25;
-			send_led_enable(true);
+		case led_control_s::COLOR_CYAN:
+			_r = 0; _g = 255; _b = 255;
 			break;
 
-		case RGBLED_MODE_PATTERN:
-			_should_run = true;
-			_counter = 0;
-			_brightness = 1.0f;
-			send_led_enable(true);
+		case led_control_s::COLOR_WHITE:
+			_r = 255; _g = 255; _b = 255;
 			break;
 
-		default:
-			PX4_WARN("mode unknown");
+		default: // led_control_s::COLOR_OFF
+			_r = 0; _g = 0; _b = 0;
 			break;
 		}
 
-		/* if it should run now, start the workq */
-		if (_should_run && !_running) {
-			_running = true;
-			work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this, 1);
-		}
-
+		send_led_rgb();
 	}
-}
 
-/**
- * Set pattern for PATTERN mode, but don't change current mode
- */
-void
-RGBLED_PWM::set_pattern(rgbled_pattern_t *pattern)
-{
-	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
-}
-
-/**
- * Sent ENABLE flag to LED driver
- */
-int
-RGBLED_PWM::send_led_enable(bool enable)
-{
-	_enable = enable;
-	send_led_rgb();
-	return (OK);
+	/* re-queue ourselves to run again later */
+	work_queue(LPWORK, &_work, (worker_t)&RGBLED_PWM::led_trampoline, this,
+		   USEC2TICK(_led_controller.maximum_update_interval()));
 }
 
 /**
@@ -535,16 +262,9 @@ int
 RGBLED_PWM::send_led_rgb()
 {
 
-	if (_enable) {
-		led_pwm_servo_set(0, _r);
-		led_pwm_servo_set(1, _g);
-		led_pwm_servo_set(2, _b);
-
-	} else {
-		led_pwm_servo_set(0, 0);
-		led_pwm_servo_set(1, 0);
-		led_pwm_servo_set(2, 0);
-	}
+	led_pwm_servo_set(0, _r);
+	led_pwm_servo_set(1, _g);
+	led_pwm_servo_set(2, _b);
 
 	return (OK);
 }
@@ -552,7 +272,8 @@ RGBLED_PWM::send_led_rgb()
 int
 RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 {
-	powersave = OK; on = _enable;
+	powersave = OK;
+	on = _r > 0 || _g > 0 || _b > 0;
 	r = _r;
 	g = _g;
 	b = _b;
@@ -562,7 +283,7 @@ RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 void
 rgbled_usage()
 {
-	PX4_WARN("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
+	PX4_INFO("missing command: try 'start', 'status', 'stop'");
 }
 
 int
@@ -592,9 +313,6 @@ rgbled_main(int argc, char *argv[])
 
 	const char *verb = argv[optind];
 
-	int fd;
-	int ret;
-
 	if (!strcmp(verb, "start")) {
 		if (g_rgbled != nullptr) {
 			errx(1, "already started");
@@ -624,68 +342,15 @@ rgbled_main(int argc, char *argv[])
 		exit(1);
 	}
 
-	if (!strcmp(verb, "test")) {
-		fd = open(RGBLED0_DEVICE_PATH, 0);
-
-		if (fd == -1) {
-			errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
-		}
-
-		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
-			{500, 500, 500, 500, 1000, 0 }	// "0" indicates end of pattern
-		};
-
-		ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
-		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
-
-		close(fd);
-		exit(ret);
-	}
-
-	if (!strcmp(verb, "info")) {
-		g_rgbled->info();
+	if (!strcmp(verb, "status")) {
+		g_rgbled->status();
 		exit(0);
 	}
 
-	if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
-		fd = open(RGBLED0_DEVICE_PATH, 0);
-
-		if (fd == -1) {
-			errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
-		}
-
-		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
-		close(fd);
-
-		/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
-		if (!strcmp(verb, "stop")) {
-			delete g_rgbled;
-			g_rgbled = nullptr;
-			exit(0);
-		}
-
-		exit(ret);
-	}
-
-	if (!strcmp(verb, "rgb")) {
-		if (argc < 5) {
-			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
-		}
-
-		fd = open(RGBLED0_DEVICE_PATH, 0);
-
-		if (fd == -1) {
-			errx(1, "Unable to open " RGBLED0_DEVICE_PATH);
-		}
-
-		rgbled_rgbset_t v;
-		v.red   = strtol(argv[2], nullptr, 0);
-		v.green = strtol(argv[3], nullptr, 0);
-		v.blue  = strtol(argv[4], nullptr, 0);
-		ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
-		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
-		close(fd);
-		exit(ret);
+	if (!strcmp(verb, "stop")) {
+		delete g_rgbled;
+		g_rgbled = nullptr;
+		exit(0);
 	}
 
 	rgbled_usage();