5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-17 21:33:57 -04:00

AP_Notify: simplify interface to avoid timers and external calls

this changes AP_Notify to use updates via notify.update() at 50Hz,
avoiding the need for the 1kHz timer. It also creates a parent class
for ToshibaLED so that the I2C and PX4 ToshibaLED drivers can be
abstracted out.

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2013-08-29 13:13:09 +10:00
parent 814ddcd787
commit 495c4bbbbe
8 changed files with 230 additions and 525 deletions

View File

@ -1,34 +1,46 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_BoardLED.h>
/*
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 <http://www.gnu.org/licenses/>.
*/
#include <AP_Notify.h>
extern const AP_HAL::HAL& hal;
// static private variable instantiation
uint16_t AP_BoardLED::_counter;
void AP_BoardLED::init(void)
{
// update LEDs as often as needed
hal.scheduler->register_timer_process( AP_BoardLED::_update );
// setup the main LEDs as outputs
hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, GPIO_OUTPUT);
hal.gpio->pinMode(HAL_GPIO_B_LED_PIN, GPIO_OUTPUT);
hal.gpio->pinMode(HAL_GPIO_C_LED_PIN, GPIO_OUTPUT);
}
void AP_BoardLED::_update(uint32_t now)
/*
main update function called at 50Hz
*/
void AP_BoardLED::update(void)
{
_counter++;
// we never want to update LEDs at a higher than 16Hz rate
if (_counter & 0x3F) {
if (_counter % 3 != 0) {
return;
}
// counter2 used to drop frequency down to 16hz
uint8_t counter2 = _counter >> 6;
uint8_t counter2 = _counter / 3;
// initialising
if (AP_Notify::flags.initialising) {

View File

@ -1,11 +1,25 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 <http://www.gnu.org/licenses/>.
*/
#ifndef __AP_HAL_BOARDLED_H__
#define __AP_HAL_BOARDLED_H__
#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_Notify.h>
#define HIGH 1
#define LOW 0
@ -48,13 +62,13 @@ class AP_BoardLED
public:
// initialise the LED driver
void init(void);
// should be called at 50Hz
void update(void);
private:
// private methods
static void _update(uint32_t now);
// private member variables
static uint16_t _counter;
// counter incremented at 50Hz
uint8_t _counter;
};
#endif // __AP_HAL_BOARDLED_H__

View File

@ -1,6 +1,39 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 <http://www.gnu.org/licenses/>.
*/
#include <AP_Notify.h>
// static flags, to allow for direct class update from device drivers
struct AP_Notify::notify_type AP_Notify::flags;
AP_Notify::update_fn_t AP_Notify::_update_fn;
// initialisation
void AP_Notify::init(void)
{
boardled.init();
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
toshibaled.init();
#endif
}
// main update function, called at 50Hz
void AP_Notify::update(void)
{
boardled.update();
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
toshibaled.update();
#endif
}

View File

@ -1,17 +1,32 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 <http://www.gnu.org/licenses/>.
*/
#ifndef __AP_NOTIFY_H__
#define __AP_NOTIFY_H__
#include <AP_Common.h>
#include <AP_BoardLED.h>
#include <ToshibaLED.h>
#include <ToshibaLED_I2C.h>
#include <ToshibaLED_PX4.h>
class AP_Notify
{
public:
/// definition of callback function
typedef void (*update_fn_t)(void);
/// notify_type - bitmask of notification types
struct notify_type {
uint16_t initialising : 1; // 1 if initialising and copter should not be moved
@ -22,41 +37,24 @@ public:
uint16_t esc_calibration: 1; // 1 if calibrating escs
};
// the notify flags are static to allow direct class access
// without declaring the object
static struct notify_type flags;
/// register_callback - allows registering functions to be called with AP_Notify::update() is called from main loop
static void register_update_function(update_fn_t fn) {_update_fn = fn;}
// initialisation
void init(void);
/// update - allow updates of leds that cannot be updated during a timed interrupt
static void update() { if (AP_Notify::_update_fn != NULL) AP_Notify::_update_fn(); }
/// To-Do: potential notifications to add
/// flight_mode
/// void flight_mode(uint8_t mode) = 0;
/// throttle failsafe
/// void fs_throttle(bool uint8_t); // 0 if throttle failsafe is cleared, 1 if activated
/// void fs_battery(bool uint8_t); // 1 if battery voltage is low or consumed amps close to battery capacity, 0 if cleared
/// void fs_gps(bool uint8_t); // 1 if we've lost gps lock and it is required for our current flightmode, 0 if cleared
/// void fs_gcs(bool uint8_t); // 1 if we've lost contact with the gcs and it is required for our current flightmode or pilot input method, 0 if cleared
/// void fence_breach(bool uint8_t); // fence type breached or 0 if cleared
/// switch_aux1(uint8_t state); // 0 if aux switch is off, 1 if in middle, 2 if high
/// switch_aux2(uint8_t state); // 0 if aux switch is off, 1 if in middle, 2 if high
/// reached_waypoint(); // called after we reach a waypoint
/// error(uint8_t subsystem_id, uint8_t error_code); // general error reporting
/// objects that we expect to create:
/// apm2, px4 leds
/// copter leds
/// blinkm
/// buzzer
//private:
static update_fn_t _update_fn;
void update(void);
private:
// individual drivers
AP_BoardLED boardled;
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
ToshibaLED_I2C toshibaled;
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
ToshibaLED_PX4 toshibaled;
#endif
};
#endif // __AP_NOTIFY_H__

View File

@ -1,173 +1,59 @@
/*
* 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.
*
ToshibaLED 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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL.h>
#include "ToshibaLED.h"
#include "AP_Notify.h"
extern const AP_HAL::HAL& hal;
// static private variable instantiation
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
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;
}
// 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 );
AP_Notify::register_update_function(ToshibaLED::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();
_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 (!_enabled) {
if (!_healthy) {
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 red value
if (red != _red_curr) {
if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM2, red>>4) == 0) {
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;
}else{
success = false;
}
}
// update the green value
if (green != _green_curr) {
if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM1, green>>4) == 0) {
_green_curr = green;
}else{
success = false;
}
}
// update the blue value
if (blue != _blue_curr) {
if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, blue>>4) == 0) {
_blue_curr = blue;
}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)
void ToshibaLED::update_colours(void)
{
static uint8_t counter; // to reduce 1khz to 10hz
static uint8_t step; // holds step of 10hz filter
// slow rate from 1khz to 10hz
// slow rate from 50Hz to 10hz
counter++;
if (counter < 100) {
if (counter < 5) {
return;
}
@ -176,7 +62,7 @@ void ToshibaLED::_scheduled_update(uint32_t now)
// move forward one step
step++;
if (step>=10) {
if (step >= 10) {
step = 0;
}
@ -187,7 +73,7 @@ void ToshibaLED::_scheduled_update(uint32_t now)
_red_des = TOSHIBA_LED_DIM;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
}else{
} else {
// even display blue light
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_DIM;
@ -196,8 +82,8 @@ void ToshibaLED::_scheduled_update(uint32_t now)
// 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) {
@ -326,13 +212,14 @@ void ToshibaLED::_scheduled_update(uint32_t now)
}
}
// update - updates led according to timed_updated. Should be called regularly from main loop
// update - updates led according to timed_updated. Should be called
// at 50Hz
void ToshibaLED::update()
{
// return immediately if not enabled
if (!_enabled) {
if (!_healthy) {
return;
}
set_rgb(_red_des,_green_des,_blue_des);
}
update_colours();
set_rgb(_red_des, _green_des, _blue_des);
}

View File

@ -1,26 +1,27 @@
/*
* 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.
*
* 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 <http://www.gnu.org/licenses/>.
*/
#ifndef __TOSHIBA_LED_H__
#define __TOSHIBA_LED_H__
#include <AP_HAL.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 0xFF // full brightness
#define TOSHIBA_LED_MEDIUM 0x80 // medium brightness
@ -31,33 +32,30 @@ class ToshibaLED {
public:
// init - initialised the LED
static void init(void);
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();
bool healthy() { return _healthy; }
// 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);
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();
// update - updates led according to timed_updated. Should be
// called at 50Hz
void update();
private:
// private methods
static void _scheduled_update(uint32_t now);
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;
// 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
// 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();
};
#endif // __TOSHIBA_LED_H__

View File

@ -1,13 +1,19 @@
/*
* ToshibaLED_PX4 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.
*
ToshibaLED PX4 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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL.h>
@ -20,246 +26,35 @@
#include <fcntl.h>
#include <unistd.h>
#include <include/device/rgbled.h>
#include <drivers/drv_rgbled.h>
#include <stdio.h>
#include <errno.h>
extern const AP_HAL::HAL& hal;
// static private variable instantiation
int ToshibaLED_PX4::_rgbled_fd; // px4 rgbled driver's file descriptor
bool ToshibaLED_PX4::_enabled; // true if the led was initialised successfully
bool ToshibaLED_PX4::_healthy; // true if the led's latest update was successful
uint8_t ToshibaLED_PX4::_red_des; // desired redness of led
uint8_t ToshibaLED_PX4::_green_des; // desired greenness of led
uint8_t ToshibaLED_PX4::_blue_des; // desired blueness of led
uint8_t ToshibaLED_PX4::_red_curr; // current redness of led
uint8_t ToshibaLED_PX4::_green_curr; // current greenness of led
uint8_t ToshibaLED_PX4::_blue_curr; // current blueness of led
void ToshibaLED_PX4::init()
bool ToshibaLED_PX4::hw_init()
{
// default LED to healthy
_healthy = true;
// open the rgb led device
_rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
if (_rgbled_fd < 0) {
if (_rgbled_fd == -1) {
hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
_healthy = false;
}else{
if (_healthy) {
_enabled = true;
}
// register update with scheduler and AP_Notify's update call
if (_healthy) {
hal.scheduler->register_timer_process( ToshibaLED_PX4::_scheduled_update );
AP_Notify::register_update_function(ToshibaLED_PX4::update);
_enabled = true;
}
return false;
}
return true;
}
// set_rgb - set color as a combination of red, green and blue values
void ToshibaLED_PX4::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
// return immediately if not enabled
if (!_enabled) {
return;
}
rgbled_rgbset_t v;
// send if desired colours aare not already set
if (green != _green_curr || blue != _blue_curr || red != _red_curr) {
struct RGBLEDSet v;
v.red = red;
v.green = green;
v.blue = blue;
int ret = ioctl(_rgbled_fd, RGBLED_SET, (unsigned long)&v);
if (ret >= 0) {
_green_curr = green;
_blue_curr = blue;
_red_curr = red;
_healthy = true;
}else{
_healthy = false;
}
}
v.red = red;
v.green = green;
v.blue = blue;
int ret = ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
return (ret == 0);
}
// _scheduled_update - updates _red, _green, _blue according to notify flags
void ToshibaLED_PX4::_scheduled_update(uint32_t now)
{
static uint8_t counter; // to reduce 1khz to 10hz
static uint8_t step; // holds step of 10hz filter
// slow rate from 1khz to 10hz
counter++;
if (counter < 100) {
return;
}
// reset counter
counter = 0;
// move forward one step
step++;
if (step>=10) {
step = 0;
}
// initialising pattern
if (AP_Notify::flags.initialising) {
if (step & 1) {
// odd steps display red light
_red_des = TOSHIBA_PX4_LED_DIM;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_OFF;
}else{
// even display blue light
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_DIM;
_green_des = TOSHIBA_PX4_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_des = TOSHIBA_PX4_LED_DIM;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_OFF;
break;
case 1:
case 4:
case 7:
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_DIM;
_green_des = TOSHIBA_PX4_LED_OFF;
break;
case 2:
case 5:
case 8:
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_DIM;
break;
case 9:
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_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 3d lock
if (AP_Notify::flags.gps_status == 3) {
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_DIM;
}else{
// flash green if armed with no gps lock
switch(step) {
case 0:
case 1:
case 2:
case 3:
case 4:
_red_des = TOSHIBA_PX4_LED_DIM;
_blue_des = TOSHIBA_PX4_LED_DIM;
_green_des = TOSHIBA_PX4_LED_DIM;
break;
case 5:
case 6:
case 7:
case 8:
case 9:
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_OFF;
break;
}
}
}else{
// flash yellow if failing pre-arm checks
if (AP_Notify::flags.pre_arm_check == 0) {
switch(step) {
case 0:
case 1:
case 2:
case 3:
case 4:
_red_des = TOSHIBA_PX4_LED_DIM;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_DIM;
break;
case 5:
case 6:
case 7:
case 8:
case 9:
// even display blue light
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_OFF;
break;
}
}else{
// solid blue if gps 3d lock
if (AP_Notify::flags.gps_status == 3) {
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_DIM;
_green_des = TOSHIBA_PX4_LED_OFF;
}else{
// flashing blue if no gps lock
switch(step) {
case 0:
case 1:
case 2:
case 3:
case 4:
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_DIM;
_green_des = TOSHIBA_PX4_LED_OFF;
break;
case 5:
case 6:
case 7:
case 8:
case 9:
// even display blue light
_red_des = TOSHIBA_PX4_LED_OFF;
_blue_des = TOSHIBA_PX4_LED_OFF;
_green_des = TOSHIBA_PX4_LED_OFF;
break;
}
}
}
}
}
// update - updates led according to timed_updated. Should be called regularly from main loop
void ToshibaLED_PX4::update()
{
// return immediately if not enabled
if (!_enabled) {
return;
}
set_rgb(_red_des,_green_des,_blue_des);
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

View File

@ -1,64 +1,32 @@
/*
* 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.
*
ToshibaLED PX4 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 <http://www.gnu.org/licenses/>.
*/
#ifndef __TOSHIBA_LED_PX4_H__
#define __TOSHIBA_LED_PX4_H__
#include <AP_HAL.h>
#include <AP_Notify.h>
#include "ToshibaLED.h"
#define TOSHIBA_PX4_LED_BRIGHT 0xFF // full brightness
#define TOSHIBA_PX4_LED_MEDIUM 0x80 // medium brightness
#define TOSHIBA_PX4_LED_DIM 0x11 // dim
#define TOSHIBA_PX4_LED_OFF 0x00 // off
class ToshibaLED_PX4 {
class ToshibaLED_PX4 : public ToshibaLED
{
public:
// constructor
ToshibaLED_PX4() {};
// init - initialised the LED
static void init(void);
// enabled - returns true if the LED was initialised successfully
static bool enabled() { return _enabled; }
// 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();
bool hw_init(void);
bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);
private:
// private methods
static void _scheduled_update(uint32_t now);
// private member variables
static int _rgbled_fd; // px4 rgbled driver's file descriptor
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
int _rgbled_fd;
};
#endif // __TOSHIBA_LED_PX4_H__