rgbled: major cleanup and bugfixes

1 files changed, 271 insertions, 198 deletions

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index ee1d472a2..74ea6d9e1 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -92,16 +92,14 @@ public:
 private:
 	work_s			_work;
 
-	rgbled_color_t		_color;
 	rgbled_mode_t		_mode;
 	rgbled_pattern_t	_pattern;
 
-	float			_brightness;
 	uint8_t			_r;
 	uint8_t			_g;
 	uint8_t			_b;
+	float			_brightness;
 
-	bool			_should_run;
 	bool			_running;
 	int			_led_interval;
 	int			_counter;
@@ -109,35 +107,33 @@ private:
 	void 			set_color(rgbled_color_t ledcolor);
 	void			set_mode(rgbled_mode_t mode);
 	void			set_pattern(rgbled_pattern_t *pattern);
-	void			set_brightness(float brightness);
 
 	static void		led_trampoline(void *arg);
 	void			led();
 
-	int			set(bool on, uint8_t r, uint8_t g, uint8_t b);
-	int			set_on(bool on);
-	int			set_rgb(uint8_t r, uint8_t g, uint8_t b);
-	int			get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+	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);
 };
 
 /* for now, we only support one RGBLED */
 namespace
 {
-	RGBLED *g_rgbled;
+RGBLED *g_rgbled;
 }
 
+void rgbled_usage();
 
 extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
 
 RGBLED::RGBLED(int bus, int rgbled) :
 	I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
-	_color(RGBLED_COLOR_OFF),
 	_mode(RGBLED_MODE_OFF),
-	_running(false),
-	_brightness(1.0f),
 	_r(0),
 	_g(0),
 	_b(0),
+	_brightness(1.0f),
+	_running(false),
 	_led_interval(0),
 	_counter(0)
 {
@@ -159,8 +155,9 @@ RGBLED::init()
 		return ret;
 	}
 
-	/* start off */
-	set(false, 0, 0, 0);
+	/* switch off LED on start */
+	send_led_enable(false);
+	send_led_rgb();
 
 	return OK;
 }
@@ -169,10 +166,10 @@ int
 RGBLED::probe()
 {
 	int ret;
-	bool on, not_powersave;
+	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, not_powersave, r, g, b);
+	ret = get(on, powersave, r, g, b);
 
 	return ret;
 }
@@ -181,15 +178,16 @@ int
 RGBLED::info()
 {
 	int ret;
-	bool on, not_powersave;
+	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, not_powersave, r, g, b);
+	ret = get(on, powersave, r, g, b);
 
 	if (ret == OK) {
 		/* we don't care about power-save mode */
 		log("state: %s", on ? "ON" : "OFF");
 		log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
 	} else {
 		warnx("failed to read led");
 	}
@@ -201,28 +199,30 @@ int
 RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	int ret = ENOTTY;
+
 	switch (cmd) {
 	case RGBLED_SET_RGB:
-		/* set the specified RGB values */
-		rgbled_rgbset_t rgbset;
-		memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset));
-		set_rgb(rgbset.red, rgbset.green, rgbset.blue);
-		set_mode(RGBLED_MODE_ON);
+		/* 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 blink speed */
+		/* 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);
+		set_pattern((rgbled_pattern_t *)arg);
 		return OK;
 
 	default:
@@ -247,33 +247,47 @@ void
 RGBLED::led()
 {
 	switch (_mode) {
-		case RGBLED_MODE_BLINK_SLOW:
-		case RGBLED_MODE_BLINK_NORMAL:
-		case RGBLED_MODE_BLINK_FAST:
-			if(_counter % 2 == 0)
-				set_on(true);
-			else
-				set_on(false);
-			break;
-		case RGBLED_MODE_BREATHE:
-			if (_counter >= 30)
-				_counter = 0;
-			if (_counter <= 15) {
-				set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
-			} else {
-				set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
-			}
-			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;
+	case RGBLED_MODE_BLINK_SLOW:
+	case RGBLED_MODE_BLINK_NORMAL:
+	case RGBLED_MODE_BLINK_FAST:
+		if (_counter >= 2)
+			_counter = 0;
 
-			set_color(_pattern.color[_counter]);
-			_led_interval = _pattern.duration[_counter];
-			break;
-		default:
-			break;
+		send_led_enable(_counter == 0);
+
+		break;
+
+	case RGBLED_MODE_BREATHE:
+
+		if (_counter >= 62)
+			_counter = 0;
+
+		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;
 	}
 
 	_counter++;
@@ -282,60 +296,106 @@ RGBLED::led()
 	work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
 }
 
+/**
+ * Parse color constant and set _r _g _b values
+ */
 void
