diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 5c0edadfac..4c438b9127 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -23,6 +23,7 @@
#include "PCA9685LED_I2C.h"
#include "NCP5623.h"
#include "OreoLED_PX4.h"
+#include "OreoLED_I2C.h"
#include "RCOutputRGBLed.h"
#include "ToneAlarm.h"
#include "ToshibaLED_I2C.h"
@@ -136,14 +137,14 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
// @User: Advanced
AP_GROUPINFO("DISPLAY_TYPE", 3, AP_Notify, _display_type, 0),
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3
+#if !HAL_MINIMIZE_FEATURES
// @Param: OREO_THEME
// @DisplayName: OreoLED Theme
// @Description: Enable/Disable Solo Oreo LED driver, 0 to disable, 1 for Aircraft theme, 2 for Rover theme
// @Values: 0:Disabled,1:Aircraft,2:Rover
// @User: Advanced
AP_GROUPINFO("OREO_THEME", 4, AP_Notify, _oreo_theme, 0),
-#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3
+#endif
#if !defined(HAL_BUZZER_PIN)
// @Param: BUZZ_PIN
@@ -261,7 +262,11 @@ void AP_Notify::add_backends(void)
if (_oreo_theme) {
ADD_BACKEND(new OreoLED_PX4(_oreo_theme));
}
-#endif // (CONFIG_HAL_BOARD == HAL_BOARD_PX4) && (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3)
+#elif !HAL_MINIMIZE_FEATURES
+ if (_oreo_theme) {
+ ADD_BACKEND(new OreoLED_I2C(0, _oreo_theme));
+ }
+#endif
break;
case Notify_LED_UAVCAN:
#if HAL_WITH_UAVCAN
diff --git a/libraries/AP_Notify/OreoLED_I2C.cpp b/libraries/AP_Notify/OreoLED_I2C.cpp
new file mode 100644
index 0000000000..40c716f1fa
--- /dev/null
+++ b/libraries/AP_Notify/OreoLED_I2C.cpp
@@ -0,0 +1,720 @@
+/*
+ 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 .
+ */
+/*
+ OreoLED I2C driver. Based primarily on ArduPilot OreoLED_PX4.cpp,
+ but with some components from orleod.cpp from px4 firmware
+*/
+
+#include
+#include
+#include
+
+#include
+#include "OreoLED_I2C.h"
+#include "AP_Notify.h"
+#include
+
+// OreoLEDs start at address 0x68 and add device number. So LED2 is at 0x6A
+#define OREOLED_BASE_I2C_ADDR 0x68
+
+#define OREOLED_BACKLEFT 0 // back left led instance number
+#define OREOLED_BACKRIGHT 1 // back right led instance number
+#define OREOLED_FRONTRIGHT 2 // front right led instance number
+#define OREOLED_FRONTLEFT 3 // front left led instance number
+#define PERIOD_SLOW 800 // slow flash rate
+#define PERIOD_FAST 500 // fast flash rate
+#define PERIOD_SUPER 150 // super fast rate
+#define PO_ALTERNATE 180 // 180 degree phase offset
+
+#define OREOLED_BOOT_CMD_BOOT_APP 0x60
+#define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2
+
+extern const AP_HAL::HAL& hal;
+
+// constructor
+OreoLED_I2C::OreoLED_I2C(uint8_t bus, uint8_t theme):
+ NotifyDevice(),
+ _bus(bus),
+ _oreo_theme(theme)
+{
+}
+
+//
+// Initialize the LEDs
+//
+bool OreoLED_I2C::init()
+{
+ // first look for led on external bus
+ _dev = std::move(hal.i2c_mgr->get_device(_bus, OREOLED_BASE_I2C_ADDR));
+ if (!_dev) {
+ return false;
+ }
+
+ // register timer
+ _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&OreoLED_I2C::update_timer, void));
+
+ // return health
+ return true;
+}
+
+// UPDATE device according to timed_updated. Called at 50Hz
+void OreoLED_I2C::update()
+{
+ if (slow_counter()) {
+ return; // slow rate from 50hz to 10hz
+ }
+
+ sync_counter(); // syncronizes LEDs every 10 seconds
+
+ if (mode_firmware_update()) {
+ return; // don't go any further if the Pixhawk is in firmware update
+ }
+
+ if (mode_init()) {
+ return; // don't go any further if the Pixhawk is initializing
+ }
+
+ if (mode_failsafe_radio()) {
+ return; // don't go any further if the Pixhawk is is in radio failsafe
+ }
+
+ set_standard_colors();
+
+ if (mode_failsafe_batt()) {
+ return; // stop here if the battery is low.
+ }
+
+ if (_pattern_override) {
+ return; // stop here if in mavlink LED control override.
+ }
+
+ if (mode_auto_flight()) {
+ return; // stop here if in an autopilot mode.
+ }
+
+ mode_pilot_flight(); // stop here if in an pilot controlled mode.
+}
+
+// Slow the update rate from 50hz to 10hz
+// Returns true if counting up
+// Returns false and resets one counter hits 5
+bool OreoLED_I2C::slow_counter()
+{
+ _slow_count++;
+ if (_slow_count < 5) {
+ return true;
+ } else {
+ _slow_count = 0;
+ return false;
+ }
+}
+
+
+// Calls resyncing the LEDs every 10 seconds
+// Always returns false, no action needed.
+void OreoLED_I2C::sync_counter()
+{
+ _sync_count++;
+ if (_sync_count > 100) {
+ _sync_count = 0;
+ send_sync();
+ }
+}
+
+
+// Procedure for when Pixhawk is in FW update / bootloader
+// Makes all LEDs go into color cycle mode
+// Returns true if firmware update in progress. False if not
+bool OreoLED_I2C::mode_firmware_update()
+{
+ if (AP_Notify::flags.firmware_update) {
+ set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_COLOUR_CYCLE);
+ return true;
+ } else {
+ return false;
+ }
+}
+
+
+// Makes all LEDs rapidly strobe blue while gyros initialize.
+bool OreoLED_I2C::mode_init()
+{
+ if (AP_Notify::flags.initialising) {
+ set_rgb(OREOLED_INSTANCE_ALL, OREOLED_PATTERN_STROBE, 0, 0, 255,0,0,0,PERIOD_SUPER,0);
+ return true;
+ } else {
+ return false;
+ }
+}
+
+
+// Procedure for when Pixhawk is in radio failsafe
+// LEDs perform alternating Red X pattern
+bool OreoLED_I2C::mode_failsafe_radio()
+{
+ if (AP_Notify::flags.failsafe_radio) {
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SLOW,0);
+ }
+ return AP_Notify::flags.failsafe_radio;
+}
+
+
+// Procedure to set standard rear LED colors in aviation theme
+// Back LEDS White for normal, yellow for GPS not usable, purple for EKF bad]
+// Returns true GPS or EKF problem, returns false if all ok
+bool OreoLED_I2C::set_standard_colors()
+{
+ if (!(AP_Notify::flags.gps_fusion)) {
+ _rear_color_r = 255;
+ _rear_color_g = 50;
+ _rear_color_b = 0;
+ return true;
+
+ } else if (AP_Notify::flags.ekf_bad) {
+ _rear_color_r = 255;
+ _rear_color_g = 0;
+ _rear_color_b = 255;
+ return true;
+
+ } else {
+ _rear_color_r = 255;
+ _rear_color_g = 255;
+ _rear_color_b = 255;
+ return false;
+ }
+}
+
+
+// Procedure to set low battery LED output
+// Colors standard
+// Fast strobe alternating front/back
+bool OreoLED_I2C::mode_failsafe_batt()
+{
+ if (AP_Notify::flags.failsafe_battery) {
+
+ switch (_oreo_theme) {
+ case OreoLED_Aircraft:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 0, 255, 0,0,0,0,PERIOD_FAST,0);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,PO_ALTERNATE);
+ break;
+
+ case OreoLED_Automobile:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE);
+ break;
+
+ default:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_FAST,0);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_FAST,PO_ALTERNATE);
+ break;
+ }
+ }
+ return AP_Notify::flags.failsafe_battery;
+}
+
+
+// Procedure for when Pixhawk is in an autopilot mode
+// Makes all LEDs strobe super fast using standard colors
+bool OreoLED_I2C::mode_auto_flight()
+{
+ switch (_oreo_theme) {
+
+ case OreoLED_Aircraft:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 0, 255, 0,0,0,0,PERIOD_SUPER,0);
+ if ((AP_Notify::flags.pre_arm_check && AP_Notify::flags.pre_arm_gps_check) || AP_Notify::flags.armed) {
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_SUPER,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_SUPER,PO_ALTERNATE);
+ } else {
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b);
+ }
+ break;
+
+ case OreoLED_Automobile:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE);
+ break;
+
+ default:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_STROBE, 255, 255, 255,0,0,0,PERIOD_SUPER,0);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, 255, 0, 0,0,0,0,PERIOD_SUPER,PO_ALTERNATE);
+ break;
+ }
+
+ return AP_Notify::flags.autopilot_mode;
+}
+
+
+// Procedure for when Pixhawk is in a pilot controlled mode
+// All LEDs use standard pattern and colors
+bool OreoLED_I2C::mode_pilot_flight()
+{
+ switch (_oreo_theme) {
+
+ case OreoLED_Aircraft:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_SOLID, 255, 0, 0);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_SOLID, 0, 255, 0);
+ if ((AP_Notify::flags.pre_arm_check && AP_Notify::flags.pre_arm_gps_check) || AP_Notify::flags.armed) {
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,0);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_STROBE, _rear_color_r, _rear_color_g, _rear_color_b,0,0,0,PERIOD_FAST,PO_ALTERNATE);
+ } else {
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, _rear_color_r, _rear_color_g, _rear_color_b);
+ }
+ break;
+
+ case OreoLED_Automobile:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_SOLID, 255, 255, 255);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_SOLID, 255, 255, 255);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, 255, 0, 0);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, 255, 0, 0);
+ break;
+
+ default:
+ set_rgb(OREOLED_FRONTLEFT, OREOLED_PATTERN_SOLID, 255, 255, 255);
+ set_rgb(OREOLED_FRONTRIGHT, OREOLED_PATTERN_SOLID, 255, 255, 255);
+ set_rgb(OREOLED_BACKLEFT, OREOLED_PATTERN_SOLID, 255, 0, 0);
+ set_rgb(OREOLED_BACKRIGHT, OREOLED_PATTERN_SOLID, 255, 0, 0);
+ break;
+ }
+
+ return true;
+}
+
+
+// set_rgb - Solid color settings only
+void OreoLED_I2C::set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue)
+{
+ set_rgb(instance, OREOLED_PATTERN_SOLID, red, green, blue);
+}
+
+
+// set_rgb - Set a color and selected pattern.
+void OreoLED_I2C::set_rgb(uint8_t instance, oreoled_pattern pattern, uint8_t red, uint8_t green, uint8_t blue)
+{
+ // get semaphore
+ WITH_SEMAPHORE(_sem);
+
+ // check for all instances
+ if (instance == OREOLED_INSTANCE_ALL) {
+ // store desired rgb for all LEDs
+ for (uint8_t i=0; iset_address(OREOLED_BASE_I2C_ADDR + cmd.led_num);
+
+ /* Calculate XOR CRC and append to the i2c write data */
+ uint8_t cmd_xor = OREOLED_BASE_I2C_ADDR + cmd.led_num;
+
+ for (uint8_t i = 0; i < cmd.num_bytes; i++) {
+ cmd_xor ^= cmd.buff[i];
+ }
+ cmd.buff[cmd.num_bytes++] = cmd_xor;
+
+ uint8_t reply[3] {};
+ bool ret = _dev->transfer(cmd.buff, cmd.num_bytes, reply, sizeof(reply));
+ //printf("command[%u] %02x %02x %02x %s -> %02x %02x %02x\n", cmd.led_num, ret?"OK":"fail", reply[0], reply[1], reply[2]);
+ return ret;
+}
+
+/*
+ send boot command to all LEDs
+ */
+void OreoLED_I2C::boot_leds(void)
+{
+ oreoled_cmd_t cmd;
+ for (uint8_t i=0; i 100) {
+ // send boot command 20 times
+ _boot_count++;
+ _last_boot_ms = now;
+ boot_leds();
+ }
+
+ // for each LED
+ for (uint8_t i=0; i> 8;
+ cmd.buff[15] = (_state_desired[i].period & 0x00FF);
+ cmd.buff[16] = OREOLED_PARAM_PHASEOFFSET;
+ cmd.buff[17] = (_state_desired[i].phase_offset & 0xFF00) >> 8;
+ cmd.buff[18] = (_state_desired[i].phase_offset & 0x00FF);
+ cmd.num_bytes = 19;
+ command_send(cmd);
+ break;
+ }
+
+ case OREOLED_MODE_SYNC: {
+ /* set I2C address to zero */
+ _dev->set_address(0);
+
+ /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/
+ uint8_t msg[] = {0x01, 0x00};
+
+ /* send I2C command */
+ _dev->transfer(msg, sizeof(msg), nullptr, 0);
+ break;
+ }
+
+ default:
+ break;
+ };
+ // save state change
+ _state_sent[i] = _state_desired[i];
+ }
+ }
+
+ // flag updates sent
+ _send_required = false;
+}
+
+
+// Handle an LED_CONTROL mavlink message
+void OreoLED_I2C::handle_led_control(mavlink_message_t *msg)
+{
+ // 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;
+ clear_state();
+ return;
+ }
+
+ if (packet.pattern == LED_CONTROL_PATTERN_CUSTOM) {
+ // Here we handle two different "sub commands",
+ // depending on the bytes in the first CUSTOM_HEADER_LENGTH
+ // of the custom pattern byte buffer
+
+ // Return if we don't have at least CUSTOM_HEADER_LENGTH bytes
+ if (packet.custom_len < CUSTOM_HEADER_LENGTH) {
+ return;
+ }
+
+ // check for the RGB0 sub-command
+ if (memcmp(packet.custom_bytes, "RGB0", CUSTOM_HEADER_LENGTH) == 0) {
+ // check to make sure the total length matches the length of the RGB0 command + data values
+ if (packet.custom_len != CUSTOM_HEADER_LENGTH + 4) {
+ return;
+ }
+
+ // check for valid pattern id
+ if (packet.custom_bytes[CUSTOM_HEADER_LENGTH] >= OREOLED_PATTERN_ENUM_COUNT) {
+ return;
+ }
+
+ // convert the first byte after the command to a oreoled_pattern
+ oreoled_pattern pattern = (oreoled_pattern)packet.custom_bytes[CUSTOM_HEADER_LENGTH];
+
+ // call the set_rgb function, using the rest of the bytes as the RGB values
+ set_rgb(packet.instance, pattern, packet.custom_bytes[CUSTOM_HEADER_LENGTH + 1], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 2], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 3]);
+
+ } else if (memcmp(packet.custom_bytes, "RGB1", CUSTOM_HEADER_LENGTH) == 0) { // check for the RGB1 sub-command
+
+ // check to make sure the total length matches the length of the RGB1 command + data values
+ if (packet.custom_len != CUSTOM_HEADER_LENGTH + 11) {
+ return;
+ }
+
+ // check for valid pattern id
+ if (packet.custom_bytes[CUSTOM_HEADER_LENGTH] >= OREOLED_PATTERN_ENUM_COUNT) {
+ return;
+ }
+
+ // convert the first byte after the command to a oreoled_pattern
+ oreoled_pattern pattern = (oreoled_pattern)packet.custom_bytes[CUSTOM_HEADER_LENGTH];
+
+ // uint16_t values are stored in custom_bytes in little endian order
+ // assume the flight controller is little endian when decoding values
+ uint16_t period =
+ ((0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 7]) << 8) |
+ (0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 8]);
+ uint16_t phase_offset =
+ ((0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 9]) << 8) |
+ (0x00FF & (uint16_t)packet.custom_bytes[CUSTOM_HEADER_LENGTH + 10]);
+
+ // call the set_rgb function, using the rest of the bytes as the RGB values
+ set_rgb(packet.instance, pattern, packet.custom_bytes[CUSTOM_HEADER_LENGTH + 1], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 2],
+ packet.custom_bytes[CUSTOM_HEADER_LENGTH + 3], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 4], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 5],
+ packet.custom_bytes[CUSTOM_HEADER_LENGTH + 6], period, phase_offset);
+ } else if (memcmp(packet.custom_bytes, "SYNC", CUSTOM_HEADER_LENGTH) == 0) { // check for the SYNC sub-command
+ // check to make sure the total length matches the length of the SYN0 command + data values
+ if (packet.custom_len != CUSTOM_HEADER_LENGTH + 0) {
+ return;
+ }
+ send_sync();
+ } else { // unrecognized command
+ return;
+ }
+ } else {
+ // other patterns sent as macro
+ set_macro(packet.instance, (oreoled_macro)packet.pattern);
+ }
+ _pattern_override = packet.pattern;
+}
+
+OreoLED_I2C::oreo_state::oreo_state()
+{
+ clear_state();
+}
+
+void OreoLED_I2C::oreo_state::clear_state()
+{
+ mode = OREOLED_MODE_NONE;
+ pattern = OREOLED_PATTERN_OFF;
+ macro = OREOLED_PARAM_MACRO_RESET;
+ red = 0;
+ green = 0;
+ blue = 0;
+ amplitude_red = 0;
+ amplitude_green = 0;
+ amplitude_blue = 0;
+ period = 0;
+ repeat = 0;
+ phase_offset = 0;
+}
+
+void OreoLED_I2C::oreo_state::send_sync()
+{
+ clear_state();
+ mode = OREOLED_MODE_SYNC;
+}
+
+void OreoLED_I2C::oreo_state::set_macro(oreoled_macro new_macro)
+{
+ clear_state();
+ mode = OREOLED_MODE_MACRO;
+ macro = new_macro;
+}
+
+void OreoLED_I2C::oreo_state::set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, uint8_t new_blue)
+{
+ clear_state();
+ mode = OREOLED_MODE_RGB;
+ pattern = new_pattern;
+ red = new_red;
+ green = new_green;
+ blue = new_blue;
+}
+
+void OreoLED_I2C::oreo_state::set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green,
+ uint8_t new_blue, uint8_t new_amplitude_red, uint8_t new_amplitude_green, uint8_t new_amplitude_blue,
+ uint16_t new_period, uint16_t new_phase_offset)
+{
+ clear_state();
+ mode = OREOLED_MODE_RGB_EXTENDED;
+ pattern = new_pattern;
+ red = new_red;
+ green = new_green;
+ blue = new_blue;
+ amplitude_red = new_amplitude_red;
+ amplitude_green = new_amplitude_green;
+ amplitude_blue = new_amplitude_blue;
+ period = new_period;
+ phase_offset = new_phase_offset;
+}
+
+bool OreoLED_I2C::oreo_state::operator==(const OreoLED_I2C::oreo_state &os)
+{
+ return ((os.mode==mode) && (os.pattern==pattern) && (os.macro==macro) && (os.red==red) && (os.green==green) && (os.blue==blue)
+ && (os.amplitude_red==amplitude_red) && (os.amplitude_green==amplitude_green) && (os.amplitude_blue==amplitude_blue)
+ && (os.period==period) && (os.repeat==repeat) && (os.phase_offset==phase_offset));
+}
diff --git a/libraries/AP_Notify/OreoLED_I2C.h b/libraries/AP_Notify/OreoLED_I2C.h
new file mode 100644
index 0000000000..4d72ac41e6
--- /dev/null
+++ b/libraries/AP_Notify/OreoLED_I2C.h
@@ -0,0 +1,202 @@
+/*
+ OreoLED I2C driver
+
+ 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
+#include "NotifyDevice.h"
+
+#define OREOLED_NUM_LEDS 4 // maximum number of individual LEDs connected to the oreo led cpu
+#define OREOLED_INSTANCE_ALL 0xff // instance number to indicate all LEDs (used for set_rgb and set_macro)
+#define OREOLED_BRIGHT 0xff // maximum brightness when flying (disconnected from usb)
+
+#define CUSTOM_HEADER_LENGTH 4 // number of bytes in the custom LED buffer that are used to identify the command
+
+class OreoLED_I2C : public NotifyDevice {
+public:
+ // constuctor
+ OreoLED_I2C(uint8_t bus, uint8_t theme);
+
+ // init - initialised the device
+ bool init(void) override;
+
+ // update - updates device according to timed_updated. Should be
+ // called at 50Hz
+ void update() override;
+
+ // handle a LED_CONTROL message, by default device ignore message
+ void handle_led_control(mavlink_message_t *msg) override;
+
+private:
+ enum oreoled_pattern {
+ OREOLED_PATTERN_OFF = 0,
+ OREOLED_PATTERN_SINE = 1,
+ OREOLED_PATTERN_SOLID = 2,
+ OREOLED_PATTERN_SIREN = 3,
+ OREOLED_PATTERN_STROBE = 4,
+ OREOLED_PATTERN_FADEIN = 5,
+ OREOLED_PATTERN_FADEOUT = 6,
+ OREOLED_PATTERN_PARAMUPDATE = 7,
+ OREOLED_PATTERN_ENUM_COUNT
+ };
+
+ /* enum of available macros defined by hardware */
+ enum oreoled_macro {
+ OREOLED_PARAM_MACRO_RESET = 0,
+ OREOLED_PARAM_MACRO_COLOUR_CYCLE = 1,
+ OREOLED_PARAM_MACRO_BREATH = 2,
+ OREOLED_PARAM_MACRO_STROBE = 3,
+ OREOLED_PARAM_MACRO_FADEIN = 4,
+ OREOLED_PARAM_MACRO_FADEOUT = 5,
+ OREOLED_PARAM_MACRO_RED = 6,
+ OREOLED_PARAM_MACRO_GREEN = 7,
+ OREOLED_PARAM_MACRO_BLUE = 8,
+ OREOLED_PARAM_MACRO_YELLOW = 9,
+ OREOLED_PARAM_MACRO_WHITE = 10,
+ OREOLED_PARAM_MACRO_AUTOMOBILE = 11,
+ OREOLED_PARAM_MACRO_AVIATION = 12,
+ OREOLED_PARAM_MACRO_ENUM_COUNT
+ };
+
+ /* enum passed to OREOLED_SET_MODE defined by hardware */
+ enum oreoled_param {
+ OREOLED_PARAM_BIAS_RED = 0,
+ OREOLED_PARAM_BIAS_GREEN = 1,
+ OREOLED_PARAM_BIAS_BLUE = 2,
+ OREOLED_PARAM_AMPLITUDE_RED = 3,
+ OREOLED_PARAM_AMPLITUDE_GREEN = 4,
+ OREOLED_PARAM_AMPLITUDE_BLUE = 5,
+ OREOLED_PARAM_PERIOD = 6,
+ OREOLED_PARAM_REPEAT = 7,
+ OREOLED_PARAM_PHASEOFFSET = 8,
+ OREOLED_PARAM_MACRO = 9,
+ OREOLED_PARAM_RESET = 10,
+ OREOLED_PARAM_APP_CHECKSUM = 11,
+ OREOLED_PARAM_ENUM_COUNT
+ };
+
+ // update_timer - called by scheduler and updates PX4 driver with commands
+ void update_timer(void);
+
+ // set_rgb - set color as a combination of red, green and blue values for one or all LEDs, pattern defaults to solid color
+ void set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue);
+
+ // set_rgb - set color as a combination of red, green and blue values for one or all LEDs, using the specified pattern
+ void set_rgb(uint8_t instance, enum oreoled_pattern pattern, uint8_t red, uint8_t green, uint8_t blue);
+
+ // set_rgb - set color as a combination of red, green and blue values for one or all LEDs, using the specified pattern and other parameters
+ void set_rgb(uint8_t instance, oreoled_pattern pattern, uint8_t red, uint8_t green, uint8_t blue,
+ uint8_t amplitude_red, uint8_t amplitude_green, uint8_t amplitude_blue,
+ uint16_t period, uint16_t phase_offset);
+
+ // set_macro - set macro for one or all LEDs
+ void set_macro(uint8_t instance, enum oreoled_macro macro);
+
+ // send_sync - force a syncronisation of the all LED's
+ void send_sync();
+
+ // functions to set LEDs to specific patterns. These functions return true if no further updates should be made to LEDs this iteration
+ bool slow_counter(void);
+ void sync_counter(void);
+ bool mode_firmware_update(void);
+ bool mode_init(void);
+ bool mode_failsafe_radio(void);
+ bool set_standard_colors(void);
+ bool mode_failsafe_batt(void);
+ bool mode_auto_flight(void);
+ bool mode_pilot_flight(void);
+
+ // Clear the desired state
+ void clear_state(void);
+
+ // oreo led modes (pattern, macro or rgb)
+ enum oreoled_mode {
+ OREOLED_MODE_NONE=0,
+ OREOLED_MODE_MACRO,
+ OREOLED_MODE_RGB,
+ OREOLED_MODE_RGB_EXTENDED,
+ OREOLED_MODE_SYNC
+ };
+
+ // Oreo LED modes
+ enum Oreo_LED_Theme {
+ OreoLED_Disabled = 0,
+ OreoLED_Aircraft = 1,
+ OreoLED_Automobile = 2,
+ };
+
+ // oreo_state structure holds possible state of an led
+ struct oreo_state {
+ enum oreoled_mode mode;
+ enum oreoled_pattern pattern;
+ enum oreoled_macro macro;
+ uint8_t red;
+ uint8_t green;
+ uint8_t blue;
+ uint8_t amplitude_red;
+ uint8_t amplitude_green;
+ uint8_t amplitude_blue;
+ uint16_t period;
+ int8_t repeat;
+ uint16_t phase_offset;
+
+ oreo_state();
+
+ void clear_state();
+
+ void send_sync();
+
+ void set_macro(oreoled_macro new_macro);
+
+ void set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, uint8_t new_blue);
+
+ void set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green,
+ uint8_t new_blue, uint8_t new_amplitude_red, uint8_t new_amplitude_green, uint8_t new_amplitude_blue,
+ uint16_t new_period, uint16_t new_phase_offset);
+
+ bool operator==(const oreo_state &os);
+ };
+
+ typedef struct {
+ uint8_t led_num;
+ uint8_t num_bytes;
+ uint8_t buff[32];
+ } oreoled_cmd_t;
+
+
+ // send a I2C command
+ bool command_send(oreoled_cmd_t &cmd);
+ void boot_leds(void);
+
+ // private members
+ uint8_t _bus;
+ HAL_Semaphore_Recursive _sem;
+ AP_HAL::OwnPtr _dev;
+ bool _send_required; // true when we need to send an update to at least one led
+ 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
+ uint8_t _oreo_theme; // theme (1=AirCraft, 2=Ground Vehicle)
+ uint8_t _rear_color_r = 255; // the rear LED red value
+ uint8_t _rear_color_g = 255; // the rear LED green value
+ uint8_t _rear_color_b = 255; // the rear LED blue value
+
+ uint8_t _slow_count;
+ uint8_t _sync_count = 80;
+ uint8_t _boot_count;
+ uint32_t _last_boot_ms;
+};
+