Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: support dual I2c toshiba LEDs 

This restructures AP_Notify to allow for multiple backends of the same type.
This commit is contained in:
Andrew Tridgell 2017-09-06 20:12:36 +10:00 committed by Lucas De Marchi
parent efc42edabe
commit 697131e94a
5 changed files with 138 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

207
libraries/AP_Notify/AP_Notify.cpp
View File

@ -34,7 +34,10 @@
AP_Notify *AP_Notify::_instance;
#define CONFIG_NOTIFY_DEVICES_COUNT 5
#define CONFIG_NOTIFY_DEVICES_MAX 6
#define TOSHIBA_LED_I2C_BUS_INTERNAL 0
#define TOSHIBA_LED_I2C_BUS_EXTERNAL 1
// table of user settable parameters
const AP_Param::GroupInfo AP_Notify::var_info[] = {
@ -91,100 +94,114 @@ AP_Notify::AP_Notify()
struct AP_Notify::notify_flags_and_values_type AP_Notify::flags;
struct AP_Notify::notify_events_type AP_Notify::events;
NotifyDevice *AP_Notify::_devices[] = {nullptr, nullptr, nullptr, nullptr, nullptr};
NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_MAX];
uint8_t AP_Notify::_num_devices;
#define ADD_BACKEND(backend) do { _devices[_num_devices++] = backend; if (_num_devices >= CONFIG_NOTIFY_DEVICES_MAX) return;} while(0)
// add notify backends to _devices array
void AP_Notify::add_backends(void)
{
if (_num_devices != 0) {
return;
}
// Notify devices for PX4 boards
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3 // Has enough memory for Oreo LEDs
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL));
ADD_BACKEND(new ToneAlarm_PX4());
ADD_BACKEND(new Display());
// Oreo LED enable/disable by NTF_OREO_THEME parameter
if (_oreo_theme) {
ADD_BACKEND(new OreoLED_PX4(_oreo_theme));
}
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 // Has its own LED board
ADD_BACKEND(new PixRacerLED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new ToneAlarm_PX4());
ADD_BACKEND(new Display());
#else // All other px4 boards use standard devices.
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new ToneAlarm_PX4());
ADD_BACKEND(new Display());
#endif
// Notify devices for VRBRAIN boards
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 // Uses px4 LED board
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new ToneAlarm_PX4());
ADD_BACKEND(new ExternalLED());
#else
ADD_BACKEND(new VRBoard_LED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new ToneAlarm_PX4());
ADD_BACKEND(new ExternalLED());
#endif
// Notify devices for linux boards
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
ADD_BACKEND(new NavioLED_I2C());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
ADD_BACKEND(new DiscreteRGBLed(4, 27, 6, false));
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new Buzzer());
ADD_BACKEND(new Display());
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new Display());
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new ToneAlarm_Linux());
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
ADD_BACKEND(new RCOutputRGBLedOff(15, 13, 14, 255));
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
ADD_BACKEND(new AP_BoardLED());
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE));
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
ADD_BACKEND(new DiscoLED());
ADD_BACKEND(new ToneAlarm_Linux());
#else // other linux
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new ToneAlarm_Linux());
#endif
#else
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
#endif
}
// initialisation
void AP_Notify::init(bool enable_external_leds)
{
// Notify devices for PX4 boards
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3 // Has enough memory for Oreo LEDs
_devices[0] = new AP_BoardLED();
_devices[1] = new ToshibaLED_I2C();
_devices[2] = new ToneAlarm_PX4();
_devices[3] = new Display();
// Oreo LED enable/disable by NTF_OREO_THEME parameter
if (_oreo_theme) {
_devices[4] = new OreoLED_PX4(_oreo_theme);
}
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 // Has its own LED board
_devices[0] = new PixRacerLED();
_devices[1] = new ToshibaLED_I2C();
_devices[2] = new ToneAlarm_PX4();
_devices[3] = new Display();
#else // All other px4 boards use standard devices.
_devices[0] = new AP_BoardLED();
_devices[1] = new ToshibaLED_I2C();
_devices[2] = new ToneAlarm_PX4();
_devices[3] = new Display();
#endif
// Notify devices for VRBRAIN boards
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 // Uses px4 LED board
_devices[0] = new AP_BoardLED();
_devices[1] = new ToshibaLED_I2C();
_devices[2] = new ToneAlarm_PX4();
_devices[3] = new ExternalLED();
#else
_devices[0] = new VRBoard_LED();
_devices[1] = new ToshibaLED_I2C();
_devices[2] = new ToneAlarm_PX4();
_devices[3] = new ExternalLED();
#endif
// Notify devices for linux boards
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
_devices[0] = new NavioLED_I2C();
_devices[1] = new ToshibaLED_I2C();
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
_devices[0] = new DiscreteRGBLed(4, 27, 6, false);
_devices[1] = new ToshibaLED_I2C();
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
_devices[0] = new AP_BoardLED();
_devices[1] = new Buzzer();
_devices[2] = new Display();
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
_devices[0] = new AP_BoardLED();
_devices[1] = new Display();
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
_devices[0] = new ToshibaLED_I2C();
_devices[1] = new ToneAlarm_Linux();
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
_devices[0] = new RCOutputRGBLedOff(15, 13, 14, 255);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
_devices[0] = new AP_BoardLED();
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
_devices[0] = new AP_BoardLED();
_devices[1] = new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
_devices[0] = new DiscoLED();
_devices[1] = new ToneAlarm_Linux();
#else
_devices[0] = new AP_BoardLED();
_devices[1] = new ToshibaLED_I2C();
_devices[2] = new ToneAlarm_Linux();
#endif
#else
_devices[0] = new AP_BoardLED();
_devices[1] = new ToshibaLED_I2C();
#endif
// add all the backends
add_backends();
// clear all flags and events
memset(&AP_Notify::flags, 0, sizeof(AP_Notify::flags));
@ -197,7 +214,7 @@ void AP_Notify::init(bool enable_external_leds)
AP_Notify::flags.external_leds = enable_external_leds;
for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
for (uint8_t i = 0; i < _num_devices; i++) {
if (_devices[i] != nullptr) {
_devices[i]->pNotify = this;
_devices[i]->init();
@ -208,7 +225,7 @@ void AP_Notify::init(bool enable_external_leds)
// main update function, called at 50Hz
void AP_Notify::update(void)
{
for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
for (uint8_t i = 0; i < _num_devices; i++) {
if (_devices[i] != nullptr) {
_devices[i]->update();
}
@ -221,7 +238,7 @@ void AP_Notify::update(void)
// handle a LED_CONTROL message
void AP_Notify::handle_led_control(mavlink_message_t *msg)
{
for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
for (uint8_t i = 0; i < _num_devices; i++) {
if (_devices[i] != nullptr) {
_devices[i]->handle_led_control(msg);
}
@ -231,7 +248,7 @@ void AP_Notify::handle_led_control(mavlink_message_t *msg)
// handle a PLAY_TUNE message
void AP_Notify::handle_play_tune(mavlink_message_t *msg)
{
for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
for (uint8_t i = 0; i < _num_devices; i++) {
if (_devices[i] != nullptr) {
_devices[i]->handle_play_tune(msg);
}

4
libraries/AP_Notify/AP_Notify.h
View File

@ -115,6 +115,9 @@ public:
// initialisation
void init(bool enable_external_leds);
// add all backends
void add_backends(void);
/// update - allow updates of leds that cannot be updated during a timed interrupt
void update(void);
@ -152,4 +155,5 @@ private:
char _flight_mode_str[5];
static NotifyDevice* _devices[];
static uint8_t _num_devices;
};

34
libraries/AP_Notify/ToshibaLED_I2C.cpp
View File

@ -27,46 +27,48 @@
extern const AP_HAL::HAL& hal;
#define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address
#define TOSHIBA_LED_I2C_BUS_INTERNAL 0
#define TOSHIBA_LED_I2C_BUS_EXTERNAL 1
#define TOSHIBA_LED_PWM0 0x01 // pwm0 register
#define TOSHIBA_LED_PWM1 0x02 // pwm1 register
#define TOSHIBA_LED_PWM2 0x03 // pwm2 register
#define TOSHIBA_LED_ENABLE 0x04 // enable register
bool ToshibaLED_I2C::hw_init()
ToshibaLED_I2C::ToshibaLED_I2C(uint8_t bus)
: ToshibaLED()
, _bus(bus)
{
}
bool ToshibaLED_I2C::hw_init(void)
{
// first look for led on external bus
_dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS_EXTERNAL, TOSHIBA_LED_I2C_ADDR));
_dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR));
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev->set_retries(10);
// enable the led
bool ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);
// on failure try the internal bus
if (!ret) {
// give back external bus semaphore
_dev->get_semaphore()->give();
// get internal I2C bus driver
_dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS_INTERNAL, TOSHIBA_LED_I2C_ADDR));
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);
return false;
}
// update the red, green and blue values to zero
uint8_t val[4] = { TOSHIBA_LED_PWM0, _led_off, _led_off, _led_off };
ret &= _dev->transfer(val, sizeof(val), nullptr, 0);
ret = _dev->transfer(val, sizeof(val), nullptr, 0);
_dev->set_retries(1);
// give back i2c semaphore
_dev->get_semaphore()->give();
_dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void));
if (ret) {
_dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void));
}
return ret;
}

3
libraries/AP_Notify/ToshibaLED_I2C.h
View File

@ -21,6 +21,8 @@
class ToshibaLED_I2C : public ToshibaLED
{
public:
ToshibaLED_I2C(uint8_t bus);
protected:
bool hw_init(void) override;
bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
@ -32,4 +34,5 @@ private:
struct {
uint8_t r, g, b;
} rgb;
uint8_t _bus;
};

2
libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.cpp
View File

@ -9,7 +9,7 @@ void blink();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static ToshibaLED_I2C toshiba_led;
static ToshibaLED_I2C toshiba_led(1);
void setup(void)
{