mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
CC-3 Modules/CCAttitude: Add oversamplign to the accel data
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2576 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
parent
21e3441f00
commit
d4b4a6ca37
@ -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 = {
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
|
@ -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
|
||||
|
Loading…
x
Reference in New Issue
Block a user