Copter: integrate notify

ArduCopter/ArduCopter.pde

@@ -73,6 +73,8 @@
 #include <AP_HAL_Empty.h>
 
 // Application dependencies
+#include <AP_Notify.h>          // Notify library
+#include <AP_BoardLED.h>        // BoardLEDs library
 #include <GCS_MAVLink.h>        // MAVLink GCS definitions
 #include <AP_GPS.h>             // ArduPilot GPS library
 #include <DataFlash.h>          // ArduPilot Mega Flash Memory Library
@@ -133,7 +135,6 @@ static AP_HAL::BetterStream* cliSerial;
 
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
-
 ////////////////////////////////////////////////////////////////////////////////
 // Parameters
 ////////////////////////////////////////////////////////////////////////////////
@@ -145,6 +146,9 @@ static Parameters g;
 // main loop scheduler
 static AP_Scheduler scheduler;
 
+// AP_BoardLED instance
+static AP_BoardLED board_led;
+
 ////////////////////////////////////////////////////////////////////////////////
 // prototypes
 ////////////////////////////////////////////////////////////////////////////////
@@ -461,9 +465,6 @@ static uint8_t pid_log_counter;
 ////////////////////////////////////////////////////////////////////////////////
 // LED output
 ////////////////////////////////////////////////////////////////////////////////
-// This is current status for the LED lights state machine
-// setting this value changes the output of the LEDs
-static uint8_t led_mode = NORMAL_LEDS;
 // Blinking indicates GPS status
 static uint8_t copter_leds_GPS_blink;
 // Blinking indicates battery status
