From 4ad83f3c8a192dab7b7b96b069856140fd7b2fcd Mon Sep 17 00:00:00 2001
From: Emile Castelnuovo <emile.castelnuovo@gmail.com>
Date: Mon, 31 Mar 2014 19:48:41 +0200
Subject: [PATCH] AP_Notify: new files and definitions for VRBRAIN board
---
libraries/AP_Notify/AP_BoardLED.h | 6 ++
libraries/AP_Notify/AP_Notify.cpp | 6 ++
libraries/AP_Notify/AP_Notify.h | 5 +
libraries/AP_Notify/ToneAlarm_VRBRAIN.cpp | 108 +++++++++++++++++++++
libraries/AP_Notify/ToneAlarm_VRBRAIN.h | 47 +++++++++
libraries/AP_Notify/ToshibaLED_VRBRAIN.cpp | 77 +++++++++++++++
libraries/AP_Notify/ToshibaLED_VRBRAIN.h | 37 +++++++
7 files changed, 286 insertions(+)
create mode 100644 libraries/AP_Notify/ToneAlarm_VRBRAIN.cpp
create mode 100644 libraries/AP_Notify/ToneAlarm_VRBRAIN.h
create mode 100644 libraries/AP_Notify/ToshibaLED_VRBRAIN.cpp
create mode 100644 libraries/AP_Notify/ToshibaLED_VRBRAIN.h
diff --git a/libraries/AP_Notify/AP_BoardLED.h b/libraries/AP_Notify/AP_BoardLED.h
index f7eccaa78..48754c3d4 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 b17d4fbcc..6f4159a38 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 70cbf07aa..b758a6494 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -26,6 +26,8 @@
#include <ToneAlarm_PX4.h>
#include <ExternalLED.h>
#include <Buzzer.h>
+#include <ToshibaLED_VRBRAIN.h>
+#include <ToneAlarm_VRBRAIN.h>
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 000000000..5004273ba
--- /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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "ToneAlarm_VRBRAIN.h"
+#include "AP_Notify.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_tone_alarm.h>
+#include <stdio.h>
+#include <errno.h>
+
+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 000000000..1780b8fc8
--- /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 <http://www.gnu.org/licenses/>.
+ */
+#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 000000000..4b06b1009
--- /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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "ToshibaLED_VRBRAIN.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_rgbled.h>
+#include <stdio.h>
+#include <errno.h>
+
+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 000000000..8773160c4
--- /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 <http://www.gnu.org/licenses/>.
+ */
+#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<uint8_t,3> last, next;
+};
+
+#endif // __TOSHIBA_LED_VRBRAIN_H__
--
GitLab