navio: switch to new led uorb interface

src/drivers/boards/rpi/board_config.h

@@ -45,3 +45,5 @@
 #define BOARD_BATTERY1_A_PER_V (15.391030303f)
 #define BOARD_HAS_NO_RESET
 #define BOARD_HAS_NO_BOOTLOADER
+
+#define BOARD_MAX_LEDS 1 // Number external of LED's this board has

src/drivers/navio_rgbled/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 		drivers__navio_gpio
+		lib__led
 	)
 
 #add_subdirectory(test)

src/drivers/navio_rgbled/navio_rgbled.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 #include <px4_posix.h>
-#include <drivers/drv_rgbled.h>
+#include <drivers/drv_led.h>
 
 #include "navio_rgbled.h"
 
@@ -41,9 +41,31 @@
 #define GPIO_LED_G      (GPIO_PIN27)
 #define GPIO_LED_B      (GPIO_PIN6)
 
+#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled"
+
+// inverted
+#define LED_ON  0
+#define LED_OFF 1
+
 using namespace DriverFramework;
 
 
+RGBLED::RGBLED(const char *name)
+	: DevObj(name,
+		 RGBLED0_DEVICE_PATH,
+		 RGBLED_BASE_DEVICE_PATH,
+		 DeviceBusType_UNKNOWN,
+		 0)
+{
+};
+
+RGBLED::~RGBLED()
+{
+	if (_led_controller.led_control_subscription() >= 0) {
+		orb_unsubscribe(_led_controller.led_control_subscription());
+	}
+};
+
 int RGBLED::start()
 {
 	int res;
@@ -61,6 +83,9 @@ int RGBLED::start()
 	_gpio.configgpio(GPIO_LED_CNF | GPIO_LED_G);
 	_gpio.configgpio(GPIO_LED_CNF | GPIO_LED_B);
 
+	// update at fixed interval
+	DevObj::setSampleInterval(_led_controller.maximum_update_interval());
+
 	return 0;
 }
 
@@ -81,100 +106,66 @@ int RGBLED::stop()
 	return 0;
 }
 
