AP_Notify: new files and definitions for VRBRAIN board

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

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
 }

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

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

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__

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

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__