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

OP-754/OPReview-374: Applied a very old patch, provided by cyr, to the latest next branch.

Browse Source
This commit is contained in:
Richard Flay (Hyper) 2013-05-04 09:10:02 +09:30
parent 137ddbf73a
commit 576d33464a
1 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -92,6 +92,7 @@ static bool accel_filter_enabled = false;
static float accels_filtered[3]; static float accels_filtered[3];
static float grot_filtered[3]; static float grot_filtered[3];
static float yawBiasRate = 0; static float yawBiasRate = 0;
static float rollPitchBiasRate = 0.0f;
static float gyroGain = 0.42; static float gyroGain = 0.42;
static int16_t accelbias[3]; static int16_t accelbias[3];
static float q[4] = {1,0,0,0}; static float q[4] = {1,0,0,0};
@ -216,17 +217,19 @@ static void AttitudeTask(void *parameters)
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias // Use accels to initialise attitude and calculate gyro bias
accelKp = 1; accelKp = 1;
accelKi = 0.9; accelKi = 0.0f;
yawBiasRate = 0.01; yawBiasRate = 0.01;
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false; accel_filter_enabled = false;
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.0f;
yawBiasRate = 0.01; yawBiasRate = 0.01;
rollPitchBiasRate = 0.01f;
accel_filter_enabled = false; accel_filter_enabled = false;
init = 0; init = 0;
} else if (init == 0) { } else if (init == 0) {
@ -234,6 +237,7 @@ static void AttitudeTask(void *parameters)
AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate); AttitudeSettingsYawBiasRateGet(&yawBiasRate);
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;
@ -357,6 +361,10 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
gyros->z += gyro_correct_int[2]; gyros->z += gyro_correct_int[2];
} }
// Force the roll & pitch gyro rates to average to zero during initialisation
gyro_correct_int[0] += - gyros->x * rollPitchBiasRate;
gyro_correct_int[1] += - gyros->y * rollPitchBiasRate;
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly) // and make it average zero (weakly)
gyro_correct_int[2] += - gyros->z * yawBiasRate; gyro_correct_int[2] += - gyros->z * yawBiasRate;
@ -428,6 +436,10 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
gyrosData->z += gyro_correct_int[2]; gyrosData->z += gyro_correct_int[2];
} }
// Force the roll & pitch gyro rates to average to zero during initialisation
gyro_correct_int[0] += - gyros->x * rollPitchBiasRate;
gyro_correct_int[1] += - gyros->y * rollPitchBiasRate;
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly) // and make it average zero (weakly)
gyro_correct_int[2] += - gyrosData->z * yawBiasRate; gyro_correct_int[2] += - gyrosData->z * yawBiasRate;