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

CC-18: Normalize the gravity vector to unity gain

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2754 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-08 16:34:31 +00:00 committed by peabody124
parent 74cc7dbfc9
commit cfe295377c
2 changed files with 21 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -133,10 +133,11 @@ static void AttitudeTask(void *parameters)
// Main task loop // Main task loop
while (1) { while (1) {
if(xTaskGetTickCount() < 5000) { if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias // For first 5 seconds use accels to get gyro bias
accelKi = 10; accelKp = 1000;
accelKp = 1; // Decrease the rate of gyro learning during init
accelKi = 100 / (xTaskGetTickCount() / 2000);
} else if (init == 0) { } else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle()); settingsUpdatedCb(AttitudeSettingsHandle());
init = 1; init = 1;
@ -219,18 +220,28 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
// Bad practice to assume structure order, but saves memory // Bad practice to assume structure order, but saves memory
float * q = &attitudeActual.q1; float * q = &attitudeActual.q1;
float * gyro = attitudeRaw->gyros; float gyro[3];
gyro[0] = attitudeRaw->gyros[0];
gyro[1] = attitudeRaw->gyros[1];
gyro[2] = attitudeRaw->gyros[2];
{ {
float * accels = attitudeRaw->accels; float * accels = attitudeRaw->accels;
float grot[3]; float grot[3];
float accel_err[3]; float accel_err[3];
// Rotate gravity to body frame and cross with accels // Rotate gravity to body frame and cross with accels
grot[0] = -9.81 * (2 * (q[1] * q[3] - q[0] * q[2])); grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -9.81 * (2 * (q[2] * q[3] + q[0] * q[1])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -9.81 * (q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) accels, (const float *) grot, accel_err); CrossProduct((const float *) accels, (const float *) grot, accel_err);
// Account for accel magnitude
float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are rad/s // Accumulate integral of error. Scale here so that units are rad/s
gyro_correct_int[0] += accel_err[0] * accelKi * dT; gyro_correct_int[0] += accel_err[0] * accelKi * dT;
gyro_correct_int[1] += accel_err[1] * accelKi * dT; gyro_correct_int[1] += accel_err[1] * accelKi * dT;
@ -281,7 +292,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
accelKp = attitudeSettings.AccelKp; accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKI; accelKi = attitudeSettings.AccelKi;
gyroGain = attitudeSettings.GyroGain; gyroGain = attitudeSettings.GyroGain;
} }
/** /**

4
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -2,8 +2,8 @@
<object name="AttitudeSettings" singleinstance="true" settings="true"> <object name="AttitudeSettings" singleinstance="true" settings="true">
<description>Settings for the @ref Attitude module used on CopterControl</description> <description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/> <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="AccelKp" units="channel" type="float" elements="1" defaultvalue="100"/>
<field name="AccelKI" units="channel" type="float" elements="1" defaultvalue="0.01"/> <field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="5"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>