From d2a0913b2d6635087c88f4956e988a67847e8bc0 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 9 Nov 2012 21:29:56 -0800 Subject: [PATCH] ACM : formatting --- ArduCopter/ArduCopter.pde | 114 ++++++++++++++++++-------------------- 1 file changed, 55 insertions(+), 59 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b0cb518d6b..b369649901 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -64,27 +64,27 @@ #include #include #include -#include // ArduPilot Mega RC Library -#include // ArduPilot GPS library -#include // Arduino I2C lib -#include // Arduino SPI lib +#include // ArduPilot Mega RC Library +#include // ArduPilot GPS library +#include // Arduino I2C lib +#include // Arduino SPI lib #include // SPI3 library -#include // for removing conflict between optical flow and dataflash on SPI3 bus -#include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Analog to Digital Converter Library +#include // for removing conflict between optical flow and dataflash on SPI3 bus +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Analog to Digital Converter Library #include #include -#include // ArduPilot Mega Magnetometer Library -#include // ArduPilot Mega Vector/Matrix math Library -#include // Curve used to linearlise throttle pwm to thrust -#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library -#include // Parent header of Timer - // (only included for makefile libpath to work) -#include // TimerProcess is the scheduler for MPU6000 reads. +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // Curve used to linearlise throttle pwm to thrust +#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library +#include // Parent header of Timer + // (only included for makefile libpath to work) +#include // TimerProcess is the scheduler for MPU6000 reads. #include #include // PI library -#include // PID library -#include // RC Channel Library +#include // PID library +#include // RC Channel Library #include // AP Motors library #include // AP Motors library for Quad #include // AP Motors library for Tri @@ -95,16 +95,16 @@ #include // AP Motors library for Heli #include // AP Motors library for Heli #include // Range finder library -#include // Optical Flow library -#include // Filter library -#include // APM FIFO Buffer +#include // Optical Flow library +#include // Filter library +#include // APM FIFO Buffer #include // Mode Filter from Filter library #include // Mode Filter from Filter library #include // GPS Lead filter #include // APM relay #include // Photo or video camera #include // Camera/Antenna mount -#include // needed for AHRS build +#include // needed for AHRS build #include // ArduPilot Mega inertial navigation library #include // Complementary filter for combining barometer altitude with accelerometers #include @@ -114,19 +114,19 @@ #include "config.h" #include "config_channels.h" -#include // MAVLink GCS definitions +#include // MAVLink GCS definitions // Local modules #include "Parameters.h" #include "GCS.h" -#include // ArduPilot Mega Declination Helper Library +#include // ArduPilot Mega Declination Helper Library // Limits library - Puts limits on the vehicle, and takes recovery actions #include -#include // a limits library module -#include // a limits library module -#include // a limits library module +#include // a limits library module +#include // a limits library module +#include // a limits library module //////////////////////////////////////////////////////////////////////////////// @@ -189,10 +189,10 @@ DataFlash_APM1 DataFlash(&spi_semaphore); // // There are three basic options related to flight sensor selection. // -// - Normal flight mode. Real sensors are used. -// - HIL Attitude mode. Most sensors are disabled, as the HIL +// - Normal flight mode. Real sensors are used. +// - HIL Attitude mode. Most sensors are disabled, as the HIL // protocol supplies attitude information directly. -// - HIL Sensors mode. Synthetic sensors are configured that +// - HIL Sensors mode. Synthetic sensors are configured that // supply data from the simulation. // @@ -288,23 +288,24 @@ AP_AHRS_MPU6000 ahrs2(&ins, g_gps); // only works with APM2 #elif HIL_MODE == HIL_MODE_SENSORS // sensor emulators -AP_ADC_HIL adc; -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; +AP_ADC_HIL adc; +AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); -AP_InertialSensor_Stub ins; -AP_AHRS_DCM ahrs(&ins, g_gps); +AP_InertialSensor_Stub ins; +AP_AHRS_DCM ahrs(&ins, g_gps); static int32_t gps_base_alt; #elif HIL_MODE == HIL_MODE_ATTITUDE -AP_ADC_HIL adc; -AP_InertialSensor_Stub ins; +AP_ADC_HIL adc; +AP_InertialSensor_Stub ins; AP_AHRS_HIL ahrs(&ins, g_gps); AP_GPS_HIL g_gps_driver(NULL); -AP_Compass_HIL compass; // never used -AP_Baro_BMP085_HIL barometer; +AP_Compass_HIL compass; // never used +AP_Baro_BMP085_HIL barometer; + #ifdef OPTFLOW_ENABLED #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); @@ -407,34 +408,34 @@ static byte oldSwitchPosition; // This was added to try and deal with biger motors //static int16_t motor_filtered[11]; -#if FRAME_CONFIG == QUAD_FRAME +#if FRAME_CONFIG == QUAD_FRAME #define MOTOR_CLASS AP_MotorsQuad #endif -#if FRAME_CONFIG == TRI_FRAME +#if FRAME_CONFIG == TRI_FRAME #define MOTOR_CLASS AP_MotorsTri #endif -#if FRAME_CONFIG == HEXA_FRAME +#if FRAME_CONFIG == HEXA_FRAME #define MOTOR_CLASS AP_MotorsHexa #endif -#if FRAME_CONFIG == Y6_FRAME +#if FRAME_CONFIG == Y6_FRAME #define MOTOR_CLASS AP_MotorsY6 #endif -#if FRAME_CONFIG == OCTA_FRAME +#if FRAME_CONFIG == OCTA_FRAME #define MOTOR_CLASS AP_MotorsOcta #endif -#if FRAME_CONFIG == OCTA_QUAD_FRAME +#if FRAME_CONFIG == OCTA_QUAD_FRAME #define MOTOR_CLASS AP_MotorsOctaQuad #endif -#if FRAME_CONFIG == HELI_FRAME +#if FRAME_CONFIG == HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli #endif -#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); +#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); #elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); #else -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); +MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); #endif //////////////////////////////////////////////////////////////////////////////// @@ -492,22 +493,22 @@ static int8_t copter_leds_nav_blink = 0; // This is used to scale GPS values for EEPROM storage // 10^7 times Decimal GPS means 1 == 1cm // This approximation makes calculations integer and it's easy to read -static const float t7 = 10000000.0; +static const float t7 = 10000000.0; // We use atan2 and other trig techniques to calaculate angles // We need to scale the longitude up to make these calcs work // to account for decreasing distance between lines of longitude away from the equator -static float scaleLongUp = 1; +static float scaleLongUp = 1; // Sometimes we need to remove the scaling for distance calcs -static float scaleLongDown = 1; +static float scaleLongDown = 1; //////////////////////////////////////////////////////////////////////////////// // Mavlink specific //////////////////////////////////////////////////////////////////////////////// // Used by Mavlink for unknow reasons -static const float radius_of_earth = 6378100; // meters +static const float radius_of_earth = 6378100; // meters // Used by Mavlink for unknow reasons -static const float gravity = 9.80665; // meters/ sec^2 +static const float gravity = 9.80665; // meters/ sec^2 // Unions for getting byte values union float_int { @@ -932,15 +933,10 @@ AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs, 1); // Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters //////////////////////////////////////////////////////////////////////////////// #ifdef AP_LIMITS - -AP_Limits limits; - +AP_Limits limits; AP_Limit_GPSLock gpslock_limit(g_gps); - AP_Limit_Geofence geofence_limit(FENCE_START_BYTE, FENCE_WP_SIZE, MAX_FENCEPOINTS, g_gps, &home, ¤t_loc); - AP_Limit_Altitude altitude_limit(¤t_loc); - #endif ////////////////////////////////////////////////////////////////////////////////