rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

CC-7: Make the gyro scale adjustable (for now at least, this was largely for

Browse Source 
debugging, let's compare values).

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2692 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-02 08:57:42 +00:00 committed by peabody124
parent 1663a838ff
commit 265737427b
2 changed files with 8 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -64,7 +64,6 @@
#define UPDATE_RATE 3
#define GYRO_NEUTRAL 1665
#define GYRO_SCALE (0.008f * 180 / M_PI)
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
// Private types
@ -169,16 +168,17 @@ static void updateSensors()
struct pios_adxl345_data accel_data;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = -(gyro[0] - GYRO_NEUTRAL) * GYRO_SCALE;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (gyro[1] - GYRO_NEUTRAL) * GYRO_SCALE;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = -(gyro[2] - GYRO_NEUTRAL) * GYRO_SCALE;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = -(gyro[0] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = -(gyro[2] - GYRO_NEUTRAL) * settings.GyroGain;
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] += gyro_correct_int[0];
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] += gyro_correct_int[1];
// Because most crafts wont get enough information from gravity to zero yaw gyro
gyro_correct_int[2] = (1-settings.GyroBiasTau) * gyro_correct_int[2] - settings.GyroBiasTau * attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z];
gyro_correct_int[2] += - attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] *
settings.AccelKI * UPDATE_RATE / 1000;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] += gyro_correct_int[2];

7
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -1,10 +1,9 @@
<xml>
<object name="AttitudeSettings" singleinstance="true" settings="true">
<description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="GyroBiasTau" units="channel" type="float" elements="1" defaultvalue="0.00001"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.00001"/>
<field name="AccelKI" units="channel" type="float" elements="1" defaultvalue="0.00001"/>
<field name="AccelILim" units="rad/s" type="float" elements="1" defaultvalue="0.5"/>
<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.01"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>