diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index d3c522442..aa0ad9bb4 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -252,12 +252,11 @@ static void AttitudeTask(void *parameters) GyrosSet(&gyros); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } - - if(cc3d) - vTaskDelayUntil(&lastSysTime, 3); } } +float gyros_passed[3]; + /** * Get an update from the sensors * @param[in] attitudeRaw Populate the UAVO instead of saving right here @@ -349,7 +348,11 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) gyro_correct_int[2] += - gyros->z * yawBiasRate; - + + gyros_passed[0] = gyros->x; + gyros_passed[1] = gyros->y; + gyros_passed[2] = gyros->z; + return 0; } @@ -364,18 +367,74 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) { static portTickType lastSysTime; uint32_t accel_samples = 0; - uint32_t gyro_samples = 0; int32_t accel_accum[3] = {0, 0, 0}; - int32_t gyro_accum[3] = {0,0,0}; float gyro_scaling = 1; + float gyros_accum[3] = {0, 0, 0}; float accel_scaling = 1; +#if defined(PIOS_INCLUDE_L3GD20) + xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); + struct pios_l3gd20_data gyro; + gyro_scaling = PIOS_L3GD20_GetScale(); + + if(xQueueReceive(gyro_queue, (void *) &gyro, 3) == errQUEUE_EMPTY) { + // 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); + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return -1; + } + + gyrosData->x = gyro.gyro_x * gyro_scaling; + gyrosData->y = -gyro.gyro_y * gyro_scaling; + gyrosData->z = -gyro.gyro_z * gyro_scaling; + gyrosData->temperature = 0; + GyrosSet(gyrosData); + + gyros_accum[0] = gyrosData->x; + gyros_accum[1] = gyrosData->y; + gyros_accum[2] = gyrosData->z; + + if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return -1; + } + + gyrosData->x = gyro.gyro_x * gyro_scaling; + gyrosData->y = -gyro.gyro_y * gyro_scaling; + gyrosData->z = -gyro.gyro_z * gyro_scaling; + gyrosData->temperature = 0; + + gyros_accum[0] += gyrosData->x; + gyros_accum[1] += gyrosData->y; + gyros_accum[2] += gyrosData->z; + + if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + return -1; + } + + gyrosData->x = gyro.gyro_x * gyro_scaling; + gyrosData->y = -gyro.gyro_y * gyro_scaling; + gyrosData->z = -gyro.gyro_z * gyro_scaling; + gyrosData->temperature = 0; + + gyros_accum[0] += gyrosData->x; + gyros_accum[1] += gyrosData->y; + gyros_accum[2] += gyrosData->z; + + gyros_passed[0] = gyros_accum[0] / 3; + gyros_passed[1] = gyros_accum[1] / 3; + gyros_passed[2] = gyros_accum[2] / 3; +#endif + #if defined(PIOS_INCLUDE_BMA180) struct pios_bma180_data accel; accel_samples = 0; bool error = false; int32_t accel_read_good; - + while((accel_read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) error = ((xTaskGetTickCount() - lastSysTime) > 5) ? true : error; if (error) { @@ -397,48 +456,13 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) } accel_scaling = PIOS_BMA180_GetScale(); #endif - -#if defined(PIOS_INCLUDE_L3GD20) - struct pios_l3gd20_data gyro; - 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; - } - - 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}; 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[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; - gyrosData->z = gyros[2] * gyro_scaling; - lastSysTime = xTaskGetTickCount(); return 0; @@ -454,7 +478,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) lastSysTime = thisSysTime; // Bad practice to assume structure order, but saves memory - float * gyros = &gyrosData->x; + float * gyros = gyros_passed; float * accels = &accelsData->x; float grot[3]; diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 312d660cc..0d206963e 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -270,22 +270,17 @@ static void SensorsTask(void *parameters) // Using L3DG20 gyro #elif defined(PIOS_INCLUDE_L3GD20) struct pios_l3gd20_data gyro; - count = 0; - while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0 && !error) - error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; - if (error) - continue; - while(read_good == 0) { - count++; - + gyro_samples = 0; + xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); + + while(xQueueReceive(gyro_queue, (void *) &gyro, 0) != errQUEUE_EMPTY) { + gyro_samples++; gyro_accum[0] += gyro.gyro_x; gyro_accum[1] += gyro.gyro_y; gyro_accum[2] += gyro.gyro_z; read_good = PIOS_L3GD20_ReadFifo(&gyro); } - - gyro_samples = count; gyro_scaling = PIOS_L3GD20_GetScale(); #else diff --git a/flight/PiOS/Common/pios_l3gd20.c b/flight/PiOS/Common/pios_l3gd20.c index 148e4ba1c..3151d26e2 100644 --- a/flight/PiOS/Common/pios_l3gd20.c +++ b/flight/PiOS/Common/pios_l3gd20.c @@ -41,12 +41,11 @@ enum pios_l3gd20_dev_magic { PIOS_L3GD20_DEV_MAGIC = 0x9d39bced, }; -#define PIOS_L3GD20_MAX_DOWNSAMPLE 10 +#define PIOS_L3GD20_MAX_DOWNSAMPLE 2 struct l3gd20_dev { uint32_t spi_id; uint32_t slave_num; - int16_t buffer[PIOS_L3GD20_MAX_DOWNSAMPLE * sizeof(struct pios_l3gd20_data)]; - t_fifo_buffer fifo; + xQueueHandle queue; const struct pios_l3gd20_cfg * cfg; enum pios_l3gd20_filter bandwidth; enum pios_l3gd20_range range; @@ -77,13 +76,18 @@ volatile bool l3gd20_configured = false; static struct l3gd20_dev * PIOS_L3GD20_alloc(void) { struct l3gd20_dev * l3gd20_dev; - + l3gd20_dev = (struct l3gd20_dev *)pvPortMalloc(sizeof(*l3gd20_dev)); if (!l3gd20_dev) return (NULL); - - fifoBuf_init(&l3gd20_dev->fifo, (uint8_t *) l3gd20_dev->buffer, sizeof(l3gd20_dev->buffer)); - + l3gd20_dev->magic = PIOS_L3GD20_DEV_MAGIC; + + l3gd20_dev->queue = xQueueCreate(PIOS_L3GD20_MAX_DOWNSAMPLE, sizeof(struct pios_l3gd20_data)); + if(l3gd20_dev->queue == NULL) { + vPortFree(l3gd20_dev); + return NULL; + } + return(l3gd20_dev); } @@ -301,24 +305,15 @@ int32_t PIOS_L3GD20_ReadID() } /** - * \brief Reads the data from the MPU6050 FIFO - * \param[out] buffer destination buffer - * \param[in] len maximum number of bytes which should be read - * \note This returns the data as X, Y, Z the temperature - * \return number of bytes transferred if operation was successful - * \return -1 if error during I2C transfer + * \brief Reads the queue handle + * \return Handle to the queue or null if invalid device */ -int32_t PIOS_L3GD20_ReadFifo(struct pios_l3gd20_data * buffer) +xQueueHandle PIOS_L3GD20_GetQueue() { if(PIOS_L3GD20_Validate(dev) != 0) - return -1; + return (xQueueHandle) NULL; - if(fifoBuf_getUsed(&dev->fifo) < sizeof(*buffer)) - return -1; - - fifoBuf_getData(&dev->fifo, (uint8_t *) buffer, sizeof(*buffer)); - - return 0; + return dev->queue; } float PIOS_L3GD20_GetScale() @@ -380,11 +375,7 @@ void PIOS_L3GD20_IRQHandler(void) memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6); data.temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); - if(fifoBuf_getFree(&dev->fifo) < sizeof(data)) { - return; - } - - fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); + xQueueSend(dev->queue, (void *) &data, 0); } #endif /* L3GD20 */ diff --git a/flight/PiOS/inc/pios_l3gd20.h b/flight/PiOS/inc/pios_l3gd20.h index 976ebef1f..83b2070b6 100644 --- a/flight/PiOS/inc/pios_l3gd20.h +++ b/flight/PiOS/inc/pios_l3gd20.h @@ -135,7 +135,7 @@ struct pios_l3gd20_cfg { /* Public Functions */ extern int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg * cfg); -extern int32_t PIOS_L3GD20_ReadFifo(struct pios_l3gd20_data * buffer); +extern xQueueHandle PIOS_L3GD20_GetQueue(); extern int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * buffer); extern int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range); extern float PIOS_L3GD20_GetScale();