Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-20 06:43:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: simplify interface to avoid timers and external calls

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Notify/AP_BoardLED.cpp
View File

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

26
libraries/AP_Notify/AP_BoardLED.h
View File

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

35
libraries/AP_Notify/AP_Notify.cpp
View File

@ -1,6 +1,39 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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> #include <AP_Notify.h>
// static flags, to allow for direct class update from device drivers
struct AP_Notify::notify_type AP_Notify::flags; 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
}

66
libraries/AP_Notify/AP_Notify.h
View File

@ -1,17 +1,32 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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__ #ifndef __AP_NOTIFY_H__
#define __AP_NOTIFY_H__ #define __AP_NOTIFY_H__
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_BoardLED.h>
#include <ToshibaLED.h>
#include <ToshibaLED_I2C.h>
#include <ToshibaLED_PX4.h>
class AP_Notify class AP_Notify
{ {
public: public:
/// definition of callback function
typedef void (*update_fn_t)(void);
/// notify_type - bitmask of notification types /// notify_type - bitmask of notification types
struct notify_type { struct notify_type {
uint16_t initialising : 1; // 1 if initialising and copter should not be moved 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 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; static struct notify_type flags;
/// register_callback - allows registering functions to be called with AP_Notify::update() is called from main loop // initialisation
static void register_update_function(update_fn_t fn) {_update_fn = fn;} void init(void);
/// update - allow updates of leds that cannot be updated during a timed interrupt /// 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(); } void update(void);
/// 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;
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__ #endif // __AP_NOTIFY_H__

187
libraries/AP_Notify/ToshibaLED.cpp
View File

@ -1,173 +1,59 @@
/* /*
* ToshibaLED Library. ToshibaLED driver
* DIYDrones 2013 */
*
* /*
* This library is free software; you can redistribute it and/or This program is free software: you can redistribute it and/or modify
* modify it under the terms of the GNU Lesser General Public it under the terms of the GNU General Public License as published by
* License as published by the Free Software Foundation; either the Free Software Foundation, either version 3 of the License, or
* version 2.1 of the License, or (at your option) any later version. (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 <AP_HAL.h>
#include "ToshibaLED.h" #include "ToshibaLED.h"
#include "AP_Notify.h"
extern const AP_HAL::HAL& hal; 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() void ToshibaLED::init()
{ {
// default LED to healthy _healthy = hw_init();
_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();
} }
// set_rgb - set color as a combination of red, green and blue values // 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) void ToshibaLED::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{ {
// return immediately if not enabled // return immediately if not enabled
if (!_enabled) { if (!_healthy) {
return; return;
} }
bool success = true; if (red != _red_curr ||
green != _green_curr ||
// get pointer to i2c bus semaphore blue != _blue_curr) {
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); // call the hardware update routine
if (hw_set_rgb(red, green, blue)) {
// 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) {
_red_curr = red; _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; _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; _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 // _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 // slow rate from 50Hz to 10hz
static uint8_t step; // holds step of 10hz filter
// slow rate from 1khz to 10hz
counter++; counter++;
if (counter < 100) { if (counter < 5) {
return; return;
} }
@ -176,7 +62,7 @@ void ToshibaLED::_scheduled_update(uint32_t now)
// move forward one step // move forward one step
step++; step++;
if (step>=10) { if (step >= 10) {
step = 0; step = 0;
} }
@ -187,7 +73,7 @@ void ToshibaLED::_scheduled_update(uint32_t now)
_red_des = TOSHIBA_LED_DIM; _red_des = TOSHIBA_LED_DIM;
_blue_des = TOSHIBA_LED_OFF; _blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF; _green_des = TOSHIBA_LED_OFF;
}else{ } else {
// even display blue light // even display blue light
_red_des = TOSHIBA_LED_OFF; _red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_DIM; _blue_des = TOSHIBA_LED_DIM;
@ -196,8 +82,8 @@ void ToshibaLED::_scheduled_update(uint32_t now)
// exit so no other status modify this pattern // exit so no other status modify this pattern
return; return;
} }
// save trim and esc calibration pattern // save trim and esc calibration pattern
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) { if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
switch(step) { 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() void ToshibaLED::update()
{ {
// return immediately if not enabled // return immediately if not enabled
if (!_enabled) { if (!_healthy) {
return; return;
} }
update_colours();
set_rgb(_red_des,_green_des,_blue_des); set_rgb(_red_des, _green_des, _blue_des);
} }

72
libraries/AP_Notify/ToshibaLED.h
View File

@ -1,26 +1,27 @@
/* /*
* AP_HAL_AVR Notify Library. * AP_Notify Library.
* * based upon a prototype library by David "Buzz" Bussenschutt.
* 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.
*
*/ */
/*
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__ #ifndef __TOSHIBA_LED_H__
#define __TOSHIBA_LED_H__ #define __TOSHIBA_LED_H__
#include <AP_HAL.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_BRIGHT 0xFF // full brightness
#define TOSHIBA_LED_MEDIUM 0x80 // medium brightness #define TOSHIBA_LED_MEDIUM 0x80 // medium brightness
@ -31,33 +32,30 @@ class ToshibaLED {
public: public:
// init - initialised the LED // init - initialised the LED
static void init(void); void init(void);
// healthy - returns true if the LED is operating properly // healthy - returns true if the LED is operating properly
static bool healthy() { return _healthy; } 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 // 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 // update - updates led according to timed_updated. Should be
static void update(); // called at 50Hz
void update();
private: protected:
// private methods // methods implemented in hardware specific classes
static void _scheduled_update(uint32_t now); virtual bool hw_init(void) = 0;
virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0;
// private member variables // meta-data common to all hw devices
static AP_HAL::Semaphore* _i2c_sem; // pointer to i2c bus semaphore uint8_t counter;
static bool _enabled; // true if the LED is operating properly uint8_t step;
static bool _healthy; // true if the LED is operating properly bool _healthy; // true if the LED is operating properly
static uint8_t _red_des, _green_des, _blue_des; // color requested by timed update 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 uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led
void update_colours();
}; };
#endif // __TOSHIBA_LED_H__ #endif // __TOSHIBA_LED_H__

263
libraries/AP_Notify/ToshibaLED_PX4.cpp
View File

@ -1,13 +1,19 @@
/* /*
* ToshibaLED_PX4 Library. ToshibaLED PX4 driver
* DIYDrones 2013 */
* /*
* This program is free software: you can redistribute it and/or modify
* This library is free software; you can redistribute it and/or it under the terms of the GNU General Public License as published by
* modify it under the terms of the GNU Lesser General Public the Free Software Foundation, either version 3 of the License, or
* License as published by the Free Software Foundation; either (at your option) any later version.
* version 2.1 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 <AP_HAL.h>
@ -20,246 +26,35 @@
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
#include <include/device/rgbled.h> #include <drivers/drv_rgbled.h>
#include <stdio.h> #include <stdio.h>
#include <errno.h> #include <errno.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// static private variable instantiation bool ToshibaLED_PX4::hw_init()
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()
{ {
// default LED to healthy
_healthy = true;
// open the rgb led device // open the rgb led device
_rgbled_fd = open(RGBLED_DEVICE_PATH, 0); _rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
if (_rgbled_fd < 0) { if (_rgbled_fd == -1) {
hal.console->printf("Unable to open " RGBLED_DEVICE_PATH); hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
_healthy = false; return 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 true;
} }
// set_rgb - set color as a combination of red, green and blue values // 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 rgbled_rgbset_t v;
if (!_enabled) {
return;
}
// send if desired colours aare not already set v.red = red;
if (green != _green_curr || blue != _blue_curr || red != _red_curr) { v.green = green;
struct RGBLEDSet v; v.blue = blue;
v.red = red;
v.green = green; int ret = ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
v.blue = blue; return (ret == 0);
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;
}
}
} }
// _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 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
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

74
libraries/AP_Notify/ToshibaLED_PX4.h
View File

@ -1,64 +1,32 @@
/* /*
* AP_HAL_AVR Notify Library. ToshibaLED PX4 driver
* */
* Copyright (c) 2013 David "Buzz" Bussenschutt. All right reserved. /*
* Rev 1.0 - 1st March 2013 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
* This library is free software; you can redistribute it and/or the Free Software Foundation, either version 3 of the License, or
* modify it under the terms of the GNU Lesser General Public (at your option) any later version.
* License as published by the Free Software Foundation; either
* version 2.1 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__ #ifndef __TOSHIBA_LED_PX4_H__
#define __TOSHIBA_LED_PX4_H__ #define __TOSHIBA_LED_PX4_H__
#include <AP_HAL.h> #include "ToshibaLED.h"
#include <AP_Notify.h>
#define TOSHIBA_PX4_LED_BRIGHT 0xFF // full brightness class ToshibaLED_PX4 : public ToshibaLED
#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 {
public: public:
bool hw_init(void);
// constructor bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);
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();
private: private:
// private methods int _rgbled_fd;
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
}; };
#endif // __TOSHIBA_LED_PX4_H__ #endif // __TOSHIBA_LED_PX4_H__