rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-1149 Uncrustify

Browse Source
This commit is contained in:
Alessio Morale 2013-12-29 18:45:44 +01:00
parent ac3d7b8d11
commit e04cef2fa4
2 changed files with 81 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
flight/modules/Attitude/attitude.c
View File

@ -103,15 +103,17 @@ static int8_t rotate = 0;
static bool zero_during_arming = false; static bool zero_during_arming = false;
static bool bias_correct_gyro = true; static bool bias_correct_gyro = true;
// static float gyros_passed[3];
// temp coefficient to calculate gyro bias // temp coefficient to calculate gyro bias
static bool apply_gyro_temp = false; static bool apply_gyro_temp = false;
static bool apply_accel_temp = false; static bool apply_accel_temp = false;
static float gyro_temp_coeff[4] = {0}; static float gyro_temp_coeff[4] = { 0 };
static float accel_temp_coeff[4] = {0}; static float accel_temp_coeff[4] = { 0 };
// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings // Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
static float gyro_scale[3] = {0}; static float gyro_scale[3] = { 0 };
static float accel_scale[3] = {0}; static float accel_scale[3] = { 0 };
// For running trim flights // For running trim flights
@ -120,15 +122,15 @@ static volatile int32_t trim_accels[3];
static volatile int32_t trim_samples; static volatile int32_t trim_samples;
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535; int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define GRAV 9.81f #define GRAV 9.81f
#define STD_CC_ACCEL_SCALE (GRAV * 0.004f) #define STD_CC_ACCEL_SCALE (GRAV * 0.004f)
/* 0.004f is gravity / LSB */ /* 0.004f is gravity / LSB */
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665 #define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f #define STD_CC_ANALOG_GYRO_GAIN 0.42f
// Used to detect CC vs CC3D // Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob; static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02) #define BOARDISCC3D (bdinfo->board_rev == 0x02)
/** /**
* Initialise the module, called on startup * Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed * \returns 0 on success or -1 if initialisation failed
@ -330,9 +332,9 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
z += -accel_data.z; z += -accel_data.z;
} while ((i < 32) && (samples_remaining > 0)); } while ((i < 32) && (samples_remaining > 0));
float accel[3] = { accel_scale[0] * (float)x / i, float accel[3] = { accel_scale[0] * (float)x / i,
accel_scale[1] * (float)y / i, accel_scale[1] * (float)y / i,
accel_scale[2] * (float)z / i }; accel_scale[2] * (float)z / i };
if (rotate) { if (rotate) {
// TODO: rotate sensors too so stabilization is well behaved // TODO: rotate sensors too so stabilization is well behaved
@ -424,13 +426,13 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
accels[1] = mpu6000_data.accel_y * accel_scale[1]; accels[1] = mpu6000_data.accel_y * accel_scale[1];
accels[2] = mpu6000_data.accel_z * accel_scale[2]; accels[2] = mpu6000_data.accel_z * accel_scale[2];
if(apply_gyro_temp) { if (apply_gyro_temp) {
gyros[0] -= gyro_temp_coeff[0] * mpu6000_data.temperature; gyros[0] -= gyro_temp_coeff[0] * mpu6000_data.temperature;
gyros[1] -= gyro_temp_coeff[1] * mpu6000_data.temperature; gyros[1] -= gyro_temp_coeff[1] * mpu6000_data.temperature;
gyros[2] -= (gyro_temp_coeff[2] + gyro_temp_coeff[3] * mpu6000_data.temperature) * mpu6000_data.temperature; gyros[2] -= (gyro_temp_coeff[2] + gyro_temp_coeff[3] * mpu6000_data.temperature) * mpu6000_data.temperature;
} }
if(apply_accel_temp){ if (apply_accel_temp) {
accels[0] -= accel_temp_coeff[0] * mpu6000_data.temperature; accels[0] -= accel_temp_coeff[0] * mpu6000_data.temperature;
accels[1] -= accel_temp_coeff[1] * mpu6000_data.temperature; accels[1] -= accel_temp_coeff[1] * mpu6000_data.temperature;
accels[2] -= accel_temp_coeff[2] * mpu6000_data.temperature; accels[2] -= accel_temp_coeff[2] * mpu6000_data.temperature;
@ -629,53 +631,53 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
accel_filter_enabled = true; accel_filter_enabled = true;
} }
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
gyro_temp_coeff[0] = accelGyroSettings.gyro_temp_coeff.X; gyro_temp_coeff[0] = accelGyroSettings.gyro_temp_coeff.X;
gyro_temp_coeff[1] = accelGyroSettings.gyro_temp_coeff.Y; gyro_temp_coeff[1] = accelGyroSettings.gyro_temp_coeff.Y;
gyro_temp_coeff[2] = accelGyroSettings.gyro_temp_coeff.Z; gyro_temp_coeff[2] = accelGyroSettings.gyro_temp_coeff.Z;
gyro_temp_coeff[3] = accelGyroSettings.gyro_temp_coeff.Z2; gyro_temp_coeff[3] = accelGyroSettings.gyro_temp_coeff.Z2;
accel_temp_coeff[0] = accelGyroSettings.accel_temp_coeff.X; accel_temp_coeff[0] = accelGyroSettings.accel_temp_coeff.X;
accel_temp_coeff[1] = accelGyroSettings.accel_temp_coeff.Y; accel_temp_coeff[1] = accelGyroSettings.accel_temp_coeff.Y;
accel_temp_coeff[2] = accelGyroSettings.accel_temp_coeff.Z; accel_temp_coeff[2] = accelGyroSettings.accel_temp_coeff.Z;
apply_gyro_temp = ( fabsf(gyro_temp_coeff[0])> 1e-6f || apply_gyro_temp = (fabsf(gyro_temp_coeff[0]) > 1e-6f ||
fabsf(gyro_temp_coeff[1])> 1e-6f || fabsf(gyro_temp_coeff[1]) > 1e-6f ||
fabsf(gyro_temp_coeff[2])> 1e-6f || fabsf(gyro_temp_coeff[2]) > 1e-6f ||
fabsf(gyro_temp_coeff[3])> 1e-6f); fabsf(gyro_temp_coeff[3]) > 1e-6f);
apply_accel_temp = (fabsf(accel_temp_coeff[0])> 1e-6f || apply_accel_temp = (fabsf(accel_temp_coeff[0]) > 1e-6f ||
fabsf(accel_temp_coeff[1])> 1e-6f || fabsf(accel_temp_coeff[1]) > 1e-6f ||
fabsf(accel_temp_coeff[2])> 1e-6f); fabsf(accel_temp_coeff[2]) > 1e-6f);
gyro_correct_int[0] = accelGyroSettings.gyro_bias.X; gyro_correct_int[0] = accelGyroSettings.gyro_bias.X;
gyro_correct_int[1] = accelGyroSettings.gyro_bias.Y; gyro_correct_int[1] = accelGyroSettings.gyro_bias.Y;
gyro_correct_int[2] = accelGyroSettings.gyro_bias.Z; gyro_correct_int[2] = accelGyroSettings.gyro_bias.Z;
if(BOARDISCC3D) { if (BOARDISCC3D) {
accelbias[0] = accelGyroSettings.accel_bias.X; accelbias[0] = accelGyroSettings.accel_bias.X;
accelbias[1] = accelGyroSettings.accel_bias.Y; accelbias[1] = accelGyroSettings.accel_bias.Y;
accelbias[2] = accelGyroSettings.accel_bias.Z; accelbias[2] = accelGyroSettings.accel_bias.Z;
gyro_scale[0] = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale (); gyro_scale[0] = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale();
gyro_scale[1] = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale (); gyro_scale[1] = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
gyro_scale[2] = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale (); gyro_scale[2] = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
accel_scale[0] = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale(); accel_scale[0] = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
accel_scale[1] = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale(); accel_scale[1] = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
accel_scale[2] = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale(); accel_scale[2] = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
} else { } else {
// Original CC with analog gyros and ADXL accel // Original CC with analog gyros and ADXL accel
accelbias[0] = accelGyroSettings.accel_bias.X; accelbias[0] = accelGyroSettings.accel_bias.X;
accelbias[1] = accelGyroSettings.accel_bias.Y; accelbias[1] = accelGyroSettings.accel_bias.Y;
accelbias[2] = accelGyroSettings.accel_bias.Z; accelbias[2] = accelGyroSettings.accel_bias.Z;
gyro_scale[0] = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN; gyro_scale[0] = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale[1] = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN; gyro_scale[1] = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN;
gyro_scale[2] = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN; gyro_scale[2] = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN;
accel_scale[0] = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE; accel_scale[0] = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE;
accel_scale[1] = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE; accel_scale[1] = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE;
@ -712,7 +714,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples; accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
// Z should average -grav // Z should average -grav
accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV; accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings); AttitudeSettingsSet(&attitudeSettings);
} else { } else {
trim_requested = false; trim_requested = false;

64
flight/modules/Sensors/sensors.c
View File

@ -90,8 +90,8 @@ static float accel_scale[3] = { 0, 0, 0 };
static float gyro_staticbias[3] = { 0, 0, 0 }; static float gyro_staticbias[3] = { 0, 0, 0 };
static float gyro_scale[3] = { 0, 0, 0 }; static float gyro_scale[3] = { 0, 0, 0 };
// temp coefficient to calculate gyro bias // temp coefficient to calculate gyro bias
static float gyro_temp_coeff[4] = {0}; static float gyro_temp_coeff[4] = { 0 };
static float accel_temp_coeff[4] = {0}; static float accel_temp_coeff[4] = { 0 };
static float R[3][3] = { static float R[3][3] = {
{ 0 } { 0 }
@ -358,7 +358,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
- accel_temp_coeff[1] * accelSensorData.temperature, - accel_temp_coeff[1] * accelSensorData.temperature,
accels[2] * accel_scaling * accel_scale[2] accels[2] * accel_scaling * accel_scale[2]
- accel_bias[2] - accel_bias[2]
- accel_temp_coeff[2] * accelSensorData.temperature}; - accel_temp_coeff[2] * accelSensorData.temperature };
if (rotate) { if (rotate) {
rot_mult(R, accels_out, accels); rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0]; accelSensorData.x = accels[0];
@ -376,15 +376,15 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
(float)gyro_accum[1] / gyro_samples, (float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples }; (float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0]
- gyro_staticbias[0] - gyro_staticbias[0]
- gyro_temp_coeff[0] * gyroSensorData.temperature, - gyro_temp_coeff[0] * gyroSensorData.temperature,
gyros[1] * gyro_scaling * gyro_scale[1] gyros[1] * gyro_scaling * gyro_scale[1]
- gyro_staticbias[1] - gyro_staticbias[1]
- gyro_temp_coeff[1] * gyroSensorData.temperature, - gyro_temp_coeff[1] * gyroSensorData.temperature,
gyros[2] * gyro_scaling * gyro_scale[2] gyros[2] * gyro_scaling * gyro_scale[2]
- gyro_staticbias[2] - gyro_staticbias[2]
- gyro_temp_coeff[2] * gyroSensorData.temperature - gyro_temp_coeff[2] * gyroSensorData.temperature
- gyro_temp_coeff[3] * gyroSensorData.temperature * gyroSensorData.temperature}; - gyro_temp_coeff[3] * gyroSensorData.temperature * gyroSensorData.temperature };
if (rotate) { if (rotate) {
rot_mult(R, gyros_out, gyros); rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0]; gyroSensorData.x = gyros[0];
@ -441,28 +441,28 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{ {
RevoCalibrationGet(&cal); RevoCalibrationGet(&cal);
AccelGyroSettingsGet(&agcal); AccelGyroSettingsGet(&agcal);
mag_bias[0] = cal.mag_bias.X; mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y; mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z; mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X; mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y; mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z; mag_scale[2] = cal.mag_scale.Z;
accel_bias[0] = agcal.accel_bias.X; accel_bias[0] = agcal.accel_bias.X;
accel_bias[1] = agcal.accel_bias.Y; accel_bias[1] = agcal.accel_bias.Y;
accel_bias[2] = agcal.accel_bias.Z; accel_bias[2] = agcal.accel_bias.Z;
accel_scale[0] = agcal.accel_scale.X; accel_scale[0] = agcal.accel_scale.X;
accel_scale[1] = agcal.accel_scale.Y; accel_scale[1] = agcal.accel_scale.Y;
accel_scale[2] = agcal.accel_scale.Z; accel_scale[2] = agcal.accel_scale.Z;
gyro_staticbias[0] = agcal.gyro_bias.X; gyro_staticbias[0] = agcal.gyro_bias.X;
gyro_staticbias[1] = agcal.gyro_bias.Y; gyro_staticbias[1] = agcal.gyro_bias.Y;
gyro_staticbias[2] = agcal.gyro_bias.Z; gyro_staticbias[2] = agcal.gyro_bias.Z;
gyro_scale[0] = agcal.gyro_scale.X; gyro_scale[0] = agcal.gyro_scale.X;
gyro_scale[1] = agcal.gyro_scale.Y; gyro_scale[1] = agcal.gyro_scale.Y;
gyro_scale[2] = agcal.gyro_scale.Z; gyro_scale[2] = agcal.gyro_scale.Z;
gyro_temp_coeff[0] = agcal.gyro_temp_coeff.X; gyro_temp_coeff[0] = agcal.gyro_temp_coeff.X;
gyro_temp_coeff[1] = agcal.gyro_temp_coeff.Y; gyro_temp_coeff[1] = agcal.gyro_temp_coeff.Y;
gyro_temp_coeff[2] = agcal.gyro_temp_coeff.Z; gyro_temp_coeff[2] = agcal.gyro_temp_coeff.Z;
gyro_temp_coeff[3] = agcal.gyro_temp_coeff.Z2; gyro_temp_coeff[3] = agcal.gyro_temp_coeff.Z2;
accel_temp_coeff[0] = agcal.accel_temp_coeff.X; accel_temp_coeff[0] = agcal.accel_temp_coeff.X;
accel_temp_coeff[1] = agcal.accel_temp_coeff.Y; accel_temp_coeff[1] = agcal.accel_temp_coeff.Y;
accel_temp_coeff[2] = agcal.accel_temp_coeff.Z; accel_temp_coeff[2] = agcal.accel_temp_coeff.Z;