zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

led drivers and controller move to uORB::Subscription

sbg
Daniel Agar 6 years ago committed by GitHub
parent
41dee3875e
commit
5ae408382b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 101 additions and 216 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 22
      boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp
  2. 2
      boards/emlid/navio2/navio_rgbled/navio_rgbled.h
  3. 85
      src/drivers/lights/rgbled/rgbled.cpp
  4. 76
      src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
  5. 75
      src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp
  6. 23
      src/lib/led/led.cpp
  7. 34
      src/lib/led/led.h

22
boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp
Unescape View File

@ -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 RGBLED::start()
{ {
int res = DevObj::init(); int res = DevObj::init();
@ -133,15 +126,14 @@ cleanup:
return res; return res;
} }
int RGBLED::stop() int
RGBLED::stop()
{ {
int res;
_gpioR.unexportPin(); _gpioR.unexportPin();
_gpioG.unexportPin(); _gpioG.unexportPin();
_gpioB.unexportPin(); _gpioB.unexportPin();
res = DevObj::stop(); int res = DevObj::stop();
if (res < 0) { if (res < 0) {
DF_LOG_ERR("could not stop DevObj"); DF_LOG_ERR("could not stop DevObj");
@ -152,13 +144,9 @@ int RGBLED::stop()
return 0; 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; LedControlData led_control_data;
if (_led_controller.update(led_control_data) == 1) { if (_led_controller.update(led_control_data) == 1) {

2
boards/emlid/navio2/navio_rgbled/navio_rgbled.h
Unescape View File

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

85
src/drivers/lights/rgbled/rgbled.cpp
Unescape View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -46,10 +46,8 @@
#include <lib/led/led.h> #include <lib/led/led.h>
#include <px4_getopt.h> #include <px4_getopt.h>
#include <px4_work_queue/ScheduledWorkItem.hpp> #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 ADDR 0x55 /**< I2C adress of TCA62724FMG */
#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ #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_NOT_POWERSAVE 0x01 /**< power-save mode not off */
#define SETTING_ENABLE 0x02 /**< on */ #define SETTING_ENABLE 0x02 /**< on */
class RGBLED : public device::I2C, public px4::ScheduledWorkItem class RGBLED : public device::I2C, public px4::ScheduledWorkItem
{ {
public: public:
RGBLED(int bus, int rgbled); RGBLED(int bus, int rgbled);
virtual ~RGBLED(); virtual ~RGBLED();
virtual int init(); virtual int init();
virtual int probe(); virtual int probe();
int status(); int status();
private: 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; LedController _led_controller;
@ -92,7 +89,7 @@ private:
int send_led_enable(bool enable); int send_led_enable(bool enable);
int send_led_rgb(); int send_led_rgb();
int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); 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 */ /* 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) : RGBLED::RGBLED(int bus, int rgbled) :
I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000), 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 int
RGBLED::init() RGBLED::init()
{ {
int ret; int ret = I2C::init();
ret = I2C::init();
if (ret != OK) { if (ret != OK) {
return ret; return ret;
@ -187,11 +174,10 @@ RGBLED::probe()
int int
RGBLED::status() RGBLED::status()
{ {
int ret;
bool on, powersave; bool on, powersave;
uint8_t r, g, b; uint8_t r, g, b;
ret = get(on, powersave, r, g, b); int ret = get(on, powersave, r, g, b);
if (ret == OK) { if (ret == OK) {
/* we don't care about power-save mode */ /* we don't care about power-save mode */
@ -212,40 +198,19 @@ void
RGBLED::Run() RGBLED::Run()
{ {
if (!_should_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; _running = false;
return; 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()) {
int led_control_sub = orb_subscribe(ORB_ID(led_control));
_led_controller.init(led_control_sub);
}
if (_param_sub >= 0) { update_params();
bool updated = false;
orb_check(_param_sub, &updated);
if (updated) { // Immediately update to change brightness
parameter_update_s pupdate; send_led_rgb();
orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
update_params();
// Immediately update to change brightness
send_led_rgb();
}
} }
LedControlData led_control_data; LedControlData led_control_data;

76
src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
Unescape View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -45,7 +45,8 @@
#include <lib/led/led.h> #include <lib/led/led.h>
#include <px4_getopt.h> #include <px4_getopt.h>
#include <px4_work_queue/ScheduledWorkItem.hpp> #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 */ #define ADDR 0x39 /**< I2C adress of NCP5623C */
@ -65,21 +66,21 @@ public:
RGBLED_NPC5623C(int bus, int rgbled); RGBLED_NPC5623C(int bus, int rgbled);
virtual ~RGBLED_NPC5623C(); virtual ~RGBLED_NPC5623C();
virtual int init(); virtual int init();
virtual int probe(); virtual int probe();
private: 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; 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) : RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) :
I2C("rgbled1", RGBLED1_DEVICE_PATH, bus, rgbled, 100000), 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; int counter = 0;
while (_running && ++counter < 10) { while (_running && ++counter < 10) {
usleep(100000); px4_usleep(100000);
} }
} }
@ -140,8 +132,7 @@ RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
int int
RGBLED_NPC5623C::init() RGBLED_NPC5623C::init()
{ {
int ret; int ret = I2C::init();
ret = I2C::init();
if (ret != OK) { if (ret != OK) {
return ret; return ret;
@ -171,40 +162,19 @@ void
RGBLED_NPC5623C::Run() RGBLED_NPC5623C::Run()
{ {
if (!_should_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; _running = false;
return; 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()) {
int led_control_sub = orb_subscribe(ORB_ID(led_control));
_led_controller.init(led_control_sub);
}
if (_param_sub >= 0) { update_params();
bool updated = false;
orb_check(_param_sub, &updated);
if (updated) { // Immediately update to change brightness
parameter_update_s pupdate; send_led_rgb();
orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
update_params();
// Immediately update to change brightness
send_led_rgb();
}
} }
LedControlData led_control_data; LedControlData led_control_data;

75
src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp
Unescape View File

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

23
src/lib/led/led.cpp
Unescape View File

@ -38,24 +38,13 @@
#include "led.h" #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) 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 // handle new state
led_control_s led_control; 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 // don't apply the new state just yet to avoid interrupting an ongoing blinking state
for (int i = 0; i < BOARD_MAX_LEDS; ++i) { for (int i = 0; i < BOARD_MAX_LEDS; ++i) {
@ -80,14 +69,18 @@ int LedController::update(LedControlData &control_data)
} }
_force_update = false; _force_update = false;
orb_check(_led_control_sub, &updated);
} }
bool had_changes = false; // did one of the outputs change? bool had_changes = false; // did one of the outputs change?
// handle state updates // handle state updates
hrt_abstime now = hrt_absolute_time(); 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 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; constexpr uint16_t breathe_duration = BREATHE_INTERVAL * BREATHE_STEPS / 100;

34
src/lib/led/led.h
Unescape View File

@ -44,6 +44,8 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_led.h> #include <drivers/drv_led.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/led_control.h>
struct LedControlDataSingle { struct LedControlDataSingle {
uint8_t color; ///< one of led_control_s::COLOR_* uint8_t color; ///< one of led_control_s::COLOR_*
@ -64,18 +66,6 @@ public:
LedController() = default; LedController() = default;
~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. * get maxium time between two consecutive calls to update() in us.
*/ */
@ -92,18 +82,16 @@ public:
*/ */
int update(LedControlData &control_data); 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 */ (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 */ (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 */ (on-to-off and off-to-on) in us */
int led_control_subscription() const { return _led_control_sub; }
private: private:
/** set control_data based on current Led states */ /** set control_data based on current Led states */
@ -186,8 +174,8 @@ private:
PerLedData _states[BOARD_MAX_LEDS]; ///< keep current LED states 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
}; };

Write Preview
Loading…
Cancel
Save