Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Notify: add ToshibaLED

Browse Source
This commit is contained in:
Randy Mackay 2013-08-09 12:17:29 +09:00 committed by Andrew Tridgell
parent e985253f1a
commit 54007854a9
4 changed files with 513 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

346
libraries/AP_Notify/ToshibaLED.cpp Normal file
View File

@ -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 Normal file
View File

@ -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 Normal file
View File

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

98
libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde Normal file
View File

@ -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();