diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index b270f5f6c2..2ac2926eb6 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 "NavigatorLED.h" #include "NeoPixel.h" #include "NCP5623.h" #include "OreoLED_I2C.h" @@ -248,7 +249,7 @@ void AP_Notify::add_backends(void) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO ADD_BACKEND(new DiscoLED()); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR - ADD_BACKEND(new DiscreteRGBLed(HAL_RGBLED_RED, HAL_RGBLED_GREEN, HAL_RGBLED_BLUE, HAL_RGBLED_NORMAL_POLARITY)); + ADD_BACKEND(new NavigatorLED()); #endif #endif // CONFIG_HAL_BOARD == HAL_BOARD_LINUX diff --git a/libraries/AP_Notify/NavigatorLED.cpp b/libraries/AP_Notify/NavigatorLED.cpp new file mode 100644 index 0000000000..8c7b706aa1 --- /dev/null +++ b/libraries/AP_Notify/NavigatorLED.cpp @@ -0,0 +1,81 @@ +/* + 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 . + */ + +// This driver is for the neopixel on the Blue Robotics Navigator board +// designed for the Raspberry Pi 4. The neopixel data in is connected to +// the Raspberry Pi's MOSI pin. The clock, chip select, and MISO pins are +// not used. The data is sent to the neopixel in 24 'SPI bytes', where each +// spi byte is formatted to appear as a single bit of data to the neopixel. + +#include +#include "AP_Notify/AP_Notify.h" +#include "NavigatorLED.h" + +#define NEOPIXEL_LED_LOW 0x18 +#define NEOPIXEL_LED_MEDIUM 0x40 +#define NEOPIXEL_LED_HIGH 0xFF +#define NEOPIXEL_LED_OFF 0x00 + +#define LED_T0 0b11000000 +#define LED_T1 0b11110000 + +extern const AP_HAL::HAL& hal; + +NavigatorLED::NavigatorLED() : + RGBLed(NEOPIXEL_LED_OFF, NEOPIXEL_LED_HIGH, NEOPIXEL_LED_MEDIUM, NEOPIXEL_LED_LOW) +{ +} + +bool NavigatorLED::init() +{ + _dev = hal.spi->get_device("led"); + if (!_dev) { + return false; + } + return true; +} + +bool NavigatorLED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + if (!_dev) { + return false; + } + + // format our spi bytes to transmit the desired color data + _setup_data(red, green, blue); + + // take i2c bus sempahore + WITH_SEMAPHORE(_dev->get_semaphore()); + + // send the new color data + return _dev->transfer(_data, sizeof(_data), nullptr, 0); +} + +// Get our bytes ready to send the desired color data +// 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) +void NavigatorLED::_setup_data(uint8_t red, uint8_t green, uint8_t blue) +{ + for (uint8_t i = 0; i < 8; i++) { + _data[i] = (green & (1<<(7-i))) ? LED_T1 : LED_T0; + } + for (uint8_t i = 0; i < 8; i++) { + _data[8 + i] = (red & (1<<(7-i))) ? LED_T1 : LED_T0; + } + for (uint8_t i = 0; i < 8; i++) { + _data[16 + i] = (blue & (1<<(7-i))) ? LED_T1 : LED_T0; + } +} diff --git a/libraries/AP_Notify/NavigatorLED.h b/libraries/AP_Notify/NavigatorLED.h new file mode 100644 index 0000000000..9f93857eb1 --- /dev/null +++ b/libraries/AP_Notify/NavigatorLED.h @@ -0,0 +1,32 @@ +/* + 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 + +class NavigatorLED: public RGBLed { +public: + NavigatorLED(); + bool init(void) override; + +protected: + bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override; + +private: + uint8_t _data[24]; + void _setup_data(uint8_t red, uint8_t green, uint8_t blue); + AP_HAL::OwnPtr _dev; +};