From e985253f1a702b4a852dbca075f3039515f57c9c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 8 Aug 2013 22:15:25 +0900 Subject: [PATCH] Copter: integrate notify --- ArduCopter/ArduCopter.pde | 15 ++-- ArduCopter/config.h | 15 ---- ArduCopter/control_modes.pde | 5 +- ArduCopter/leds.pde | 157 ----------------------------------- ArduCopter/motors.pde | 3 - ArduCopter/radio.pde | 7 +- ArduCopter/setup.pde | 2 +- ArduCopter/system.pde | 22 +---- ArduCopter/test.pde | 4 - 9 files changed, 16 insertions(+), 214 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5e540229af..0fcf5dbc04 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -73,6 +73,8 @@ #include // Application dependencies +#include // Notify library +#include // BoardLEDs library #include // MAVLink GCS definitions #include // ArduPilot GPS library #include // 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 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 55f62244ca..5febe128ba 100644 --- a/ArduCopter/config.h +++ b/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) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 0f79b23e38..434e169747 100644 --- a/ArduCopter/control_modes.pde +++ b/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; - clear_leds(); + notify.flags.save_trim = false; } } } diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index f7885e25c3..2b0be51537 100644 --- a/ArduCopter/leds.pde +++ b/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 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 552f85a1f4..e94f755dff 100644 --- a/ArduCopter/motors.pde +++ b/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 diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index f587dc7730..9d21f890e6 100644 --- a/ArduCopter/radio.pde +++ b/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) { - delay(200); - dancing_light(); - } + while(1) {} }else{ cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n")); // clear esc flag diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index c35cd29d1f..8fa6a7674a 100644 --- a/ArduCopter/setup.pde +++ b/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(); } } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 89f4d97f08..048ead5a43 100644 --- a/ArduCopter/system.pde +++ b/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(C_LED_PIN, on ? LED_ON : LED_OFF); + //digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON); + //digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF); } /* diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index b4cb45633c..333094edad 100644 --- a/ArduCopter/test.pde +++ b/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) {