AP_Notify: early draft of library

libraries/AP_Notify/AP_BoardLED.cpp

@@ -0,0 +1,160 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_BoardLED.h>
+
+extern const AP_HAL::HAL& hal;
+
+// static private variable instantiation
+uint16_t AP_BoardLED::_counter;
+
+void AP_BoardLED::init(void)
+{
+    // update LEDs as often as needed
+    hal.scheduler->register_timer_process( AP_BoardLED::_update );	
+
+    // setup the main LEDs as outputs
+    hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, GPIO_OUTPUT);
+    hal.gpio->pinMode(HAL_GPIO_B_LED_PIN, GPIO_OUTPUT);
+    hal.gpio->pinMode(HAL_GPIO_C_LED_PIN, GPIO_OUTPUT);
+}
+
+void AP_BoardLED::_update(uint32_t now)
+{
+    _counter++;
+
+    // we never want to update LEDs at a higher than 16Hz rate
+    if (_counter & 0x3F) {
+        return;
+    }
+
+    // counter2 used to drop frequency down to 16hz
+    uint8_t counter2 = _counter >> 6;
+
+    // initialising
+    if (notify.flags.initialising) {
+        // blink LEDs A and C at 8Hz (full cycle) during initialisation
+        if (counter2 & 1) {
+            hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+            hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
+        } else {
+            hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+            hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON);
+        }
+        return;
+	}
+
+    // save trim
+    if (notify.flags.save_trim) {
+        static uint8_t save_trim_counter = 0;
+        if ((counter2 & 0x2) == 0) {
+            save_trim_counter++;
+        }
+        switch(save_trim_counter) {
+            case 0:
+                hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
+                hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+                break;
+
+            case 1:
+                hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+                hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_ON);
+                break;
+
+            case 2:
+                hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
+                hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON);
+                break;
+
+            default:
+                save_trim_counter = -1;
+        }
+        return;
+    }
+
+    // arming light
+    static uint8_t arm_counter = 0;
+	if (notify.flags.armed) {
+        // red led solid
+        hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+    }else{
+        if ((counter2 & 0x2) == 0) {
+            arm_counter++;
+        }
+        if (notify.flags.pre_arm_check) {
+            // passed pre-arm checks so slower single flash
+            switch(arm_counter) {
+                case 0:
+                case 1:
+                case 2:
+                    hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+                    break;
+                case 3:
+                case 4:
+                case 5:
+                    hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+                    break;
+                default:
+                    // reset counter to restart the sequence
+                    arm_counter = -1;
+                    break;
+            }
+            // disarmed and passing pre-arm checks, blink at about 2hz
+            //if ((counter2 & 0x7) == 0) {
+            //    hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
+            //}
+        }else{
+            // disarmed and failing pre-arm checks, double blink
+            //if (counter2 & 0x4) {
+            //    hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
+            //}
+            // failed pre-arm checks so double flash
+            switch(arm_counter) {
+                case 0:
+                case 1:
+                    hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+                    break;
+                case 2:
+                    hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+                    break;
+                case 3:
+                case 4:
+                    hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+                    break;
+                case 5:
+                case 6:
+                    hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+                    break;
+                default:
+                    arm_counter = -1;
+                    break;
+            }
+        }
+    }
+
+    // gps light
+    switch (notify.flags.gps_status) {
+        case 0:
+            // no GPS attached
+            hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
+            break;
+
+        case 1:
+            // GPS attached but no lock, blink at 4Hz
+            if ((counter2 & 0x3) == 0) {
+                hal.gpio->toggle(HAL_GPIO_C_LED_PIN);
+            }
+            break;
+
+        case 2:
+            // GPS attached but 2D lock, blink more slowly (around 2Hz)
+            if ((counter2 & 0x7) == 0) {
+                hal.gpio->toggle(HAL_GPIO_C_LED_PIN);
+            }
+            break;
+
+        case 3:
+            // GPS attached but 2D lock, blink more slowly (around 2Hz)
+            hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON);
+            break;        
+    }
+}

libraries/AP_Notify/AP_BoardLED.h

