From 54007854a9ac7f5f6ea651cc4173b684da2cf3e6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Aug 2013 12:17:29 +0900 Subject: [PATCH] Notify: add ToshibaLED --- libraries/AP_Notify/ToshibaLED.cpp | 346 ++++++++++++++++++ libraries/AP_Notify/ToshibaLED.h | 68 ++++ .../examples/ToshibaLED_test/Makefile | 1 + .../ToshibaLED_test/ToshibaLED_test.pde | 98 +++++ 4 files changed, 513 insertions(+) create mode 100644 libraries/AP_Notify/ToshibaLED.cpp create mode 100644 libraries/AP_Notify/ToshibaLED.h create mode 100644 libraries/AP_Notify/examples/ToshibaLED_test/Makefile create mode 100644 libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde diff --git a/libraries/AP_Notify/ToshibaLED.cpp b/libraries/AP_Notify/ToshibaLED.cpp new file mode 100644 index 0000000000..be06f6c251 --- /dev/null +++ b/libraries/AP_Notify/ToshibaLED.cpp @@ -0,0 +1,346 @@ +/* + * ToshibaLED Library. + * DIYDrones 2013 + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + */ + +#include +#include "ToshibaLED.h" + +extern const AP_HAL::HAL& hal; + +// static private variable instantiation +uint16_t ToshibaLED::_counter; +bool ToshibaLED::_enabled; // true if the led was initialised successfully +bool ToshibaLED::_healthy; // true if the led's latest update was successful +uint8_t ToshibaLED::_red_des; // desired redness of led +uint8_t ToshibaLED::_green_des; // desired greenness of led +uint8_t ToshibaLED::_blue_des; // desired blueness of led +uint8_t ToshibaLED::_red_curr; // current redness of led +uint8_t ToshibaLED::_green_curr; // current greenness of led +uint8_t ToshibaLED::_blue_curr; // current blueness of led + +// constructor +ToshibaLED::ToshibaLED() +{ +} + +void ToshibaLED::init() +{ + // default LED to healthy + _healthy = true; + + // 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)) { + _healthy = false; + return; + } + + // set i2c bus to low speed - it seems to work at high speed even though the datasheet doesn't say this + //hal.i2c->setHighSpeed(false); + + // enable the led + _healthy = (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_ENABLE, 0x03) == 0); + + // update the red, green and blue values to zero + _healthy &= (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, TOSHIBA_LED_OFF) == 0); + _healthy &= (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM1, TOSHIBA_LED_OFF) == 0); + _healthy &= (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM2, TOSHIBA_LED_OFF) == 0); + + // register update with scheduler + if (_healthy) { + hal.scheduler->register_timer_process( ToshibaLED::_scheduled_update ); + _enabled = true; + } + + // give back i2c semaphore + i2c_sem->give(); +} + +// on - turn LED on +void ToshibaLED::on() +{ + // return immediately if not enabled + if (!_enabled) { + return; + } + + // 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)) { + //_healthy = false; + return; + } + + // try writing to the register + _healthy = (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_ENABLE, 0x03) == 0); + + // give back i2c semaphore + i2c_sem->give(); +} + +// off - turn LED off +void ToshibaLED::off() +{ + // return immediately if not enabled + if (!_enabled) { + return; + } + + // 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)) { + //_healthy = false; + return; + } + + // try writing to the register + _healthy = (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_ENABLE, 0x00) == 0); + + // give back i2c semaphore + i2c_sem->give(); +} + +// 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 (!_enabled) { + return; + } + + bool success = true; + + // 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)) { + _healthy = false; + return; + } + + // update the green value + if (green != _green_curr) { + if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, green) == 0) { + _green_curr = green; + }else{ + success = false; + } + } + + // update the blue value + if (blue != _blue_curr) { + if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM1, blue) == 0) { + _blue_curr = blue; + }else{ + success = false; + } + } + + // update the red value + if (red != _red_curr) { + if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM2, red) == 0) { + _red_curr = red; + }else{ + success = false; + } + } + + // set healthy status + _healthy = success; + + // give back i2c semaphore + i2c_sem->give(); +} + +// _scheduled_update - updates _red, _green, _blue according to notify flags +void ToshibaLED::_scheduled_update(uint32_t now) +{ + _counter++; + + // we never want to update LEDs at a higher than 16Hz rate + if (_counter & 0x3F) { + return; + } + + // counter2 used to drop frequency down to 16hz + uint8_t counter2 = _counter >> 6; + + // initialising pattern + static uint8_t init_counter = 0; + init_counter++; + if (notify.flags.initialising) { + // blink LED red and blue alternatively + if (init_counter == 1) { + // turn on red + _red_des = TOSHIBA_LED_DIM; + _blue_des = 0; + _green_des = 0; + }else{ + // turn on blue + _red_des = 0; + _blue_des = TOSHIBA_LED_DIM; + _green_des = 0; + init_counter = 0; + } + return; + } + + // save trim pattern + if (notify.flags.save_trim) { + static uint8_t save_trim_counter = 0; + if ((counter2 & 0x2) == 0) { + save_trim_counter++; + } + switch(save_trim_counter) { + case 0: + _red_des = TOSHIBA_LED_DIM; + _blue_des = TOSHIBA_LED_OFF; + _green_des = TOSHIBA_LED_OFF; + break; + + case 1: + _red_des = TOSHIBA_LED_OFF; + _blue_des = TOSHIBA_LED_DIM; + _green_des = TOSHIBA_LED_OFF; + break; + + case 2: + _red_des = TOSHIBA_LED_OFF; + _blue_des = TOSHIBA_LED_OFF; + _green_des = TOSHIBA_LED_DIM; + break; + + default: + save_trim_counter = -1; + } + return; + } + + // armed and gps + static uint8_t arm_or_gps = 0; // 0 = displaying arming state, 1 = displaying gps state + // switch between showing arming and gps state every second + if (counter2 == 0) { + arm_or_gps = !arm_or_gps; + // turn off both red and blue + _red_des = TOSHIBA_LED_OFF; + _blue_des = TOSHIBA_LED_OFF; + _green_des = TOSHIBA_LED_OFF; + } + + // displaying arming state + if (arm_or_gps == 0) { + static uint8_t arm_counter = 0; + if (notify.flags.armed) { + // red led solid + _red_des = TOSHIBA_LED_DIM; + }else{ + if ((counter2 & 0x2) == 0) { + arm_counter++; + } + if (notify.flags.pre_arm_check) { + // passed pre-arm checks so slower single flash + switch(arm_counter) { + case 0: + case 1: + case 2: + _red_des = TOSHIBA_LED_DIM; + break; + case 3: + case 4: + case 5: + _red_des = TOSHIBA_LED_OFF; + break; + default: + // reset counter to restart the sequence + arm_counter = -1; + break; + } + }else{ + // failed pre-arm checks so double flash + switch(arm_counter) { + case 0: + case 1: + _red_des = TOSHIBA_LED_DIM; + break; + case 2: + _red_des = TOSHIBA_LED_OFF; + break; + case 3: + case 4: + _red_des = TOSHIBA_LED_DIM; + break; + case 5: + case 6: + _red_des = TOSHIBA_LED_OFF; + break; + default: + arm_counter = -1; + break; + } + } + } + }else{ + // gps light + switch (notify.flags.gps_status) { + case 0: + // no GPS attached + _blue_des = TOSHIBA_LED_OFF; + break; + + case 1: + // GPS attached but no lock, blink at 4Hz + if ((counter2 & 0x3) == 0) { + // toggle blue + if (_blue_des == TOSHIBA_LED_OFF) { + _blue_des = TOSHIBA_LED_DIM; + }else{ + _blue_des = TOSHIBA_LED_OFF; + } + } + break; + + case 2: + // GPS attached but 2D lock, blink more slowly (around 2Hz) + if ((counter2 & 0x7) == 0) { + // toggle blue + if (_blue_des == TOSHIBA_LED_OFF) { + _blue_des = TOSHIBA_LED_DIM; + }else{ + _blue_des = TOSHIBA_LED_OFF; + } + } + break; + + case 3: + // 3D lock so become solid + _blue_des = TOSHIBA_LED_DIM; + break; + } + } +} + +// update - updates led according to timed_updated. Should be called regularly from main loop +void ToshibaLED::update() +{ + // return immediately if not enabled + if (!_enabled) { + return; + } + + set_rgb(_red_des,_blue_des,_green_des); +} \ No newline at end of file diff --git a/libraries/AP_Notify/ToshibaLED.h b/libraries/AP_Notify/ToshibaLED.h new file mode 100644 index 0000000000..1733bf1ce7 --- /dev/null +++ b/libraries/AP_Notify/ToshibaLED.h @@ -0,0 +1,68 @@ +/* + * AP_HAL_AVR Notify Library. + * + * Copyright (c) 2013 David "Buzz" Bussenschutt. All right reserved. + * Rev 1.0 - 1st March 2013 + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + */ +#ifndef __TOSHIBA_LED_H__ +#define __TOSHIBA_LED_H__ + +#include +//#include "AP_HAL_AVR_Namespace.h" +#include + +#define TOSHIBA_LED_ADDRESS 0x55 // default I2C bus address +#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 + +#define TOSHIBA_LED_BRIGHT 0x0F // full brightness +#define TOSHIBA_LED_MEDIUM 0x0A // medium brightness +#define TOSHIBA_LED_DIM 0x01 // dim // was 0x05 +#define TOSHIBA_LED_OFF 0x00 // dim // was 0x05 + +class ToshibaLED { +public: + + // constructor + ToshibaLED(); + + // init - initialised the LED + static void init(void); + + // healthy - returns true if the LED is operating properly + static bool healthy() { return _healthy; } + + // on - turn LED on + static void on(); + + // off - turn LED off + static void off(); + + // set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15 + static void set_rgb(uint8_t red, uint8_t green, uint8_t blue); + + // update - updates led according to timed_updated. Should be called regularly from main loop + static void update(); + +private: + // private methods + static void _scheduled_update(uint32_t now); + + // private member variables + static AP_HAL::Semaphore* _i2c_sem; // pointer to i2c bus semaphore + static bool _enabled; // true if the LED is operating properly + static bool _healthy; // true if the LED is operating properly + static uint8_t _red_des, _green_des, _blue_des; // color requested by timed update + static uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led + static uint16_t _counter; // used to slow the update rate +}; + +#endif // __TOSHIBA_LED_H__ diff --git a/libraries/AP_Notify/examples/ToshibaLED_test/Makefile b/libraries/AP_Notify/examples/ToshibaLED_test/Makefile new file mode 100644 index 0000000000..f5daf25151 --- /dev/null +++ b/libraries/AP_Notify/examples/ToshibaLED_test/Makefile @@ -0,0 +1 @@ +include ../../../../mk/apm.mk diff --git a/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde b/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde new file mode 100644 index 0000000000..4373a3be18 --- /dev/null +++ b/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde @@ -0,0 +1,98 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include +#include +#include +#include // Notify library +#include + +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; + +ToshibaLED toshiba_led; +uint8_t led_state; +uint8_t red, green, blue; + +void setup(void) +{ + // display welcome message + hal.console->print_P(PSTR("Toshiba LED test ver 0.1\n")); + + // initialise LED + toshiba_led.init(); + + // check if healthy + if (!toshiba_led.healthy()) { + hal.console->print_P(PSTR("Failed to initialise Toshiba LED\n")); + } + + // turn on initialising notification + notify.flags.initialising = false; + notify.flags.save_trim = true; + notify.flags.gps_status = 1; + notify.flags.armed = 1; + notify.flags.pre_arm_check = 1; +} + +void loop(void) +{ + // blink test + //hal.console->print_P(PSTR("Blink test\n")); + //blink(); + /* + // full spectrum test + hal.console->print_P(PSTR("Spectrum test\n")); + full_spectrum(); + */ + + // update the toshiba led + toshiba_led.update(); + + // wait 1/100th of a second + hal.scheduler->delay(10); +} + +// full_spectrum - runs through the full spectrum of colours the led can display +void full_spectrum() +{ + // turn on led + toshiba_led.on(); + + // go through the full range of colours but only up to the dim light level + for (uint8_t red=0; red<=0x05; red++) { + for (uint8_t green=0; green<=0x05; green++) { + for (uint8_t blue=0; blue<=0x05; blue++) { + toshiba_led.set_rgb(red,green,blue); + hal.scheduler->delay(5); + } + } + } +} + +// blink - blink the led at 10hz for 10 seconds +void blink() +{ + // set colour to red + toshiba_led.set_rgb(TOSHIBA_LED_DIM,0,0); + + // full spectrum test + for (uint8_t c=0; c<=2; c++ ) { + if (c==0) { + toshiba_led.set_rgb(TOSHIBA_LED_DIM,0,0); // red + }else if (c==1) { + toshiba_led.set_rgb(0,TOSHIBA_LED_DIM,0); // green + }else{ + toshiba_led.set_rgb(0,0,TOSHIBA_LED_DIM); // blue + } + for (uint8_t i=0; i<10; i++) { + toshiba_led.on(); + hal.scheduler->delay(100); + toshiba_led.off(); + hal.scheduler->delay(100); + } + } +} + +AP_HAL_MAIN();