From 4846ba119e96c6dcfca83bd321852aa7c8302b6a Mon Sep 17 00:00:00 2001 From: uncrustify Date: Thu, 16 Aug 2012 22:40:30 -0700 Subject: [PATCH] uncrustify libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde --- .../AP_AHRS/examples/AHRS_Test/AHRS_Test.pde | 148 +++++++++--------- 1 file changed, 74 insertions(+), 74 deletions(-) diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde index 56b03ba3a4..723ad0db02 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde +++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde @@ -32,19 +32,19 @@ FastSerialPort0(Serial); FastSerialPort1(Serial1); Arduino_Mega_ISR_Registry isr_registry; -AP_TimerProcess scheduler; +AP_TimerProcess scheduler; #ifdef DESKTOP_BUILD -AP_Compass_HIL compass; +AP_Compass_HIL compass; #else AP_Compass_HMC5843 compass; #endif #ifdef APM2_HARDWARE - AP_InertialSensor_MPU6000 ins( 53 ); +AP_InertialSensor_MPU6000 ins( 53 ); # else - AP_ADC_ADS7844 adc; - AP_InertialSensor_Oilpan ins( &adc ); +AP_ADC_ADS7844 adc; +AP_InertialSensor_Oilpan ins( &adc ); #endif static GPS *g_gps; @@ -58,109 +58,109 @@ AP_AHRS_DCM ahrs(&imu, g_gps); //AP_AHRS_Quaternion ahrs(&imu, g_gps); //AP_AHRS_MPU6000 ahrs(&imu, g_gps, &ins); // only works with APM2 -AP_Baro_BMP085_HIL barometer; +AP_Baro_BMP085_HIL barometer; #ifdef APM2_HARDWARE -# define A_LED_PIN 27 -# define C_LED_PIN 25 -# define LED_ON LOW -# define LED_OFF HIGH -# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD + # define A_LED_PIN 27 + # define C_LED_PIN 25 + # define LED_ON LOW + # define LED_OFF HIGH + # define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD #else -# define A_LED_PIN 37 -# define C_LED_PIN 35 -# define LED_ON HIGH -# define LED_OFF LOW -# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD + # define A_LED_PIN 37 + # define C_LED_PIN 35 + # define LED_ON HIGH + # define LED_OFF LOW + # define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #endif static 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); } void setup(void) { - Serial.begin(115200); - Serial.println("Starting up..."); + Serial.begin(115200); + Serial.println("Starting up..."); #ifndef DESKTOP_BUILD - I2c.begin(); - I2c.timeOut(5); - I2c.setSpeed(true); + I2c.begin(); + I2c.timeOut(5); + I2c.setSpeed(true); #endif - SPI.begin(); - SPI.setClockDivider(SPI_CLOCK_DIV16); + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV16); #ifdef APM2_HARDWARE - // we need to stop the barometer from holding the SPI bus - pinMode(40, OUTPUT); - digitalWrite(40, HIGH); + // we need to stop the barometer from holding the SPI bus + pinMode(40, OUTPUT); + digitalWrite(40, HIGH); #endif - isr_registry.init(); - scheduler.init(&isr_registry); + isr_registry.init(); + scheduler.init(&isr_registry); - imu.init(IMU::COLD_START, delay, flash_leds, &scheduler); - imu.init_accel(delay, flash_leds); + imu.init(IMU::COLD_START, delay, flash_leds, &scheduler); + imu.init_accel(delay, flash_leds); - compass.set_orientation(MAG_ORIENTATION); - ahrs.init(); + compass.set_orientation(MAG_ORIENTATION); + ahrs.init(); - if( compass.init() ) { - Serial.printf("Enabling compass\n"); - ahrs.set_compass(&compass); - } else { - Serial.printf("No compass detected\n"); - } - g_gps = &g_gps_driver; + if( compass.init() ) { + Serial.printf("Enabling compass\n"); + ahrs.set_compass(&compass); + } else { + Serial.printf("No compass detected\n"); + } + g_gps = &g_gps_driver; #if WITH_GPS - g_gps->init(); + g_gps->init(); #endif } void loop(void) { - static uint16_t counter; - static uint32_t last_t, last_print, last_compass; - uint32_t now = micros(); - float heading = 0; + static uint16_t counter; + static uint32_t last_t, last_print, last_compass; + uint32_t now = micros(); + float heading = 0; - if (last_t == 0) { - last_t = now; - return; - } - last_t = now; + if (last_t == 0) { + last_t = now; + return; + } + last_t = now; - if (now - last_compass > 100*1000UL && - compass.read()) { - heading = compass.calculate_heading(ahrs.get_dcm_matrix()); - // read compass at 10Hz - last_compass = now; + if (now - last_compass > 100*1000UL && + compass.read()) { + heading = compass.calculate_heading(ahrs.get_dcm_matrix()); + // read compass at 10Hz + last_compass = now; #if WITH_GPS - g_gps->update(); + g_gps->update(); #endif - } + } - ahrs.update(); - counter++; + ahrs.update(); + counter++; - if (now - last_print >= 0.5e6) { - Vector3f drift = ahrs.get_gyro_drift(); - Serial.printf_P(PSTR("r:%4.1f p:%4.1f y:%4.1f drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"), - ToDeg(ahrs.roll), - ToDeg(ahrs.pitch), - ToDeg(ahrs.yaw), - ToDeg(drift.x), - ToDeg(drift.y), - ToDeg(drift.z), - compass.use_for_yaw()?ToDeg(heading):0.0, - (1.0e6*counter)/(now-last_print)); - last_print = now; - counter = 0; - } + if (now - last_print >= 0.5e6) { + Vector3f drift = ahrs.get_gyro_drift(); + Serial.printf_P(PSTR("r:%4.1f p:%4.1f y:%4.1f drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"), + ToDeg(ahrs.roll), + ToDeg(ahrs.pitch), + ToDeg(ahrs.yaw), + ToDeg(drift.x), + ToDeg(drift.y), + ToDeg(drift.z), + compass.use_for_yaw() ? ToDeg(heading) : 0.0, + (1.0e6*counter)/(now-last_print)); + last_print = now; + counter = 0; + } }