zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Notify: add ToshibaLED

master
Randy Mackay 12 years ago committed by Andrew Tridgell
parent
e985253f1a
commit
54007854a9
4 changed files with 513 additions and 0 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. 346
      libraries/AP_Notify/ToshibaLED.cpp
  2. 68
      libraries/AP_Notify/ToshibaLED.h
  3. 1
      libraries/AP_Notify/examples/ToshibaLED_test/Makefile
  4. 98
      libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde

346
libraries/AP_Notify/ToshibaLED.cpp
Unescape View File

@ -0,0 +1,346 @@ @@ -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 <AP_HAL.h>
#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);
}

68
libraries/AP_Notify/ToshibaLED.h
Unescape View File

@ -0,0 +1,68 @@ @@ -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 <AP_HAL.h>
//#include "AP_HAL_AVR_Namespace.h"
#include <AP_Notify.h>
#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__

1
libraries/AP_Notify/examples/ToshibaLED_test/Makefile
Unescape View File

@ -0,0 +1 @@ @@ -0,0 +1 @@
include ../../../../mk/apm.mk

98
libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde
Unescape View File

@ -0,0 +1,98 @@ @@ -0,0 +1,98 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Param.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_Notify.h> // Notify library
#include <ToshibaLED.h>
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();
Write Preview
Loading…
Cancel
Save