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:
parent
814ddcd787
commit
495c4bbbbe
@ -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) {
|
||||
|
@ -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__
|
||||
|
@ -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
|
||||
}
|
||||
|
@ -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__
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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__
|
||||
|
@ -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
|
||||
|
@ -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__
|
||||
|
Loading…
Reference in New Issue
Block a user