mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
CC-7: Make the gyro scale adjustable (for now at least, this was largely for
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:
parent
1663a838ff
commit
265737427b
@ -64,7 +64,6 @@
|
|||||||
|
|
||||||
#define UPDATE_RATE 3
|
#define UPDATE_RATE 3
|
||||||
#define GYRO_NEUTRAL 1665
|
#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)
|
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
|
||||||
// Private types
|
// Private types
|
||||||
@ -169,16 +168,17 @@ static void updateSensors()
|
|||||||
|
|
||||||
struct pios_adxl345_data accel_data;
|
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_X] = -(gyro[0] - GYRO_NEUTRAL) * settings.GyroGain;
|
||||||
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (gyro[1] - GYRO_NEUTRAL) * GYRO_SCALE;
|
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (gyro[1] - GYRO_NEUTRAL) * settings.GyroGain;
|
||||||
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = -(gyro[2] - GYRO_NEUTRAL) * GYRO_SCALE;
|
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
|
// 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_X] += gyro_correct_int[0];
|
||||||
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] += gyro_correct_int[1];
|
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] += gyro_correct_int[1];
|
||||||
|
|
||||||
// Because most crafts wont get enough information from gravity to zero yaw gyro
|
// 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];
|
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] += gyro_correct_int[2];
|
||||||
|
|
||||||
|
|
||||||
|
@ -1,10 +1,9 @@
|
|||||||
<xml>
|
<xml>
|
||||||
<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="GyroBiasTau" units="channel" type="float" elements="1" defaultvalue="0.00001"/>
|
<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.00001"/>
|
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.01"/>
|
||||||
<field name="AccelKI" units="channel" type="float" elements="1" defaultvalue="0.00001"/>
|
<field name="AccelKI" units="channel" type="float" elements="1" defaultvalue="0.01"/>
|
||||||
<field name="AccelILim" units="rad/s" type="float" elements="1" defaultvalue="0.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"/>
|
||||||
|
Loading…
Reference in New Issue
Block a user