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

Use the updated L3GD20 format and make attitude.c use the gyro data from the

Browse Source 
fifo
This commit is contained in:
James Cotton 2012-01-24 15:52:17 -06:00
parent c9263cb8c0
commit b6c056fef2
3 changed files with 36 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -143,7 +143,7 @@ static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = {
static const struct pios_l3gd20_cfg pios_l3gd20_cfg = {
.exti_cfg = &pios_exti_l3gd20_cfg,
.gyro_range = PIOS_L3GD20_SCALE_500_DEG,
.range = PIOS_L3GD20_SCALE_500_DEG,
};
#endif /* PIOS_INCLUDE_L3GD20 */
@ -687,12 +687,13 @@ void PIOS_Board_Init(void) {
if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
PIOS_Assert(0);
}
PIOS_L3GD20_Attach(pios_spi_gyro_id);
PIOS_L3GD20_Init(&pios_l3gd20_cfg);
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
PIOS_Assert(PIOS_L3GD20_Test() == 0);
#endif /* PIOS_INCLUDE_L3GD20 */
#if defined(PIOS_INCLUDE_BMA180)
PIOS_BMA180_Init(pios_spi_flash_accel_id, 0, &pios_bma180_cfg);
PIOS_Assert(PIOS_BMA180_Test() == 0);
#endif /* PIOS_INCLUDE_BMA180 */
break;
default:

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

@ -381,17 +381,15 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
// Unfortunately if the BMA180 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstart
// it.
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data);
PIOS_BMA180_ReadAccels(&accel);
lastSysTime = xTaskGetTickCount();
return -1;
}
while(accel_read_good == 0) {
accel_samples++;
accel_accum[0] += -accel.x;
accel_accum[1] += -accel.y;
accel_accum[0] += accel.x;
accel_accum[1] += accel.y;
accel_accum[2] += accel.z;
accel_read_good = PIOS_BMA180_ReadFifo(&accel);
@ -400,37 +398,40 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
#endif
#if defined(PIOS_INCLUDE_L3GD20)
// int32_t gyro_read_good;
// gyro_samples = 0;
// bool gyro_error = false;
//
// while((gyro_read_good = PIOS_L3GD20_ReadFifo(&gyro) != 0) && !gyro_error)
// gyro_error = ((xTaskGetTickCount() - lastSysTime) > 5) ? true : gyro_error;
// while(gyro_read_good == 0) {
// gyro_samples++;
//
// gyro_accum[0] += gyro.gyro_x;
// gyro_accum[1] += gyro.gyro_y;
// gyro_accum[2] += gyro.gyro_z;
//
// gyro_read_good = PIOS_L3GD20_ReadFifo(&gyro);
// }
if(PIOS_L3GD20_ReadGyros(&gyro) < 0)
int32_t gyro_read_good;
gyro_samples = 0;
bool gyro_error = false;
while((gyro_read_good = PIOS_L3GD20_ReadFifo(&gyro) != 0) && !gyro_error)
gyro_error = ((xTaskGetTickCount() - lastSysTime) > 5) ? true : gyro_error;
if (gyro_error) {
// Unfortunately if the L3GD20 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstart
// it.
PIOS_L3GD20_ReadGyros(&gyro);
lastSysTime = xTaskGetTickCount();
return -1;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
gyro_samples =1;
}
while(gyro_read_good == 0) {
gyro_samples++;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
gyro_read_good = PIOS_L3GD20_ReadFifo(&gyro);
}
gyro_scaling = PIOS_L3GD20_GetScale();
#endif
float accels[3] = {(float) accel_accum[1] / accel_samples, (float) accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples};
float accels[3] = {(float) -accel_accum[1] / accel_samples, (float) -accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples};
accelsData->x = accels[0] * accel_scaling - accelbias[0];
accelsData->y = accels[1] * accel_scaling - accelbias[1];
accelsData->z = accels[2] * accel_scaling - accelbias[2];
float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples};
float gyros[3] = {(float) gyro_accum[0] / gyro_samples, (float) -gyro_accum[1] / gyro_samples, -(float) gyro_accum[2] / gyro_samples};
gyrosData->x = gyros[0] * gyro_scaling;
gyrosData->y = gyros[1] * gyro_scaling;

7
flight/Revolution/System/pios_board.c
View File

@ -1577,7 +1577,7 @@ static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = {
static const struct pios_l3gd20_cfg pios_l3gd20_cfg = {
.exti_cfg = &pios_exti_l3gd20_cfg,
.gyro_range = PIOS_L3GD20_SCALE_500_DEG,
.range = PIOS_L3GD20_SCALE_500_DEG,
};
#endif /* PIOS_INCLUDE_L3GD20 */
@ -1761,14 +1761,15 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_BMA180)
PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
PIOS_Assert(PIOS_BMA180_Test() == 0);
#endif
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Attach(pios_spi_gyro_id);
PIOS_MPU6000_Init(&pios_mpu6000_cfg);
PIOS_Assert(PIOS_MPU6000_Test() == 0);
#elif defined(PIOS_INCLUDE_L3GD20)
PIOS_L3GD20_Attach(pios_spi_gyro_id);
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
PIOS_Assert(PIOS_L3GD20_Test() == 0);
PIOS_L3GD20_Init(&pios_l3gd20_cfg);
#else
PIOS_Assert(0);
#endif