zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Notify: added OreoLED_I2C driver 

this is for Solo on ChibiOS, but is usable with any board
mission-4.1.18
Andrew Tridgell 6 years ago
parent
deea50731d
commit
78dc575258
3 changed files with 930 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 11
      libraries/AP_Notify/AP_Notify.cpp
  2. 720
      libraries/AP_Notify/OreoLED_I2C.cpp
  3. 202
      libraries/AP_Notify/OreoLED_I2C.h

11
libraries/AP_Notify/AP_Notify.cpp
Unescape View File

@ -23,6 +23,7 @@ @@ -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[] = { @@ -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) @@ -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

720
libraries/AP_Notify/OreoLED_I2C.cpp
Unescape View File

@ -0,0 +1,720 @@ @@ -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 <http://www.gnu.org/licenses/>.
*/
/*
OreoLED I2C driver. Based primarily on ArduPilot OreoLED_PX4.cpp,
but with some components from orleod.cpp from px4 firmware
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_Common/Semaphore.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include "OreoLED_I2C.h"
#include "AP_Notify.h"
#include <utility>
// 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; i<OREOLED_NUM_LEDS; i++) {
_state_desired[i].set_rgb(pattern, red, green, blue);
if (!(_state_desired[i] == _state_sent[i])) {
_send_required = true;
}
}
} else if (instance < OREOLED_NUM_LEDS) {
// store desired rgb for one LED
_state_desired[instance].set_rgb(pattern, red, green, blue);
if (!(_state_desired[instance] == _state_sent[instance])) {
_send_required = true;
}
}
}
// set_rgb - Sets a color, pattern, and uses extended options for amplitude, period, and phase offset
void OreoLED_I2C::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)
{
WITH_SEMAPHORE(_sem);
// check for all instances
if (instance == OREOLED_INSTANCE_ALL) {
// store desired rgb for all LEDs
for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
_state_desired[i].set_rgb(pattern, red, green, blue, amplitude_red, amplitude_green, amplitude_blue, period, phase_offset);
if (!(_state_desired[i] == _state_sent[i])) {
_send_required = true;
}
}
} else if (instance < OREOLED_NUM_LEDS) {
// store desired rgb for one LED
_state_desired[instance].set_rgb(pattern, red, green, blue, amplitude_red, amplitude_green, amplitude_blue, period, phase_offset);
if (!(_state_desired[instance] == _state_sent[instance])) {
_send_required = true;
}
}
}
// set_macro - set macro for one or all LEDs
void OreoLED_I2C::set_macro(uint8_t instance, oreoled_macro macro)
{
WITH_SEMAPHORE(_sem);
// check for all instances
if (instance == OREOLED_INSTANCE_ALL) {
// store desired macro for all LEDs
for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
_state_desired[i].set_macro(macro);
if (!(_state_desired[i] == _state_sent[i])) {
_send_required = true;
}
}
} else if (instance < OREOLED_NUM_LEDS) {
// store desired macro for one LED
_state_desired[instance].set_macro(macro);
if (!(_state_desired[instance] == _state_sent[instance])) {
_send_required = true;
}
}
}
// send_sync - force a syncronisation of the all LED's
void OreoLED_I2C::send_sync()
{
WITH_SEMAPHORE(_sem);
// store desired macro for all LEDs
for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
_state_desired[i].send_sync();
if (!(_state_desired[i] == _state_sent[i])) {
_send_required = true;
}
}
}
// Clear the desired state
void OreoLED_I2C::clear_state(void)
{
WITH_SEMAPHORE(_sem);
for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
_state_desired[i].clear_state();
}
_send_required = false;
}
/*
send a command onto the I2C bus
*/
bool OreoLED_I2C::command_send(oreoled_cmd_t &cmd)
{
//printf("sending %u\n", cmd.num_bytes);
_dev->set_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<OREOLED_NUM_LEDS; i++) {
cmd.led_num = i;
cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP;
cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE;
cmd.buff[2] = OREOLED_BASE_I2C_ADDR + i;
cmd.num_bytes = 3;
command_send(cmd);
}
}
// update_timer - called by scheduler and updates PX4 driver with commands
void OreoLED_I2C::update_timer(void)
{
// exit immediately if send not required, or state is being updated
if (!_send_required) {
return;
}
WITH_SEMAPHORE(_sem);
uint32_t now = AP_HAL::millis();
if (_boot_count < 20 &&
now - _last_boot_ms > 100) {
// send boot command 20 times
_boot_count++;
_last_boot_ms = now;
boot_leds();
}
// for each LED
for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
// check for state change
if (true) {
switch (_state_desired[i].mode) {
case OREOLED_MODE_MACRO: {
oreoled_cmd_t cmd {};
cmd.led_num = i;
cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE;
cmd.buff[1] = OREOLED_PARAM_MACRO;
cmd.buff[2] = _state_desired[i].macro;
cmd.num_bytes = 3;
command_send(cmd);
break;
}
case OREOLED_MODE_RGB: {
oreoled_cmd_t cmd {};
cmd.led_num = i;
cmd.buff[0] = _state_desired[i].pattern;
cmd.buff[1] = OREOLED_PARAM_BIAS_RED;
cmd.buff[2] = _state_desired[i].red;
cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN;
cmd.buff[4] = _state_desired[i].green;
cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE;
cmd.buff[6] = _state_desired[i].blue;
cmd.num_bytes = 7;
command_send(cmd);
break;
}
case OREOLED_MODE_RGB_EXTENDED: {
oreoled_cmd_t cmd {};
cmd.led_num = i;
cmd.buff[0] = _state_desired[i].pattern;
cmd.buff[1] = OREOLED_PARAM_BIAS_RED;
cmd.buff[2] = _state_desired[i].red;
cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN;
cmd.buff[4] = _state_desired[i].green;
cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE;
cmd.buff[6] = _state_desired[i].blue;
cmd.buff[7] = OREOLED_PARAM_AMPLITUDE_RED;
cmd.buff[8] = _state_desired[i].amplitude_red;
cmd.buff[9] = OREOLED_PARAM_AMPLITUDE_GREEN;
cmd.buff[10] = _state_desired[i].amplitude_green;
cmd.buff[11] = OREOLED_PARAM_AMPLITUDE_BLUE;
cmd.buff[12] = _state_desired[i].amplitude_blue;
// Note: The Oreo LED controller expects to receive uint16 values
// in little endian order
cmd.buff[13] = OREOLED_PARAM_PERIOD;
cmd.buff[14] = (_state_desired[i].period & 0xFF00) >> 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));
}

202
libraries/AP_Notify/OreoLED_I2C.h
Unescape View File

@ -0,0 +1,202 @@ @@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#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<AP_HAL::I2CDevice> _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;
};
Write Preview
Loading…
Cancel
Save