-int RGBLED::devIOCTL(unsigned long request, unsigned long arg)
+void RGBLED::_measure()
 {
-	int ret = ENOTTY;
-	rgbled_rgbset_t *rgb;
-
-	switch (request) {
-	case RGBLED_SET_RGB:
-		ret = 0;
-		rgb = (rgbled_rgbset_t *)arg;
-		_rgb.red = (rgb->red != 0) ? LED_ON : LED_OFF;
-		_rgb.green = (rgb->green != 0) ? LED_ON : LED_OFF;
-		_rgb.blue = (rgb->blue != 0) ? LED_ON : LED_OFF;
-		_gpio.gpiowrite(GPIO_LED_R, _rgb.red);
-		_gpio.gpiowrite(GPIO_LED_G, _rgb.green);
-		_gpio.gpiowrite(GPIO_LED_B, _rgb.blue);
-		break;
-
-	case RGBLED_SET_COLOR:
-		if (arg > _max_color) {
-			ret = ENOTSUP;
-
-		} else {
-			_rgb = _rgbsets[arg];
-			_gpio.gpiowrite(GPIO_LED_R, _rgb.red);
-			_gpio.gpiowrite(GPIO_LED_G, _rgb.green);
-			_gpio.gpiowrite(GPIO_LED_B, _rgb.blue);
-			ret = 0;
-		}
+	if (!_led_controller.is_init()) {
+		int led_control_sub = orb_subscribe(ORB_ID(led_control));
+		_led_controller.init(led_control_sub);
+	}
 
-		break;
+	LedControlData led_control_data;
 
-	case RGBLED_SET_MODE:
-		ret = 0;
+	if (_led_controller.update(led_control_data) == 1) {
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			_gpio.gpiowrite(GPIO_LED_R, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
+			break;
 
-		switch (arg) {
-		case RGBLED_MODE_ON:
-			DevObj::setSampleInterval(0);
+		case led_control_s::COLOR_GREEN:
+			_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_G, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
 			break;
 
-		case RGBLED_MODE_BLINK_SLOW:
-			DevObj::setSampleInterval(2000 * 1000);
+		case led_control_s::COLOR_BLUE:
+			_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_B, LED_ON);
 			break;
 
-		case RGBLED_MODE_BLINK_NORMAL:
-			DevObj::setSampleInterval(500 * 1000);
+		case led_control_s::COLOR_AMBER: //make it the same as yellow
+		case led_control_s::COLOR_YELLOW:
+			_gpio.gpiowrite(GPIO_LED_R, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_G, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
 			break;
 
-		case RGBLED_MODE_BLINK_FAST:
-			DevObj::setSampleInterval(100 * 1000);
+		case led_control_s::COLOR_PURPLE:
+			_gpio.gpiowrite(GPIO_LED_R, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_B, LED_ON);
 			break;
 
-		case RGBLED_MODE_BREATHE:
-			DevObj::setSampleInterval(1500 * 1000);
+		case led_control_s::COLOR_CYAN:
+			_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_G, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_B, LED_ON);
 			break;
 
-		default:
-			ret = ENOTSUP;
-		}
+		case led_control_s::COLOR_WHITE:
+			_gpio.gpiowrite(GPIO_LED_R, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_G, LED_ON);
+			_gpio.gpiowrite(GPIO_LED_B, LED_ON);
+			break;
 
-		if (!m_work_handle.isValid()) {
-			// this can fail
-			DevObj::start();
+		default: // led_control_s::COLOR_OFF
+			_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
+			_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
+			break;
 		}
-
-		break;
-
-	case RGBLED_PLAY_SCRIPT_NAMED:
-	case RGBLED_PLAY_SCRIPT:
-	case RGBLED_SET_USER_SCRIPT:
-	case RGBLED_SET_PATTERN:
-		ret = ENOTSUP;
-		break;
-
-	default:
-		ret = DevObj::devIOCTL(request, arg);
-		break;
-	}
-
-	return ret;
-}
-
-void RGBLED::_measure()
-{
-	if (_turn) {
-		_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
-		_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
-		_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
-		_turn = false;
-
-	} else {
-		_gpio.gpiowrite(GPIO_LED_R, _rgb.red);
-		_gpio.gpiowrite(GPIO_LED_G, _rgb.green);
-		_gpio.gpiowrite(GPIO_LED_B, _rgb.blue);
-		_turn = true;
 	}
 }
 

src/drivers/navio_rgbled/navio_rgbled.h

@@ -36,12 +36,7 @@
 
 #include "navio_gpio.h"
 
-#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled"
-#define RGBLED_DEVICE_PATH  "/dev/rgbled0"
-
-// inverted
-#define LED_ON  0
-#define LED_OFF 1
+#include <lib/led/led.h>
 
 
 using namespace navio_gpio;
@@ -49,39 +44,16 @@ using namespace navio_gpio;
 class RGBLED : public DevObj
 {
 public:
-	RGBLED(const char *name) :
-		DevObj(name,
-		       RGBLED_DEVICE_PATH,
-		       RGBLED_BASE_DEVICE_PATH,
-		       DeviceBusType_UNKNOWN,
-		       0),
-		_rgbsets{
-		{LED_OFF, LED_OFF, LED_OFF}, /* OFF */
-		{LED_ON,  LED_OFF, LED_OFF}, /* red */
-		{LED_ON,  LED_ON,  LED_OFF}, /* yellow */
-		{LED_ON,  LED_OFF, LED_ON},  /* purple */
-		{LED_OFF, LED_ON,  LED_OFF}, /* green */
-		{LED_OFF, LED_OFF, LED_ON},  /* blue */
-		{LED_ON,  LED_ON,  LED_ON},  /* white */
-	},
-	_max_color(7),
-		   _rgb{LED_OFF, LED_OFF, LED_OFF},
-		   _turn(true)
-	{ };
-	virtual ~RGBLED()
-	{ };
+	RGBLED(const char *name);
+	virtual ~RGBLED();
 
 	int start();
 	int stop();
-	int devIOCTL(unsigned long request, unsigned long arg);
 
 protected:
 	void _measure();
 
 private:
+	LedController _led_controller;
 	Gpio _gpio;
-	const rgbled_rgbset_t _rgbsets[7];
-	const int _max_color;
-	rgbled_rgbset_t _rgb;
-	bool _turn;
 };