PX4 Firmware
PX4 Autopilot Software http://px4.io
blinkm.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012-2016 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 blinkm.cpp
36  *
37  * Driver for the BlinkM LED controller connected via I2C.
38  *
39  * Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
40  * blinkm start
41  *
42  * To start the system monitor put in the next line after the blinkm start:
43  * blinkm systemmonitor
44  *
45  *
46  * Description:
47  * After startup, the Application checked how many lipo cells are connected to the System.
48  * The recognized number off cells, will be blinked 5 times in purple color.
49  * 2 Cells = 2 blinks
50  * ...
51  * 6 Cells = 6 blinks
52  * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
53  *
54  * System disarmed and safe:
55  * The BlinkM should light solid cyan.
56  *
57  * System safety off but not armed:
58  * The BlinkM should light flashing orange
59  *
60  * System armed:
61  * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
62  *
63  * X-X-X-X-_-_-_-_-_-_-
64  * -------------------------
65  * G G G M
66  * P P P O
67  * S S S D
68  * E
69  *
70  * (X = on, _=off)
71  *
72  * The first 3 blinks indicates the status of the GPS-Signal (red):
73  * 0-4 satellites = X-X-X-X-X-_-_-_-_-_-
74  * 5 satellites = X-X-_-X-X-_-_-_-_-_-
75  * 6 satellites = X-_-_-X-X-_-_-_-_-_-
76  * >=7 satellites = _-_-_-X-X-_-_-_-_-_-
77  * If no GPS is found the first 3 blinks are white
78  *
79  * The fourth Blink indicates the Flightmode:
80  * MANUAL : amber
81  * STABILIZED : yellow
82  * HOLD : blue
83  * AUTO : green
84  *
85  * Battery Warning (low Battery Level):
86  * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X
87  *
88  * Battery Alert (critical Battery Level)
89  * Continuously blinking in red X-X-X-X-X-X-X-X-X-X
90  *
91  * General Error (no uOrb Data)
92  * Continuously blinking in white X-X-X-X-X-X-X-X-X-X
93  *
94  */
95 
96 #include <ctype.h>
97 #include <string.h>
98 #include <strings.h>
99 
100 #include <drivers/device/i2c.h>
101 #include <drivers/drv_blinkm.h>
102 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
103 #include <uORB/Subscription.hpp>
104 #include <uORB/topics/actuator_armed.h>
105 #include <uORB/topics/battery_status.h>
106 #include <uORB/topics/safety.h>
107 #include <uORB/topics/vehicle_control_mode.h>
108 #include <uORB/topics/vehicle_gps_position.h>
109 #include <uORB/topics/vehicle_status.h>
110 
111 static const int LED_ONTIME = 120;
112 static const int LED_OFFTIME = 120;
113 static const int LED_BLINK = 1;
114 static const int LED_NOBLINK = 0;
115 
116 class BlinkM : public device::I2C, public px4::ScheduledWorkItem
117 {
118 public:
119  BlinkM(int bus, int blinkm);
120  virtual ~BlinkM() = default;
121 
122 
123  virtual int init();
124  virtual int probe();
125  virtual int setMode(int mode);
126  virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
127 
128  static const char *const script_names[];
129 
130 private:
131  enum ScriptID {
132  USER = 0,
133  RGB,
134  WHITE_FLASH,
135  RED_FLASH,
136  GREEN_FLASH,
137  BLUE_FLASH,
138  CYAN_FLASH,
139  MAGENTA_FLASH,
140  YELLOW_FLASH,
141  BLACK,
142  HUE_CYCLE,
143  MOOD_LIGHT,
144  VIRTUAL_CANDLE,
145  WATER_REFLECTIONS,
146  OLD_NEON,
147  THE_SEASONS,
148  THUNDERSTORM,
149  STOP_LIGHT,
150  MORSE_CODE
151  };
152 
153  enum ledColors {
154  LED_OFF,
155  LED_RED,
156  LED_ORANGE,
157  LED_YELLOW,
158  LED_PURPLE,
159  LED_GREEN,
160  LED_BLUE,
161  LED_CYAN,
162  LED_WHITE,
163  LED_AMBER
164  };
165 
166  int led_color_1;
167  int led_color_2;
168  int led_color_3;
169  int led_color_4;
170  int led_color_5;
171  int led_color_6;
172  int led_color_7;
173  int led_color_8;
174  int led_blink;
175 
176  bool systemstate_run;
177 
178  uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
179  uORB::Subscription battery_status_sub{ORB_ID(battery_status)};
180  uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
181  uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
182  uORB::Subscription actuator_armed_sub{ORB_ID(actuator_armed)};
183  uORB::Subscription safety_sub{ORB_ID(safety)};
184 
185  int num_of_cells;
186  int detected_cells_runcount;
187  int t_led_color[8];
188  int t_led_blink;
189  int led_thread_runcount;
190  int led_interval;
191 
192  bool detected_cells_blinked;
193  bool led_thread_ready;
194 
195  int num_of_used_sats;
196 
197  void setLEDColor(int ledcolor);
198 
199  void Run() override;
200 
201  int set_rgb(uint8_t r, uint8_t g, uint8_t b);
202 
203  int fade_rgb(uint8_t r, uint8_t g, uint8_t b);
204  int fade_hsb(uint8_t h, uint8_t s, uint8_t b);
205 
206  int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b);
207  int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b);
208 
209  int set_fade_speed(uint8_t s);
210 
211  int play_script(uint8_t script_id);
212  int play_script(const char *script_name);
213  int stop_script();
214 
215  int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3);
216  int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]);
217  int set_script(uint8_t length, uint8_t repeats);
218 
219  int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b);
220 
221  int get_firmware_version(uint8_t version[2]);
222 };
223 
224 /* for now, we only support one BlinkM */
225 namespace
226 {
227 BlinkM *g_blinkm;
228 }
229 
230 /* list of script names, must match script ID numbers */
231 const char *const BlinkM::script_names[] = {
232  "USER",
233  "RGB",
234  "WHITE_FLASH",
235  "RED_FLASH",
236  "GREEN_FLASH",
237  "BLUE_FLASH",
238  "CYAN_FLASH",
239  "MAGENTA_FLASH",
240  "YELLOW_FLASH",
241  "BLACK",
242  "HUE_CYCLE",
243  "MOOD_LIGHT",
244  "VIRTUAL_CANDLE",
245  "WATER_REFLECTIONS",
246  "OLD_NEON",
247  "THE_SEASONS",
248  "THUNDERSTORM",
249  "STOP_LIGHT",
250  "MORSE_CODE",
251  nullptr
252 };
253 
254 
255 extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
256 
257 BlinkM::BlinkM(int bus, int blinkm) :
258  I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm, 100000),
259  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
260  led_color_1(LED_OFF),
261  led_color_2(LED_OFF),
262  led_color_3(LED_OFF),
263  led_color_4(LED_OFF),
264  led_color_5(LED_OFF),
265  led_color_6(LED_OFF),
266  led_color_7(LED_OFF),
267  led_color_8(LED_OFF),
268  led_blink(LED_NOBLINK),
269  systemstate_run(false),
270  num_of_cells(0),
271  detected_cells_runcount(0),
272  t_led_color{0},
273  t_led_blink(0),
274  led_thread_runcount(0),
275  led_interval(1000),
276  detected_cells_blinked(false),
277  led_thread_ready(true),
278  num_of_used_sats(0)
279 {
280 }
281 
282 int
283 BlinkM::init()
284 {
285  int ret = I2C::init();
286 
287  if (ret != OK) {
288  warnx("I2C init failed");
289  return ret;
290  }
291 
292  stop_script();
293  set_rgb(0, 0, 0);
294 
295  return OK;
296 }
297 
298 int
299 BlinkM::setMode(int mode)
300 {
301  if (mode == 1) {
302  if (systemstate_run == false) {
303  stop_script();
304  set_rgb(0, 0, 0);
305  systemstate_run = true;
306  ScheduleNow();
307  }
308 
309  } else {
310  systemstate_run = false;
311  }
312 
313  return OK;
314 }
315 
316 int
317 BlinkM::probe()
318 {
319  uint8_t version[2];
320  int ret;
321 
322  ret = get_firmware_version(version);
323 
324  if (ret == OK) {
325  DEVICE_DEBUG("found BlinkM firmware version %c%c", version[1], version[0]);
326  }
327 
328  return ret;
329 }
330 
331 int
332 BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
333 {
334  int ret = ENOTTY;
335 
336  switch (cmd) {
337  case BLINKM_PLAY_SCRIPT_NAMED:
338  if (arg == 0) {
339  ret = EINVAL;
340  break;
341  }
342 
343  ret = play_script((const char *)arg);
344  break;
345 
346  case BLINKM_PLAY_SCRIPT:
347  ret = play_script(arg);
348  break;
349 
350  case BLINKM_SET_USER_SCRIPT: {
351  if (arg == 0) {
352  ret = EINVAL;
353  break;
354  }
355 
356  unsigned lines = 0;
357  const uint8_t *script = (const uint8_t *)arg;
358 
359  while ((lines < 50) && (script[1] != 0)) {
360  ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]);
361 
362  if (ret != OK) {
363  break;
364  }
365 
366  script += 5;
367  }
368 
369  if (ret == OK) {
370  ret = set_script(lines, 0);
371  }
372 
373  break;
374  }
375 
376  default:
377  break;
378  }
379 
380  return ret;
381 }
382 
383 
384 void
385 BlinkM::Run()
386 {
387  if (led_thread_ready == true) {
388  if (!detected_cells_blinked) {
389  if (num_of_cells > 0) {
390  t_led_color[0] = LED_PURPLE;
391  }
392 
393  if (num_of_cells > 1) {
394  t_led_color[1] = LED_PURPLE;
395  }
396 
397  if (num_of_cells > 2) {
398  t_led_color[2] = LED_PURPLE;
399  }
400 
401  if (num_of_cells > 3) {
402  t_led_color[3] = LED_PURPLE;
403  }
404 
405  if (num_of_cells > 4) {
406  t_led_color[4] = LED_PURPLE;
407  }
408 
409  if (num_of_cells > 5) {
410  t_led_color[5] = LED_PURPLE;
411  }
412 
413  t_led_color[6] = LED_OFF;
414  t_led_color[7] = LED_OFF;
415  t_led_blink = LED_BLINK;
416 
417  } else {
418  t_led_color[0] = led_color_1;
419  t_led_color[1] = led_color_2;
420  t_led_color[2] = led_color_3;
421  t_led_color[3] = led_color_4;
422  t_led_color[4] = led_color_5;
423  t_led_color[5] = led_color_6;
424  t_led_color[6] = led_color_7;
425  t_led_color[7] = led_color_8;
426  t_led_blink = led_blink;
427  }
428 
429  led_thread_ready = false;
430  }
431 
432  if (led_thread_runcount & 1) {
433  if (t_led_blink) {
434  setLEDColor(LED_OFF);
435  }
436 
437  led_interval = LED_OFFTIME;
438 
439  } else {
440  setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]);
441  //led_interval = (led_thread_runcount & 1) : LED_ONTIME;
442  led_interval = LED_ONTIME;
443  }
444 
445  if (led_thread_runcount == 15) {
446  int no_data_vehicle_status = 0;
447  int no_data_battery_status = 0;
448  int no_data_vehicle_control_mode = 0;
449  int no_data_actuator_armed = 0;
450  int no_data_vehicle_gps_position = 0;
451 
452 
453  vehicle_status_s vehicle_status_raw{};
454  bool new_data_vehicle_status = vehicle_status_sub.updated();
455 
456  if (new_data_vehicle_status) {
457  vehicle_status_sub.copy(&vehicle_status_raw);
458  no_data_vehicle_status = 0;
459 
460  } else {
461  no_data_vehicle_status++;
462 
463  if (no_data_vehicle_status >= 3) {
464  no_data_vehicle_status = 3;
465  }
466  }
467 
468 
469  battery_status_s battery_status{};
470  bool new_data_battery_status = battery_status_sub.updated();
471 
472  if (new_data_battery_status) {
473  battery_status_sub.copy(&battery_status);
474 
475  } else {
476  no_data_battery_status++;
477 
478  if (no_data_battery_status >= 3) {
479  no_data_battery_status = 3;
480  }
481  }
482 
483 
484  vehicle_control_mode_s vehicle_control_mode{};
485  bool new_data_vehicle_control_mode = vehicle_control_mode_sub.updated();
486 
487  if (new_data_vehicle_control_mode) {
488  vehicle_control_mode_sub.copy(&vehicle_control_mode);
489  no_data_vehicle_control_mode = 0;
490 
491  } else {
492  no_data_vehicle_control_mode++;
493 
494  if (no_data_vehicle_control_mode >= 3) {
495  no_data_vehicle_control_mode = 3;
496  }
497  }
498 
499 
500  actuator_armed_s actuator_armed{};
501  bool new_data_actuator_armed = actuator_armed_sub.updated();
502 
503  if (new_data_actuator_armed) {
504  actuator_armed_sub.copy(&actuator_armed);
505  no_data_actuator_armed = 0;
506 
507  } else {
508  no_data_actuator_armed++;
509 
510  if (no_data_actuator_armed >= 3) {
511  no_data_actuator_armed = 3;
512  }
513  }
514 
515 
516  vehicle_gps_position_s vehicle_gps_position_raw{};
517  bool new_data_vehicle_gps_position = vehicle_gps_position_sub.updated();
518 
519  if (new_data_vehicle_gps_position) {
520  vehicle_gps_position_sub.copy(&vehicle_gps_position_raw);
521  no_data_vehicle_gps_position = 0;
522 
523  } else {
524  no_data_vehicle_gps_position++;
525 
526  if (no_data_vehicle_gps_position >= 3) {
527  no_data_vehicle_gps_position = 3;
528  }
529  }
530 
531 
532  /* update safety topic */
533  safety_s safety{};
534  bool new_data_safety = safety_sub.updated();
535 
536  if (new_data_safety) {
537  safety_sub.copy(&safety);
538  }
539 
540  /* get number of used satellites in navigation */
541  num_of_used_sats = vehicle_gps_position_raw.satellites_used;
542 
543  if (new_data_battery_status || no_data_battery_status < 3) {
544  if (num_of_cells == 0) {
545 
546  num_of_cells = battery_status.cell_count;
547 
548  } else {
549  if (battery_status.warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
550  /* LED Pattern for battery critical alerting */
551  led_color_1 = LED_RED;
552  led_color_2 = LED_RED;
553  led_color_3 = LED_RED;
554  led_color_4 = LED_RED;
555  led_color_5 = LED_RED;
556  led_color_6 = LED_RED;
557  led_color_7 = LED_RED;
558  led_color_8 = LED_RED;
559  led_blink = LED_BLINK;
560 
561  } else if (vehicle_status_raw.rc_signal_lost) {
562  /* LED Pattern for FAILSAFE */
563  led_color_1 = LED_BLUE;
564  led_color_2 = LED_BLUE;
565  led_color_3 = LED_BLUE;
566  led_color_4 = LED_BLUE;
567  led_color_5 = LED_BLUE;
568  led_color_6 = LED_BLUE;
569  led_color_7 = LED_BLUE;
570  led_color_8 = LED_BLUE;
571  led_blink = LED_BLINK;
572 
573  } else if (battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) {
574  /* LED Pattern for battery low warning */
575  led_color_1 = LED_YELLOW;
576  led_color_2 = LED_YELLOW;
577  led_color_3 = LED_YELLOW;
578  led_color_4 = LED_YELLOW;
579  led_color_5 = LED_YELLOW;
580  led_color_6 = LED_YELLOW;
581  led_color_7 = LED_YELLOW;
582  led_color_8 = LED_YELLOW;
583  led_blink = LED_BLINK;
584 
585  } else {
586  /* no battery warnings here */
587 
588  if (actuator_armed.armed == false) {
589  /* system not armed */
590  if (safety.safety_off) {
591  led_color_1 = LED_ORANGE;
592  led_color_2 = LED_ORANGE;
593  led_color_3 = LED_ORANGE;
594  led_color_4 = LED_ORANGE;
595  led_color_5 = LED_ORANGE;
596  led_color_6 = LED_ORANGE;
597  led_color_7 = LED_ORANGE;
598  led_color_8 = LED_ORANGE;
599  led_blink = LED_BLINK;
600 
601  } else {
602  led_color_1 = LED_CYAN;
603  led_color_2 = LED_CYAN;
604  led_color_3 = LED_CYAN;
605  led_color_4 = LED_CYAN;
606  led_color_5 = LED_CYAN;
607  led_color_6 = LED_CYAN;
608  led_color_7 = LED_CYAN;
609  led_color_8 = LED_CYAN;
610  led_blink = LED_NOBLINK;
611  }
612 
613  } else {
614  /* armed system - initial led pattern */
615  led_color_1 = LED_RED;
616  led_color_2 = LED_RED;
617  led_color_3 = LED_RED;
618  led_color_4 = LED_OFF;
619  led_color_5 = LED_OFF;
620  led_color_6 = LED_OFF;
621  led_color_7 = LED_OFF;
622  led_color_8 = LED_OFF;
623  led_blink = LED_BLINK;
624 
625  if (new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
626 
627  /* indicate main control state */
628  if (vehicle_control_mode.flag_control_auto_enabled) {
629  led_color_4 = LED_BLUE;
630 
631  } else if (vehicle_control_mode.flag_control_position_enabled) {
632  led_color_4 = LED_GREEN;
633 
634  } else if (vehicle_control_mode.flag_control_altitude_enabled) {
635  led_color_4 = LED_YELLOW;
636 
637  } else if (vehicle_control_mode.flag_control_manual_enabled) {
638  led_color_4 = LED_WHITE;
639 
640  } else {
641  led_color_4 = LED_OFF;
642  }
643 
644  led_color_5 = led_color_4;
645  }
646 
647  if (new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
648  /* handling used satus */
649  if (num_of_used_sats >= 7) {
650  led_color_1 = LED_OFF;
651  led_color_2 = LED_OFF;
652  led_color_3 = LED_OFF;
653 
654  } else if (num_of_used_sats == 6) {
655  led_color_2 = LED_OFF;
656  led_color_3 = LED_OFF;
657 
658  } else if (num_of_used_sats == 5) {
659  led_color_3 = LED_OFF;
660  }
661 
662  } else {
663  /* no vehicle_gps_position data */
664  led_color_1 = LED_WHITE;
665  led_color_2 = LED_WHITE;
666  led_color_3 = LED_WHITE;
667 
668  }
669 
670  }
671  }
672  }
673 
674  } else {
675  /* LED Pattern for general Error - no vehicle_status can retrieved */
676  led_color_1 = LED_WHITE;
677  led_color_2 = LED_WHITE;
678  led_color_3 = LED_WHITE;
679  led_color_4 = LED_WHITE;
680  led_color_5 = LED_WHITE;
681  led_color_6 = LED_WHITE;
682  led_color_7 = LED_WHITE;
683  led_color_8 = LED_WHITE;
684  led_blink = LED_BLINK;
685 
686  }
687 
688  /*
689  printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
690  vehicle_status_raw.voltage_battery,
691  vehicle_status_raw.flag_system_armed,
692  vehicle_status_raw.flight_mode,
693  num_of_cells,
694  no_data_vehicle_status,
695  no_data_vehicle_gps_position,
696  num_of_used_sats,
697  vehicle_gps_position_raw.fix_type,
698  vehicle_gps_position_raw.satellites_visible);
699  */
700 
701  led_thread_runcount = 0;
702  led_thread_ready = true;
703  led_interval = LED_OFFTIME;
704 
705  if (detected_cells_runcount < 4) {
706  detected_cells_runcount++;
707 
708  } else {
709  detected_cells_blinked = true;
710  }
711 
712  } else {
713  led_thread_runcount++;
714  }
715 
716  if (systemstate_run == true) {
717  /* re-queue ourselves to run again later */
718  ScheduleDelayed(led_interval);
719 
720  } else {
721  stop_script();
722  set_rgb(0, 0, 0);
723  }
724 }
725 
726 void BlinkM::setLEDColor(int ledcolor)
727 {
728  switch (ledcolor) {
729  case LED_OFF: // off
730  set_rgb(0, 0, 0);
731  break;
732 
733  case LED_RED: // red
734  set_rgb(255, 0, 0);
735  break;
736 
737  case LED_ORANGE: // orange
738  set_rgb(255, 150, 0);
739  break;
740 
741  case LED_YELLOW: // yellow
742  set_rgb(200, 200, 0);
743  break;
744 
745  case LED_PURPLE: // purple
746  set_rgb(255, 0, 255);
747  break;
748 
749  case LED_GREEN: // green
750  set_rgb(0, 255, 0);
751  break;
752 
753  case LED_BLUE: // blue
754  set_rgb(0, 0, 255);
755  break;
756 
757  case LED_CYAN: // cyan
758  set_rgb(0, 128, 128);
759  break;
760 
761  case LED_WHITE: // white
762  set_rgb(255, 255, 255);
763  break;
764 
765  case LED_AMBER: // amber
766  set_rgb(255, 65, 0);
767  break;
768  }
769 }
770 
771 int
772 BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b)
773 {
774  const uint8_t msg[4] = { 'n', r, g, b };
775 
776  return transfer(msg, sizeof(msg), nullptr, 0);
777 }
778 
779 int
780 BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b)
781 {
782  const uint8_t msg[4] = { 'c', r, g, b };
783 
784  return transfer(msg, sizeof(msg), nullptr, 0);
785 }
786 
787 int
788 BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b)
789 {
790  const uint8_t msg[4] = { 'h', h, s, b };
791 
792  return transfer(msg, sizeof(msg), nullptr, 0);
793 }
794 
795 int
796 BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b)
797 {
798  const uint8_t msg[4] = { 'C', r, g, b };
799 
800  return transfer(msg, sizeof(msg), nullptr, 0);
801 }
802 
803 int
804 BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b)
805 {
806  const uint8_t msg[4] = { 'H', h, s, b };
807 
808  return transfer(msg, sizeof(msg), nullptr, 0);
809 }
810 
811 int
812 BlinkM::set_fade_speed(uint8_t s)
813 {
814  const uint8_t msg[2] = { 'f', s };
815 
816  return transfer(msg, sizeof(msg), nullptr, 0);
817 }
818 
819 int
820 BlinkM::play_script(uint8_t script_id)
821 {
822  const uint8_t msg[4] = { 'p', script_id, 0, 0 };
823 
824  return transfer(msg, sizeof(msg), nullptr, 0);
825 }
826 
827 int
828 BlinkM::play_script(const char *script_name)
829 {
830  /* handle HTML colour encoding */
831  if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) {
832  char code[3];
833  uint8_t r, g, b;
834 
835  code[2] = '\0';
836 
837  code[0] = script_name[1];
838  code[1] = script_name[2];
839  r = strtol(code, 0, 16);
840  code[0] = script_name[3];
841  code[1] = script_name[4];
842  g = strtol(code, 0, 16);
843  code[0] = script_name[5];
844  code[1] = script_name[6];
845  b = strtol(code, 0, 16);
846 
847  stop_script();
848  return set_rgb(r, g, b);
849  }
850 
851  for (unsigned i = 0; script_names[i] != nullptr; i++)
852  if (!strcasecmp(script_name, script_names[i])) {
853  return play_script(i);
854  }
855 
856  return -1;
857 }
858 
859 int
860 BlinkM::stop_script()
861 {
862  const uint8_t msg[1] = { 'o' };
863 
864  return transfer(msg, sizeof(msg), nullptr, 0);
865 }
866 
867 int
868 BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3)
869 {
870  const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 };
871 
872  return transfer(msg, sizeof(msg), nullptr, 0);
873 }
874 
875 int
876 BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4])
877 {
878  const uint8_t msg[3] = { 'R', 0, line };
879  uint8_t result[5];
880 
881  int ret = transfer(msg, sizeof(msg), result, sizeof(result));
882 
883  if (ret == OK) {
884  ticks = result[0];
885  cmd[0] = result[1];
886  cmd[1] = result[2];
887  cmd[2] = result[3];
888  cmd[3] = result[4];
889  }
890 
891  return ret;
892 }
893 
894 int
895 BlinkM::set_script(uint8_t len, uint8_t repeats)
896 {
897  const uint8_t msg[4] = { 'L', 0, len, repeats };
898 
899  return transfer(msg, sizeof(msg), nullptr, 0);
900 }
901 
902 int
903 BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
904 {
905  const uint8_t msg = 'g';
906  uint8_t result[3];
907 
908  int ret = transfer(&msg, sizeof(msg), result, sizeof(result));
909 
910  if (ret == OK) {
911  r = result[0];
912  g = result[1];
913  b = result[2];
914  }
915 
916  return ret;
917 }
918 
919 int
920 BlinkM::get_firmware_version(uint8_t version[2])
921 {
922  const uint8_t msg = 'Z';
923 
924  return transfer(&msg, sizeof(msg), version, 2);
925 }
926 
927 void blinkm_usage();
928 
929 void blinkm_usage()
930 {
931  warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
932  warnx("options:");
933  warnx("\t-b --bus i2cbus (3)");
934  warnx("\t-a --blinkmaddr blinkmaddr (9)");
935 }
936 
937 int
938 blinkm_main(int argc, char *argv[])
939 {
940 
941  int i2cdevice = PX4_I2C_BUS_EXPANSION;
942  int blinkmadr = 9;
943 
944  int x;
945 
946  if (argc < 2) {
947  blinkm_usage();
948  return 1;
949  }
950 
951  for (x = 1; x < argc; x++) {
952  if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
953  if (argc > x + 1) {
954  i2cdevice = atoi(argv[x + 1]);
955  }
956  }
957 
958  if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) {
959  if (argc > x + 1) {
960  blinkmadr = atoi(argv[x + 1]);
961  }
962  }
963 
964  }
965 
966  if (!strcmp(argv[1], "start")) {
967  if (g_blinkm != nullptr) {
968  warnx("already started");
969  return 1;
970  }
971 
972  g_blinkm = new BlinkM(i2cdevice, blinkmadr);
973 
974  if (g_blinkm == nullptr) {
975  warnx("new failed");
976  return 1;
977  }
978 
979  if (OK != g_blinkm->init()) {
980  delete g_blinkm;
981  g_blinkm = nullptr;
982  warnx("init failed");
983  return 1;
984  }
985 
986  return 0;
987  }
988 
989 
990  if (g_blinkm == nullptr) {
991  PX4_ERR("not started");
992  blinkm_usage();
993  return 0;
994  }
995 
996  if (!strcmp(argv[1], "systemstate")) {
997  g_blinkm->setMode(1);
998  return 0;
999  }
1000 
1001  if (!strcmp(argv[1], "ledoff")) {
1002  g_blinkm->setMode(0);
1003  return 0;
1004  }
1005 
1006 
1007  if (!strcmp(argv[1], "list")) {
1008  for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) {
1009  PX4_ERR(" %s", BlinkM::script_names[i]);
1010  }
1011 
1012  PX4_ERR(" <html color number>");
1013  return 0;
1014  }
1015 
1016  /* things that require access to the device */
1017  int fd = px4_open(BLINKM0_DEVICE_PATH, 0);
1018 
1019  if (fd < 0) {
1020  warn("can't open BlinkM device");
1021  return 1;
1022  }
1023 
1024  g_blinkm->setMode(0);
1025 
1026  if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) {
1027  return 0;
1028  }
1029 
1030  px4_close(fd);
1031 
1032  blinkm_usage();
1033  return 0;
1034 }
BLINKM_PLAY_SCRIPT_NAMED
#define BLINKM_PLAY_SCRIPT_NAMED
play the named script in *(char *)arg, repeating forever
Definition: drv_blinkm.h:59
battery_status.h
Subscription.hpp
blinkm_main
__EXPORT int blinkm_main(int argc, char *argv[])
Definition: blinkm.cpp:938
LED_OFFTIME
static const int LED_OFFTIME
Definition: blinkm.cpp:112
blinkm_usage
void blinkm_usage()
Definition: blinkm.cpp:929
__EXPORT
Definition: I2C.hpp:51
drv_blinkm.h
BlinkM driver API.
LED_ONTIME
static const int LED_ONTIME
Definition: blinkm.cpp:111
vehicle_status_s
Definition: vehicle_status.h:94
vehicle_status.h
BLINKM_PLAY_SCRIPT
#define BLINKM_PLAY_SCRIPT
play the numbered script in (arg), repeating forever
Definition: drv_blinkm.h:62
vehicle_control_mode_s
Definition: vehicle_control_mode.h:51
msg
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
safety
static struct safety_s safety
Definition: Commander.cpp:140
ORB_ID
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
ToneAlarmInterface::init
void init()
Activates/configures the hardware registers.
actuator_armed.h
BLINKM0_DEVICE_PATH
#define BLINKM0_DEVICE_PATH
Definition: drv_blinkm.h:49
vehicle_gps_position.h
warnx
#define warnx(...)
Definition: err.h:95
safety_s
Definition: safety.h:51
uORB::Subscription::updated
bool updated()
Check if there is a new update.
Definition: Subscription.hpp:98
battery_status
Definition: parameters.cpp:46
fd
int fd
Definition: dataman.cpp:146
vehicle_gps_position_s
Definition: vehicle_gps_position.h:51
LED_BLINK
static const int LED_BLINK
Definition: blinkm.cpp:113
battery_status_s
Definition: battery_status.h:56
cdev::file_t
Definition: cdev_platform.hpp:20
px4_open
int px4_open(const char *path, int flags,...)
Definition: cdev_platform.cpp:148
BLINKM_SET_USER_SCRIPT
#define BLINKM_SET_USER_SCRIPT
Set the user script; (arg) is a pointer to an array of script lines, where each line is an array of f...
Definition: drv_blinkm.h:70
px4
Definition: bst.cpp:62
LED_NOBLINK
static const int LED_NOBLINK
Definition: blinkm.cpp:114
OK
#define OK
Definition: uavcan_main.cpp:71
mode
mode
Definition: vtol_type.h:76
uORB::Subscription::copy
bool copy(void *dst)
Copy the struct.
Definition: Subscription.hpp:121
vehicle_control_mode.h
warn
#define warn(...)
Definition: err.h:94
safety_s::safety_off
bool safety_off
Definition: safety.h:55
actuator_armed_s
Definition: actuator_armed.h:51
DEVICE_DEBUG
#define DEVICE_DEBUG(FMT,...)
Definition: Device.hpp:52
px4_close
int px4_close(int fd)
Definition: cdev_platform.cpp:216
i2c.h
Base class for devices connected via I2C.
px4_ioctl
int px4_ioctl(int fd, int cmd, unsigned long arg)
Definition: cdev_platform.cpp:287