From 89cd74c35f6339866438ef48dcd8b2c2ec05e1ee Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 26 Feb 2015 16:40:41 +0900
Subject: [PATCH] Notify: OreoLED supports handle_led_control

includes support for send bytes ioctl
---
 libraries/AP_Notify/OreoLED_PX4.cpp | 71 ++++++++++++++++++++++++++++-
 libraries/AP_Notify/OreoLED_PX4.h   |  7 +++
 2 files changed, 77 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Notify/OreoLED_PX4.cpp b/libraries/AP_Notify/OreoLED_PX4.cpp
index 87115b059a..d5793c4e86 100644
--- a/libraries/AP_Notify/OreoLED_PX4.cpp
+++ b/libraries/AP_Notify/OreoLED_PX4.cpp
@@ -44,7 +44,8 @@ OreoLED_PX4::OreoLED_PX4() : NotifyDevice(),
     _overall_health(false),
     _oreoled_fd(-1),
     _send_required(false),
-    _state_desired_semaphore(false)
+    _state_desired_semaphore(false),
+    _pattern_override(0)
 {
     // initialise desired and sent state
     memset(_state_desired,0,sizeof(_state_desired));
@@ -84,6 +85,13 @@ void OreoLED_PX4::update()
         return;
     }
 
+    // return immediately if custom pattern has been sent
+    if (OreoLED_PX4::_pattern_override != 0) {
+        // reset stage so patterns will be resent once override clears
+        last_stage = 0;
+        return;
+    }
+
     // slow rate from 50Hz to 10hz
     counter++;
     if (counter < 5) {
@@ -297,6 +305,28 @@ void OreoLED_PX4::set_macro(uint8_t instance, oreoled_macro macro)
     _state_desired_semaphore = false;
 }
 
+// send_bytes - send bytes to one or all LEDs
+void OreoLED_PX4::send_bytes(uint8_t instance, uint8_t num_bytes, uint8_t bytes[OREOLED_CMD_LENGTH_MAX])
+{
+    // return immediately if no healthy leds
+    if (!_overall_health) {
+        return;
+    }
+
+    // set semaphore
+    _state_desired_semaphore = true;
+
+    // send bytes to some or all leds
+    oreoled_cmd_t new_cmd;
+    new_cmd.led_num = instance;
+    new_cmd.num_bytes = num_bytes;
+    memcpy(new_cmd.buff, bytes, OREOLED_CMD_LENGTH_MAX);
+    ioctl(_oreoled_fd, OREOLED_SEND_BYTES, (unsigned long)&new_cmd);
+
+    // release semaphore
+    _state_desired_semaphore = false;
+}
+
 // update_timer - called by scheduler and updates PX4 driver with commands
 void OreoLED_PX4::update_timer(void)
 {
@@ -341,4 +371,43 @@ void OreoLED_PX4::update_timer(void)
     _send_required = false;
 }
 
+// handle a LED_CONTROL message
+void OreoLED_PX4::handle_led_control(mavlink_message_t *msg)
+{
+    // exit immediately if unhealthy
+    if (!_overall_health) {
+        return;
+    }
+
+    // decode mavlink message
+    mavlink_led_control_t packet;
+    mavlink_msg_led_control_decode(msg, &packet);
+
+    // exit immediately if instance is invalid
+    if (packet.instance >= OREOLED_NUM_LEDS && packet.instance != OREOLED_INSTANCE_ALL) {
+        return;
+    }
+
+    // if pattern is OFF, we clear pattern override so normal lighting should resume
+    if (packet.pattern == LED_CONTROL_PATTERN_OFF) {
+        _pattern_override = 0;
+        return;
+    }
+
+    // custom pattern
+    if (packet.pattern == LED_CONTROL_PATTERN_CUSTOM) {
+        // sanity check length
+        if (packet.custom_len <= OREOLED_CMD_LENGTH_MAX) {
+            // send bytes
+            send_bytes(packet.instance, packet.custom_len, packet.custom_bytes);
+            _pattern_override = packet.pattern;
+        }
+        return;
+    }
+
+    // other patterns sent as macro
+    set_macro(packet.instance, (oreoled_macro)packet.pattern);
+    _pattern_override = packet.pattern;
+}
+
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_Notify/OreoLED_PX4.h b/libraries/AP_Notify/OreoLED_PX4.h
index e83942d29a..38c533dd12 100644
--- a/libraries/AP_Notify/OreoLED_PX4.h
+++ b/libraries/AP_Notify/OreoLED_PX4.h
@@ -42,6 +42,9 @@ public:
     // healthy - return true if at least one LED is responding
     bool healthy() const { return _overall_health; }
 
+    // handle a LED_CONTROL message, by default device ignore message
+    void handle_led_control(mavlink_message_t *msg);
+
 private:
     // update_timer - called by scheduler and updates PX4 driver with commands
     void update_timer(void);
@@ -52,6 +55,9 @@ private:
     // set_macrxo - set macro for one or all LEDs
     void set_macro(uint8_t instance, enum oreoled_macro macro);
 
+    // send_bytes - send bytes to one or all LEDs
+    void send_bytes(uint8_t instance, uint8_t num_bytes, uint8_t bytes[OREOLED_CMD_LENGTH_MAX]);
+
     // oreo led modes (pattern, macro or rgb)
     enum oreoled_mode {
         OREOLED_MODE_PATTERN,
@@ -81,6 +87,7 @@ private:
     volatile bool _state_desired_semaphore;         // true when we are updating the state desired values to ensure they are not sent prematurely
     oreo_state _state_desired[OREOLED_NUM_LEDS];    // desired state
     oreo_state _state_sent[OREOLED_NUM_LEDS];       // last state sent to led
+    uint8_t _pattern_override;                      // holds last processed pattern override, 0 if we are not overriding a pattern
 };
 
 #endif // __OREOLED_PX4_H__