diff --git a/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp b/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp index 5241a40097..0094038838 100644 --- a/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp +++ b/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) { diff --git a/boards/emlid/navio2/navio_rgbled/navio_rgbled.h b/boards/emlid/navio2/navio_rgbled/navio_rgbled.h index aa25935dcd..55fc3f8fa8 100644 --- a/boards/emlid/navio2/navio_rgbled/navio_rgbled.h +++ b/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(); diff --git a/src/drivers/lights/rgbled/rgbled.cpp b/src/drivers/lights/rgbled/rgbled.cpp index f5b8e9a663..073eed08d7 100644 --- a/src/drivers/lights/rgbled/rgbled.cpp +++ b/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 #include #include -#include "uORB/topics/parameter_update.h" - -#define RGBLED_ONTIME 120 -#define RGBLED_OFFTIME 120 +#include +#include #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; - uint8_t _g; - uint8_t _b; - volatile bool _running; - volatile bool _should_run; - bool _leds_enabled; - int _param_sub; + float _brightness{1.0f}; + float _max_brightness{1.0f}; + + uint8_t _r{0}; + uint8_t _g{0}; + uint8_t _b{0}; + 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())), - _brightness(1.0f), - _max_brightness(1.0f), - _r(0), - _g(0), - _b(0), - _running(false), - _should_run(true), - _leds_enabled(true), - _param_sub(-1) + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) { } @@ -133,8 +121,7 @@ RGBLED::~RGBLED() int RGBLED::init() { - int ret; - ret = I2C::init(); + int 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) { - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - } + if (_param_sub.updated()) { + // clear update + parameter_update_s pupdate; + _param_sub.copy(&pupdate); - if (!_led_controller.is_init()) { - int led_control_sub = orb_subscribe(ORB_ID(led_control)); - _led_controller.init(led_control_sub); - } + update_params(); - if (_param_sub >= 0) { - bool updated = false; - 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(); - } + // Immediately update to change brightness + send_led_rgb(); } LedControlData led_control_data; diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp index 2a994348ca..3dd2726ce6 100755 --- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp +++ b/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 #include #include -#include "uORB/topics/parameter_update.h" +#include +#include #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 _max_brightness; + float _brightness{1.0f}; + float _max_brightness{1.0f}; - uint8_t _r; - uint8_t _g; - uint8_t _b; - volatile bool _running; - volatile bool _should_run; - bool _leds_enabled; - int _param_sub; + uint8_t _r{0}; + uint8_t _g{0}; + uint8_t _b{0}; + volatile bool _running{false}; + volatile bool _should_run{true}; + bool _leds_enabled{true}; + 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())), - _brightness(1.0f), - _max_brightness(1.0f), - _r(0), - _g(0), - _b(0), - _running(false), - _should_run(true), - _leds_enabled(true), - _param_sub(-1) + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) { } @@ -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; - ret = I2C::init(); + int 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) { - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - } + if (_param_sub.updated()) { + // clear update + parameter_update_s pupdate; + _param_sub.copy(&pupdate); - if (!_led_controller.is_init()) { - int led_control_sub = orb_subscribe(ORB_ID(led_control)); - _led_controller.init(led_control_sub); - } + update_params(); - if (_param_sub >= 0) { - bool updated = false; - 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(); - } + // Immediately update to change brightness + send_led_rgb(); } LedControlData led_control_data; diff --git a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp index c8bb872aa8..f50490bd35 100644 --- a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp @@ -45,10 +45,6 @@ #include #include #include -#include - -#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; } diff --git a/src/lib/led/led.cpp b/src/lib/led/led.cpp index 120a8eb4f9..e4202e6078 100644 --- a/src/lib/led/led.cpp +++ b/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; - - orb_check(_led_control_sub, &updated); - - while (updated || _force_update) { + while (_led_control_sub.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; diff --git a/src/lib/led/led.h b/src/lib/led/led.h index e6cc84482b..93fa9166e4 100644 --- a/src/lib/led/led.h +++ b/src/lib/led/led.h @@ -44,6 +44,8 @@ #include #include +#include +#include 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 const int BREATHE_STEPS = 64; /**< number of steps in breathe mode for a full on-off cycle */ + static constexpr int BREATHE_INTERVAL = 25 * 1000; /**< single step when in breathe mode */ + 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 - hrt_abstime _last_update_call; - 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 + uORB::Subscription _led_control_sub{ORB_ID(led_control)}; ///< uorb subscription + hrt_abstime _last_update_call{0}; + 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 };