PX4 Firmware
PX4 Autopilot Software http://px4.io
rgbled_ncp5623c.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018-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_ncp5623c.cpp
36  *
37  * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C.
38  *
39  * @author CUAVcaijie <caijie@cuav.net>
40  */
41 
42 #include <string.h>
43 
44 #include <drivers/device/i2c.h>
45 #include <lib/led/led.h>
46 #include <lib/parameters/param.h>
47 #include <px4_platform_common/getopt.h>
48 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
49 #include <uORB/Subscription.hpp>
51 
52 
53 #define ADDR 0x39 /**< I2C adress of NCP5623C */
54 
55 #define NCP5623_LED_CURRENT 0x20 /**< Current register */
56 #define NCP5623_LED_PWM0 0x40 /**< pwm0 register */
57 #define NCP5623_LED_PWM1 0x60 /**< pwm1 register */
58 #define NCP5623_LED_PWM2 0x80 /**< pwm2 register */
59 
60 #define NCP5623_LED_BRIGHT 0x1f /**< full brightness */
61 #define NCP5623_LED_OFF 0x00 /**< off */
62 
63 
64 class RGBLED_NPC5623C : public device::I2C, public px4::ScheduledWorkItem
65 {
66 public:
67  RGBLED_NPC5623C(int bus, int rgbled);
68  virtual ~RGBLED_NPC5623C();
69 
70  virtual int init();
71  virtual int probe();
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_rgb();
92  void update_params();
93 
94  int write(uint8_t reg, uint8_t data);
95 };
96 
97 /* for now, we only support one RGBLED */
98 namespace
99 {
100 RGBLED_NPC5623C *g_rgbled = nullptr;
101 }
102 
103 void rgbled_ncp5623c_usage();
104 
105 extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]);
106 
107 RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) :
108  I2C("rgbled1", RGBLED1_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
124 RGBLED_NPC5623C::write(uint8_t reg, uint8_t data)
125 {
126  uint8_t msg[1] = { 0x00 };
127  msg[0] = ((reg & 0xe0) | (data & 0x1f));
128 
129  int ret = transfer(&msg[0], 1, nullptr, 0);
130 
131  return ret;
132 }
133 
134 int
136 {
137  int ret = I2C::init();
138 
139  if (ret != OK) {
140  return ret;
141  }
142 
143  update_params();
144 
145  _running = true;
146 
147  ScheduleNow();
148 
149  return OK;
150 }
151 
152 int
154 {
155  _retries = 4;
156 
157  return write(NCP5623_LED_CURRENT, 0x00);
158 }
159 
160 /**
161  * Main loop function
162  */
163 void
165 {
166  if (!_should_run) {
167  _running = false;
168  return;
169  }
170 
171  // check for parameter updates
173  // clear update
174  parameter_update_s pupdate;
175  _parameter_update_sub.copy(&pupdate);
176 
177  // update parameters from storage
178  update_params();
179 
180  // Immediately update to change brightness
181  send_led_rgb();
182  }
183 
184  LedControlData led_control_data;
185 
186  if (_led_controller.update(led_control_data) == 1) {
187  switch (led_control_data.leds[0].color) {
188  case led_control_s::COLOR_RED:
189  _r = NCP5623_LED_BRIGHT; _g = 0; _b = 0;
190  break;
191 
192  case led_control_s::COLOR_GREEN:
193  _r = 0; _g = NCP5623_LED_BRIGHT; _b = 0;
194  break;
195 
196  case led_control_s::COLOR_BLUE:
197  _r = 0; _g = 0; _b = NCP5623_LED_BRIGHT;
198  break;
199 
200  case led_control_s::COLOR_AMBER: //make it the same as yellow
201  case led_control_s::COLOR_YELLOW:
203  break;
204 
205  case led_control_s::COLOR_PURPLE:
207  break;
208 
209  case led_control_s::COLOR_CYAN:
211  break;
212 
213  case led_control_s::COLOR_WHITE:
215  break;
216 
217  default: // led_control_s::COLOR_OFF
218  _r = 0; _g = 0; _b = 0;
219  break;
220  }
221 
222  _brightness = (float)led_control_data.leds[0].brightness / 255.f;
223  send_led_rgb();
224 
225  }
226 
227  /* re-queue ourselves to run again later */
228  ScheduleDelayed(_led_controller.maximum_update_interval());
229 }
230 
231 /**
232  * Send RGB PWM settings to LED driver according to current color and brightness
233  */
234 int
236 {
237 
238  uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80};
239  uint8_t brightness = 0x1f * _max_brightness;
240 
241  msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f);
242  msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f);
243  msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f);
244  msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f);
245 
246  return transfer(&msg[0], 7, nullptr, 0);
247 }
248 
249 void
251 {
252  int32_t maxbrt = 31;
253  param_get(param_find("LED_RGB1_MAXBRT"), &maxbrt);
254  maxbrt = maxbrt > 31 ? 31 : maxbrt;
255  maxbrt = maxbrt < 0 ? 0 : maxbrt;
256 
257  if (maxbrt == 0) {
258  maxbrt = 1;
259  }
260 
261  _max_brightness = maxbrt / 31.0f;
262 }
263 
264 void
266 {
267  PX4_INFO("missing command: try 'start', 'stop'");
268  PX4_INFO("options:");
269  PX4_INFO(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
270  PX4_INFO(" -a addr (0x%x)", ADDR);
271 }
272 
273 int
274 rgbled_ncp5623c_main(int argc, char *argv[])
275 {
276  int i2cdevice = -1;
277  int rgbledadr = ADDR; /* 7bit */
278 
279  int ch;
280 
281  /* jump over start/off/etc and look at options first */
282  int myoptind = 1;
283  const char *myoptarg = nullptr;
284 
285  while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
286  switch (ch) {
287  case 'a':
288  rgbledadr = strtol(myoptarg, nullptr, 0);
289  break;
290 
291  case 'b':
292  i2cdevice = strtol(myoptarg, nullptr, 0);
293  break;
294 
295  default:
297  return 1;
298  }
299  }
300 
301  if (myoptind >= argc) {
303  return 1;
304  }
305 
306  const char *verb = argv[myoptind];
307 
308  if (!strcmp(verb, "start")) {
309  if (g_rgbled != nullptr) {
310  PX4_WARN("already started");
311  return 1;
312  }
313 
314  if (i2cdevice == -1) {
315  // try the external bus first
316  i2cdevice = PX4_I2C_BUS_EXPANSION;
317  g_rgbled = new RGBLED_NPC5623C(PX4_I2C_BUS_EXPANSION, rgbledadr);
318 
319  if (g_rgbled != nullptr && OK != g_rgbled->init()) {
320  delete g_rgbled;
321  g_rgbled = nullptr;
322  }
323 
324  if (g_rgbled == nullptr) {
325  // fall back to default bus
326  if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
327  PX4_WARN("no RGB led on bus #%d", i2cdevice);
328  return 1;
329  }
330 
331  i2cdevice = PX4_I2C_BUS_LED;
332  }
333  }
334 
335  if (g_rgbled == nullptr) {
336  g_rgbled = new RGBLED_NPC5623C(i2cdevice, rgbledadr);
337 
338  if (g_rgbled == nullptr) {
339  PX4_WARN("alloc failed");
340  return 1;
341  }
342 
343  if (OK != g_rgbled->init()) {
344  delete g_rgbled;
345  g_rgbled = nullptr;
346  PX4_WARN("no RGB led on bus #%d", i2cdevice);
347  return 1;
348  }
349  }
350 
351  return 0;
352  }
353 
354  /* need the driver past this point */
355  if (g_rgbled == nullptr) {
356  PX4_WARN("not started");
358  return 1;
359  }
360 
361  if (!strcmp(verb, "stop")) {
362  delete g_rgbled;
363  g_rgbled = nullptr;
364  return 0;
365  }
366 
368  return 1;
369 }
#define ADDR
I2C adress of NCP5623C.
virtual int init()
__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
LedControlDataSingle leds[BOARD_MAX_LEDS]
Definition: led.h:55
Definition: I2C.hpp:51
volatile bool _should_run
int write(uint8_t reg, uint8_t data)
#define NCP5623_LED_PWM1
pwm1 register
LedController _led_controller
#define NCP5623_LED_PWM0
pwm0 register
uint8_t brightness
brightness in [0, 255]
Definition: led.h:52
virtual int probe()
#define NCP5623_LED_CURRENT
Current register.
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
void rgbled_ncp5623c_usage()
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
RGBLED_NPC5623C(int bus, int rgbled)
void init()
Activates/configures the hardware registers.
#define NCP5623_LED_BRIGHT
full brightness
uint8_t * data
Definition: dataman.cpp:149
#define RGBLED1_DEVICE_PATH
Definition: drv_led.h:60
static unsigned counter
Definition: safety.c:56
bool updated()
Check if there is a new update.
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
int send_led_rgb()
Send RGB PWM settings to LED driver according to current color and brightness.
#define NCP5623_LED_PWM2
pwm2 register
uORB::Subscription _parameter_update_sub
volatile bool _running
int update(LedControlData &control_data)
Update and retrieve the Led state.
Definition: led.cpp:41
void Run() override
Main loop function.
__EXPORT int rgbled_ncp5623c_main(int argc, char *argv[])
Definition: bst.cpp:62
Led controller helper class, used by Led drivers.
#define OK
Definition: uavcan_main.cpp:71
bool copy(void *dst)
Copy the struct.
class LedController Handles the led_control topic: blinking, priorities and state updates...
Definition: led.h:63
virtual ~RGBLED_NPC5623C()
Base class for devices connected via I2C.