dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

led drivers and controller move to uORB::Subscription

This commit is contained in:
Daniel Agar 2019-08-20 14:03:23 -04:00 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp
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 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) {

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

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

85
src/drivers/lights/rgbled/rgbled.cpp
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
* 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"
#define RGBLED_ONTIME 120
#define RGBLED_OFFTIME 120
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#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;

76
src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
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
* 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 _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;

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

@ -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;
}

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

@ -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;

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

@ -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 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
};