rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

CC-3 Modules/CCAttitude: Add oversamplign to the accel data

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2576 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-24 07:52:08 +00:00 committed by peabody124
parent 21e3441f00
commit d4b4a6ca37
4 changed files with 27 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/CopterControl/System/pios_board.c
View File

@ -106,7 +106,7 @@ const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16,
},
.use_crc = FALSE,
.dma = {

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

@ -93,7 +93,6 @@ int32_t CCAttitudeInitialize(void)
*/
static void CCAttitudeTask(void *parameters)
{
portTickType lastSysTime;
// AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
@ -104,6 +103,7 @@ static void CCAttitudeTask(void *parameters)
// Main task loop
while (1) {
//portTickType lastSysTime;
//PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS);
// TODO: register the adc callback, push the data onto a queue (safe for thread)
@ -112,8 +112,8 @@ static void CCAttitudeTask(void *parameters)
updateAttitude();
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, UPDATE_RATE / portTICK_RATE_MS);
//vTaskDelay(UPDATE_RATE / portTICK_RATE_MS);
//vTaskDelayUntil(&lastSysTime, UPDATE_RATE / portTICK_RATE_MS);
vTaskDelay(UPDATE_RATE / portTICK_RATE_MS);
}
}
@ -125,7 +125,7 @@ void updateSensors()
struct pios_adxl345_data accel_data;
static float gyro_bias[3] = {0,0,0};
static const float tau = 0.999f;
static const float tau = 0.9999f;
attitudeRaw.gyros[ATTITUDERAW_GYROS_X] = PIOS_ADC_PinGet(1);
attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] = PIOS_ADC_PinGet(2);
@ -143,15 +143,29 @@ void updateSensors()
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] -= gyro_bias[1];
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] -= gyro_bias[2];
attitudeRaw.gyrotemp[0] = PIOS_ADXL345_Read(&accel_data);
// Get the accel data
uint8_t i = 0;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = 0;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = 0;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] = 0;
do {
i++;
attitudeRaw.gyrotemp[0] = PIOS_ADXL345_Read(&accel_data);
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] += (float) accel_data.x * 0.004f * 9.81;
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.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] /= i;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] /= i;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] /= i;
attitudeRaw.accels[ATTITUDERAW_ACCELS_X] = accel_data.x;
attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] = accel_data.y;
attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] = accel_data.z;
attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = (float) accel_data.x * 0.004f * 9.81;
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;
AttitudeRawSet(&attitudeRaw);
}

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

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

3
flight/PiOS/inc/pios_adxl345.h
View File

@ -21,6 +21,9 @@
#define ADXL_RATE_100 0x0A
#define ADXL_RATE_200 0x0B
#define ADXL_RATE_400 0x0C
#define ADXL_RATE_800 0x0D
#define ADXL_RATE_1600 0x0E
#define ADXL_RATE_3200 0x0F
#define ADXL_POWER_ADDR 0x2D
#define ADXL_MEAURE 0x08