Plane: integrate AP_Notify

ArduPlane/ArduPlane.pde

@@ -70,6 +70,11 @@
 #include <AP_SpdHgtControl.h>
 #include <AP_TECS.h>
 
+#include <AP_Notify.h>      // Notify library
+#include <AP_BoardLED.h>    // BoardLEDs library
+#include <ToshibaLED.h>     // ToshibaLED library
+#include <ToshibaLED_PX4.h> // ToshibaLED library for PX4
+
 // Pre-AP_HAL compatibility
 #include "compat.h"
 
@@ -117,7 +122,7 @@ static Parameters g;
 
 // main loop scheduler
 static AP_Scheduler scheduler;
-
+ 
 // mapping between input channels
 static RCMapper rcmap;
 
@@ -384,8 +389,12 @@ static struct {
 ////////////////////////////////////////////////////////////////////////////////
 // LED output
 ////////////////////////////////////////////////////////////////////////////////
-// state of the GPS light (on/off)
-static bool GPS_light;
+static AP_BoardLED board_led;
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+static ToshibaLED_PX4 toshiba_led;
+#else
+static ToshibaLED toshiba_led;
+#endif
 
 ////////////////////////////////////////////////////////////////////////////////
 // GPS variables
@@ -713,6 +722,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { read_battery,           5,   1000 },
     { compass_accumulate,     1,   1500 },
     { barometer_accumulate,   1,    900 }, // 20
+    { update_toshiba_led,     1,    100 },
     { one_second_loop,       50,   3900 },
     { check_long_failsafe,   15,   1000 },
     { airspeed_ratio_update, 50,   1000 },
@@ -733,6 +743,16 @@ void setup() {
     // load the default values of variables listed in var_info[]
     AP_Param::setup_sketch_defaults();
 
+    // arduplane does not use arming nor pre-arm checks
+    AP_Notify::flags.armed = true;
+    AP_Notify::flags.pre_arm_check = true;
+
+    // initialise board leds
+    board_led.init();
+
+    // initialise toshiba led
+    toshiba_led.init();
+
     rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
 
     vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
@@ -992,7 +1012,6 @@ static void update_GPS(void)
 {
     static uint32_t last_gps_reading;
     g_gps->update();
-    update_GPS_light();
 
     if (g_gps->last_message_time_ms() != last_gps_reading) {
         last_gps_reading = g_gps->last_message_time_ms();

ArduPlane/system.pde

@@ -483,37 +483,13 @@ static void startup_INS_ground(bool do_accel_init)
     digitalWrite(C_LED_PIN, LED_OFF);
 }
 
-
-static void update_GPS_light(void)
+// update_toshiba_led - updates the status of the toshiba led
+// should be called at 50hz
+static void update_toshiba_led()
 {
-    // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
-    // ---------------------------------------------------------------------
-    switch (g_gps->status()) {
-        case GPS::NO_FIX:
-        case GPS::GPS_OK_FIX_2D:
-            // check if we've blinked since the last gps update
-            if (g_gps->valid_read) {
-                g_gps->valid_read = false;
-                GPS_light = !GPS_light;                     // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
-                if (GPS_light) {
-                    digitalWrite(C_LED_PIN, LED_OFF);
-                }else{
-                    digitalWrite(C_LED_PIN, LED_ON);
-                }
-            }
-            break;
-
-        case GPS::GPS_OK_FIX_3D:
-            digitalWrite(C_LED_PIN, LED_ON);                  //Turn LED C on when gps has valid fix AND home is set.
-            break;
-
-        default:
-            digitalWrite(C_LED_PIN, LED_OFF);
-            break;
-    }
+    AP_Notify::update();
 }
 
-
 static void resetPerfData(void) {
     mainLoop_count                  = 0;
     G_Dt_max                                = 0;

ArduPlane/test.pde

@@ -443,10 +443,6 @@ test_gps(uint8_t argc, const Menu::arg *argv)
     while(1) {
         delay(100);
 
-        // Blink GPS LED if we don't have a fix
-        // ------------------------------------
-        update_GPS_light();
-
         g_gps->update();
 
         if (g_gps->new_data) {