-RGBLED::set_color(rgbled_color_t color) {
+RGBLED::set_color(rgbled_color_t color)
+{
+	switch (color) {
+	case RGBLED_COLOR_OFF:
+		_r = 0;
+		_g = 0;
+		_b = 0;
+		break;
 
-	_color = color;
+	case RGBLED_COLOR_RED:
+		_r = 255;
+		_g = 0;
+		_b = 0;
+		break;
 
-	switch (color) {
-		case RGBLED_COLOR_OFF:		// off
-			set_rgb(0,0,0);
-			break;
-		case RGBLED_COLOR_RED:		// red
-			set_rgb(255,0,0);
-			break;
-		case RGBLED_COLOR_YELLOW:	// yellow
-			set_rgb(255,70,0);
-			break;
-		case RGBLED_COLOR_PURPLE:	// purple
-			set_rgb(255,0,255);
-			break;
-		case RGBLED_COLOR_GREEN:	// green
-			set_rgb(0,255,0);
-			break;
-		case RGBLED_COLOR_BLUE:		// blue
-			set_rgb(0,0,255);
-			break;
-		case RGBLED_COLOR_WHITE:	// white
-			set_rgb(255,255,255);
-			break;
-		case RGBLED_COLOR_AMBER:	// amber
-			set_rgb(255,20,0);
-			break;
-		case RGBLED_COLOR_DIM_RED:	// red
-			set_rgb(90,0,0);
-			break;
-		case RGBLED_COLOR_DIM_YELLOW:	// yellow
-			set_rgb(80,30,0);
-			break;
-		case RGBLED_COLOR_DIM_PURPLE:	// purple
-			set_rgb(45,0,45);
-			break;
-		case RGBLED_COLOR_DIM_GREEN:	// green
-			set_rgb(0,90,0);
-			break;
-		case RGBLED_COLOR_DIM_BLUE:	// blue
-			set_rgb(0,0,90);
-			break;
-		case RGBLED_COLOR_DIM_WHITE:	// white
-			set_rgb(30,30,30);
-			break;
-		case RGBLED_COLOR_DIM_AMBER:	// amber
-			set_rgb(80,20,0);
-			break;
-		default:
-			warnx("color unknown");
-			break;
+	case RGBLED_COLOR_YELLOW:
+		_r = 255;
+		_g = 70;
+		_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 = 20;
+		_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:
+		warnx("color unknown");
+		break;
 	}
 }
 
@@ -343,51 +403,70 @@ void
 RGBLED::set_mode(rgbled_mode_t mode)
 {
 	_mode = mode;
+	bool should_run = false;
 
 	switch (mode) {
-		case RGBLED_MODE_OFF:
-			_should_run = false;
-			set_on(false);
-			break;
-		case RGBLED_MODE_ON:
-			_should_run = false;
-			set_on(true);
-			break;
-		case RGBLED_MODE_BLINK_SLOW:
-			_should_run = true;
-			_led_interval = 2000;
-			break;
-		case RGBLED_MODE_BLINK_NORMAL:
-			_should_run = true;
-			_led_interval = 500;
-			break;
-		case RGBLED_MODE_BLINK_FAST:
-			_should_run = true;
-			_led_interval = 100;
-			break;
-		case RGBLED_MODE_BREATHE:
-			_should_run = true;
-			set_on(true);
-			_counter = 0;
-			_led_interval = 1000/15;
-			break;
-		case RGBLED_MODE_PATTERN:
-			_should_run = true;
-			set_on(true);
-			_counter = 0;
-			break;
-		default:
-			warnx("mode unknown");
-			break;
+	case RGBLED_MODE_OFF:
+		send_led_enable(false);
+		break;
+
+	case RGBLED_MODE_ON:
+		_brightness = 1.0f;
+		send_led_rgb();
+		send_led_enable(true);
+		break;
+
+	case RGBLED_MODE_BLINK_SLOW:
+		should_run = true;
+		_counter = 0;
+		_led_interval = 2000;
+		_brightness = 1.0f;
+		send_led_rgb();
+		break;
+
+	case RGBLED_MODE_BLINK_NORMAL:
+		should_run = true;
+		_counter = 0;
+		_led_interval = 500;
+		_brightness = 1.0f;
+		send_led_rgb();
+		break;
+
+	case RGBLED_MODE_BLINK_FAST:
+		should_run = true;
+		_counter = 0;
+		_led_interval = 100;
+		_brightness = 1.0f;
+		send_led_rgb();
+		break;
+
+	case RGBLED_MODE_BREATHE:
+		should_run = true;
+		_counter = 0;
+		_led_interval = 25;
+		send_led_enable(true);
+		break;
+
+	case RGBLED_MODE_PATTERN:
+		should_run = true;
+		_counter = 0;
+		_brightness = 1.0f;
+		send_led_enable(true);
+		break;
+
+	default:
+		warnx("mode unknown");
+		break;
 	}
 
 	/* if it should run now, start the workq */
-	if (_should_run && !_running) {
+	if (should_run && !_running) {
 		_running = true;
 		work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
 	}
+
 	/* if it should stop, then cancel the workq */
-	if (!_should_run && _running) {
+	if (!should_run && _running) {
 		_running = false;
 		work_cancel(LPWORK, &_work);
 	}
@@ -397,66 +476,44 @@ void
 RGBLED::set_pattern(rgbled_pattern_t *pattern)
 {
 	memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
-
 	set_mode(RGBLED_MODE_PATTERN);
 }
 
-void
-RGBLED::set_brightness(float brightness) {
-
-	_brightness = brightness;
-	set_rgb(_r, _g, _b);
-}
-
-int
-RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
-{
-	uint8_t settings_byte = 0;
-
-	if (on)
-		settings_byte |= SETTING_ENABLE;
-/* powersave not used */
-//	if (not_powersave)
-		settings_byte |= SETTING_NOT_POWERSAVE;
-
-	const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
-
-	return transfer(msg, sizeof(msg), nullptr, 0);
-}
-
+/**
+ * Sent ENABLE flag to LED driver
+ */
 int
-RGBLED::set_on(bool on)
+RGBLED::send_led_enable(bool enable)
 {
 	uint8_t settings_byte = 0;
 
-	if (on)
+	if (enable)
 		settings_byte |= SETTING_ENABLE;
 
-/* powersave not used */
-//	if (not_powersave)
-		settings_byte |= SETTING_NOT_POWERSAVE;
+	settings_byte |= SETTING_NOT_POWERSAVE;
 
 	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
 
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
 int
-RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+RGBLED::send_led_rgb()
 {
-	/* save the RGB values in case we want to change the brightness later */
-	_r = r;
-	_g = g;
-	_b = b;
-
-	const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)};
-
+	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
+	const uint8_t msg[6] = {
+		SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
+		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
+		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+	};
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
-
 int
-RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 {
 	uint8_t result[2];
 	int ret;
@@ -465,24 +522,23 @@ RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 
 	if (ret == OK) {
 		on = result[0] & SETTING_ENABLE;
-		not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+		powersave = !(result[0] & SETTING_NOT_POWERSAVE);
 		/* XXX check, looks wrong */
-		r = (result[0] & 0x0f)*255/15;
-		g = (result[1] & 0xf0)*255/15;
-		b = (result[1] & 0x0f)*255/15;
+		r = (result[0] & 0x0f) << 4;
+		g = (result[1] & 0xf0);
+		b = (result[1] & 0x0f) << 4;
 	}
 
 	return ret;
 }
 
-void rgbled_usage();
-
-
-void rgbled_usage() {
-	warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
+void
+rgbled_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
 	warnx("options:");
 	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
-	errx(0, "    -a addr (0x%x)", ADDR);
+	warnx("    -a addr (0x%x)", ADDR);
 }
 
 int
@@ -492,17 +548,21 @@ rgbled_main(int argc, char *argv[])
 	int rgbledadr = ADDR; /* 7bit */
 
 	int ch;
+
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
+	while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
 		switch (ch) {
 		case 'a':
 			rgbledadr = strtol(optarg, NULL, 0);
 			break;
+
 		case 'b':
 			i2cdevice = strtol(optarg, NULL, 0);
 			break;
+
 		default:
 			rgbled_usage();
+			exit(0);
 		}
 	}
 
@@ -519,17 +579,21 @@ rgbled_main(int argc, char *argv[])
 			// try the external bus first
 			i2cdevice = PX4_I2C_BUS_EXPANSION;
 			g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
 			if (g_rgbled != nullptr && OK != g_rgbled->init()) {
 				delete g_rgbled;
 				g_rgbled = nullptr;
 			}
+
 			if (g_rgbled == nullptr) {
 				// fall back to default bus
 				i2cdevice = PX4_I2C_BUS_LED;
 			}
 		}
+
 		if (g_rgbled == nullptr) {
 			g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
 			if (g_rgbled == nullptr)
 				errx(1, "new failed");
 
@@ -545,21 +609,24 @@ rgbled_main(int argc, char *argv[])
 
 	/* need the driver past this point */
 	if (g_rgbled == nullptr) {
-	    warnx("not started");
-	    rgbled_usage();
-	    exit(0);
+		warnx("not started");
+		rgbled_usage();
+		exit(0);
 	}
 
 	if (!strcmp(verb, "test")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
 
-		rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF},
-					     {200,              200,                200,               400             } };
+		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 }
+		};
 
 		ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
 
 		close(fd);
 		exit(ret);
@@ -570,33 +637,39 @@ rgbled_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
-		/* although technically it doesn't stop, this is the excepted syntax */
+	if (!strcmp(verb, "off")) {
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
+
 		ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
 		close(fd);
 		exit(ret);
 	}
 
 	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+		}
+
 		fd = open(RGBLED_DEVICE_PATH, 0);
+
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
-		if (argc < 5) {
-			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
-		}
+
 		rgbled_rgbset_t v;
 		v.red   = strtol(argv[2], NULL, 0);
 		v.green = strtol(argv[3], NULL, 0);
 		v.blue  = strtol(argv[4], NULL, 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);
 	}
 
 	rgbled_usage();
+	exit(0);
 }