AP_Notify: add ToneAlarm backend for ChibiOS

libraries/AP_Notify/AP_Notify.cpp

@@ -24,6 +24,7 @@
 #include "OreoLED_PX4.h"
 #include "RCOutputRGBLed.h"
 #include "ToneAlarm_Linux.h"
+#include "ToneAlarm_ChibiOS.h"
 #include "ToneAlarm_PX4.h"
 #include "ToshibaLED.h"
 #include "ToshibaLED_I2C.h"
@@ -33,6 +34,8 @@
 #include "Led_Sysfs.h"
 #include <stdio.h>
 
+extern const AP_HAL::HAL& hal;
+
 AP_Notify *AP_Notify::_instance;
 
 #define CONFIG_NOTIFY_DEVICES_MAX 6
@@ -136,6 +139,14 @@ void AP_Notify::add_backends(void)
     ADD_BACKEND(new Display());
   #endif
 
+// Notify devices for ChibiOS boards
+#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    ADD_BACKEND(new ToneAlarm_ChibiOS());
+    ADD_BACKEND(new PixRacerLED());
+    ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
+    ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL));
+    ADD_BACKEND(new Display());
+
 // Notify devices for VRBRAIN boards
 #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN  
   #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 // Uses px4 LED board

libraries/AP_Notify/PixRacerLED.cpp

@@ -15,7 +15,8 @@
 
 #include "PixRacerLED.h"
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 \
+    || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 
 extern const AP_HAL::HAL& hal;
 

libraries/AP_Notify/ToneAlarm_ChibiOS.cpp

@@ -0,0 +1,101 @@
+/*
+  ToneAlarm Linux 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/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#include "ToneAlarm_ChibiOS.h"
+#include <AP_HAL_ChibiOS/ToneAlarm.h>
+#include "AP_Notify.h"
+
+extern const AP_HAL::HAL& hal;
+
+bool ToneAlarm_ChibiOS::init()
+{
+    // open the tone alarm device
+    _initialized = hal.util->toneAlarm_init();
+    if (!_initialized) {
+        hal.console->printf("AP_Notify: Failed to initialise ToneAlarm");
+        return false;
+    }
+
+    // set initial boot states. This prevents us issuing 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_ChibiOS::play_tune(uint8_t tune_number)
+{
+    hal.util->toneAlarm_set_tune(tune_number);
+    return true;
+}
+
+
+// update - updates led according to timed_updated.  Should be called at 50Hz
+void ToneAlarm_ChibiOS::update()
+{
+    // exit immediately if we haven't initialised successfully
+    if (!_initialized) {
+        return;
+    }
+
+    // exit if buzzer is not enabled
+    if (pNotify->buzzer_enabled() == false) {
+        return;
+    }
+
+    // check for arming failure
+    if (AP_Notify::events.arming_failed) {
+        play_tune(TONE_ARMING_FAILURE_TUNE);
+    }
+
+    // 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 parachute release
+    if (flags.parachute_release != AP_Notify::flags.parachute_release) {
+        flags.parachute_release = AP_Notify::flags.parachute_release;
+        if (flags.parachute_release) {
+            // parachute release warning tune
+            play_tune(TONE_PARACHUTE_RELEASE_TUNE);
+        }
+    }
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

libraries/AP_Notify/ToneAlarm_ChibiOS.h

@@ -0,0 +1,43 @@
+/*
+  ToneAlarm Linux 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/>.
+ */
+#pragma once
+
+#include "NotifyDevice.h"
+
+class ToneAlarm_ChibiOS: public NotifyDevice
+{
+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(uint8_t tune_number);
+
+    bool _initialized = false;
+
+    /// tonealarm_type - bitmask of states we track
+    struct tonealarm_type {
+        bool armed              : 1;    // false = disarmed, true = armed
+        bool failsafe_battery   : 1;    // true if battery failsafe
+        bool parachute_release  : 1;    // true if parachute is being released
+    } flags;
+};