From 19ae5c30eb8949a6aa8f44ab4d63c2f6f706bf5f Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 13 Dec 2012 20:12:39 -0800 Subject: [PATCH] ACM : Formatting --- ArduCopter/ArduCopter.pde | 32 ++++++------- ArduCopter/GCS.pde | 94 --------------------------------------- ArduCopter/Log.pde | 6 +-- ArduCopter/navigation.pde | 10 ++--- ArduCopter/setup.pde | 34 +++++++------- ArduCopter/system.pde | 6 +-- 6 files changed, 44 insertions(+), 138 deletions(-) delete mode 100644 ArduCopter/GCS.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5b27ee139b..e10571e2b0 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -868,7 +868,7 @@ static int16_t gps_fix_count; // Time in microseconds of main control loop static uint32_t fast_loopTimer; // Time in microseconds of 50hz control loop -static uint32_t fiftyhz_loopTimer = 0; +static uint32_t fiftyhz_loopTimer; // Counters for branching from 10 hz control loop static byte medium_loopCounter; // Counters for branching from 3 1/3hz control loop @@ -894,7 +894,7 @@ static uint8_t save_trim_counter; // Reference to the AP relay object - APM1 only AP_Relay relay; -// a pin for reading the receiver RSSI voltage. The scaling by 0.25 +// a pin for reading the receiver RSSI voltage. The scaling by 0.25 // is to take the 0 to 1024 range down to an 8 bit range for MAVLink AP_AnalogSource_Arduino RSSI_pin(-1, 0.25); @@ -961,8 +961,8 @@ void loop() // 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; + G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops + fast_loopTimer = timer; // for mainloop failure monitoring mainLoop_count++; @@ -981,7 +981,7 @@ void loop() #endif // store the micros for the 50 hz timer - fiftyhz_loopTimer = timer; + fiftyhz_loopTimer = timer; // check for new GPS messages // -------------------------- @@ -1379,7 +1379,7 @@ static void super_slow_loop() // agmatthews - USERHOOKS #ifdef USERHOOK_SUPERSLOWLOOP USERHOOK_SUPERSLOWLOOP -#endif +#endif } // called at 100hz but data from sensor only arrives at 20 Hz @@ -1642,7 +1642,7 @@ void update_roll_pitch_mode(void) } switch(roll_pitch_mode) { - case ROLL_PITCH_ACRO: + case ROLL_PITCH_ACRO: #if FRAME_CONFIG == HELI_FRAME if(g.axis_enabled) { @@ -1720,7 +1720,7 @@ void update_roll_pitch_mode(void) roll_pitch_toy(); break; } - + #if FRAME_CONFIG != HELI_FRAME if(g.rc_3.control_in == 0 && control_mode <= ACRO) { reset_rate_I(); @@ -1870,7 +1870,7 @@ void update_throttle_mode(void) #else update_throttle_cruise(g.rc_3.control_in); #endif //HELI_FRAME - + // check if we've taken off yet if (!ap.takeoff_complete && motors.armed()) { @@ -1998,19 +1998,19 @@ static void update_trig(void){ yawvector.normalize(); cos_pitch_x = safe_sqrt(1 - (temp.c.x * temp.c.x)); // level = 1 - cos_roll_x = temp.c.z / cos_pitch_x; // level = 1 + cos_roll_x = temp.c.z / cos_pitch_x; // level = 1 - cos_pitch_x = constrain(cos_pitch_x, 0, 1.0); + cos_pitch_x = constrain(cos_pitch_x, 0, 1.0); // this relies on constrain() of infinity doing the right thing, // which it does do in avr-libc - cos_roll_x = constrain(cos_roll_x, -1.0, 1.0); + cos_roll_x = constrain(cos_roll_x, -1.0, 1.0); - sin_yaw_y = yawvector.x; // 1y = north - cos_yaw_x = yawvector.y; // 0x = north + sin_yaw_y = yawvector.x; // 1y = north + cos_yaw_x = yawvector.y; // 0x = north // added to convert earth frame to body frame for rate controllers - sin_pitch = -temp.c.x; - sin_roll = temp.c.y / cos_pitch_x; + sin_pitch = -temp.c.x; + sin_roll = temp.c.y / cos_pitch_x; //flat: // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, diff --git a/ArduCopter/GCS.pde b/ArduCopter/GCS.pde deleted file mode 100644 index c8fa2dc6d0..0000000000 --- a/ArduCopter/GCS.pde +++ /dev/null @@ -1,94 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/* - * GCS Protocol - * - * 4 Ardupilot Header - * D - * 5 Payload length - * 1 Message ID - * 1 Message Version - * 9 Payload byte 1 - * 8 Payload byte 2 - * 7 Payload byte 3 - * A Checksum byte 1 - * B Checksum byte 2 - * - */ - -/* - * #if GCS_PORT == 3 - # define SendSerw Serial3.write - # define SendSer Serial3.print - ##else - # define SendSerw Serial.write - # define SendSer Serial.print - ##endif - # - # byte mess_buffer[60]; - # byte buff_pointer; - # - # // Unions for getting byte values - # union f_out{ - # byte bytes[4]; - # float value; - # } floatOut; - # - # union i_out { - # byte bytes[2]; - # int16_t value; - # } intOut; - # - # union l_out{ - # byte bytes[4]; - # int32_t value; - # } longOut; - # - # // Add binary values to the buffer - # void write_byte(byte val) - # { - # mess_buffer[buff_pointer++] = val; - # } - # - # void write_int(int16_t val ) - # { - # intOut.value = val; - # mess_buffer[buff_pointer++] = intOut.bytes[0]; - # mess_buffer[buff_pointer++] = intOut.bytes[1]; - # } - # - # void write_float(float val) - # { - # floatOut.value = val; - # mess_buffer[buff_pointer++] = floatOut.bytes[0]; - # mess_buffer[buff_pointer++] = floatOut.bytes[1]; - # mess_buffer[buff_pointer++] = floatOut.bytes[2]; - # mess_buffer[buff_pointer++] = floatOut.bytes[3]; - # } - # - # void write_long(int32_t val) - # { - # longOut.value = val; - # mess_buffer[buff_pointer++] = longOut.bytes[0]; - # mess_buffer[buff_pointer++] = longOut.bytes[1]; - # mess_buffer[buff_pointer++] = longOut.bytes[2]; - # mess_buffer[buff_pointer++] = longOut.bytes[3]; - # } - # - # void flush(byte id) - # { - # byte mess_ck_a = 0; - # byte mess_ck_b = 0; - # byte i; - # - # SendSer("4D"); // This is the message preamble - # SendSerw(buff_pointer); // Length - # SendSerw(2); // id - # - # for (i = 0; i < buff_pointer; i++) { - # SendSerw(mess_buffer[i]); - # } - # - # buff_pointer = 0; - # } - */ \ No newline at end of file diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 778ce142a3..49c4cf0c0f 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -5,9 +5,9 @@ // Code to Write and Read packets from DataFlash log memory // Code to interact with the user to dump or erase logs - #define HEAD_BYTE1 0xA3 // Decimal 163 - #define HEAD_BYTE2 0x95 // Decimal 149 - #define END_BYTE 0xBA // Decimal 186 +#define HEAD_BYTE1 0xA3 // Decimal 163 +#define HEAD_BYTE2 0x95 // Decimal 149 +#define END_BYTE 0xBA // Decimal 186 // These are function definitions so the Menu can be constructed before the functions diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 5497368812..3a654a0b65 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -13,7 +13,7 @@ static void update_navigation() // used to calculate speed in X and Y, iterms // ------------------------------------------ - dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0; + dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0; nav_last_gps_update = millis(); // prevent runup from bad GPS @@ -292,7 +292,7 @@ static void update_nav_wp() loiter_sum += loiter_delta; circle_angle += (circle_rate * dTnav); - //1° = 0.0174532925 radians + //1deg = 0.0174532925 radians // wrap if (circle_angle > 6.28318531) @@ -331,8 +331,8 @@ static void update_nav_wp() nav_lon = g.pid_loiter_rate_lon.get_integrator(); nav_lat = g.pid_loiter_rate_lon.get_integrator(); - nav_lon = constrain(nav_lon, -2000, 2000); // 20° - nav_lat = constrain(nav_lat, -2000, 2000); // 20° + nav_lon = constrain(nav_lon, -2000, 2000); // 20m + nav_lat = constrain(nav_lat, -2000, 2000); // 20m break; } } @@ -617,4 +617,4 @@ static int32_t wrap_180(int32_t error) static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t deg_per_sec) { return wrap_360(current_yaw + constrain(wrap_180(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec)); -} \ No newline at end of file +} diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index bdf14e46bc..33d4ee551d 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -3,27 +3,27 @@ #if CLI_ENABLED == ENABLED // Functions called from the setup menu -static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); -static int8_t setup_motors (uint8_t argc, const Menu::arg *argv); -static int8_t setup_accel (uint8_t argc, const Menu::arg *argv); -static int8_t setup_accel_scale (uint8_t argc, const Menu::arg *argv); -static int8_t setup_frame (uint8_t argc, const Menu::arg *argv); -static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); -static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); -static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); -static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); -static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); -static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); -static int8_t setup_tune (uint8_t argc, const Menu::arg *argv); -static int8_t setup_range (uint8_t argc, const Menu::arg *argv); +static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); +static int8_t setup_motors (uint8_t argc, const Menu::arg *argv); +static int8_t setup_accel (uint8_t argc, const Menu::arg *argv); +static int8_t setup_accel_scale (uint8_t argc, const Menu::arg *argv); +static int8_t setup_frame (uint8_t argc, const Menu::arg *argv); +static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); +static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); +static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); +static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); +static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); +static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); +static int8_t setup_tune (uint8_t argc, const Menu::arg *argv); +static int8_t setup_range (uint8_t argc, const Menu::arg *argv); //static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); -static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); -static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv); +static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); +static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv); #if FRAME_CONFIG == HELI_FRAME -static int8_t setup_heli (uint8_t argc, const Menu::arg *argv); -static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv); +static int8_t setup_heli (uint8_t argc, const Menu::arg *argv); +static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv); #endif // Command/function table for the setup menu diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 214ae5c385..85c07387f4 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -364,7 +364,7 @@ static void startup_ground(void) // Warm up and read Gyro offsets // ----------------------------- - ins.init(AP_InertialSensor::COLD_START, + ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, mavlink_delay, flash_leds, &timer_scheduler); #if CLI_ENABLED == ENABLED @@ -407,8 +407,8 @@ static void set_mode(byte mode) mode = STABILIZE; } - control_mode = mode; - control_mode = constrain(control_mode, 0, NUM_MODES - 1); + control_mode = mode; + control_mode = constrain(control_mode, 0, NUM_MODES - 1); // used to stop fly_aways // set to false if we have low throttle