From d40011acf303572d78e38881b4411749131a57d6 Mon Sep 17 00:00:00 2001 From: Staroselskii Georgii Date: Fri, 14 Nov 2014 17:14:40 +0300 Subject: [PATCH] AP_Notify: added abstract Led and RGBLed classes --- libraries/AP_Notify/AP_BoardLED.cpp | 3 +- libraries/AP_Notify/AP_BoardLED.h | 5 +- libraries/AP_Notify/AP_Notify.cpp | 77 +++--- libraries/AP_Notify/AP_Notify.h | 36 +-- libraries/AP_Notify/Buzzer.cpp | 5 +- libraries/AP_Notify/Buzzer.h | 6 +- libraries/AP_Notify/ExternalLED.cpp | 5 +- libraries/AP_Notify/ExternalLED.h | 5 +- libraries/AP_Notify/Led.cpp | 0 libraries/AP_Notify/Led.h | 37 +++ libraries/AP_Notify/NavioLED.cpp | 29 +++ libraries/AP_Notify/NavioLED.h | 31 +++ libraries/AP_Notify/NavioLED_I2C.cpp | 88 +++++++ libraries/AP_Notify/NavioLED_I2C.h | 29 +++ libraries/AP_Notify/NotifyDevice.h | 14 ++ libraries/AP_Notify/RGBLed.cpp | 315 +++++++++++++++++++++++++ libraries/AP_Notify/RGBLed.h | 63 +++++ libraries/AP_Notify/ToneAlarm_Linux.h | 4 +- libraries/AP_Notify/ToneAlarm_PX4.h | 4 +- libraries/AP_Notify/ToshibaLED.cpp | 287 +--------------------- libraries/AP_Notify/ToshibaLED.h | 36 +-- libraries/AP_Notify/ToshibaLED_I2C.cpp | 2 +- 22 files changed, 697 insertions(+), 384 deletions(-) create mode 100644 libraries/AP_Notify/Led.cpp create mode 100644 libraries/AP_Notify/Led.h create mode 100644 libraries/AP_Notify/NavioLED.cpp create mode 100644 libraries/AP_Notify/NavioLED.h create mode 100644 libraries/AP_Notify/NavioLED_I2C.cpp create mode 100644 libraries/AP_Notify/NavioLED_I2C.h create mode 100644 libraries/AP_Notify/NotifyDevice.h create mode 100644 libraries/AP_Notify/RGBLed.cpp create mode 100644 libraries/AP_Notify/RGBLed.h diff --git a/libraries/AP_Notify/AP_BoardLED.cpp b/libraries/AP_Notify/AP_BoardLED.cpp index 244f600b88..045d6faf44 100644 --- a/libraries/AP_Notify/AP_BoardLED.cpp +++ b/libraries/AP_Notify/AP_BoardLED.cpp @@ -19,7 +19,7 @@ extern const AP_HAL::HAL& hal; -void AP_BoardLED::init(void) +bool AP_BoardLED::init(void) { // setup the main LEDs as outputs hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, HAL_GPIO_OUTPUT); @@ -30,6 +30,7 @@ void AP_BoardLED::init(void) hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF); hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF); hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF); + return true; } /* diff --git a/libraries/AP_Notify/AP_BoardLED.h b/libraries/AP_Notify/AP_BoardLED.h index 3a368a388a..0c331af4f9 100644 --- a/libraries/AP_Notify/AP_BoardLED.h +++ b/libraries/AP_Notify/AP_BoardLED.h @@ -20,6 +20,7 @@ #include #include +#include "Led.h" #define HIGH 1 #define LOW 0 @@ -70,11 +71,11 @@ #error "Unknown board type in AP_Notify" #endif -class AP_BoardLED +class AP_BoardLED: public Led { public: // initialise the LED driver - void init(void); + bool init(void); // should be called at 50Hz void update(void); diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index ce4a71725c..85cbcfe8fa 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -19,52 +19,53 @@ // static flags, to allow for direct class update from device drivers struct AP_Notify::notify_type AP_Notify::flags; +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + AP_BoardLED boardled; + ToshibaLED_PX4 toshibaled; + ToneAlarm_PX4 tonealarm; + NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled, &tonealarm}; +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 + AP_BoardLED boardled; + ExternalLED externalled; + Buzzer buzzer; + NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &externalled, &buzzer}; +#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + Buzzer buzzer; + AP_BoardLED boardled; + ToshibaLED_I2C toshibaled; + ExternalLED externalled; + NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled, &externalled, &buzzer}; +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO + AP_BoardLED boardled; + NavioLED_I2C navioled; + NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &navioled}; + #else + AP_BoardLED boardled; + ToshibaLED_I2C toshibaled; + ToneAlarm_Linux tonealarm; + NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled, &tonealarm}; + #endif +#else + AP_BoardLED boardled; + ToshibaLED_I2C toshibaled; + NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled}; +#endif + // initialisation void AP_Notify::init(bool enable_external_leds) { AP_Notify::flags.external_leds = enable_external_leds; - boardled.init(); - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - toshibaled.init(); - tonealarm.init(); -#endif -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX - toshibaled.init(); - tonealarm.init(); -#endif -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 - externalled.init(); - buzzer.init(); -#endif -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - toshibaled.init(); - externalled.init(); - buzzer.init(); -#endif + for (int i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { + _devices[i]->init(); + } } // main update function, called at 50Hz void AP_Notify::update(void) { - boardled.update(); - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - toshibaled.update(); - tonealarm.update(); -#endif -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX - toshibaled.update(); - tonealarm.update(); -#endif -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 - externalled.update(); - buzzer.update(); -#endif -#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - toshibaled.update(); - externalled.update(); - buzzer.update(); -#endif + for (int i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { + _devices[i]->update(); + } } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index a49aa837f8..b6696f9a24 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -25,9 +25,26 @@ #include #include #include +#include #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + #define CONFIG_NOTIFY_DEVICES_COUNT 3 +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 + #define CONFIG_NOTIFY_DEVICES_COUNT 3 +#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + #define CONFIG_NOTIFY_DEVICES_COUNT 4 +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO + #define CONFIG_NOTIFY_DEVICES_COUNT 2 + #else + #define CONFIG_NOTIFY_DEVICES_COUNT 3 + #endif +#else + #define CONFIG_NOTIFY_DEVICES_COUNT 2 +#endif + class AP_Notify { public: @@ -63,24 +80,7 @@ public: void update(void); private: - // individual drivers - AP_BoardLED boardled; -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - ToshibaLED_PX4 toshibaled; - ToneAlarm_PX4 tonealarm; -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - ToshibaLED_I2C toshibaled; - ToneAlarm_Linux tonealarm; -#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 - ExternalLED externalled; - Buzzer buzzer; -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - ToshibaLED_I2C toshibaled; - ExternalLED externalled; - Buzzer buzzer; -#else - ToshibaLED_I2C toshibaled; -#endif + static NotifyDevice* _devices[CONFIG_NOTIFY_DEVICES_COUNT]; }; #endif // __AP_NOTIFY_H__ diff --git a/libraries/AP_Notify/Buzzer.cpp b/libraries/AP_Notify/Buzzer.cpp index fc89e4274b..5db1b2f4a1 100644 --- a/libraries/AP_Notify/Buzzer.cpp +++ b/libraries/AP_Notify/Buzzer.cpp @@ -23,11 +23,11 @@ extern const AP_HAL::HAL& hal; -void Buzzer::init() +bool Buzzer::init() { // return immediately if disabled if (!AP_Notify::flags.external_leds) { - return; + return false; } // setup the pin and ensure it's off @@ -38,6 +38,7 @@ void Buzzer::init() // warning in plane and rover on every boot _flags.armed = AP_Notify::flags.armed; _flags.failsafe_battery = AP_Notify::flags.failsafe_battery; + return true; } // update - updates led according to timed_updated. Should be called at 50Hz diff --git a/libraries/AP_Notify/Buzzer.h b/libraries/AP_Notify/Buzzer.h index c2d7cba03c..e46f1f7f68 100644 --- a/libraries/AP_Notify/Buzzer.h +++ b/libraries/AP_Notify/Buzzer.h @@ -30,7 +30,9 @@ #define BUZZER_ARMING_BUZZ_MS 3000 // arming buzz length in milliseconds (i.e. 3 seconds) -class Buzzer +#include "NotifyDevice.h" + +class Buzzer: public NotifyDevice { public: /// Constructor @@ -42,7 +44,7 @@ public: {} /// init - initialise the buzzer - void init(void); + bool init(void); /// update - updates buzzer according to timed_updated. Should be called at 50Hz void update(); diff --git a/libraries/AP_Notify/ExternalLED.cpp b/libraries/AP_Notify/ExternalLED.cpp index 540059707b..d6e076213f 100644 --- a/libraries/AP_Notify/ExternalLED.cpp +++ b/libraries/AP_Notify/ExternalLED.cpp @@ -20,11 +20,11 @@ extern const AP_HAL::HAL& hal; -void ExternalLED::init(void) +bool ExternalLED::init(void) { // return immediately if disabled if (!AP_Notify::flags.external_leds) { - return; + return false; } // setup the main LEDs as outputs @@ -38,6 +38,7 @@ void ExternalLED::init(void) hal.gpio->write(EXTERNAL_LED_GPS, HAL_GPIO_LED_OFF); hal.gpio->write(EXTERNAL_LED_MOTOR1, HAL_GPIO_LED_OFF); hal.gpio->write(EXTERNAL_LED_MOTOR2, HAL_GPIO_LED_OFF); + return true; } /* diff --git a/libraries/AP_Notify/ExternalLED.h b/libraries/AP_Notify/ExternalLED.h index e4a037fe51..1f991d624d 100644 --- a/libraries/AP_Notify/ExternalLED.h +++ b/libraries/AP_Notify/ExternalLED.h @@ -21,6 +21,7 @@ #include #include #include +#include "Led.h" #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 #define EXTERNAL_LED_ARMED 61 // Armed LED - AN7 @@ -44,14 +45,14 @@ #define EXTERNAL_LED_MOTOR2 0 #endif -class ExternalLED +class ExternalLED: public Led { public: // constructor ExternalLED() : _counter(0), _counter2(0), _pattern(NONE), _pattern_counter(0) {} // initialise the LED driver - void init(void); + bool init(void); // should be called at 50Hz void update(void); diff --git a/libraries/AP_Notify/Led.cpp b/libraries/AP_Notify/Led.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Notify/Led.h b/libraries/AP_Notify/Led.h new file mode 100644 index 0000000000..c62828ae37 --- /dev/null +++ b/libraries/AP_Notify/Led.h @@ -0,0 +1,37 @@ +/* + * AP_Notify Library. + * based upon a prototype library by David "Buzz" Bussenschutt. + */ + +/* + 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 . + */ + +#ifndef __LED_H__ +#define __LED_H__ + +#include +#include "NotifyDevice.h" + +class Led: public NotifyDevice { +public: + virtual ~Led() {} + // init - initialised the LED + virtual bool init(void) = 0; + // update - updates led according to timed_updated. Should be + // called at 50Hz + virtual void update() = 0; +}; + +#endif //__LED_H__ diff --git a/libraries/AP_Notify/NavioLED.cpp b/libraries/AP_Notify/NavioLED.cpp new file mode 100644 index 0000000000..8d799c829b --- /dev/null +++ b/libraries/AP_Notify/NavioLED.cpp @@ -0,0 +1,29 @@ +/* + NavioLED 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. + +*/ + +#include "NavioLED.h" + +#define NAVIO_LED_BRIGHT 0x0 // full brightness +#define NAVIO_LED_MEDIUM 0x7F // medium brightness +#define NAVIO_LED_DIM 0x4F // dim brightness +#define NAVIO_LED_OFF 0xFF // off + +NavioLED::NavioLED() : + RGBLed(NAVIO_LED_OFF, NAVIO_LED_BRIGHT, NAVIO_LED_MEDIUM, NAVIO_LED_DIM) +{ + +} diff --git a/libraries/AP_Notify/NavioLED.h b/libraries/AP_Notify/NavioLED.h new file mode 100644 index 0000000000..027b9557d2 --- /dev/null +++ b/libraries/AP_Notify/NavioLED.h @@ -0,0 +1,31 @@ +/* + * AP_Notify Library. + * based upon a prototype library by David "Buzz" Bussenschutt. + */ + +/* + 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 . + */ + +#ifndef __NAVIO_LED_H__ +#define __NAVIO_LED_H__ + +#include "RGBLed.h" + +class NavioLED: public RGBLed { +public: + NavioLED(); +}; + +#endif diff --git a/libraries/AP_Notify/NavioLED_I2C.cpp b/libraries/AP_Notify/NavioLED_I2C.cpp new file mode 100644 index 0000000000..8d72c7800f --- /dev/null +++ b/libraries/AP_Notify/NavioLED_I2C.cpp @@ -0,0 +1,88 @@ +/* + NavioLED 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 . + */ + +#include +#include "NavioLED_I2C.h" + +#define PCA9685_ADDRESS 0x40 +#define PCA9685_PWM 0x6 + +extern const AP_HAL::HAL& hal; + +bool NavioLED_I2C::hw_init() +{ + // get pointer to i2c bus semaphore + AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); + + // take i2c bus sempahore + if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + + // disable recording of i2c lockup errors + hal.i2c->ignore_errors(true); + + // enable the led + bool ret = true; + + // re-enable recording of i2c lockup errors + hal.i2c->ignore_errors(false); + + // give back i2c semaphore + i2c_sem->give(); + + return ret; +} + +// set_rgb - set color as a combination of red, green and blue values +bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + // get pointer to i2c bus semaphore + AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); + + // exit immediately if we can't take the semaphore + if (i2c_sem == NULL || !i2c_sem->take(5)) { + return false; + } + + uint16_t red_adjusted = red * 0x10; + uint16_t green_adjusted = green * 0x10; + uint16_t blue_adjusted = blue * 0x10; + + uint8_t blue_channel_lsb = blue_adjusted & 0xFF; + uint8_t blue_channel_msb = blue_adjusted >> 8; + + uint8_t green_channel_lsb = green_adjusted & 0xFF; + uint8_t green_channel_msb = green_adjusted >> 8; + + uint8_t red_channel_lsb = red_adjusted & 0xFF; + uint8_t red_channel_msb = red_adjusted >> 8; + + + uint8_t transaction[] = {0x00, 0x00, blue_channel_lsb, blue_channel_msb, + 0x00, 0x00, green_channel_lsb, green_channel_msb, + 0x00, 0x00, red_channel_lsb, red_channel_msb + }; + + + bool success = (hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_PWM, sizeof(transaction), transaction) == 0); + + // give back i2c semaphore + i2c_sem->give(); + return success; +} diff --git a/libraries/AP_Notify/NavioLED_I2C.h b/libraries/AP_Notify/NavioLED_I2C.h new file mode 100644 index 0000000000..59e7b1f695 --- /dev/null +++ b/libraries/AP_Notify/NavioLED_I2C.h @@ -0,0 +1,29 @@ +/* + NavioLED 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 . + */ +#ifndef __NAVIO_LED_I2C_H__ +#define __NAVIO_LED_I2C_H__ + +#include "NavioLED.h" + +class NavioLED_I2C : public NavioLED +{ +protected: + virtual bool hw_init(void); + virtual bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b); +}; + +#endif // __TOSHIBA_LED_I2C_H__ diff --git a/libraries/AP_Notify/NotifyDevice.h b/libraries/AP_Notify/NotifyDevice.h new file mode 100644 index 0000000000..0264755c0c --- /dev/null +++ b/libraries/AP_Notify/NotifyDevice.h @@ -0,0 +1,14 @@ +#ifndef __NOTIFYDEVICE_H__ +#define __NOTIFYDEVICE_H__ + +class NotifyDevice { +public: + virtual ~NotifyDevice() {} + // init - initialised the device + virtual bool init(void) = 0; + // update - updates device according to timed_updated. Should be + // called at 50Hz + virtual void update() = 0; +}; + +#endif diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp new file mode 100644 index 0000000000..4afbf2aad6 --- /dev/null +++ b/libraries/AP_Notify/RGBLed.cpp @@ -0,0 +1,315 @@ +/* + Generic RGBLed 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. + +*/ + + +#include +#include +#include "Led.h" +#include "AP_Notify.h" + +extern const AP_HAL::HAL& hal; + +RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim): + _led_off(led_off), + _led_bright(led_bright), + _led_medium(led_medium), + _led_dim(led_dim) + +{ + +} + +bool RGBLed::init() +{ + _healthy = hw_init(); + return _healthy; +} + +// set_rgb - set color as a combination of red, green and blue values +void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + // return immediately if not enabled + if (!_healthy) { + return; + } + + if (red != _red_curr || + green != _green_curr || + blue != _blue_curr) { + // call the hardware update routine + if (hw_set_rgb(red, green, blue)) { + _red_curr = red; + _green_curr = green; + _blue_curr = blue; + } + } +} + + +// _scheduled_update - updates _red, _green, _blue according to notify flags +void RGBLed::update_colours(void) +{ + uint8_t brightness = _led_bright; + // slow rate from 50Hz to 10hz + counter++; + if (counter < 5) { + return; + } + + // reset counter + counter = 0; + + // move forward one step + step++; + if (step >= 10) { + step = 0; + } + + // use dim light when connected through USB + if (hal.gpio->usb_connected()) { + brightness = _led_dim; + } + + // initialising pattern + if (AP_Notify::flags.initialising) { + if (step & 1) { + // odd steps display red light + _red_des = brightness; + _blue_des = _led_off; + _green_des = _led_off; + } else { + // even display blue light + _red_des = _led_off; + _blue_des = brightness; + _green_des = _led_off; + } + + // exit so no other status modify this pattern + return; + } + + // save trim and esc calibration pattern + if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) { + switch(step) { + case 0: + case 3: + case 6: + // red on + _red_des = brightness; + _blue_des = _led_off; + _green_des = _led_off; + break; + + case 1: + case 4: + case 7: + // blue on + _red_des = _led_off; + _blue_des = brightness; + _green_des = _led_off; + break; + + case 2: + case 5: + case 8: + // green on + _red_des = _led_off; + _blue_des = _led_off; + _green_des = brightness; + break; + + case 9: + // all off + _red_des = _led_off; + _blue_des = _led_off; + _green_des = _led_off; + break; + } + // exit so no other status modify this pattern + return; + } + + // radio and battery failsafe patter: flash yellow + // gps failsafe pattern : flashing yellow and blue + // baro glitching pattern : flashing yellow and purple + // ekf_bad pattern : flashing yellow and red + if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery || + AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching || + AP_Notify::flags.baro_glitching || + AP_Notify::flags.ekf_bad) { + switch(step) { + case 0: + case 1: + case 2: + case 3: + case 4: + // yellow on + _red_des = brightness; + _blue_des = _led_off; + _green_des = brightness; + break; + case 5: + case 6: + case 7: + case 8: + case 9: + if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) { + // blue on for gps failsafe or glitching + _red_des = _led_off; + _blue_des = brightness; + _green_des = _led_off; + } else if (AP_Notify::flags.baro_glitching) { + // purple on if baro glitching + _red_des = brightness; + _blue_des = brightness; + _green_des = _led_off; + } else if (AP_Notify::flags.ekf_bad) { + // red on if ekf bad + _red_des = brightness; + _blue_des = _led_off; + _green_des = _led_off; + }else{ + // all off for radio or battery failsafe + _red_des = _led_off; + _blue_des = _led_off; + _green_des = _led_off; + } + break; + } + // exit so no other status modify this pattern + return; + } + + // solid green or flashing green if armed + if (AP_Notify::flags.armed) { + // solid green if armed with GPS 3d lock + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { + _red_des = _led_off; + _blue_des = _led_off; + _green_des = brightness; + }else{ + // solid blue if armed with no GPS lock + _red_des = _led_off; + _blue_des = brightness; + _green_des = _led_off; + } + return; + }else{ + // double flash yellow if failing pre-arm checks + if (!AP_Notify::flags.pre_arm_check) { + switch(step) { + case 0: + case 1: + case 4: + case 5: + // yellow on + _red_des = brightness; + _blue_des = _led_off; + _green_des = brightness; + break; + case 2: + case 3: + case 6: + case 7: + case 8: + case 9: + // all off + _red_des = _led_off; + _blue_des = _led_off; + _green_des = _led_off; + break; + } + }else{ + // flashing green if disarmed with GPS 3d lock + // flashing blue if disarmed with no gps lock + switch(step) { + case 0: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = brightness; + } + break; + case 1: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = _led_off; + } + break; + case 2: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = brightness; + } + break; + case 3: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = _led_off; + } + break; + case 4: + _red_des = _led_off; + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { + // flashing green if disarmed with GPS 3d lock + _blue_des = _led_off; + _green_des = brightness; + }else{ + // flashing blue if disarmed with no gps lock + _blue_des = brightness; + _green_des = _led_off; + } + break; + case 5: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = _led_off; + } + break; + + case 6: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = brightness; + } + break; + + case 7: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = _led_off; + } + break; + case 8: + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + _green_des = brightness; + } + break; + case 9: + // all off + _red_des = _led_off; + _blue_des = _led_off; + _green_des = _led_off; + break; + } + } + } +} + +// update - updates led according to timed_updated. Should be called +// at 50Hz +void RGBLed::update() +{ + // return immediately if not enabled + if (!_healthy) { + return; + } + update_colours(); + set_rgb(_red_des, _green_des, _blue_des); +} diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h new file mode 100644 index 0000000000..5795f03ca8 --- /dev/null +++ b/libraries/AP_Notify/RGBLed.h @@ -0,0 +1,63 @@ +/* + * AP_Notify Library. + * based upon a prototype library by David "Buzz" Bussenschutt. + */ + +/* + 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 . + */ + +#ifndef __RGBLED_H__ +#define __RGBLED_H__ + +#include +#include "Led.h" + +class RGBLed: public Led { +public: + RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim); + + // init - initialised the LED + virtual bool init(void); + + // healthy - returns true if the LED is operating properly + virtual bool healthy() { return _healthy; } + + // set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15 + virtual void set_rgb(uint8_t red, uint8_t green, uint8_t blue); + + // update - updates led according to timed_updated. Should be + // called at 50Hz + virtual void update(); + +protected: + // methods implemented in hardware specific classes + virtual bool hw_init(void) = 0; + virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0; + + // meta-data common to all hw devices + uint8_t counter; + uint8_t step; + bool _healthy; // true if the LED is operating properly + uint8_t _red_des, _green_des, _blue_des; // color requested by timed update + uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led + uint8_t _led_off; + uint8_t _led_bright; + uint8_t _led_medium; + uint8_t _led_dim; +private: + virtual void update_colours(); +}; + +#endif //__RGBLED_H__ diff --git a/libraries/AP_Notify/ToneAlarm_Linux.h b/libraries/AP_Notify/ToneAlarm_Linux.h index 28b0e7146d..b6dbf38074 100644 --- a/libraries/AP_Notify/ToneAlarm_Linux.h +++ b/libraries/AP_Notify/ToneAlarm_Linux.h @@ -20,7 +20,9 @@ #include "ToneAlarm_Linux.h" -class ToneAlarm_Linux +#include "NotifyDevice.h" + +class ToneAlarm_Linux: public NotifyDevice { public: ToneAlarm_Linux(): diff --git a/libraries/AP_Notify/ToneAlarm_PX4.h b/libraries/AP_Notify/ToneAlarm_PX4.h index 3d44ce2942..fd52d8a1d1 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.h +++ b/libraries/AP_Notify/ToneAlarm_PX4.h @@ -18,9 +18,9 @@ #ifndef __TONE_ALARM_PX4_H__ #define __TONE_ALARM_PX4_H__ -#include "ToneAlarm_PX4.h" +#include "NotifyDevice.h" -class ToneAlarm_PX4 +class ToneAlarm_PX4: public NotifyDevice { public: /// init - initialised the tone alarm diff --git a/libraries/AP_Notify/ToshibaLED.cpp b/libraries/AP_Notify/ToshibaLED.cpp index 1460c0538e..265d6cb193 100644 --- a/libraries/AP_Notify/ToshibaLED.cpp +++ b/libraries/AP_Notify/ToshibaLED.cpp @@ -17,288 +17,15 @@ along with this program. If not, see . */ -#include -#include #include "ToshibaLED.h" -#include "AP_Notify.h" -extern const AP_HAL::HAL& hal; +#define TOSHIBA_LED_BRIGHT 0xFF // full brightness +#define TOSHIBA_LED_MEDIUM 0x80 // medium brightness +#define TOSHIBA_LED_DIM 0x11 // dim +#define TOSHIBA_LED_OFF 0x00 // off -void ToshibaLED::init() +ToshibaLED::ToshibaLED(): + RGBLed(TOSHIBA_LED_OFF, TOSHIBA_LED_BRIGHT, TOSHIBA_LED_MEDIUM, TOSHIBA_LED_DIM) { - _healthy = hw_init(); -} - -// set_rgb - set color as a combination of red, green and blue values -void ToshibaLED::set_rgb(uint8_t red, uint8_t green, uint8_t blue) -{ - // return immediately if not enabled - if (!_healthy) { - return; - } - - if (red != _red_curr || - green != _green_curr || - blue != _blue_curr) { - // call the hardware update routine - if (hw_set_rgb(red, green, blue)) { - _red_curr = red; - _green_curr = green; - _blue_curr = blue; - } - } -} - -// _scheduled_update - updates _red, _green, _blue according to notify flags -void ToshibaLED::update_colours(void) -{ - uint8_t brightness = TOSHIBA_LED_BRIGHT; - // slow rate from 50Hz to 10hz - counter++; - if (counter < 5) { - return; - } - - // reset counter - counter = 0; - - // move forward one step - step++; - if (step >= 10) { - step = 0; - } - - // use dim light when connected through USB - if (hal.gpio->usb_connected()) { - brightness = TOSHIBA_LED_DIM; - } - - // initialising pattern - if (AP_Notify::flags.initialising) { - if (step & 1) { - // odd steps display red light - _red_des = brightness; - _blue_des = TOSHIBA_LED_OFF; - _green_des = TOSHIBA_LED_OFF; - } else { - // even display blue light - _red_des = TOSHIBA_LED_OFF; - _blue_des = brightness; - _green_des = TOSHIBA_LED_OFF; - } - - // exit so no other status modify this pattern - return; - } - - // save trim and esc calibration pattern - if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) { - switch(step) { - case 0: - case 3: - case 6: - // red on - _red_des = brightness; - _blue_des = TOSHIBA_LED_OFF; - _green_des = TOSHIBA_LED_OFF; - break; - - case 1: - case 4: - case 7: - // blue on - _red_des = TOSHIBA_LED_OFF; - _blue_des = brightness; - _green_des = TOSHIBA_LED_OFF; - break; - - case 2: - case 5: - case 8: - // green on - _red_des = TOSHIBA_LED_OFF; - _blue_des = TOSHIBA_LED_OFF; - _green_des = brightness; - break; - - case 9: - // all off - _red_des = TOSHIBA_LED_OFF; - _blue_des = TOSHIBA_LED_OFF; - _green_des = TOSHIBA_LED_OFF; - break; - } - // exit so no other status modify this pattern - return; - } - - // radio and battery failsafe patter: flash yellow - // gps failsafe pattern : flashing yellow and blue - // baro glitching pattern : flashing yellow and purple - // ekf_bad pattern : flashing yellow and red - if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery || - AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching || - AP_Notify::flags.baro_glitching || - AP_Notify::flags.ekf_bad) { - switch(step) { - case 0: - case 1: - case 2: - case 3: - case 4: - // yellow on - _red_des = brightness; - _blue_des = TOSHIBA_LED_OFF; - _green_des = brightness; - break; - case 5: - case 6: - case 7: - case 8: - case 9: - if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) { - // blue on for gps failsafe or glitching - _red_des = TOSHIBA_LED_OFF; - _blue_des = brightness; - _green_des = TOSHIBA_LED_OFF; - } else if (AP_Notify::flags.baro_glitching) { - // purple on if baro glitching - _red_des = brightness; - _blue_des = brightness; - _green_des = TOSHIBA_LED_OFF; - } else if (AP_Notify::flags.ekf_bad) { - // red on if ekf bad - _red_des = brightness; - _blue_des = TOSHIBA_LED_OFF; - _green_des = TOSHIBA_LED_OFF; - }else{ - // all off for radio or battery failsafe - _red_des = TOSHIBA_LED_OFF; - _blue_des = TOSHIBA_LED_OFF; - _green_des = TOSHIBA_LED_OFF; - } - break; - } - // exit so no other status modify this pattern - return; - } - - // solid green or flashing green if armed - if (AP_Notify::flags.armed) { - // solid green if armed with GPS 3d lock - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { - _red_des = TOSHIBA_LED_OFF; - _blue_des = TOSHIBA_LED_OFF; - _green_des = brightness; - }else{ - // solid blue if armed with no GPS lock - _red_des = TOSHIBA_LED_OFF; - _blue_des = brightness; - _green_des = TOSHIBA_LED_OFF; - } - return; - }else{ - // double flash yellow if failing pre-arm checks - if (!AP_Notify::flags.pre_arm_check) { - switch(step) { - case 0: - case 1: - case 4: - case 5: - // yellow on - _red_des = brightness; - _blue_des = TOSHIBA_LED_OFF; - _green_des = brightness; - break; - case 2: - case 3: - case 6: - case 7: - case 8: - case 9: - // all off - _red_des = TOSHIBA_LED_OFF; - _blue_des = TOSHIBA_LED_OFF; - _green_des = TOSHIBA_LED_OFF; - break; - } - }else{ - // flashing green if disarmed with GPS 3d lock - // flashing blue if disarmed with no gps lock - switch(step) { - case 0: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = brightness; - } - break; - case 1: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = TOSHIBA_LED_OFF; - } - break; - case 2: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = brightness; - } - break; - case 3: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = TOSHIBA_LED_OFF; - } - break; - case 4: - _red_des = TOSHIBA_LED_OFF; - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { - // flashing green if disarmed with GPS 3d lock - _blue_des = TOSHIBA_LED_OFF; - _green_des = brightness; - }else{ - // flashing blue if disarmed with no gps lock - _blue_des = brightness; - _green_des = TOSHIBA_LED_OFF; - } - break; - case 5: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = TOSHIBA_LED_OFF; - } - break; - - case 6: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = brightness; - } - break; - - case 7: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = TOSHIBA_LED_OFF; - } - break; - case 8: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { - _green_des = brightness; - } - break; - case 9: - // all off - _red_des = TOSHIBA_LED_OFF; - _blue_des = TOSHIBA_LED_OFF; - _green_des = TOSHIBA_LED_OFF; - break; - } - } - } -} - -// update - updates led according to timed_updated. Should be called -// at 50Hz -void ToshibaLED::update() -{ - // return immediately if not enabled - if (!_healthy) { - return; - } - update_colours(); - set_rgb(_red_des, _green_des, _blue_des); + } diff --git a/libraries/AP_Notify/ToshibaLED.h b/libraries/AP_Notify/ToshibaLED.h index 4d66e36204..9758241ca1 100644 --- a/libraries/AP_Notify/ToshibaLED.h +++ b/libraries/AP_Notify/ToshibaLED.h @@ -21,41 +21,11 @@ #ifndef __TOSHIBA_LED_H__ #define __TOSHIBA_LED_H__ -#include +#include "RGBLed.h" -#define TOSHIBA_LED_BRIGHT 0xFF // full brightness -#define TOSHIBA_LED_MEDIUM 0x80 // medium brightness -#define TOSHIBA_LED_DIM 0x11 // dim -#define TOSHIBA_LED_OFF 0x00 // off - -class ToshibaLED { +class ToshibaLED: public RGBLed { public: - - // init - initialised the LED - void init(void); - - // healthy - returns true if the LED is operating properly - bool healthy() { return _healthy; } - - // set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15 - void set_rgb(uint8_t red, uint8_t green, uint8_t blue); - - // update - updates led according to timed_updated. Should be - // called at 50Hz - void update(); - -protected: - // methods implemented in hardware specific classes - virtual bool hw_init(void) = 0; - virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0; - - // meta-data common to all hw devices - uint8_t counter; - uint8_t step; - bool _healthy; // true if the LED is operating properly - uint8_t _red_des, _green_des, _blue_des; // color requested by timed update - uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led - void update_colours(); + ToshibaLED(); }; #endif // __TOSHIBA_LED_H__ diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp index 5d01dffd49..f8d0a6b679 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.cpp +++ b/libraries/AP_Notify/ToshibaLED_I2C.cpp @@ -45,7 +45,7 @@ bool ToshibaLED_I2C::hw_init() bool ret = (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_ENABLE, 0x03) == 0); // update the red, green and blue values to zero - uint8_t val[3] = { TOSHIBA_LED_OFF, TOSHIBA_LED_OFF, TOSHIBA_LED_OFF }; + uint8_t val[3] = { _led_off, _led_off, _led_off }; ret &= (hal.i2c->writeRegisters(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, 3, val) == 0); // re-enable recording of i2c lockup errors