From aa602b5e1b6202e76ba30c41b9562c8179e36cf9 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 2 Apr 2019 10:56:00 -0700 Subject: [PATCH] AP_Notify: add NeoPixel driver --- libraries/AP_Notify/AP_Notify.cpp | 6 +- libraries/AP_Notify/AP_Notify.h | 1 + libraries/AP_Notify/NeoPixel.cpp | 141 ++++++++++++++++++++++++++++++ libraries/AP_Notify/NeoPixel.h | 68 ++++++++++++++ 4 files changed, 215 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Notify/NeoPixel.cpp create mode 100644 libraries/AP_Notify/NeoPixel.h diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 0a6283a2a8..eb506e29df 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -21,6 +21,7 @@ #include "Display.h" #include "ExternalLED.h" #include "PCA9685LED_I2C.h" +#include "NeoPixel.h" #include "NCP5623.h" #include "OreoLED_I2C.h" #include "RCOutputRGBLed.h" @@ -144,7 +145,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 + // @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 // @User: Advanced AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, BUILD_DEFAULT_LED_TYPE), @@ -251,6 +252,9 @@ void AP_Notify::add_backends(void) case Notify_LED_PCA9685LED_I2C_External: ADD_BACKEND(new PCA9685LED_I2C()); break; + case Notify_LED_NeoPixel: + ADD_BACKEND(new NeoPixel()); + break; case Notify_LED_OreoLED: #if !HAL_MINIMIZE_FEATURES if (_oreo_theme) { diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 93bf77ff24..4c8e885218 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -69,6 +69,7 @@ public: Notify_LED_UAVCAN = (1 << 5), // UAVCAN RGB LED 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_MAX }; diff --git a/libraries/AP_Notify/NeoPixel.cpp b/libraries/AP_Notify/NeoPixel.cpp new file mode 100644 index 0000000000..5d4f3b454b --- /dev/null +++ b/libraries/AP_Notify/NeoPixel.cpp @@ -0,0 +1,141 @@ +/* + 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 . + */ + +#include "AP_Notify/AP_Notify.h" +#include "NeoPixel.h" +#include "SRV_Channel/SRV_Channel.h" + +// This limit is from the dshot driver rcout groups limit +#define AP_NOTIFY_NEOPIXEL_MAX_INSTANCES 4 + +// Datasheet: https://cdn-shop.adafruit.com/datasheets/WS2812B.pdf +// 24bit msg as 3 byte GRB (not RGB) where first bit is G7, and last bit is B0 +// (first) G7|G6|G5|G4|G3|G2|G1|G0|R7|R6|R5|R4|R3|R2|R1|R0|B7|B6|B5|B4|B3|B2|B1|B0 (last) + +#define NEOPIXEL_LED_LOW 0x33 +#define NEOPIXEL_LED_MEDIUM 0x7F +#define NEOPIXEL_LED_HIGH 0xFF +#define NEOPIXEL_LED_OFF 0x00 + +extern const AP_HAL::HAL& hal; + +NeoPixel::NeoPixel() : + RGBLed(NEOPIXEL_LED_OFF, NEOPIXEL_LED_HIGH, NEOPIXEL_LED_MEDIUM, NEOPIXEL_LED_LOW) +{ +} + +bool NeoPixel::hw_init() +{ + NeoPixel::init_ports(); + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&NeoPixel::timer, void)); + return true; +} + +uint16_t NeoPixel::init_ports() +{ + static uint16_t last_mask = 0; + uint16_t mask = 0; + for (uint16_t i=0; iset_output_mode(mask, AP_HAL::RCOutput::MODE_NEOPIXEL); + } + 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 = NeoPixel::init_ports(); + } + if (enable_mask == 0) { + // nothing is enabled, no pins set as LED output + return; + } + + +#if NEOPIXEL_WHITE_STROBE + if (now_ms - _white_long_ms >= 2000) { + //start white light + _white_long_ms = now_ms; + _white_short_ms = now_ms; // start 100ms WHITE pulse + hw_set_rgb(0xFF,0xFF,0xFF); + } else if (_white_short_ms > 0 && now_ms - _white_short_ms >= 100) { + // stop white light + _white_short_ms = 0; + hw_set_rgb(_last_rgb.r, _last_rgb.g, _last_rgb.b); + } +#endif +} + +bool NeoPixel::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + // always rememebr this even when disabled so when we enable it will show correct color + NeoPixel::RGB value {}; + value.r = red; + value.g = green; + value.b = blue; + +#if NEOPIXEL_WHITE_STROBE + if (_white_short_ms == 0) { + // if not during a WHITE BLINK then record last LED + _last_rgb.rgb = value.rgb; + } +#endif + + if (enable_mask == 0) { + // nothing is enabled, no pins set as LED output + return true; + } + + NeoPixel::write_LED(value); + return true; +} + +void NeoPixel::write_LED(NeoPixel::RGB value) +{ + for (uint16_t i=0; i= AP_NOTIFY_NEOPIXEL_MAX_INSTANCES) { + return; + } + hal.rcout->set_neopixel_rgb_data(instance, value.rgb); +} + +void NeoPixel::write_LED(uint16_t instance, uint8_t red, uint8_t green, uint8_t blue) +{ + NeoPixel::RGB value {}; + value.r = red; + value.g = green; + value.b = blue; + + NeoPixel::write_LED(instance, value); +} diff --git a/libraries/AP_Notify/NeoPixel.h b/libraries/AP_Notify/NeoPixel.h new file mode 100644 index 0000000000..a0bdcb361e --- /dev/null +++ b/libraries/AP_Notify/NeoPixel.h @@ -0,0 +1,68 @@ +/* + 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 . + */ +#pragma once + +#include "RGBLed.h" +#include + +#define NEOPIXEL_WHITE_STROBE 0 + +class NeoPixel: public RGBLed { +public: + NeoPixel(); + + typedef union { + struct PACKED { + // **NOTE** These are GRB, not RGB + uint8_t b; + uint8_t r; + uint8_t g; + uint8_t unused; + }; + uint32_t rgb; + } RGB; + + static void write_LED(NeoPixel::RGB value); + static void write_LED(uint16_t instance, NeoPixel::RGB value); + static void write_LED(uint16_t instance, uint8_t red, uint8_t green, uint8_t blue); + static 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; + + // perdiodic tick to re-init + uint32_t _last_init_ms; + + // periodic callback + void timer(); + + HAL_Semaphore_Recursive _sem; + + +#if NEOPIXEL_WHITE_STROBE + // remember last RGB so we can resume after a white pulse + RGB _last_rgb; + uint32_t _white_long_ms; + uint32_t _white_short_ms; +#endif + + + +};