From 0a33a7c15a1a50401d5ab3bb678408bb66565b5f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Dec 2014 15:37:24 +0900 Subject: [PATCH] Notify: OreoLED_PX4 driver --- libraries/AP_Notify/OreoLED_PX4.cpp | 344 ++++++++++++++++++++++++++++ libraries/AP_Notify/OreoLED_PX4.h | 86 +++++++ 2 files changed, 430 insertions(+) create mode 100644 libraries/AP_Notify/OreoLED_PX4.cpp create mode 100644 libraries/AP_Notify/OreoLED_PX4.h diff --git a/libraries/AP_Notify/OreoLED_PX4.cpp b/libraries/AP_Notify/OreoLED_PX4.cpp new file mode 100644 index 0000000000..5adf6b6e05 --- /dev/null +++ b/libraries/AP_Notify/OreoLED_PX4.cpp @@ -0,0 +1,344 @@ +/* + OreoLED 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 + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include "OreoLED_PX4.h" +#include "AP_Notify.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define OREOLED_BACKLEFT 0 // back left led instance number +#define OREOLED_BACKRIGHT 1 // back right led instance number +#define OREOLED_FRONTRIGHT 2 // front right led instance number +#define OREOLED_FRONTLEFT 3 // front left led instance number + +// constructor +OreoLED_PX4::OreoLED_PX4() : NotifyDevice(), + _overall_health(false), + _oreoled_fd(-1), + _send_required(false), + _state_desired_semaphore(false) +{ + // initialise desired and sent state + memset(_state_desired,0,sizeof(_state_desired)); + memset(_state_sent,0,sizeof(_state_sent)); +} + +// init - initialised the device +bool OreoLED_PX4::init() +{ + // open the device + _oreoled_fd = open(OREOLED0_DEVICE_PATH, O_RDWR); + if (_oreoled_fd == -1) { + hal.console->printf("Unable to open " OREOLED0_DEVICE_PATH); + _overall_health = false; + } else { + // set overall health + _overall_health = true; + // register timer + hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&OreoLED_PX4::update_timer)); + } + + // return health + return _overall_health; +} + +// update - updates device according to timed_updated. Should be +// called at 50Hz +void OreoLED_PX4::update() +{ + static uint8_t counter = 0; // counter to reduce rate from 50hz to 10hz + static uint8_t step = 0; // step to control pattern + static uint8_t last_stage = 0; // unique id of the last messages sent to the LED, used to reduce resends which disrupt some patterns + uint8_t brightness = OREOLED_BRIGHT; + + // return immediately if not healthy + if (!_overall_health) { + return; + } + + // slow rate from 50Hz to 10hz + counter++; + if (counter < 5) { + return; + } + counter = 0; + + // use dim light when connected through USB + if (hal.gpio->usb_connected()) { + brightness = OREOLED_DIM; + } + + // move forward one step + step++; + if (step >= 10) { + step = 0; + } + + // initialising pattern + if (AP_Notify::flags.initialising) { + if (step & 1) { + // left side on + set_rgb(OREOLED_FRONTLEFT, brightness, brightness, brightness); // white + set_rgb(OREOLED_BACKLEFT, brightness, 0, 0); // red + // right side off + set_rgb(OREOLED_FRONTRIGHT, 0, 0, 0); + set_rgb(OREOLED_BACKRIGHT, 0, 0, 0); + } else { + // left side off + set_rgb(OREOLED_FRONTLEFT, 0, 0, 0); + set_rgb(OREOLED_BACKLEFT, 0, 0, 0); + // right side on + set_rgb(OREOLED_FRONTRIGHT, brightness, brightness, brightness); // white + set_rgb(OREOLED_BACKRIGHT, brightness, 0, 0); // red + } + + // record stage + last_stage = 1; + + // 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 + set_rgb(OREOLED_INSTANCE_ALL, brightness, 0, 0); + break; + + case 1: + case 4: + case 7: + // blue + set_rgb(OREOLED_INSTANCE_ALL, 0, 0, brightness); + break; + + case 2: + case 5: + case 8: + // green on + set_rgb(OREOLED_INSTANCE_ALL, 0, brightness, 0); + break; + + case 9: + // all off + set_rgb(OREOLED_INSTANCE_ALL, 0, 0, 0); + break; + } + // record stage + last_stage = 2; + // exit so no other status modify this pattern + return; + } + + // radio and battery failsafe patter: flash yellow + // gps failsafe pattern : flashing yellow and blue + // baro glitching pattern : flashing yellow and purple + // ekf_bad pattern : flashing yellow and red + if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery || + AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching || + AP_Notify::flags.baro_glitching || + AP_Notify::flags.ekf_bad) { + switch(step) { + case 0: + case 1: + case 2: + case 3: + case 4: + // yellow on + set_rgb(OREOLED_INSTANCE_ALL, brightness, brightness, 0); + break; + case 5: + case 6: + case 7: + case 8: + case 9: + if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) { + // blue on for gps failsafe + set_rgb(OREOLED_INSTANCE_ALL, 0, 0, brightness); + } else if (AP_Notify::flags.baro_glitching) { + // purple on if baro glitching + set_rgb(OREOLED_INSTANCE_ALL, brightness, 0, brightness); + } else if (AP_Notify::flags.ekf_bad) { + // red on if ekf bad + set_rgb(OREOLED_INSTANCE_ALL, brightness, 0, 0); + }else{ + // all off for radio or battery failsafe + set_rgb(OREOLED_INSTANCE_ALL, 0, 0, 0); + } + break; + } + // record stage + last_stage = 3; + // exit so no other status modify this pattern + return; + } + + // send colours (later we will set macro if required) + if (last_stage < 10) { + set_macro(OREOLED_FRONTLEFT, OREOLED_PARAM_MACRO_WHITE); // white + set_macro(OREOLED_FRONTRIGHT, OREOLED_PARAM_MACRO_WHITE); // white + set_macro(OREOLED_BACKLEFT, OREOLED_PARAM_MACRO_RED); // red + set_macro(OREOLED_BACKRIGHT, OREOLED_PARAM_MACRO_RED); // red + last_stage = 10; + } else if (last_stage >= 10) { + // check arming status + if (AP_Notify::flags.armed) { + // reset macro will return LEDs to solid + set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_RESET); + } else { + // start breathing macro + set_macro(OREOLED_INSTANCE_ALL, OREOLED_PARAM_MACRO_BREATH); + } + last_stage = 11; + } +} + +// set_rgb - set color as a combination of red, green and blue values for one or all LEDs +void OreoLED_PX4::set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue) +{ + // return immediately if no healty leds + if (!_overall_health) { + return; + } + + // get semaphore + _state_desired_semaphore = true; + + // check for all instances + if (instance == OREOLED_INSTANCE_ALL) { + // store desired rgb for all LEDs + for (uint8_t i=0; i. + */ +#ifndef __OREOLED_PX4_H__ +#define __OREOLED_PX4_H__ + +#include +#include "NotifyDevice.h" +#include + +#define OREOLED_NUM_LEDS 4 // maximum number of individual LEDs connected to the oreo led cpu +#define OREOLED_INSTANCE_ALL 0xff // instance number to indicate all LEDs (used for set_rgb and set_macro) +#define OREOLED_BRIGHT 0xff // maximum brightness when flying (disconnected from usb) +#define OREOLED_DIM 0x0f // dim when connected to USB + +class OreoLED_PX4 : public NotifyDevice +{ +public: + // constuctor + OreoLED_PX4(); + + // init - initialised the device + bool init(void); + + // update - updates device according to timed_updated. Should be + // called at 50Hz + void update(); + + // healthy - return true if at least one LED is responding + bool healthy() const { return _overall_health; } + +private: + // update_timer - called by scheduler and updates PX4 driver with commands + void update_timer(void); + + // set_rgb - set color as a combination of red, green and blue values for one or all LEDs + void set_rgb(uint8_t instance, uint8_t red, uint8_t green, uint8_t blue); + + // set_macrxo - set macro for one or all LEDs + void set_macro(uint8_t instance, enum oreoled_macro macro); + + // oreo led modes (pattern, macro or rgb) + enum oreoled_mode { + OREOLED_MODE_PATTERN, + OREOLED_MODE_MACRO, + OREOLED_MODE_RGB + }; + + // oreo_state structure holds possible state of an led + struct oreo_state { + enum oreoled_mode mode; + enum oreoled_pattern pattern; + enum oreoled_macro macro; + uint8_t red; + uint8_t green; + uint8_t blue; + + // operator== + inline bool operator==(const oreo_state &os) { + return ((os.mode==mode) && (os.pattern==pattern) && (os.macro==macro) && (os.red==red) && (os.green==green) && (os.blue==blue)); + } + }; + + // private members + bool _overall_health; // overall health + int _oreoled_fd; // file descriptor + bool _send_required; // true when we need to send an update to at least one led + volatile bool _state_desired_semaphore; // true when we are updating the state desired values to ensure they are not sent prematurely + oreo_state _state_desired[OREOLED_NUM_LEDS]; // desired state + oreo_state _state_sent[OREOLED_NUM_LEDS]; // last state sent to led +}; + +#endif // __OREOLED_PX4_H__