rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

CC-1 CC-8 Cache the oversampled gyro value and use that to get rid of glitches.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2652 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-01 02:17:27 +00:00 committed by peabody124
parent bbbdea26b9
commit 037a63ee1d
2 changed files with 19 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/Modules/CCAttitude/ccattitude.c
View File

@ -63,7 +63,7 @@
#define UPDATE_RATE 2 /* ms */
#define GYRO_NEUTRAL 1665
#define GYRO_SCALE (0.010f * 180 / M_PI)
#define GYRO_SCALE (0.008f * 180 / M_PI)
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
// Private types
@ -74,6 +74,9 @@ static xTaskHandle taskHandle;
// Private functions
static void CCAttitudeTask(void *parameters);
void adc_callback(float * data);
float gyro[3] = {0, 0, 0};
void updateSensors();
void updateAttitude();
@ -97,9 +100,10 @@ static void CCAttitudeTask(void *parameters)
// AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_ADC_SetCallback(adc_callback);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Main task loop
@ -128,13 +132,9 @@ void updateSensors()
static float gyro_bias[3] = {0,0,0};
static const float tau = 0.9999f;
attitudeRaw.gyros[ATTITUDERAW_GYROS_X] = PIOS_ADC_PinGet(1);
attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] = PIOS_ADC_PinGet(2);
attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] = PIOS_ADC_PinGet(3);
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = -(attitudeRaw.gyros[ATTITUDERAW_GYROS_X] - GYRO_NEUTRAL) * GYRO_SCALE;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] - GYRO_NEUTRAL) * GYRO_SCALE;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = (attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] - GYRO_NEUTRAL) * GYRO_SCALE;
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;
gyro_bias[0] = tau * gyro_bias[0] + (1-tau) * attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X];
gyro_bias[1] = tau * gyro_bias[1] + (1-tau) * attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y];
@ -158,6 +158,7 @@ void updateSensors()
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] += -(float) accel_data.y * 0.004f * 9.81;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] += -(float) accel_data.z * 0.004f * 9.81;
} while ( (i < 32) && (attitudeRaw.gyrotemp[0] > 0) );
attitudeRaw.gyrotemp[1] = i;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] /= i;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] /= i;
@ -216,6 +217,13 @@ void updateAttitude()
AttitudeActualSet(&attitudeActual);
}
void adc_callback(float * data)
{
gyro[0] = data[1];
gyro[1] = data[2];
gyro[2] = data[3];
}
/**
* @}
* @}

2
flight/PiOS/Common/pios_adxl345.c
View File

@ -80,7 +80,7 @@ void PIOS_ADXL345_SetMeasure(uint8_t enable)
void PIOS_ADXL345_Init()
{
PIOS_ADXL345_ReleaseBus();
PIOS_ADXL345_SelectRate(ADXL_RATE_1600);
PIOS_ADXL345_SelectRate(ADXL_RATE_3200);
PIOS_ADXL345_SetRange(ADXL_RANGE_8G);
PIOS_ADXL345_FifoDepth(16);
PIOS_ADXL345_SetMeasure(1);