led drivers and controller move to uORB::Subscription

boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp

@@ -55,13 +55,6 @@ RGBLED::RGBLED(const char *name)
 {
 };
 
-RGBLED::~RGBLED()
-{
-	if (_led_controller.led_control_subscription() >= 0) {
-		orb_unsubscribe(_led_controller.led_control_subscription());
-	}
-};
-
 int RGBLED::start()
 {
 	int res = DevObj::init();
@@ -133,15 +126,14 @@ cleanup:
 	return res;
 }
 
-int RGBLED::stop()
+int
+RGBLED::stop()
 {
-	int res;
-
 	_gpioR.unexportPin();
 	_gpioG.unexportPin();
 	_gpioB.unexportPin();
 
-	res = DevObj::stop();
+	int res = DevObj::stop();
 
 	if (res < 0) {
 		DF_LOG_ERR("could not stop DevObj");
@@ -152,13 +144,9 @@ int RGBLED::stop()
 	return 0;
 }
 
-void RGBLED::_measure()
+void
+RGBLED::_measure()
 {
-	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) {

boards/emlid/navio2/navio_rgbled/navio_rgbled.h

@@ -41,7 +41,7 @@ class RGBLED : public DriverFramework::DevObj
 {
 public:
 	RGBLED(const char *name);
-	virtual ~RGBLED();
+	virtual ~RGBLED() = default;
 
 	int start();
 	int stop();

src/drivers/lights/rgbled/rgbled.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,10 +46,8 @@
 #include <lib/led/led.h>
 #include <px4_getopt.h>
 #include <px4_work_queue/ScheduledWorkItem.hpp>
-#include "uORB/topics/parameter_update.h"
+#include <uORB/Subscription.hpp>
-
+#include <uORB/topics/parameter_update.h>
-#define RGBLED_ONTIME 120
-#define RGBLED_OFFTIME 120
 
 #define ADDR			0x55	/**< I2C adress of TCA62724FMG */
 #define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
@@ -61,29 +59,28 @@
 #define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
 #define SETTING_ENABLE   	0x02	/**< on */
 
-
 class RGBLED : public device::I2C, public px4::ScheduledWorkItem
 {
 public:
 	RGBLED(int bus, int rgbled);
 	virtual ~RGBLED();
 
-
 	virtual int		init();
 	virtual int		probe();
-	int		status();
+	int			status();
 
 private:
-	float			_brightness;
-	float			_max_brightness;
 
-	uint8_t			_r;
+	float			_brightness{1.0f};
-	uint8_t			_g;
+	float			_max_brightness{1.0f};
-	uint8_t			_b;
+
-	volatile bool		_running;
+	uint8_t			_r{0};
-	volatile bool		_should_run;
+	uint8_t			_g{0};
-	bool			_leds_enabled;
+	uint8_t			_b{0};
-	int			_param_sub;
+	volatile bool		_running{false};
+	volatile bool		_should_run{true};
+	bool			_leds_enabled{true};
+	uORB::Subscription	_param_sub{ORB_ID(parameter_update)};
 
 	LedController		_led_controller;
 
@@ -92,7 +89,7 @@ private:
 	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);
-	void		update_params();
+	void			update_params();
 };
 
 /* for now, we only support one RGBLED */
@@ -107,16 +104,7 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
 
 RGBLED::RGBLED(int bus, int rgbled) :
 	I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000),
-	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
-	_brightness(1.0f),
-	_max_brightness(1.0f),
-	_r(0),
-	_g(0),
-	_b(0),
-	_running(false),
-	_should_run(true),
-	_leds_enabled(true),
-	_param_sub(-1)
 {
 }
 
@@ -133,8 +121,7 @@ RGBLED::~RGBLED()
 int
 RGBLED::init()
 {
-	int ret;
+	int ret = I2C::init();
-	ret = I2C::init();
 
 	if (ret != OK) {
 		return ret;
@@ -187,11 +174,10 @@ RGBLED::probe()
 int
 RGBLED::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 */
@@ -212,40 +198,19 @@ void
 RGBLED::Run()
 {
 	if (!_should_run) {
-		if (_param_sub >= 0) {
-			orb_unsubscribe(_param_sub);
-		}
-
-		int led_control_sub = _led_controller.led_control_subscription();
-
-		if (led_control_sub >= 0) {
-			orb_unsubscribe(led_control_sub);
-		}
-
 		_running = false;
 		return;
 	}
 
-	if (_param_sub < 0) {
+	if (_param_sub.updated()) {
-		_param_sub = orb_subscribe(ORB_ID(parameter_update));
+		// clear update
-	}
+		parameter_update_s pupdate;
+		_param_sub.copy(&pupdate);
 
-	if (!_led_controller.is_init()) {
+		update_params();
-		int led_control_sub = orb_subscribe(ORB_ID(led_control));
-		_led_controller.init(led_control_sub);
-	}
 
-	if (_param_sub >= 0) {
+		// Immediately update to change brightness
-		bool updated = false;
+		send_led_rgb();
-		orb_check(_param_sub, &updated);
-
-		if (updated) {
-			parameter_update_s pupdate;
-			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
-			update_params();
-			// Immediately update to change brightness
-			send_led_rgb();
-		}
 	}
 
 	LedControlData led_control_data;

src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,7 +45,8 @@
 #include <lib/led/led.h>
 #include <px4_getopt.h>
 #include <px4_work_queue/ScheduledWorkItem.hpp>
-#include "uORB/topics/parameter_update.h"
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/parameter_update.h>
 
 
 #define ADDR			0x39	/**< I2C adress of NCP5623C */
@@ -65,21 +66,21 @@ public:
 	RGBLED_NPC5623C(int bus, int rgbled);
 	virtual ~RGBLED_NPC5623C();
 
-
 	virtual int		init();
 	virtual int		probe();
+
 private:
 
-	float			_brightness;
+	float			_brightness{1.0f};
-	float			_max_brightness;
+	float			_max_brightness{1.0f};
 
-	uint8_t			_r;
+	uint8_t			_r{0};
-	uint8_t			_g;
+	uint8_t			_g{0};
-	uint8_t			_b;
+	uint8_t			_b{0};
-	volatile bool		_running;
+	volatile bool		_running{false};
-	volatile bool		_should_run;
+	volatile bool		_should_run{true};
-	bool			_leds_enabled;
+	bool			_leds_enabled{true};
-	int			_param_sub;
+	uORB::Subscription	_param_sub{ORB_ID(parameter_update)};
 
 	LedController		_led_controller;
 
@@ -103,16 +104,7 @@ extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]);
 
 RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) :
 	I2C("rgbled1", RGBLED1_DEVICE_PATH, bus, rgbled, 100000),
-	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
-	_brightness(1.0f),
-	_max_brightness(1.0f),
-	_r(0),
-	_g(0),
-	_b(0),
-	_running(false),
-	_should_run(true),
-	_leds_enabled(true),
-	_param_sub(-1)
 {
 }
 
@@ -122,7 +114,7 @@ RGBLED_NPC5623C::~RGBLED_NPC5623C()
 	int counter = 0;
 
 	while (_running && ++counter < 10) {
-		usleep(100000);
+		px4_usleep(100000);
 	}
 }
 
@@ -140,8 +132,7 @@ RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
 int
 RGBLED_NPC5623C::init()
 {
-	int ret;
+	int ret = I2C::init();
-	ret = I2C::init();
 
 	if (ret != OK) {
 		return ret;
@@ -171,40 +162,19 @@ void
 RGBLED_NPC5623C::Run()
 {
 	if (!_should_run) {
-		if (_param_sub >= 0) {
-			orb_unsubscribe(_param_sub);
-		}
-
-		int led_control_sub = _led_controller.led_control_subscription();
-
-		if (led_control_sub >= 0) {
-			orb_unsubscribe(led_control_sub);
-		}
-
 		_running = false;
 		return;
 	}
 
-	if (_param_sub < 0) {
+	if (_param_sub.updated()) {
-		_param_sub = orb_subscribe(ORB_ID(parameter_update));
+		// clear update
-	}
+		parameter_update_s pupdate;
+		_param_sub.copy(&pupdate);
 
-	if (!_led_controller.is_init()) {
+		update_params();
-		int led_control_sub = orb_subscribe(ORB_ID(led_control));
-		_led_controller.init(led_control_sub);
-	}
 
-	if (_param_sub >= 0) {
+		// Immediately update to change brightness
-		bool updated = false;
+		send_led_rgb();
-		orb_check(_param_sub, &updated);
-
-		if (updated) {
-			parameter_update_s pupdate;
-			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
-			update_params();
-			// Immediately update to change brightness
-			send_led_rgb();
-		}
 	}
 
 	LedControlData led_control_data;

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			_r{0};
-	uint8_t			_g;
+	uint8_t			_g{0};
-	uint8_t			_b;
+	uint8_t			_b{0};
 
-	volatile bool		_running;
+	volatile bool		_running{false};
-	volatile bool		_should_run;
+	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),
+	ScheduledWorkItem(px4::wq_configurations::lp_default)
-	_r(0),
-	_g(0),
-	_b(0),
-	_running(false),
-	_should_run(true)
 {
 }
 
@@ -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;
 }

src/lib/led/led.cpp

@@ -38,24 +38,13 @@
 
 #include "led.h"
 
-int LedController::init(int led_control_sub)
-{
-	_led_control_sub = led_control_sub;
-	_last_update_call = hrt_absolute_time();
-	return 0;
-}
-
 int LedController::update(LedControlData &control_data)
 {
-	bool updated = false;
+	while (_led_control_sub.updated() || _force_update) {
-
-	orb_check(_led_control_sub, &updated);
-
-	while (updated || _force_update) {
 		// handle new state
 		led_control_s led_control;
 
-		if (orb_copy(ORB_ID(led_control), _led_control_sub, &led_control) == 0) {
+		if (_led_control_sub.copy(&led_control)) {
 
 			// don't apply the new state just yet to avoid interrupting an ongoing blinking state
 			for (int i = 0; i < BOARD_MAX_LEDS; ++i) {
@@ -80,14 +69,18 @@ int LedController::update(LedControlData &control_data)
 		}
 
 		_force_update = false;
-
-		orb_check(_led_control_sub, &updated);
 	}
 
 	bool had_changes = false; // did one of the outputs change?
 
 	// handle state updates
 	hrt_abstime now = hrt_absolute_time();
+
+	if (_last_update_call == 0) {
+		_last_update_call = now;
+		return 0;
+	}
+
 	uint16_t blink_delta_t = (uint16_t)((now - _last_update_call) / 100); // Note: this is in 0.1ms
 	constexpr uint16_t breathe_duration = BREATHE_INTERVAL * BREATHE_STEPS / 100;
 

src/lib/led/led.h

@@ -44,6 +44,8 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_led.h>
 
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/led_control.h>
 
 struct LedControlDataSingle {
 	uint8_t color; ///< one of led_control_s::COLOR_*
@@ -64,18 +66,6 @@ public:
 	LedController() = default;
 	~LedController() = default;
 
-	/**
-	 * initialize. Call this once before using the object
-	 * @param led_control_sub uorb subscription for led_control
-	 * @return 0 on success, <0 on error otherwise
-	 */
-	int init(int led_control_sub);
-
-	/**
-	 * check if already initialized
-	 */
-	bool is_init() const { return _led_control_sub >= 0; }
-
 	/**
 	 * get maxium time between two consecutive calls to update() in us.
 	 */
@@ -92,18 +82,16 @@ public:
 	 */
 	int update(LedControlData &control_data);
 
-	static const int BREATHE_INTERVAL = 25 * 1000; /**< single step when in breathe mode */
+	static constexpr int BREATHE_INTERVAL = 25 * 1000; /**< single step when in breathe mode */
-	static const int BREATHE_STEPS = 64; /**< number of steps in breathe mode for a full on-off cycle */
+	static constexpr int BREATHE_STEPS = 64; /**< number of steps in breathe mode for a full on-off cycle */
 
-	static const int BLINK_FAST_DURATION = 100 * 1000; /**< duration of half a blinking cycle
+	static constexpr int BLINK_FAST_DURATION = 100 * 1000; /**< duration of half a blinking cycle
 									(on-to-off and off-to-on) in us */
-	static const int BLINK_NORMAL_DURATION = 500 * 1000; /**< duration of half a blinking cycle
+	static constexpr int BLINK_NORMAL_DURATION = 500 * 1000; /**< duration of half a blinking cycle
 									(on-to-off and off-to-on) in us */
-	static const int BLINK_SLOW_DURATION = 2000 * 1000; /**< duration of half a blinking cycle
+	static constexpr int BLINK_SLOW_DURATION = 2000 * 1000; /**< duration of half a blinking cycle
 									(on-to-off and off-to-on) in us */
 
-	int led_control_subscription() const { return _led_control_sub; }
-
 private:
 
 	/** set control_data based on current Led states */
@@ -186,8 +174,8 @@ private:
 
 	PerLedData _states[BOARD_MAX_LEDS]; ///< keep current LED states
 
-	int _led_control_sub = -1; ///< uorb subscription
+	uORB::Subscription _led_control_sub{ORB_ID(led_control)}; ///< uorb subscription
-	hrt_abstime _last_update_call;
+	hrt_abstime _last_update_call{0};
-	bool _force_update = true; ///< force an orb_copy in the beginning
+	bool _force_update{true}; ///< force an orb_copy in the beginning
-	bool _breathe_enabled = false; ///< true if at least one of the led's is currently in breathe mode
+	bool _breathe_enabled{false}; ///< true if at least one of the led's is currently in breathe mode
 };