led drivers and controller move to uORB::Subscription

src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp

@@ -45,10 +45,6 @@
 #include <lib/led/led.h>
 #include <px4_getopt.h>
 #include <px4_work_queue/ScheduledWorkItem.hpp>
-#include <systemlib/err.h>
-
-#define RGBLED_ONTIME 120
-#define RGBLED_OFFTIME 120
 
 class RGBLED_PWM : public device::CDev, public px4::ScheduledWorkItem
 {
@@ -56,19 +52,18 @@ public:
 	RGBLED_PWM();
 	virtual ~RGBLED_PWM();
 
-
 	virtual int		init();
 	virtual int		probe();
-	int		status();
+	int			status();
 
 private:
 
-	uint8_t			_r;
-	uint8_t			_g;
-	uint8_t			_b;
+	uint8_t			_r{0};
+	uint8_t			_g{0};
+	uint8_t			_b{0};
 
-	volatile bool		_running;
-	volatile bool		_should_run;
+	volatile bool		_running{false};
+	volatile bool		_should_run{true};
 
 	LedController		_led_controller;
 
@@ -93,12 +88,7 @@ RGBLED_PWM *g_rgbled = nullptr;
 
 RGBLED_PWM::RGBLED_PWM() :
 	CDev("rgbled_pwm", RGBLED_PWM0_DEVICE_PATH),
-	ScheduledWorkItem(px4::wq_configurations::lp_default),
-	_r(0),
-	_g(0),
-	_b(0),
-	_running(false),
-	_should_run(true)
+	ScheduledWorkItem(px4::wq_configurations::lp_default)
 {
 }
 
@@ -108,7 +98,7 @@ RGBLED_PWM::~RGBLED_PWM()
 	int counter = 0;
 
 	while (_running && ++counter < 10) {
-		usleep(100000);
+		px4_usleep(100000);
 	}
 }
 
@@ -128,14 +118,19 @@ RGBLED_PWM::init()
 	return OK;
 }
 
+int
+RGBLED_PWM::probe()
+{
+	return PX4_OK;
+}
+
 int
 RGBLED_PWM::status()
 {
-	int ret;
 	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, powersave, r, g, b);
+	int ret = get(on, powersave, r, g, b);
 
 	if (ret == OK) {
 		/* we don't care about power-save mode */
@@ -148,11 +143,6 @@ RGBLED_PWM::status()
 
 	return ret;
 }
-int
-RGBLED_PWM::probe()
-{
-	return (OK);
-}
 
 /**
  * Main loop function
@@ -161,21 +151,10 @@ void
 RGBLED_PWM::Run()
 {
 	if (!_should_run) {
-		int led_control_sub = _led_controller.led_control_subscription();
-
-		if (led_control_sub >= 0) {
-			orb_unsubscribe(led_control_sub);
-		}
-
 		_running = false;
 		return;
 	}
 
-	if (!_led_controller.is_init()) {
-		int led_control_sub = orb_subscribe(ORB_ID(led_control));
-		_led_controller.init(led_control_sub);
-	}
-
 	LedControlData led_control_data;
 
 	if (_led_controller.update(led_control_data) == 1) {
@@ -280,58 +259,60 @@ rgbled_pwm_main(int argc, char *argv[])
 
 		default:
 			rgbled_usage();
-			exit(0);
+			return 1;
 		}
 	}
 
 	if (myoptind >= argc) {
 		rgbled_usage();
-		exit(0);
+		return 1;
 	}
 
 	const char *verb = argv[myoptind];
 
-
 	if (!strcmp(verb, "start")) {
 		if (g_rgbled != nullptr) {
-			errx(1, "already started");
+			PX4_WARN("already started");
+			return 1;
 		}
 
 		if (g_rgbled == nullptr) {
 			g_rgbled = new RGBLED_PWM();
 
 			if (g_rgbled == nullptr) {
-				errx(1, "new failed");
+				PX4_WARN("alloc failed");
+				return 1;
 			}
 
 			if (OK != g_rgbled->init()) {
 				delete g_rgbled;
 				g_rgbled = nullptr;
-				errx(1, "init failed");
+				PX4_ERR("init failed");
+				return 1;
 			}
 		}
 
-		exit(0);
+		return 0;
 	}
 
 	/* need the driver past this point */
 	if (g_rgbled == nullptr) {
 		PX4_WARN("not started");
 		rgbled_usage();
-		exit(1);
+		return 1;
 	}
 
 	if (!strcmp(verb, "status")) {
 		g_rgbled->status();
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(verb, "stop")) {
 		delete g_rgbled;
 		g_rgbled = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	rgbled_usage();
-	exit(0);
+	return 1;
 }