PX4 Firmware
PX4 Autopilot Software http://px4.io
rgbled.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file rgbled.cpp
36  *
37  * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
38  *
39  * @author Julian Oes <julian@px4.io>
40  * @author Anton Babushkin <anton.babushkin@me.com>
41  */
42 
43 #include <string.h>
44 
45 #include <drivers/device/i2c.h>
46 #include <lib/led/led.h>
47 #include <lib/parameters/param.h>
48 #include <px4_platform_common/getopt.h>
49 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
50 #include <uORB/Subscription.hpp>
52 
53 #define ADDR 0x55 /**< I2C adress of TCA62724FMG */
54 #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */
55 #define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */
56 #define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */
57 #define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */
58 #define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/
59 
60 #define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */
61 #define SETTING_ENABLE 0x02 /**< on */
62 
63 class RGBLED : public device::I2C, public px4::ScheduledWorkItem
64 {
65 public:
66  RGBLED(int bus, int rgbled);
67  virtual ~RGBLED();
68 
69  virtual int init();
70  virtual int probe();
71  int status();
72 
73 private:
74 
75  float _brightness{1.0f};
76  float _max_brightness{1.0f};
77 
78  uint8_t _r{0};
79  uint8_t _g{0};
80  uint8_t _b{0};
81  volatile bool _running{false};
82  volatile bool _should_run{true};
83  bool _leds_enabled{true};
84 
86 
88 
89  void Run() override;
90 
91  int send_led_enable(bool enable);
92  int send_led_rgb();
93  int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
94  void update_params();
95 };
96 
97 /* for now, we only support one RGBLED */
98 namespace
99 {
100 RGBLED *g_rgbled = nullptr;
101 }
102 
103 void rgbled_usage();
104 
105 extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
106 
107 RGBLED::RGBLED(int bus, int rgbled) :
108  I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000),
109  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id()))
110 {
111 }
112 
114 {
115  _should_run = false;
116  int counter = 0;
117 
118  while (_running && ++counter < 10) {
119  px4_usleep(100000);
120  }
121 }
122 
123 int
125 {
126  int ret = I2C::init();
127 
128  if (ret != OK) {
129  return ret;
130  }
131 
132  /* switch off LED on start */
133  send_led_enable(false);
134  send_led_rgb();
135 
136  update_params();
137 
138  _running = true;
139 
140  // kick off work queue
141  ScheduleNow();
142 
143  return OK;
144 }
145 
146 int
148 {
149  int ret;
150  bool on, powersave;
151  uint8_t r, g, b;
152 
153  /**
154  this may look strange, but is needed. There is a serial
155  EEPROM (Microchip-24aa01) that responds to a bunch of I2C
156  addresses, including the 0x55 used by this LED device. So
157  we need to do enough operations to be sure we are talking
158  to the right device. These 3 operations seem to be enough,
159  as the 3rd one consistently fails if no RGBLED is on the bus.
160  */
161 
162  unsigned prevretries = _retries;
163  _retries = 4;
164 
165  if ((ret = get(on, powersave, r, g, b)) != OK ||
166  (ret = send_led_enable(false) != OK) ||
167  (ret = send_led_enable(false) != OK)) {
168  return ret;
169  }
170 
171  _retries = prevretries;
172 
173  return ret;
174 }
175 
176 int
178 {
179  bool on, powersave;
180  uint8_t r, g, b;
181 
182  int ret = get(on, powersave, r, g, b);
183 
184  if (ret == OK) {
185  /* we don't care about power-save mode */
186  DEVICE_LOG("state: %s", on ? "ON" : "OFF");
187  DEVICE_LOG("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
188 
189  } else {
190  PX4_WARN("failed to read led");
191  }
192 
193  return ret;
194 }
195 
196 /**
197  * Main loop function
198  */
199 void
201 {
202  if (!_should_run) {
203  _running = false;
204  return;
205  }
206 
207  // check for parameter updates
209  // clear update
210  parameter_update_s pupdate;
211  _parameter_update_sub.copy(&pupdate);
212 
213  // update parameters from storage
214  update_params();
215 
216  // Immediately update to change brightness
217  send_led_rgb();
218  }
219 
220  LedControlData led_control_data;
221 
222  if (_led_controller.update(led_control_data) == 1) {
223  switch (led_control_data.leds[0].color) {
224  case led_control_s::COLOR_RED:
225  _r = 255; _g = 0; _b = 0;
226  send_led_enable(true);
227  break;
228 
229  case led_control_s::COLOR_GREEN:
230  _r = 0; _g = 255; _b = 0;
231  send_led_enable(true);
232  break;
233 
234  case led_control_s::COLOR_BLUE:
235  _r = 0; _g = 0; _b = 255;
236  send_led_enable(true);
237  break;
238 
239  case led_control_s::COLOR_AMBER: //make it the same as yellow
240  case led_control_s::COLOR_YELLOW:
241  _r = 255; _g = 255; _b = 0;
242  send_led_enable(true);
243  break;
244 
245  case led_control_s::COLOR_PURPLE:
246  _r = 255; _g = 0; _b = 255;
247  send_led_enable(true);
248  break;
249 
250  case led_control_s::COLOR_CYAN:
251  _r = 0; _g = 255; _b = 255;
252  send_led_enable(true);
253  break;
254 
255  case led_control_s::COLOR_WHITE:
256  _r = 255; _g = 255; _b = 255;
257  send_led_enable(true);
258  break;
259 
260  default: // led_control_s::COLOR_OFF
261  _r = 0; _g = 0; _b = 0;
262  send_led_enable(false);
263  break;
264  }
265 
266  _brightness = (float)led_control_data.leds[0].brightness / 255.f;
267 
268  send_led_rgb();
269  }
270 
271  /* re-queue ourselves to run again later */
272  ScheduleDelayed(_led_controller.maximum_update_interval());
273 }
274 
275 /**
276  * Sent ENABLE flag to LED driver
277  */
278 int
280 {
281  if (_leds_enabled && enable) {
282  // already enabled
283  return 0;
284  }
285 
286  _leds_enabled = enable;
287  uint8_t settings_byte = 0;
288 
289  if (enable) {
290  settings_byte |= SETTING_ENABLE;
291  }
292 
293  settings_byte |= SETTING_NOT_POWERSAVE;
294 
295  const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
296 
297  return transfer(msg, sizeof(msg), nullptr, 0);
298 }
299 
300 /**
301  * Send RGB PWM settings to LED driver according to current color and brightness
302  */
303 int
305 {
306  /* To scale from 0..255 -> 0..15 shift right by 4 bits */
307  const uint8_t msg[6] = {
308  SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness * _max_brightness + 0.5f),
309  SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness * _max_brightness + 0.5f),
310  SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness * _max_brightness + 0.5f)
311  };
312  return transfer(msg, sizeof(msg), nullptr, 0);
313 }
314 
315 int
316 RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
317 {
318  uint8_t result[2] = {0, 0};
319  int ret;
320 
321  ret = transfer(nullptr, 0, &result[0], 2);
322 
323  if (ret == OK) {
324  on = ((result[0] >> 4) & SETTING_ENABLE);
325  powersave = !((result[0] >> 4) & SETTING_NOT_POWERSAVE);
326  /* XXX check, looks wrong */
327  r = (result[0] & 0x0f) << 4;
328  g = (result[1] & 0xf0);
329  b = (result[1] & 0x0f) << 4;
330  }
331 
332  return ret;
333 }
334 
335 void
337 {
338  int32_t maxbrt = 15;
339  param_get(param_find("LED_RGB_MAXBRT"), &maxbrt);
340  maxbrt = maxbrt > 15 ? 15 : maxbrt;
341  maxbrt = maxbrt < 0 ? 0 : maxbrt;
342 
343  // A minimum of 2 "on" steps is required for breathe effect
344  if (maxbrt == 1) {
345  maxbrt = 2;
346  }
347 
348  _max_brightness = maxbrt / 15.0f;
349 }
350 
351 void
353 {
354  PX4_INFO("missing command: try 'start', 'status', 'stop'");
355  PX4_INFO("options:");
356  PX4_INFO(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
357  PX4_INFO(" -a addr (0x%x)", ADDR);
358 }
359 
360 int
361 rgbled_main(int argc, char *argv[])
362 {
363  int i2cdevice = -1;
364  int rgbledadr = ADDR; /* 7bit */
365 
366  int ch;
367 
368  /* jump over start/off/etc and look at options first */
369  int myoptind = 1;
370  const char *myoptarg = nullptr;
371 
372  while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
373  switch (ch) {
374  case 'a':
375  rgbledadr = strtol(myoptarg, nullptr, 0);
376  break;
377 
378  case 'b':
379  i2cdevice = strtol(myoptarg, nullptr, 0);
380  break;
381 
382  default:
383  rgbled_usage();
384  return 1;
385  }
386  }
387 
388  if (myoptind >= argc) {
389  rgbled_usage();
390  return 1;
391  }
392 
393  const char *verb = argv[myoptind];
394 
395  if (!strcmp(verb, "start")) {
396  if (g_rgbled != nullptr) {
397  PX4_WARN("already started");
398  return 1;
399  }
400 
401  if (i2cdevice == -1) {
402  // try the external bus first
403  i2cdevice = PX4_I2C_BUS_EXPANSION;
404  g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
405 
406  if (g_rgbled != nullptr && OK != g_rgbled->init()) {
407  delete g_rgbled;
408  g_rgbled = nullptr;
409  }
410 
411  if (g_rgbled == nullptr) {
412  // fall back to default bus
413  if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
414  PX4_WARN("no RGB led on bus #%d", i2cdevice);
415  return 1;
416  }
417 
418  i2cdevice = PX4_I2C_BUS_LED;
419  }
420  }
421 
422  if (g_rgbled == nullptr) {
423  g_rgbled = new RGBLED(i2cdevice, rgbledadr);
424 
425  if (g_rgbled == nullptr) {
426  PX4_WARN("alloc failed");
427  return 1;
428  }
429 
430  if (OK != g_rgbled->init()) {
431  delete g_rgbled;
432  g_rgbled = nullptr;
433  PX4_WARN("no RGB led on bus #%d", i2cdevice);
434  return 1;
435  }
436  }
437 
438  return 0;
439  }
440 
441  /* need the driver past this point */
442  if (g_rgbled == nullptr) {
443  PX4_WARN("not started");
444  rgbled_usage();
445  return 1;
446  }
447 
448  if (!strcmp(verb, "status")) {
449  g_rgbled->status();
450  return 0;
451  }
452 
453  if (!strcmp(verb, "stop")) {
454  delete g_rgbled;
455  g_rgbled = nullptr;
456  return 0;
457  }
458 
459  rgbled_usage();
460  return 1;
461 }
#define ADDR
I2C adress of TCA62724FMG.
Definition: rgbled.cpp:53
void update_params()
Definition: rgbled.cpp:336
virtual int probe()
Definition: rgbled.cpp:147
#define SUB_ADDR_PWM0
blue (without auto-increment)
Definition: rgbled.cpp:55
volatile bool _running
Definition: rgbled.cpp:81
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
int maximum_update_interval() const
get maxium time between two consecutive calls to update() in us.
Definition: led.h:72
float _max_brightness
Definition: rgbled.cpp:76
LedControlDataSingle leds[BOARD_MAX_LEDS]
Definition: led.h:55
virtual int init()
Definition: rgbled.cpp:124
Definition: I2C.hpp:51
int status()
Definition: rgbled.cpp:177
#define SUB_ADDR_PWM2
red (without auto-increment)
Definition: rgbled.cpp:57
bool _leds_enabled
Definition: rgbled.cpp:83
uint8_t _g
Definition: rgbled.cpp:79
uint8_t brightness
brightness in [0, 255]
Definition: led.h:52
Global flash based parameter store.
uint8_t color
one of led_control_s::COLOR_*
Definition: led.h:51
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
#define SUB_ADDR_SETTINGS
settings (without auto-increment)
Definition: rgbled.cpp:58
void Run() override
Main loop function.
Definition: rgbled.cpp:200
LedController _led_controller
Definition: rgbled.cpp:87
void init()
Activates/configures the hardware registers.
#define DEVICE_LOG(FMT,...)
Definition: Device.hpp:51
int send_led_rgb()
Send RGB PWM settings to LED driver according to current color and brightness.
Definition: rgbled.cpp:304
uORB::Subscription _parameter_update_sub
Definition: rgbled.cpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
RGBLED(int bus, int rgbled)
Definition: rgbled.cpp:107
static unsigned counter
Definition: safety.c:56
#define SUB_ADDR_PWM1
green (without auto-increment)
Definition: rgbled.cpp:56
bool updated()
Check if there is a new update.
void rgbled_usage()
Definition: rgbled.cpp:352
uint8_t _b
Definition: rgbled.cpp:80
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
__EXPORT int rgbled_main(int argc, char *argv[])
Definition: rgbled.cpp:361
#define SETTING_ENABLE
on
Definition: rgbled.cpp:61
volatile bool _should_run
Definition: rgbled.cpp:82
virtual ~RGBLED()
Definition: rgbled.cpp:113
uint8_t _r
Definition: rgbled.cpp:78
int update(LedControlData &control_data)
Update and retrieve the Led state.
Definition: led.cpp:41
Definition: bst.cpp:62
Led controller helper class, used by Led drivers.
float _brightness
Definition: rgbled.cpp:75
#define OK
Definition: uavcan_main.cpp:71
int send_led_enable(bool enable)
Sent ENABLE flag to LED driver.
Definition: rgbled.cpp:279
#define RGBLED0_DEVICE_PATH
Definition: drv_led.h:59
bool copy(void *dst)
Copy the struct.
#define SETTING_NOT_POWERSAVE
power-save mode not off
Definition: rgbled.cpp:60
class LedController Handles the led_control topic: blinking, priorities and state updates...
Definition: led.h:63
int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
Definition: rgbled.cpp:316
Base class for devices connected via I2C.