@@ -884,6 +885,9 @@ void setup() {
     // Load the default values of variables listed in var_info[]s
     AP_Param::setup_sketch_defaults();
 
+    // initialise board leds
+    board_led.init();
+
 #if CONFIG_SONAR == ENABLED
  #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
     sonar_analog_source = new AP_ADC_AnalogSource(
@@ -1190,8 +1194,6 @@ static void medium_loop()
         USERHOOK_MEDIUMLOOP
 #endif
 
-        // update board leds
-        update_board_leds();
 #if COPTER_LEDS == ENABLED
         update_copter_leds();
 #endif
@@ -1355,7 +1357,6 @@ static void update_GPS(void)
     static uint8_t ground_start_count  = 10;
 
     g_gps->update();
-    update_GPS_light();
 
     if (g_gps->new_data && last_gps_time != g_gps->time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
         // clear new data flag

ArduCopter/config.h

@@ -168,9 +168,6 @@
 // LED and IO Pins
 //
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
- # define A_LED_PIN        37
- # define B_LED_PIN        36
- # define C_LED_PIN        35
  # define LED_ON           HIGH
  # define LED_OFF          LOW
  # define PUSHBUTTON_PIN   41
@@ -178,9 +175,6 @@
  # define BATTERY_VOLT_PIN      0      // Battery voltage on A0
  # define BATTERY_CURR_PIN      1      // Battery current on A1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
- # define A_LED_PIN        27
- # define B_LED_PIN        26
- # define C_LED_PIN        25
  # define LED_ON           LOW
  # define LED_OFF          HIGH
  # define PUSHBUTTON_PIN   (-1)
@@ -188,9 +182,6 @@
  # define BATTERY_VOLT_PIN      1      // Battery voltage on A1
  # define BATTERY_CURR_PIN      2      // Battery current on A2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
- # define A_LED_PIN        27
- # define B_LED_PIN        26
- # define C_LED_PIN        25
  # define LED_ON           LOW
  # define LED_OFF          HIGH
  # define PUSHBUTTON_PIN   (-1)
@@ -198,9 +189,6 @@
  # define BATTERY_VOLT_PIN 1      // Battery voltage on A1
  # define BATTERY_CURR_PIN 2      // Battery current on A2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
- # define A_LED_PIN        27
- # define B_LED_PIN        26
- # define C_LED_PIN        25
  # define LED_ON           LOW
  # define LED_OFF          HIGH
  # define PUSHBUTTON_PIN   (-1)
@@ -209,9 +197,6 @@
  # define BATTERY_CURR_PIN -1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
 // XXX these are just copied, may not make sense
- # define A_LED_PIN        27
- # define B_LED_PIN        26
- # define C_LED_PIN        25
  # define LED_ON           LOW
  # define LED_OFF          HIGH
  # define PUSHBUTTON_PIN   (-1)

ArduCopter/control_modes.pde

@@ -295,7 +295,7 @@ static void auto_trim()
         auto_trim_counter--;
 
         // flash the leds
-        led_mode = SAVE_TRIM_LEDS;
+        notify.flags.save_trim = true;
 
         // calculate roll trim adjustment
         float roll_trim_adjustment = ToRad((float)g.rc_1.control_in / 4000.0f);
@@ -313,8 +313,7 @@ static void auto_trim()
         // on last iteration restore leds and accel gains to normal
         if(auto_trim_counter == 0) {
             ahrs.set_fast_gains(false);
-            led_mode = NORMAL_LEDS;
+            notify.flags.save_trim = false;
-            clear_leds();
         }
     }
 }

ArduCopter/leds.pde

@@ -1,162 +1,5 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-// update_board_leds - updates leds on board
-// should be called at 10hz
-static void update_board_leds()
-{
-    // we need to slow down the calls to the GPS and dancing lights to 3.33hz
-    static uint8_t counter = 0;
-    if(++counter >= 3){
-        counter = 0;
-    }
-
-    switch(led_mode) {
-    case NORMAL_LEDS:
-        update_arming_light();
-        if (counter == 0) {
-            update_GPS_light();
-        }
-        break;
-
-    case SAVE_TRIM_LEDS:
-        if (counter == 0) {
-            dancing_light();
-        }
-        break;
-    }
-}
-
-static void update_GPS_light(void)
-{
-    // 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;
-                ap_system.GPS_light = !ap_system.GPS_light;                     // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
-                if (ap_system.GPS_light) {
-                    digitalWrite(C_LED_PIN, LED_OFF);
-                }else{
-                    digitalWrite(C_LED_PIN, LED_ON);
-                }
-            }
-            break;
-
-        case GPS::GPS_OK_FIX_3D:
-            if(ap.home_is_set) {
-                digitalWrite(C_LED_PIN, LED_ON);                  //Turn LED C on when gps has valid fix AND home is set.
-            } else {
-                digitalWrite(C_LED_PIN, LED_OFF);
-            }
-            break;
-
-        default:
-            digitalWrite(C_LED_PIN, LED_OFF);
-            break;
-    }
-}
-
-static void update_arming_light(void)
-{
-    // counter to control state
-    static int8_t counter = 0;
-    counter++;
-
-    // disarmed
-    if(!motors.armed()) {
-        if(!ap.pre_arm_check) {
-            // failed pre-arm checks so double flash
-            switch(counter) {
-                case 0:
-                    ap_system.arming_light = true;
-                    break;
-                case 1:
-                    ap_system.arming_light = false;
-                    break;
-                case 2:
-                    ap_system.arming_light = true;
-                    break;
-                case 3:
-                case 4:
-                    ap_system.arming_light = false;
-                    break;
-                default:
-                    // reset counter to restart the sequence
-                    counter = -1;
-                    break;
-            }
-        }else{
-            // passed pre-arm checks so slower single flash
-            switch(counter) {
-                case 0:
-                case 1:
-                case 2:
-                    ap_system.arming_light = true;
-                    break;
-                case 3:
-                case 4:
-                case 5:
-                    ap_system.arming_light = false;
-                    break;
-                default:
-                    // reset counter to restart the sequence
-                    counter = -1;
-                    break;
-            }
-        }
-        // set arming led from arming_light flag
-        if(ap_system.arming_light) {
-            digitalWrite(A_LED_PIN, LED_ON);
-        }else{
-            digitalWrite(A_LED_PIN, LED_OFF);
-        }
-    }else{
-        // armed
-        if(!ap_system.arming_light) {
-            ap_system.arming_light = true;
-            digitalWrite(A_LED_PIN, LED_ON);
-        }
-    }
-}
-
-static void dancing_light()
-{
-    static uint8_t step;
-
-    if (step++ == 3)
-        step = 0;
-
-    switch(step)
-    {
-    case 0:
-        digitalWrite(C_LED_PIN, LED_OFF);
-        digitalWrite(A_LED_PIN, LED_ON);
-        break;
-
-    case 1:
-        digitalWrite(A_LED_PIN, LED_OFF);
-        digitalWrite(B_LED_PIN, LED_ON);
-        break;
-
-    case 2:
-        digitalWrite(B_LED_PIN, LED_OFF);
-        digitalWrite(C_LED_PIN, LED_ON);
-        break;
-    }
-}
-
-static void clear_leds()
-{
-    digitalWrite(A_LED_PIN, LED_OFF);
-    digitalWrite(B_LED_PIN, LED_OFF);
-    digitalWrite(C_LED_PIN, LED_OFF);
-    ap_system.arming_light = false;
-    led_mode = NORMAL_LEDS;
-}
-
 /////////////////////////////////////////////////////////////////////////////////////////////
 //	Copter LEDS by Robert Lefebvre
 //	Based on the work of U4eake, Bill Sanford, Max Levine, and Oliver

