From 07a7a1acd8a5c2fe18d224867b68b2384f0c12ac Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Mon, 19 Nov 2012 01:16:07 +0900 Subject: [PATCH] ArduCopter: replaced digitalRead and digitalWrite with faster calls improved performance logging to dataflash --- ArduCopter/ArduCopter.pde | 41 +++++------- ArduCopter/Attitude.pde | 8 +-- ArduCopter/Log.pde | 23 ++++--- ArduCopter/config.h | 6 +- ArduCopter/leds.pde | 136 +++++++++++++++++++------------------- ArduCopter/perf_info.pde | 50 ++++++++++++++ ArduCopter/sensors.pde | 4 +- ArduCopter/setup.pde | 8 +-- ArduCopter/system.pde | 6 +- ArduCopter/test.pde | 4 +- 10 files changed, 170 insertions(+), 116 deletions(-) create mode 100644 ArduCopter/perf_info.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index df4b485b2c..ce2c2301f5 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -108,6 +108,7 @@ #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library #include // Complementary filter for combining barometer altitude with accelerometers +#include // faster digital write for LEDs #include // Configuration @@ -230,7 +231,7 @@ AP_Baro_MS5611 barometer; AP_Compass_HMC5843 compass; #endif - #ifdef OPTFLOW_ENABLED + #if OPTFLOW == ENABLED #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); #else @@ -307,17 +308,18 @@ AP_GPS_HIL g_gps_driver(NULL); AP_Compass_HIL compass; // never used AP_Baro_BMP085_HIL barometer; -#ifdef OPTFLOW_ENABLED -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #if OPTFLOW == ENABLED + #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); -#else + #else AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); -#endif -#endif + #endif // CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + #endif // OPTFLOW == ENABLED + #ifdef DESKTOP_BUILD #include SITL sitl; - #endif + #endif // DESKTOP_BUILD static int32_t gps_base_alt; #else #error Unrecognised HIL_MODE setting. @@ -942,7 +944,9 @@ void loop() Log_Write_Data(DATA_FAST_LOOP, (int32_t)(timer - fast_loopTimer)); #endif - //PORTK |= B00010000; + // check loop time + perf_info_check_loop_time(timer - fast_loopTimer); + G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops fast_loopTimer = timer; @@ -965,9 +969,6 @@ void loop() // store the micros for the 50 hz timer fiftyhz_loopTimer = timer; - // port manipulation for external timing of main loops - //PORTK |= B01000000; - // check for new GPS messages // -------------------------- update_GPS(); @@ -991,14 +992,13 @@ void loop() counter_one_herz = 0; } perf_mon_counter++; - if (perf_mon_counter > 600 ) { + if (perf_mon_counter >= 500 ) { // 500 iterations at 50hz = 10 seconds if (g.log_bitmask & MASK_LOG_PM) Log_Write_Performance(); - + perf_info_reset(); gps_fix_count = 0; perf_mon_counter = 0; } - //PORTK &= B10111111; } } else { #ifdef DESKTOP_BUILD @@ -1017,12 +1017,7 @@ void loop() } } - // port manipulation for external timing of main loops - //PORTK &= B11101111; - } -// PORTK |= B01000000; -// PORTK &= B10111111; // Main loop - 100hz static void fast_loop() @@ -1052,11 +1047,11 @@ static void fast_loop() // optical flow // -------------------- -#ifdef OPTFLOW_ENABLED +#if OPTFLOW == ENABLED if(g.optflow_enabled) { update_optical_flow(); } -#endif +#endif // OPTFLOW == ENABLED // Read radio and 3-position switch on radio // ----------------------------------------- @@ -1397,7 +1392,7 @@ static void super_slow_loop() } // called at 100hz but data from sensor only arrives at 20 Hz -#ifdef OPTFLOW_ENABLED +#if OPTFLOW == ENABLED static void update_optical_flow(void) { static uint32_t last_of_update = 0; @@ -1418,7 +1413,7 @@ static void update_optical_flow(void) } } } -#endif +#endif // OPTFLOW == ENABLED // called at 50hz static void update_GPS(void) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index a43543b79a..215f66af43 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -600,7 +600,7 @@ static int16_t heli_get_angle_boost(int16_t throttle) static int32_t get_of_roll(int32_t input_roll) { -#ifdef OPTFLOW_ENABLED +#if OPTFLOW == ENABLED static float tot_x_cm = 0; // total distance from target static uint32_t last_of_roll_update = 0; int32_t new_roll = 0; @@ -648,13 +648,13 @@ get_of_roll(int32_t input_roll) return input_roll+of_roll; #else return input_roll; -#endif +#endif // OPTFLOW == ENABLED } static int32_t get_of_pitch(int32_t input_pitch) { -#ifdef OPTFLOW_ENABLED +#if OPTFLOW == ENABLED static float tot_y_cm = 0; // total distance from target static uint32_t last_of_pitch_update = 0; int32_t new_pitch = 0; @@ -703,5 +703,5 @@ get_of_pitch(int32_t input_pitch) return input_pitch+of_pitch; #else return input_pitch; -#endif +#endif // OPTFLOW == ENABLED } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 98b1275808..d8105645a6 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -508,7 +508,7 @@ static void Log_Read_Motors() // Write an optical flow packet. Total length : 30 bytes static void Log_Write_Optflow() { - #ifdef OPTFLOW_ENABLED + #if OPTFLOW == ENABLED DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_OPTFLOW_MSG); @@ -522,13 +522,12 @@ static void Log_Write_Optflow() DataFlash.WriteLong(of_roll); DataFlash.WriteLong(of_pitch); DataFlash.WriteByte(END_BYTE); - #endif + #endif // OPTFLOW == ENABLED } // Read an optical flow packet. static void Log_Read_Optflow() { - #ifdef OPTFLOW_ENABLED int16_t temp1 = DataFlash.ReadInt(); // 1 int16_t temp2 = DataFlash.ReadInt(); // 2 int16_t temp3 = DataFlash.ReadInt(); // 3 @@ -549,7 +548,6 @@ static void Log_Read_Optflow() temp7, (long)temp8, (long)temp9); - #endif } // Write an Nav Tuning packet. Total length : 24 bytes @@ -673,10 +671,13 @@ static void Log_Write_Performance() DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - DataFlash.WriteByte(0); //1 - was adc_constraints + DataFlash.WriteByte(0); //1 DataFlash.WriteByte(ahrs.renorm_range_count); //2 DataFlash.WriteByte(ahrs.renorm_blowup_count); //3 DataFlash.WriteByte(gps_fix_count); //4 + DataFlash.WriteInt(perf_info_get_num_long_running()); //5 - number of long running loops + DataFlash.WriteLong(perf_info_get_num_loops()); //6 - total number of loops + DataFlash.WriteLong(perf_info_get_max_time()); //7 - time of longest running loop DataFlash.WriteByte(END_BYTE); } @@ -687,13 +688,19 @@ static void Log_Read_Performance() int8_t temp2 = DataFlash.ReadByte(); int8_t temp3 = DataFlash.ReadByte(); int8_t temp4 = DataFlash.ReadByte(); + uint16_t temp5 = DataFlash.ReadInt(); + uint32_t temp6 = DataFlash.ReadLong(); + uint32_t temp7 = DataFlash.ReadLong(); - //1 2 3 4 - Serial.printf_P(PSTR("PM, %d, %d, %d, %d\n"), + // 1 2 3 4 5 6 7 + Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %u, %lu, %lu\n"), (int)temp1, (int)temp2, (int)temp3, - (int)temp4); + (int)temp4, + temp5, + temp6, + temp7); } // Write a command processing packet. Total length : 21 bytes diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 95c98780c5..8ba7e73097 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -348,10 +348,10 @@ ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included - #define OPTFLOW_ENABLED + #define OPTFLOW ENABLED #endif -#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) - # define OPTFLOW DISABLED +#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) + # define OPTFLOW DISABLED #endif #ifndef OPTFLOW_ORIENTATION # define OPTFLOW_ORIENTATION AP_OPTICALFLOW_ADNS3080_PINS_FORWARD diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 18b2232629..28949b17c0 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -1,3 +1,5 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + static void update_lights() { switch(led_mode) { @@ -20,9 +22,9 @@ static void update_GPS_light(void) case (2): if(ap.home_is_set) { // JLN update - digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set. + digitalWriteFast(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); + digitalWriteFast(C_LED_PIN, LED_OFF); } break; @@ -30,16 +32,16 @@ static void update_GPS_light(void) if (g_gps->valid_read == true) { 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); + digitalWriteFast(C_LED_PIN, LED_OFF); }else{ - digitalWrite(C_LED_PIN, LED_ON); + digitalWriteFast(C_LED_PIN, LED_ON); } g_gps->valid_read = false; } break; default: - digitalWrite(C_LED_PIN, LED_OFF); + digitalWriteFast(C_LED_PIN, LED_OFF); break; } } @@ -51,14 +53,14 @@ static void update_motor_light(void) // blink if(ap_system.motor_light) { - digitalWrite(A_LED_PIN, LED_ON); + digitalWriteFast(A_LED_PIN, LED_ON); }else{ - digitalWrite(A_LED_PIN, LED_OFF); + digitalWriteFast(A_LED_PIN, LED_OFF); } }else{ if(!ap_system.motor_light) { ap_system.motor_light = true; - digitalWrite(A_LED_PIN, LED_ON); + digitalWriteFast(A_LED_PIN, LED_ON); } } } @@ -73,26 +75,26 @@ static void dancing_light() switch(step) { case 0: - digitalWrite(C_LED_PIN, LED_OFF); - digitalWrite(A_LED_PIN, LED_ON); + digitalWriteFast(C_LED_PIN, LED_OFF); + digitalWriteFast(A_LED_PIN, LED_ON); break; case 1: - digitalWrite(A_LED_PIN, LED_OFF); - digitalWrite(B_LED_PIN, LED_ON); + digitalWriteFast(A_LED_PIN, LED_OFF); + digitalWriteFast(B_LED_PIN, LED_ON); break; case 2: - digitalWrite(B_LED_PIN, LED_OFF); - digitalWrite(C_LED_PIN, LED_ON); + digitalWriteFast(B_LED_PIN, LED_OFF); + digitalWriteFast(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); + digitalWriteFast(A_LED_PIN, LED_OFF); + digitalWriteFast(B_LED_PIN, LED_OFF); + digitalWriteFast(C_LED_PIN, LED_OFF); ap_system.motor_light = false; led_mode = NORMAL_LEDS; } @@ -191,56 +193,56 @@ static void update_copter_leds(void) } static void copter_leds_reset(void) { - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); - digitalWrite(COPTER_LED_3, COPTER_LED_OFF); - digitalWrite(COPTER_LED_4, COPTER_LED_OFF); - digitalWrite(COPTER_LED_5, COPTER_LED_OFF); - digitalWrite(COPTER_LED_6, COPTER_LED_OFF); - digitalWrite(COPTER_LED_7, COPTER_LED_OFF); - digitalWrite(COPTER_LED_8, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_5, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF); } static void copter_leds_on(void) { if ( !bitRead(g.copter_leds_mode, 2) ) { - digitalWrite(COPTER_LED_1, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_1, COPTER_LED_ON); } #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 if ( !bitRead(g.copter_leds_mode, 3) ) { - digitalWrite(COPTER_LED_2, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); } #else - digitalWrite(COPTER_LED_2, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); #endif if ( !bitRead(g.copter_leds_mode, 1) ) { - digitalWrite(COPTER_LED_3, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_3, COPTER_LED_ON); } - digitalWrite(COPTER_LED_4, COPTER_LED_ON); - digitalWrite(COPTER_LED_5, COPTER_LED_ON); - digitalWrite(COPTER_LED_6, COPTER_LED_ON); - digitalWrite(COPTER_LED_7, COPTER_LED_ON); - digitalWrite(COPTER_LED_8, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_4, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_5, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_6, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_7, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_8, COPTER_LED_ON); } static void copter_leds_off(void) { if ( !bitRead(g.copter_leds_mode, 2) ) { - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF); } #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 if ( !bitRead(g.copter_leds_mode, 3) ) { - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); } #else - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); #endif if ( !bitRead(g.copter_leds_mode, 1) ) { - digitalWrite(COPTER_LED_3, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF); } - digitalWrite(COPTER_LED_4, COPTER_LED_OFF); - digitalWrite(COPTER_LED_5, COPTER_LED_OFF); - digitalWrite(COPTER_LED_6, COPTER_LED_OFF); - digitalWrite(COPTER_LED_7, COPTER_LED_OFF); - digitalWrite(COPTER_LED_8, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_5, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF); } static void copter_leds_slow_blink(void) { @@ -271,42 +273,42 @@ static void copter_leds_oscillate(void) { copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds if ( !bitRead(g.copter_leds_mode, 2) ) { - digitalWrite(COPTER_LED_1, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_1, COPTER_LED_ON); } #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 if ( !bitRead(g.copter_leds_mode, 3) ) { - digitalWrite(COPTER_LED_2, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); } #else - digitalWrite(COPTER_LED_2, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_2, COPTER_LED_ON); #endif if ( !bitRead(g.copter_leds_mode, 1) ) { - digitalWrite(COPTER_LED_3, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF); } - digitalWrite(COPTER_LED_4, COPTER_LED_OFF); - digitalWrite(COPTER_LED_5, COPTER_LED_ON); - digitalWrite(COPTER_LED_6, COPTER_LED_ON); - digitalWrite(COPTER_LED_7, COPTER_LED_OFF); - digitalWrite(COPTER_LED_8, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_5, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_6, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF); }else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) { if ( !bitRead(g.copter_leds_mode, 2) ) { - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF); } #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 if ( !bitRead(g.copter_leds_mode, 3) ) { - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); } #else - digitalWrite(COPTER_LED_2, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF); #endif if ( !bitRead(g.copter_leds_mode, 1) ) { - digitalWrite(COPTER_LED_3, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_3, COPTER_LED_ON); } - digitalWrite(COPTER_LED_4, COPTER_LED_ON); - digitalWrite(COPTER_LED_5, COPTER_LED_OFF); - digitalWrite(COPTER_LED_6, COPTER_LED_OFF); - digitalWrite(COPTER_LED_7, COPTER_LED_ON); - digitalWrite(COPTER_LED_8, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_4, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_5, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_7, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_8, COPTER_LED_ON); } else copter_leds_motor_blink = 0; // start blink cycle again } @@ -314,11 +316,11 @@ static void copter_leds_oscillate(void) { static void copter_leds_GPS_on(void) { - digitalWrite(COPTER_LED_3, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_3, COPTER_LED_ON); } static void copter_leds_GPS_off(void) { - digitalWrite(COPTER_LED_3, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF); } static void copter_leds_GPS_slow_blink(void) { @@ -345,19 +347,19 @@ static void copter_leds_GPS_fast_blink(void) { } static void copter_leds_aux_off(void){ - digitalWrite(COPTER_LED_1, COPTER_LED_OFF); + digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF); } static void copter_leds_aux_on(void){ - digitalWrite(COPTER_LED_1, COPTER_LED_ON); + digitalWriteFast(COPTER_LED_1, COPTER_LED_ON); } void piezo_on(){ - digitalWrite(PIEZO_PIN,HIGH); + digitalWriteFast(PIEZO_PIN,HIGH); } void piezo_off(){ - digitalWrite(PIEZO_PIN,LOW); + digitalWriteFast(PIEZO_PIN,LOW); } void piezo_beep(){ // Note! This command should not be used in time sensitive loops diff --git a/ArduCopter/perf_info.pde b/ArduCopter/perf_info.pde new file mode 100644 index 0000000000..3f3d98b125 --- /dev/null +++ b/ArduCopter/perf_info.pde @@ -0,0 +1,50 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// +// high level performance monitoring +// +// we measure the main loop time +// + +#define PERF_INFO_OVERTIME_THRESHOLD_MICROS 10500 + +uint16_t perf_info_loop_count; +uint32_t perf_info_max_time; +uint16_t perf_info_long_running; + +// perf_info_reset - reset all records of loop time to zero +void perf_info_reset() +{ + perf_info_loop_count = 0; + perf_info_max_time = 0; + perf_info_long_running = 0; +} + +// perf_info_check_loop_time - check latest loop time vs min, max and overtime threshold +void perf_info_check_loop_time(uint32_t time_in_micros) +{ + perf_info_loop_count++; + if( time_in_micros > perf_info_max_time) { + perf_info_max_time = time_in_micros; + } + if( time_in_micros > PERF_INFO_OVERTIME_THRESHOLD_MICROS ) { + perf_info_long_running++; + } +} + +// perf_info_get_long_running_percentage - get number of long running loops as a percentage of the total number of loops +uint32_t perf_info_get_num_loops() +{ + return perf_info_loop_count; +} + +// perf_info_get_max_time - return maximum loop time (in microseconds) +uint32_t perf_info_get_max_time() +{ + return perf_info_max_time; +} + +// perf_info_get_num_long_running - get number of long running loops +uint32_t perf_info_get_num_long_running() +{ + return perf_info_long_running; +} \ No newline at end of file diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index ec4cc0d1c3..3c8a05eed2 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -50,7 +50,7 @@ static void init_compass() static void init_optflow() { -#ifdef OPTFLOW_ENABLED +#if OPTFLOW == ENABLED if( optflow.init(false, &timer_scheduler, &spi_semaphore, &spi3_semaphore) == false ) { g.optflow_enabled = false; Serial.print_P(PSTR("\nFailed to Init OptFlow ")); @@ -65,7 +65,7 @@ static void init_optflow() // resume timer timer_scheduler.resume_timer(); -#endif +#endif // OPTFLOW == ENABLED } // read_battery - check battery voltage and current and invoke failsafe if necessary diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index b8d31dcbc4..d3393caaf2 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -715,7 +715,7 @@ static void clear_offsets() static int8_t setup_optflow(uint8_t argc, const Menu::arg *argv) { - #ifdef OPTFLOW_ENABLED + #if OPTFLOW == ENABLED if (!strcmp_P(argv[1].str, PSTR("on"))) { g.optflow_enabled = true; init_optflow(); @@ -731,7 +731,7 @@ setup_optflow(uint8_t argc, const Menu::arg *argv) g.optflow_enabled.save(); report_optflow(); - #endif + #endif // OPTFLOW == ENABLED return 0; } @@ -857,7 +857,7 @@ static void report_flight_modes() void report_optflow() { - #ifdef OPTFLOW_ENABLED + #if OPTFLOW == ENABLED Serial.printf_P(PSTR("OptFlow\n")); print_divider(); @@ -868,7 +868,7 @@ void report_optflow() // degrees(g.optflow_fov)); print_blanks(2); - #endif + #endif // OPTFLOW == ENABLED } #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 9d29d41f5e..ffb0bbe56c 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -62,7 +62,7 @@ static void init_ardupilot() // USB_MUX_PIN pinMode(USB_MUX_PIN, INPUT); - ap_system.usb_connected = !digitalRead(USB_MUX_PIN); + ap_system.usb_connected = !digitalReadFast(USB_MUX_PIN); if (!ap_system.usb_connected) { // USB is not connected, this means UART0 may be a Xbee, with // its darned bricking problem. We can't write to it for at @@ -612,7 +612,7 @@ static void update_throttle_cruise(int16_t tmp) static boolean check_startup_for_CLI() { - return (digitalRead(SLIDE_SWITCH_PIN) == 0); + return (digitalReadFast(SLIDE_SWITCH_PIN) == 0); } #endif // CLI_ENABLED @@ -639,7 +639,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) #if USB_MUX_PIN > 0 static void check_usb_mux(void) { - bool usb_check = !digitalRead(USB_MUX_PIN); + bool usb_check = !digitalReadFast(USB_MUX_PIN); if (usb_check == ap_system.usb_connected) { return; } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index b3122b5850..bb359fe6f7 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -935,7 +935,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) static int8_t test_optflow(uint8_t argc, const Menu::arg *argv) { -#ifdef OPTFLOW_ENABLED +#if OPTFLOW == ENABLED if(g.optflow_enabled) { Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); print_hit_enter(); @@ -964,7 +964,7 @@ test_optflow(uint8_t argc, const Menu::arg *argv) #else print_test_disabled(); return (0); -#endif +#endif // OPTFLOW == ENABLED }