diff --git a/libraries/AP_Notify/AP_BoardLED.h b/libraries/AP_Notify/AP_BoardLED.h index f7eccaa783..48754c3d43 100644 --- a/libraries/AP_Notify/AP_BoardLED.h +++ b/libraries/AP_Notify/AP_BoardLED.h @@ -60,6 +60,12 @@ # define HAL_GPIO_C_LED_PIN -1 # define HAL_GPIO_LED_ON LOW # define HAL_GPIO_LED_OFF HIGH +#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + # define HAL_GPIO_A_LED_PIN 27 + # define HAL_GPIO_B_LED_PIN 26 + # define HAL_GPIO_C_LED_PIN 25 + # define HAL_GPIO_LED_ON LOW + # define HAL_GPIO_LED_OFF HIGH #else #error "Unknown board type in AP_Notify" #endif diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index b17d4fbcc8..6f4159a385 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -34,6 +34,9 @@ void AP_Notify::init(bool enable_external_leds) externalled.init(); buzzer.init(); #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + tonealarm.init(); +#endif } // main update function, called at 50Hz @@ -49,4 +52,7 @@ void AP_Notify::update(void) externalled.update(); buzzer.update(); #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + tonealarm.update(); +#endif } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 70cbf07aaa..b758a64946 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -26,6 +26,8 @@ #include #include #include +#include +#include class AP_Notify { @@ -69,6 +71,9 @@ private: ToshibaLED_I2C toshibaled; ExternalLED externalled; Buzzer buzzer; +#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + ToshibaLED_VRBRAIN toshibaled; + ToneAlarm_VRBRAIN tonealarm; #else ToshibaLED_I2C toshibaled; #endif diff --git a/libraries/AP_Notify/ToneAlarm_VRBRAIN.cpp b/libraries/AP_Notify/ToneAlarm_VRBRAIN.cpp new file mode 100644 index 0000000000..5004273bac --- /dev/null +++ b/libraries/AP_Notify/ToneAlarm_VRBRAIN.cpp @@ -0,0 +1,108 @@ +/* + ToneAlarm VRBRAIN 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_VRBRAIN +#include "ToneAlarm_VRBRAIN.h" +#include "AP_Notify.h" + +#include +#include +#include +#include + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +bool ToneAlarm_VRBRAIN::init() +{ + // open the tone alarm device + _tonealarm_fd = open(TONEALARM_DEVICE_PATH, 0); + if (_tonealarm_fd == -1) { + hal.console->printf("Unable to open " TONEALARM_DEVICE_PATH); + return false; + } + + // set initial boot states. This prevents us issueing a arming + // warning in plane and rover on every boot + flags.armed = AP_Notify::flags.armed; + flags.failsafe_battery = AP_Notify::flags.failsafe_battery; + return true; +} + +// play_tune - play one of the pre-defined tunes +bool ToneAlarm_VRBRAIN::play_tune(const uint8_t tune_number) +{ + int ret = ioctl(_tonealarm_fd, TONE_SET_ALARM, tune_number); + return (ret == 0); +} + + +// update - updates led according to timed_updated. Should be called at 50Hz +void ToneAlarm_VRBRAIN::update() +{ + // exit immediately if we haven't initialised successfully + if (_tonealarm_fd == -1) { + return; + } + + // check if arming status has changed + if (flags.armed != AP_Notify::flags.armed) { + flags.armed = AP_Notify::flags.armed; + if (flags.armed) { + // arming tune + play_tune(TONE_ARMING_WARNING_TUNE); + }else{ + // disarming tune + play_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } + } + + // check if battery status has changed + if (flags.failsafe_battery != AP_Notify::flags.failsafe_battery) { + flags.failsafe_battery = AP_Notify::flags.failsafe_battery; + if (flags.failsafe_battery) { + // low battery warning tune + play_tune(TONE_BATTERY_WARNING_FAST_TUNE); + } + } + + // check gps glitch + if (flags.gps_glitching != AP_Notify::flags.gps_glitching) { + flags.gps_glitching = AP_Notify::flags.gps_glitching; + if (flags.gps_glitching) { + // gps glitch warning tune + play_tune(TONE_GPS_WARNING_TUNE); + } + } + + // check gps failsafe + if (flags.failsafe_gps != AP_Notify::flags.failsafe_gps) { + flags.failsafe_gps = AP_Notify::flags.failsafe_gps; + if (flags.failsafe_gps) { + // gps glitch warning tune + play_tune(TONE_GPS_WARNING_TUNE); + } + } +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN diff --git a/libraries/AP_Notify/ToneAlarm_VRBRAIN.h b/libraries/AP_Notify/ToneAlarm_VRBRAIN.h new file mode 100644 index 0000000000..1780b8fc87 --- /dev/null +++ b/libraries/AP_Notify/ToneAlarm_VRBRAIN.h @@ -0,0 +1,47 @@ +/* + ToneAlarm VRBRAIN 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 __TONE_ALARM_VRBRAIN_H__ +#define __TONE_ALARM_VRBRAIN_H__ + +#include "ToneAlarm_VRBRAIN.h" + +class ToneAlarm_VRBRAIN +{ +public: + /// init - initialised the tone alarm + bool init(void); + + /// update - updates led according to timed_updated. Should be called at 50Hz + void update(); + +private: + /// play_tune - play one of the pre-defined tunes + bool play_tune(const uint8_t tune_number); + + int _tonealarm_fd; // file descriptor for the tone alarm + + /// tonealarm_type - bitmask of states we track + struct tonealarm_type { + uint8_t armed : 1; // 0 = disarmed, 1 = armed + uint8_t failsafe_battery : 1; // 1 if battery failsafe + uint8_t gps_glitching : 1; // 1 if gps position is not good + uint8_t failsafe_gps : 1; // 1 if gps failsafe + } flags; +}; + +#endif // __TONE_ALARM_VRBRAIN_H__ diff --git a/libraries/AP_Notify/ToshibaLED_VRBRAIN.cpp b/libraries/AP_Notify/ToshibaLED_VRBRAIN.cpp new file mode 100644 index 0000000000..4b06b10092 --- /dev/null +++ b/libraries/AP_Notify/ToshibaLED_VRBRAIN.cpp @@ -0,0 +1,77 @@ +/* + ToshibaLED VRBRAIN 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_VRBRAIN +#include "ToshibaLED_VRBRAIN.h" + +#include +#include +#include +#include + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +bool ToshibaLED_VRBRAIN::hw_init() +{ + // open the rgb led device + _rgbled_fd = open(RGBLED_DEVICE_PATH, 0); + if (_rgbled_fd == -1) { + hal.console->printf("Unable to open " RGBLED_DEVICE_PATH); + return false; + } + ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + last.zero(); + next.zero(); + hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_VRBRAIN::update_timer)); + return true; +} + +// set_rgb - set color as a combination of red, green and blue values +bool ToshibaLED_VRBRAIN::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + hal.scheduler->suspend_timer_procs(); + next[0] = red; + next[1] = green; + next[2] = blue; + hal.scheduler->resume_timer_procs(); + return true; +} + +void ToshibaLED_VRBRAIN::update_timer(void) +{ + if (last == next) { + return; + } + rgbled_rgbset_t v; + + v.red = next[0]; + v.green = next[1]; + v.blue = next[2]; + + ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v); + + last = next; +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN diff --git a/libraries/AP_Notify/ToshibaLED_VRBRAIN.h b/libraries/AP_Notify/ToshibaLED_VRBRAIN.h new file mode 100644 index 0000000000..8773160c42 --- /dev/null +++ b/libraries/AP_Notify/ToshibaLED_VRBRAIN.h @@ -0,0 +1,37 @@ +/* + ToshibaLED VRBRAIN 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_VRBRAIN_H__ +#define __TOSHIBA_LED_VRBRAIN_H__ + +#include "ToshibaLED.h" +#include "AP_Math.h" +#include "vectorN.h" + +class ToshibaLED_VRBRAIN : public ToshibaLED +{ +public: + bool hw_init(void); + bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b); +private: + int _rgbled_fd; + void update_timer(void); + + VectorN last, next; +}; + +#endif // __TOSHIBA_LED_VRBRAIN_H__