ArduCopter/motors.pde

@@ -182,9 +182,6 @@ static void init_arm_motors()
     // set hover throttle
     motors.set_mid_throttle(g.throttle_mid);
 
-    // update leds on board
-    update_arming_light();
-
 #if COPTER_LEDS == ENABLED
     piezo_beep_twice();
 #endif

ArduCopter/radio.pde

@@ -74,11 +74,10 @@ static void init_rc_out()
             g.esc_calibrate.set_and_save(1);
             // display message on console
             cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
+            // turn on esc calibration notification
+            notify.flags.esc_calibration = true;
             // block until we restart
-            while(1) {
+            while(1) {}
-                delay(200);
-                dancing_light();
-            }
         }else{
             cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
             // clear esc flag

ArduCopter/setup.pde

@@ -1356,7 +1356,7 @@ init_esc()
     while(1) {
         read_radio();
         delay(100);
-        dancing_light();
+        notify.flags.esc_calibration = true;
         motors.throttle_pass_through();
     }
 }

ArduCopter/system.pde

@@ -123,16 +123,6 @@ static void init_ardupilot()
     //
     report_version();
 
-    // setup IO pins
-    pinMode(A_LED_PIN, OUTPUT);                                 // GPS status LED
-    digitalWrite(A_LED_PIN, LED_OFF);
-
-    pinMode(B_LED_PIN, OUTPUT);                         // GPS status LED
-    digitalWrite(B_LED_PIN, LED_OFF);
-
-    pinMode(C_LED_PIN, OUTPUT);                         // GPS status LED
-    digitalWrite(C_LED_PIN, LED_OFF);
-
     relay.init(); 
 
 #if COPTER_LEDS == ENABLED
@@ -303,10 +293,6 @@ static void startup_ground(void)
     ahrs2.set_fast_gains(true);
 #endif
 
-    // reset the leds
-    // ---------------------------
-    clear_leds();
-
     // when we re-calibrate the gyros,
     // all previous I values are invalid
     reset_I_all();
@@ -367,10 +353,6 @@ static bool set_mode(uint8_t mode)
     bool success = false;
     bool ignore_checks = !motors.armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform
 
-    // report the GPS and Motor arming status
-    // To-Do: this should be initialised somewhere else related to the LEDs
-    led_mode = NORMAL_LEDS;
-
     switch(mode) {
         case ACRO:
             success = true;
@@ -632,8 +614,8 @@ static void check_usb_mux(void)
  */
 void flash_leds(bool on)
 {
-    digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON);
+    //digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON);
-    digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
+    //digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
 }
 
 /*

ArduCopter/test.pde

@@ -264,10 +264,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) {