@@ -0,0 +1,60 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_HAL_BOARDLED_H__
+#define __AP_HAL_BOARDLED_H__
+
+#include <AP_Common.h>
+#include <AP_HAL.h>
+#include <AP_Notify.h>
+
+#define HIGH 1
+#define LOW 0
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+ # define HAL_GPIO_A_LED_PIN        37
+ # define HAL_GPIO_B_LED_PIN        36
+ # define HAL_GPIO_C_LED_PIN        35
+ # define HAL_GPIO_LED_ON           HIGH
+ # define HAL_GPIO_LED_OFF          LOW
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ # 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
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+ # 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
+#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
+ # 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
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
+// XXX these are just copied, may not make sense
+ # 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
+#endif
+
+class AP_BoardLED
+{
+public:
+    // initialise the LED driver
+    void init(void);
+
+private:
+    // private methods
+    static void _update(uint32_t now);
+
+    // private member variables
+    static uint16_t _counter;
+};
+
+#endif // __AP_HAL_BOARDLED_H__

libraries/AP_Notify/AP_Notify.cpp

@@ -0,0 +1,8 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_Notify.h>
+
+AP_Notify notify;
+/// Constructor
+//AP_Notify::AP_Notify()
+//{
+//}

libraries/AP_Notify/AP_Notify.h

@@ -0,0 +1,55 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_NOTIFY_H__
+#define __AP_NOTIFY_H__
+
+#include <AP_Common.h>
+
+class AP_Notify
+{
+public:
+
+    /// notify_type - bitmask of notification types
+    struct notify_type {
+        uint16_t initialising   : 1;    // 1 if initialising and copter should not be moved
+        uint16_t gps_status     : 2;    // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock
+        uint16_t armed          : 1;    // 0 = disarmed, 1 = armed
+        uint16_t pre_arm_check  : 1;    // 0 = failing checks, 1 = passed
+        uint16_t save_trim      : 1;    // 1 if gathering trim data
+        uint16_t esc_calibration: 1;    // 1 if calibrating escs
+    } flags;
+
+    /// Constructor
+    //Notify();
+    
+    /// To-Do: potential notifications to add
+
+    /// flight_mode 
+    /// void flight_mode(uint8_t mode) = 0;
+
+    /// throttle failsafe
+    /// void fs_throttle(bool uint8_t);     // 0 if throttle failsafe is cleared, 1 if activated
+    /// void fs_battery(bool uint8_t);      // 1 if battery voltage is low or consumed amps close to battery capacity, 0 if cleared
+    /// void fs_gps(bool uint8_t);          // 1 if we've lost gps lock and it is required for our current flightmode, 0 if cleared
+    /// void fs_gcs(bool uint8_t);          // 1 if we've lost contact with the gcs and it is required for our current flightmode or pilot input method, 0 if cleared
+    /// void fence_breach(bool uint8_t);    // fence type breached or 0 if cleared
+
+    /// switch_aux1(uint8_t state);     // 0 if aux switch is off, 1 if in middle, 2 if high
+    /// switch_aux2(uint8_t state);     // 0 if aux switch is off, 1 if in middle, 2 if high
+
+    /// reached_waypoint();             // called after we reach a waypoint
+
+    /// error(uint8_t subsystem_id, uint8_t error_code);    // general error reporting
+
+    /// objects that we expect to create:
+    /// apm2, px4 leds
+    /// copter leds
+    /// blinkm
+    /// buzzer
+
+};
+
+// declare a static instance so that it can be accessed easily from anywhere
+extern AP_Notify notify;
+
+#endif	// __AP_NOTIFY_H__

libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde

@@ -0,0 +1,39 @@
+/*
+ *       Example of AC_Notify library .
+ *       DIYDrones.com
+ */
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Param.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+#include <AP_Notify.h>          // Notify library
+#include <AP_BoardLED.h>        // Board LED library
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+// create board led object
+AP_BoardLED board_led;
+
+void setup()
+{
+    hal.console->println("AP_Notify library test");
+
+    // initialise the board leds
+    board_led.init();
+
+    // turn on initialising notification
+    //notify.flags.initialising = true;
+    notify.flags.gps_status = 1;
+    notify.flags.armed = 1;
+    notify.flags.pre_arm_check = 1;
+}
+
+void loop()
+{
+    hal.scheduler->delay(1000);
+}
+
+AP_HAL_MAIN();

libraries/AP_Notify/examples/AP_Notify_test/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk