rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +01:00
Code Issues Releases Activity

Disable accel smoothing during init/arming so it does not interfere

Browse Source
This commit is contained in:
Erik Gustavsson 2012-11-02 09:12:24 +01:00
parent b7bdf9861d
commit f63db712d0
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/Modules/Attitude/attitude.c
View File

@ -87,6 +87,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0; static float accelKi = 0;
static float accelKp = 0; static float accelKp = 0;
static float accel_alpha = 0; static float accel_alpha = 0;
static float accel_alpha_setting = 0;
static float yawBiasRate = 0; static float yawBiasRate = 0;
static float gyroGain = 0.42; static float gyroGain = 0.42;
static int16_t accelbias[3]; static int16_t accelbias[3];
@ -216,18 +217,21 @@ static void AttitudeTask(void *parameters)
accelKp = 1; accelKp = 1;
accelKi = 0.9; accelKi = 0.9;
yawBiasRate = 0.23; yawBiasRate = 0.23;
accel_alpha = 0;
init = 0; init = 0;
} }
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1; accelKp = 1;
accelKi = 0.9; accelKi = 0.9;
yawBiasRate = 0.23; yawBiasRate = 0.23;
accel_alpha = 0;
init = 0; init = 0;
} else if (init == 0) { } else if (init == 0) {
// Reload settings (all the rates) // Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate); AttitudeSettingsYawBiasRateGet(&yawBiasRate);
accel_alpha = accel_alpha_setting;
init = 1; init = 1;
} }
@ -543,10 +547,12 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module. // Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025; const float fakeDt = 0.0025;
if(attitudeSettings.AccelTau < 0.0001) if(attitudeSettings.AccelTau < 0.0001)
accel_alpha = 0; // not trusting this to resolve to 0 accel_alpha_setting = 0; // not trusting this to resolve to 0
else else
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); accel_alpha_setting = expf(-fakeDt / attitudeSettings.AccelTau);
accel_alpha = accel_alpha_setting;
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;