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

Fix indentation

Browse Source
This commit is contained in:
Alessio Morale 2013-05-23 00:47:38 +02:00
parent 697874b315
commit 711f1ad35b
1 changed files with 21 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
flight/modules/Attitude/revolution/attitude.c
View File

@ -387,24 +387,25 @@ static int32_t updateAttitudeComplementary(bool first_run)
attitudeSettings.AccelKp = 1.0f; attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f; attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f; attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false; accel_filter_enabled = false;
rollPitchBiasRate = 0.01f; rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f; attitudeSettings.MagKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f; attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f; attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f; attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false; accel_filter_enabled = false;
rollPitchBiasRate = 0.01f; rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f; attitudeSettings.MagKp = 1.0f;
init = 0; init = 0;
} else if (init == 0) { } else if (init == 0) {
// Reload settings (all the rates) // Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
rollPitchBiasRate = 0.0f; rollPitchBiasRate = 0.0f;
if (accel_alpha > 0.0f) if (accel_alpha > 0.0f) {
accel_filter_enabled = true; accel_filter_enabled = true;
init = 1; }
init = 1;
} }
GyrosGet(&gyrosData); GyrosGet(&gyrosData);
@ -437,10 +438,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude // Account for accel magnitude
accel_mag = accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]; accel_mag = accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2];
accel_mag = sqrtf(accel_mag); accel_mag = sqrtf(accel_mag);
//TODO! check accel vector magnitude value for correctness // TODO! check accel vector magnitude value for correctness
accel_err[0] /= accel_mag; accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag; accel_err[1] /= accel_mag;
@ -448,17 +449,16 @@ static int32_t updateAttitudeComplementary(bool first_run)
float grot_mag; float grot_mag;
if (accel_filter_enabled) { if (accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
}
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); // TODO! check grot_mag value for correctness.
} else {
grot_mag = 1.0f;
}
// TODO! check grot_mag value for correctness. accel_err[0] /= (accel_mag * grot_mag);
accel_err[1] /= (accel_mag * grot_mag);
accel_err[0] /= (accel_mag * grot_mag); accel_err[2] /= (accel_mag * grot_mag);
accel_err[1] /= (accel_mag * grot_mag);
accel_err[2] /= (accel_mag * grot_mag);
if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) { if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) {
@ -498,15 +498,14 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias; GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias); GyrosBiasGet(&gyrosBias);
if(revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate; gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate; gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate;
gyrosBias.z -= - gyrosData.z * rollPitchBiasRate; gyrosBias.z -= -gyrosData.z * rollPitchBiasRate;
} else { } else {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - (gyrosData.x - gyrosBias.x) * rollPitchBiasRate;; gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - (gyrosData.x - gyrosBias.x) * rollPitchBiasRate;;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - (gyrosData.y - gyrosBias.y) * rollPitchBiasRate; gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - (gyrosData.y - gyrosBias.y) * rollPitchBiasRate;
gyrosBias.z -= - (gyrosData.z - gyrosBias.z) * rollPitchBiasRate; gyrosBias.z -= -(gyrosData.z - gyrosBias.z) * rollPitchBiasRate;
// if the raw values are not adjusted, we need to adjust here. // if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x; gyrosData.x -= gyrosBias.x;