static void PIOS_L3GD20_Task(void *parameters)
{
while (1) {
// Wait for data ready interrupt
if (PIOS_Semaphore_Take(pios_l3gd20_dev->data_ready_sema, PIOS_SEMAPHORE_TIMEOUT_MAX) != true)
continue;
struct pios_l3gd20_data data;
PIOS_L3GD20_ReadGyros(&data);
// TODO: This reordering is specific to the FlyingF3 chip placement. Whenever
// this code is used on another board add an orientation mapping to the configuration
struct pios_sensor_gyro_data normalized_data;
float scale = PIOS_L3GD20_GetScale();
normalized_data.y = data.gyro_x * scale;
normalized_data.x = data.gyro_y * scale;
normalized_data.z = -data.gyro_z * scale;
normalized_data.temperature = data.temperature;
PIOS_Queue_Send(pios_l3gd20_dev->queue, (void *)&normalized_data, 0);
}
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
*/
bool PIOS_L3GD20_IRQHandler(void)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0 || pios_l3gd20_dev->configured == false)
return false;
struct pios_l3gd20_data data;
uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 };
uint8_t rec[7];
/* This code duplicates ReadGyros above but uses ClaimBusIsr */
bool woken = false;
if (PIOS_L3GD20_ClaimBusIsr(&woken) != 0)
return woken;
if (PIOS_SPI_TransferBlock(pios_l3gd20_dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBusIsr(&woken);
return woken;
}
PIOS_L3GD20_ReleaseBusIsr(&woken);
memcpy((uint8_t *)&data.gyro_x, &rec[1], 6);
// TODO: This reordering is specific to the FlyingF3 chip placement. Whenever
// this code is used on another board add an orientation mapping to the configuration
struct pios_sensor_gyro_data normalized_data;
float scale = PIOS_L3GD20_GetScale();
normalized_data.y = data.gyro_x * scale;
normalized_data.x = data.gyro_y * scale;
normalized_data.z = -data.gyro_z * scale;
normalized_data.temperature = PIOS_L3GD20_GetRegIsr(PIOS_L3GD20_OUT_TEMP, &woken);
PIOS_Queue_Send_FromISR(pios_l3gd20_dev->queue, &normalized_data, &woken);
return woken;
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
*/
bool PIOS_L3GD20_IRQHandler(void)
{
l3gd20_irq++;
struct pios_l3gd20_data data;
uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0};
uint8_t rec[7];
/* This code duplicates ReadGyros above but uses ClaimBusIsr */
bool woken = false;
if(PIOS_L3GD20_ClaimBusIsr(&woken) != 0)
return woken;
if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBusIsr(&woken);
return woken;
}
PIOS_L3GD20_ReleaseBusIsr(&woken);
memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6);
// TODO: This reordering is specific to the FlyingF3 chip placement. Whenever
// this code is used on another board add an orientation mapping to the configuration
struct pios_sensor_gyro_data normalized_data;
float scale = PIOS_L3GD20_GetScale();
normalized_data.y = data.gyro_x * scale;
normalized_data.x = data.gyro_y * scale;
normalized_data.z = -data.gyro_z * scale;
normalized_data.temperature = PIOS_L3GD20_GetRegIsr(PIOS_L3GD20_OUT_TEMP, &woken);
portBASE_TYPE xHigherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &normalized_data, &xHigherPriorityTaskWoken);
return woken || (xHigherPriorityTaskWoken == pdTRUE);
}
static void SensorsTask(void *parameters)
{
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 = 0;
float accel_scaling = 0;
static int32_t timeval;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
UAVObjEvent ev;
settingsUpdatedCb(&ev);
const struct pios_board_info * bdinfo = &pios_board_info_blob;
switch(bdinfo->board_rev) {
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
gyro_test = PIOS_L3GD20_Test();
#endif
#if defined(PIOS_INCLUDE_BMA180)
accel_test = PIOS_BMA180_Test();
#endif
break;
case 0x02:
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
accel_test = gyro_test;
#endif
break;
default:
PIOS_DEBUG_Assert(0);
}
#if defined(PIOS_INCLUDE_HMC5883)
mag_test = PIOS_HMC5883_Test();
#else
mag_test = 0;
#endif
if(accel_test < 0 || gyro_test < 0 || mag_test < 0) {
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
vTaskDelay(10);
}
}
// Main task loop
lastSysTime = xTaskGetTickCount();
bool error = false;
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (error) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false;
} else {
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
}
for (int i = 0; i < 3; i++) {
accel_accum[i] = 0;
gyro_accum[i] = 0;
}
accel_samples = 0;
gyro_samples = 0;
AccelsData accelsData;
GyrosData gyrosData;
switch(bdinfo->board_rev) {
case 0x01: // L3GD20 + BMA180 board
#if defined(PIOS_INCLUDE_BMA180)
{
struct pios_bma180_data accel;
int32_t read_good;
int32_t count;
count = 0;
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error) {
// Unfortunately if the BMA180 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstarts
// it.
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data);
continue;
}
while(read_good == 0) {
count++;
accel_accum[1] += accel.x;
accel_accum[0] += accel.y;
accel_accum[2] -= accel.z;
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale();
// Get temp from last reading
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
}
#endif
#if defined(PIOS_INCLUDE_L3GD20)
{
struct pios_l3gd20_data gyro;
gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) {
error = true;
continue;
}
gyro_samples = 1;
gyro_accum[1] += gyro.gyro_x;
gyro_accum[0] += gyro.gyro_y;
gyro_accum[2] -= gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale();
// Get temp from last reading
gyrosData.temperature = gyro.temperature;
}
#endif
break;
case 0x02: // MPU6000 board
case 0x03: // MPU6000 board
#if defined(PIOS_INCLUDE_MPU6000)
{
struct pios_mpu6000_data mpu6000_data;
xQueueHandle queue = PIOS_MPU6000_GetQueue();
while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY)
{
gyro_accum[0] += mpu6000_data.gyro_x;
gyro_accum[1] += mpu6000_data.gyro_y;
gyro_accum[2] += mpu6000_data.gyro_z;
accel_accum[0] += mpu6000_data.accel_x;
accel_accum[1] += mpu6000_data.accel_y;
accel_accum[2] += mpu6000_data.accel_z;
gyro_samples ++;
accel_samples ++;
}
if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true;
continue;
}
gyro_scaling = PIOS_MPU6000_GetScale();
accel_scaling = PIOS_MPU6000_GetAccelScale();
gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
}
#endif /* PIOS_INCLUDE_MPU6000 */
break;
default:
PIOS_DEBUG_Assert(0);
}
// Scale the accels
float accels[3] = {(float) accel_accum[0] / accel_samples,
(float) accel_accum[1] / accel_samples,
(float) accel_accum[2] / accel_samples};
float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]};
if (rotate) {
rot_mult(R, accels_out, accels);
accelsData.x = accels[0];
accelsData.y = accels[1];
accelsData.z = accels[2];
} else {
accelsData.x = accels_out[0];
accelsData.y = accels_out[1];
accelsData.z = accels_out[2];
}
AccelsSet(&accelsData);
// Scale the gyros
float gyros[3] = {(float) gyro_accum[0] / gyro_samples,
(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[2] / gyro_samples};
float gyros_out[3] = {gyros[0] * gyro_scaling,
gyros[1] * gyro_scaling,
gyros[2] * gyro_scaling};
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0];
gyrosData.y = gyros[1];
gyrosData.z = gyros[2];
} else {
gyrosData.x = gyros_out[0];
gyrosData.y = gyros_out[1];
gyrosData.z = gyros_out[2];
}
if (bias_correct_gyro) {
// Apply bias correction to the gyros from the state estimator
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
GyrosSet(&gyrosData);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
float mags[3] = {-(float) values[1] * mag_scale[0] - mag_bias[0],
-(float) values[0] * mag_scale[1] - mag_bias[1],
-(float) values[2] * mag_scale[2] - mag_bias[2]};
if (rotate) {
float mag_out[3];
rot_mult(R, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
} else {
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
}
// Correct for mag bias and update if the rate is non zero
if(cal.MagBiasNullingRate > 0)
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
lastSysTime = xTaskGetTickCount();
}
}