mirror of https://github.com/ArduPilot/ardupilot
Notify: add ToshibaLED
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
e985253f1a
commit
54007854a9
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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__
|
||||
|
|
@ -0,0 +1 @@
|
|||
include ../../../../mk/apm.mk
|
||||
|
|
@ -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();
|
||||
Loading…
Reference in New Issue