AP_Notify: support ProfiLEDs

libraries/AP_Notify/AP_Notify.cpp

@@ -35,6 +35,7 @@
 #include "SITL_SFML_LED.h"
 #include <stdio.h>
 #include "AP_BoardLED2.h"
+#include "ProfiLED.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -145,7 +146,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     // @Param: LED_TYPES
     // @DisplayName: LED Driver Types
     // @Description: Controls what types of LEDs will be enabled
-    // @Bitmask: 0:Build in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:UAVCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel
+    // @Bitmask: 0:Build in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:UAVCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED
     // @User: Advanced
     AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, BUILD_DEFAULT_LED_TYPE),
 
@@ -164,7 +165,14 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     // @Range: 0 100
     // @Units: %
     AP_GROUPINFO("BUZZ_VOLUME", 8, AP_Notify, _buzzer_volume, 100),
-    
+
+    // @Param: LED_LEN
+    // @DisplayName: Serial LED String Length
+    // @Description: The number of Serial LED's to use for notifications (NeoPixel's and ProfiLED)
+    // @Range: 1 32
+    // @User: Advanced
+    AP_GROUPINFO("LED_LEN", 9, AP_Notify, _led_len, 1),
+
     AP_GROUPEND
 };
 
@@ -262,6 +270,9 @@ void AP_Notify::add_backends(void)
             case Notify_LED_NeoPixel:
                 ADD_BACKEND(new NeoPixel());
                 break;
+            case Notify_LED_ProfiLED:
+                ADD_BACKEND(new ProfiLED());
+                break;
             case Notify_LED_OreoLED:
 #if !HAL_MINIMIZE_FEATURES
                 if (_oreo_theme) {

libraries/AP_Notify/AP_Notify.h

@@ -70,6 +70,7 @@ public:
         Notify_LED_NCP5623_I2C_External     = (1 << 6), // External NCP5623
         Notify_LED_NCP5623_I2C_Internal     = (1 << 7), // Internal NCP5623
         Notify_LED_NeoPixel                 = (1 << 8), // NeoPixel 5050 AdaFruit 1655 SK6812  Worldsemi WS2812B
+        Notify_LED_ProfiLED                 = (1 << 9), // ProfiLED
         Notify_LED_MAX
     };
 
@@ -160,6 +161,7 @@ public:
     uint8_t get_buzz_pin() const  { return _buzzer_pin; }
     uint8_t get_buzz_level() const  { return _buzzer_level; }
     uint8_t get_buzz_volume() const  { return _buzzer_volume; }
+    uint8_t get_led_len() const {return _led_len; }
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     HAL_Semaphore sf_window_mutex;
@@ -184,6 +186,7 @@ private:
     AP_Int32 _led_type;
     AP_Int8 _buzzer_level;
     AP_Int8 _buzzer_volume;
+    AP_Int8 _led_len;
 
     char _send_text[NOTIFY_TEXT_BUFFER_SIZE];
     uint32_t _send_text_updated_millis; // last time text changed

libraries/AP_Notify/NeoPixel.cpp

@@ -36,17 +36,10 @@
 extern const AP_HAL::HAL& hal;
 
 NeoPixel::NeoPixel() :
-    RGBLed(NEOPIXEL_LED_OFF, NEOPIXEL_LED_HIGH, NEOPIXEL_LED_MEDIUM, NEOPIXEL_LED_LOW)
+    SerialLED(NEOPIXEL_LED_OFF, NEOPIXEL_LED_HIGH, NEOPIXEL_LED_MEDIUM, NEOPIXEL_LED_LOW)
 {
 }
 
-bool NeoPixel::hw_init()
-{
-    init_ports();
-    hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&NeoPixel::timer, void));
-    return true;
-}
-
 uint16_t NeoPixel::init_ports()
 {
     uint16_t mask = 0;
@@ -58,40 +51,20 @@ uint16_t NeoPixel::init_ports()
         mask |= SRV_Channels::get_output_channel_mask(fn);
     }
 
-    if (mask != 0) {
-        for (uint16_t chan=0; chan<16; chan++) {
-            if ((1U<<chan) & mask) {
-                hal.rcout->set_neopixel_num_LEDs(chan, HAL_NEOPIXEL_COUNT);
-            }
-        }
+    if (mask == 0) {
+        return 0;
     }
-    last_mask = mask;
-    return mask;
-}
 
