ACM : Formatting

This commit is contained in:
Jason Short 2012-12-13 20:12:39 -08:00
parent dda74afa21
commit 19ae5c30eb
6 changed files with 44 additions and 138 deletions

View File

@ -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
@ -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
// --------------------------
@ -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,

View File

@ -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;
# }
*/

View File

@ -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

View File

@ -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;
}
}

View File

@ -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

View File

@ -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