From 495c4bbbbe6500d5931af037321c5bf47dcedf1c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Aug 2013 13:13:09 +1000 Subject: [PATCH] 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 --- libraries/AP_Notify/AP_BoardLED.cpp | 32 ++- libraries/AP_Notify/AP_BoardLED.h | 26 ++- libraries/AP_Notify/AP_Notify.cpp | 35 +++- libraries/AP_Notify/AP_Notify.h | 66 +++---- libraries/AP_Notify/ToshibaLED.cpp | 187 ++++-------------- libraries/AP_Notify/ToshibaLED.h | 72 ++++--- libraries/AP_Notify/ToshibaLED_PX4.cpp | 263 +++---------------------- libraries/AP_Notify/ToshibaLED_PX4.h | 74 ++----- 8 files changed, 230 insertions(+), 525 deletions(-) diff --git a/libraries/AP_Notify/AP_BoardLED.cpp b/libraries/AP_Notify/AP_BoardLED.cpp index 7d021322b3..5502175490 100644 --- a/libraries/AP_Notify/AP_BoardLED.cpp +++ b/libraries/AP_Notify/AP_BoardLED.cpp @@ -1,34 +1,46 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include +/* + 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 . + */ + +#include 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) { diff --git a/libraries/AP_Notify/AP_BoardLED.h b/libraries/AP_Notify/AP_BoardLED.h index 58b2e43eae..14d960605f 100644 --- a/libraries/AP_Notify/AP_BoardLED.h +++ b/libraries/AP_Notify/AP_BoardLED.h @@ -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 . + */ + #ifndef __AP_HAL_BOARDLED_H__ #define __AP_HAL_BOARDLED_H__ #include #include -#include #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__ diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 964a85fae7..af24d593e9 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -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 . + */ + #include +// 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; \ No newline at end of file +// 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 +} diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 4408bd1621..a0f0ba34b4 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -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 . + */ + #ifndef __AP_NOTIFY_H__ #define __AP_NOTIFY_H__ #include +#include +#include +#include +#include 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__ diff --git a/libraries/AP_Notify/ToshibaLED.cpp b/libraries/AP_Notify/ToshibaLED.cpp index cbd30975a7..35a5ac639f 100644 --- a/libraries/AP_Notify/ToshibaLED.cpp +++ b/libraries/AP_Notify/ToshibaLED.cpp @@ -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 . */ #include #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); -} \ No newline at end of file + update_colours(); + set_rgb(_red_des, _green_des, _blue_des); +} diff --git a/libraries/AP_Notify/ToshibaLED.h b/libraries/AP_Notify/ToshibaLED.h index e623772e5f..4d66e36204 100644 --- a/libraries/AP_Notify/ToshibaLED.h +++ b/libraries/AP_Notify/ToshibaLED.h @@ -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 . + */ + #ifndef __TOSHIBA_LED_H__ #define __TOSHIBA_LED_H__ #include -#include - -#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__ diff --git a/libraries/AP_Notify/ToshibaLED_PX4.cpp b/libraries/AP_Notify/ToshibaLED_PX4.cpp index 8c9c83c6c2..c646a49eb6 100644 --- a/libraries/AP_Notify/ToshibaLED_PX4.cpp +++ b/libraries/AP_Notify/ToshibaLED_PX4.cpp @@ -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 . */ #include @@ -20,246 +26,35 @@ #include #include -#include +#include #include #include 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 \ No newline at end of file +#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_Notify/ToshibaLED_PX4.h b/libraries/AP_Notify/ToshibaLED_PX4.h index b9406e8a24..21ecb1b4f5 100644 --- a/libraries/AP_Notify/ToshibaLED_PX4.h +++ b/libraries/AP_Notify/ToshibaLED_PX4.h @@ -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 . */ #ifndef __TOSHIBA_LED_PX4_H__ #define __TOSHIBA_LED_PX4_H__ -#include -#include +#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__