fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 14:50:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

led drivers and controller move to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-08-20 14:03:23 -04:00
committed by GitHub
parent 41dee3875e
commit 5ae408382b
7 changed files with 101 additions and 216 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/lights/rgbled/rgbled.cpp
+25 -60
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;
src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
+23 -53
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;
src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp
+28 -47
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;
}