-void NeoPixel::timer()
-{
-    WITH_SEMAPHORE(_sem);
-
-    const uint32_t now_ms = AP_HAL::millis();
-    if (now_ms - _last_init_ms >= 1000) {
-        _last_init_ms = now_ms;
-        enable_mask = init_ports();
-    }
-}
-
-bool NeoPixel::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
-{
-    if (enable_mask == 0) {
-        // nothing is enabled, no pins set as LED output
-        return true;
+    AP_SerialLED *led = AP_SerialLED::get_singleton();
+    if (led == nullptr) {
+        return 0;
     }
 
     for (uint16_t chan=0; chan<16; chan++) {
-        if ((1U<<chan) & enable_mask) {
-            hal.rcout->set_neopixel_rgb_data(chan, -1, red, green, blue);
+        if ((1U<<chan) & mask) {
+            led->set_num_neopixel(chan+1, (pNotify->get_led_len()));
         }
     }
-    hal.rcout->neopixel_send();
-    return true;
+
+    return mask;
 }

libraries/AP_Notify/NeoPixel.h

@@ -15,33 +15,12 @@
 #pragma once
 
 #include "RGBLed.h"
+#include "SerialLED.h"
 #include <AP_Common/AP_Common.h>
 
-class NeoPixel: public RGBLed {
+class NeoPixel: public SerialLED {
 public:
     NeoPixel();
 
-    struct {
-        uint8_t b;
-        uint8_t r;
-        uint8_t g;
-    } RGB;
-
-    uint16_t init_ports();
-
-protected:
-    bool hw_init(void) override;
-    bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
-
-private:
-    uint16_t enable_mask;
-    uint16_t last_mask;
-
-    // perdiodic tick to re-init
-    uint32_t    _last_init_ms;
-
-    // periodic callback
-    void timer();
-    
-    HAL_Semaphore_Recursive _sem;
+    uint16_t init_ports() override;
 };

libraries/AP_Notify/ProfiLED.cpp

@@ -0,0 +1,62 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_Notify/AP_Notify.h"
+#include "ProfiLED.h"
+#include "SRV_Channel/SRV_Channel.h"
+
+// This limit is from the dshot driver rc out groups limit, we need at least one channel for clock
+#define AP_NOTIFY_ProfiLED_MAX_INSTANCES        3
+
+#define ProfiLED_LOW    0x33
+#define ProfiLED_MEDIUM 0x7F
+#define ProfiLED_HIGH   0xFF
+#define ProfiLED_OFF    0x00
+
+extern const AP_HAL::HAL& hal;
+
+ProfiLED::ProfiLED() :
+    SerialLED(ProfiLED_OFF, ProfiLED_HIGH, ProfiLED_MEDIUM, ProfiLED_LOW)
+{
+}
+
+uint16_t ProfiLED::init_ports()
+{
+    uint16_t mask = 0;
+    for (uint16_t i=0; i<AP_NOTIFY_ProfiLED_MAX_INSTANCES; i++) {
+        const SRV_Channel::Aux_servo_function_t fn = (SRV_Channel::Aux_servo_function_t)((uint8_t)SRV_Channel::k_ProfiLED_1 + i);
+        if (!SRV_Channels::function_assigned(fn)) {
+            continue;
+        }
+        mask |= SRV_Channels::get_output_channel_mask(fn);
+    }
+
+    if (mask == 0) {
+        return 0;
+    }
+
+    AP_SerialLED *led = AP_SerialLED::get_singleton();
+    if (led == nullptr) {
+        return 0;
+    }
+
+    for (uint16_t chan=0; chan<16; chan++) {
+        if ((1U<<chan) & mask) {
+            led->set_num_profiled(chan+1, (pNotify->get_led_len()));
+        }
+    }
+
+    return mask;
+}

libraries/AP_Notify/ProfiLED.h

@@ -0,0 +1,27 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "RGBLed.h"
+#include "SerialLED.h"
+#include <AP_Common/AP_Common.h>
+
+class ProfiLED: public SerialLED {
+public:
+    ProfiLED();
+
+    uint16_t init_ports() override;
+
+};

libraries/AP_Notify/SerialLED.cpp

@@ -0,0 +1,69 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_Notify/AP_Notify.h"
+#include "SerialLED.h"
+
+extern const AP_HAL::HAL& hal;
+
+SerialLED::SerialLED(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim) :
+    RGBLed(led_off, led_bright, led_medium, led_dim)
+{
+}
+
+bool SerialLED::hw_init()
+{
+    init_ports();
+    hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&SerialLED::timer, void));
+    return true;
+}
+
+void SerialLED::timer()
+{
+    WITH_SEMAPHORE(_sem);
+
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - _last_init_ms >= 1000) {
+        _last_init_ms = now_ms;
+        enable_mask = init_ports();
+    }
+}
+
+bool SerialLED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
+{
+    if (enable_mask == 0) {
+        // nothing is enabled, no pins set as LED output
+        return true;
+    }
+
+    AP_SerialLED *led = AP_SerialLED::get_singleton();
+    if (led == nullptr) {
+        return false;
+    }
+
+    for (uint16_t chan=0; chan<16; chan++) {
+        if ((1U<<chan) & enable_mask) {
+            led->set_RGB(chan+1, -1, red, green, blue);
+        }
+    }
+
+    for (uint16_t chan=0; chan<16; chan++) {
+        if ((1U<<chan) & enable_mask) {
+            led->send(chan+1);
+        }
+    }
+
+    return true;
+}

libraries/AP_Notify/SerialLED.h

@@ -0,0 +1,49 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "RGBLed.h"
+#include <AP_Common/AP_Common.h>
+#include <AP_SerialLED/AP_SerialLED.h>
+
+class SerialLED: public RGBLed {
+public:
+    SerialLED(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim);
+
+    struct {
+        uint8_t b;
+        uint8_t r;
+        uint8_t g;
+    } RGB;
+
+    virtual uint16_t init_ports() { return 0; };
+
+protected:
+
+    bool hw_init(void) override;
+
+    bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
+
+private:
+    uint16_t enable_mask;
+
+    // perdiodic tick to re-init
+    uint32_t    _last_init_ms;
+
+    // periodic callback
+    void timer();
+
+    HAL_Semaphore _sem;
+};