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

CC-35: Separate term for CC yaw bias rate and others

Browse Source 
Note: AttitudeSettings changed so you'll need to re-zero it

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3090 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-03-28 18:42:48 +00:00 committed by peabody124
parent cdb521ac4a
commit bd5925d526
2 changed files with 5 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -81,6 +81,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0;
static float accelKp = 0;
static float yawBiasRate = 0;
static float gyroGain = 0.42;
static int16_t accelbias[3];
@ -137,6 +138,7 @@ static void AttitudeTask(void *parameters)
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
yawBiasRate = 0.01 / (1 + xTaskGetTickCount() / 5000);
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
@ -177,29 +179,9 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
// Because most crafts wont get enough information from gravity to zero yaw gyro
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] *
accelKi;
yawBiasRate;
// Get the accel data
/* uint8_t i = 0;
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = 0;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = 0;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = 0;
do {
i++;
attitudeRaw->gyrotemp[0] = PIOS_ADXL345_Read(&accel_data);
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] += (float) accel_data.x * 0.004f * 9.81;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] += -(float) accel_data.y * 0.004f * 9.81;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] += -(float) accel_data.z * 0.004f * 9.81;
} while ( (i < 32) && (attitudeRaw->gyrotemp[0] > 0) );
attitudeRaw->gyrotemp[1] = i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] /= i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] /= i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] /= i;
*/
int32_t x = 0;
int32_t y = 0;
int32_t z = 0;
@ -312,6 +294,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];

1
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -5,6 +5,7 @@
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.01"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>