diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 764999928..a6caf26be 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -47,12 +47,11 @@ */ #include - +#include #include #include #include #include -#include #include #include #include @@ -60,14 +59,23 @@ #include #include #include - #include - +#include // Private constants -#define STACK_SIZE_BYTES 1000 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) +#define STACK_SIZE_BYTES 1000 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); +#define MAX_SENSORS_PER_INSTANCE 2 +#ifdef PIOS_INCLUDE_WDG +#define RELOAD_WDG() PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS) +#define REGISTER_WDG() PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS) +#else +#define RELOAD_WDG() +#define REGISTER_WDG() +#endif + +static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); +static const uint32_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS; // Interval in number of sample to recalculate temp bias #define TEMP_CALIB_INTERVAL 30 @@ -75,41 +83,55 @@ static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); // LPF #define TEMP_DT (1.0f / PIOS_SENSOR_RATE) #define TEMP_LPF_FC 5.0f -static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC)); +static const float temp_alpha = LPF_ALPHA(TEMP_DT, TEMP_LPF_FC); #define ZERO_ROT_ANGLE 0.00001f // Private types +typedef struct { + // used to accumulate all samples in a task iteration + Vector3i32 accum[2]; + int32_t temperature; + uint32_t count; +} sensor_fetch_context; + +#define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16)) +typedef union { + PIOS_SENSORS_3Axis_SensorsWithTemp sensorSample3Axis; + PIOS_SENSORS_1Axis_SensorsWithTemp sensorSample1Axis; +} sensor_data; + #define PIOS_INSTRUMENT_MODULE #include -PERF_DEFINE_COUNTER(counterGyroSamples); -PERF_DEFINE_COUNTER(counterSensorPeriod); - -// Counters: -// - 0x53000001 Sensor fetch rate(period) -// - 0x53000002 number of gyro samples read for each loop - // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); +static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample); +static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor); +static void clearContext(sensor_fetch_context *sensor_context); + +static void handleAccel(float *samples, float temperature); +static void handleGyro(float *samples, float temperature); +static void handleMag(float *samples, float temperature); + +static void updateAccelTempBias(float temperature); +static void updateGyroTempBias(float temperature); + // Private variables +static sensor_data *source_data; static xTaskHandle sensorsTaskHandle; RevoCalibrationData cal; AccelGyroSettingsData agcal; -#ifdef PIOS_INCLUDE_HMC5X83 -#include -extern pios_hmc5x83_dev_t onboard_mag; -#endif - // These values are initialized by settings but can be updated by the attitude algorithm static float mag_bias[3] = { 0, 0, 0 }; static float mag_transform[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; -// temp coefficient to calculate gyro bias + +// Variables used to handle temperature bias static volatile bool gyro_temp_calibrated = false; static volatile bool accel_temp_calibrated = false; @@ -117,29 +139,21 @@ static float accel_temperature = NAN; static float gyro_temperature = NAN; static float accel_temp_bias[3] = { 0 }; static float gyro_temp_bias[3] = { 0 }; -static uint8_t temp_calibration_count = 0; - +static uint8_t accel_temp_calibration_count = 0; +static uint8_t gyro_temp_calibration_count = 0; static float R[3][3] = { { 0 } }; static int8_t rotate = 0; -/** - * API for sensor fusion algorithms: - * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro) - * Stores all the queues the algorithm will pull data from - * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias) - * Update() -- queries queues and updates the attitude estiamte - */ - - /** * Initialise the module. Called before the start function * \returns 0 on success or -1 if initialisation failed */ int32_t SensorsInitialize(void) { + source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE); GyroSensorInitialize(); AccelSensorInitialize(); MagSensorInitialize(); @@ -165,10 +179,7 @@ int32_t SensorsStart(void) // Start main task xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS); -#endif - + REGISTER_WDG(); return 0; } @@ -184,82 +195,47 @@ int32_t mag_test; * The sensor task. This polls the gyros at 500 Hz and pumps that data to * stabilization and to the attitude loop * - * This function has a lot of if/defs right now to allow these configurations: - * 1. BMA180 accel and MPU6000 gyro - * 2. MPU6000 gyro and accel - * 3. BMA180 accel and L3GD20 gyro */ uint32_t sensor_dt_us; static void SensorsTask(__attribute__((unused)) 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; + sensor_fetch_context sensor_context; + bool error = false; + const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList(); + PIOS_SENSORS_Instance *sensor; AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); + settingsUpdatedCb(NULL); - 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); + // Test sensors + bool sensors_test = true; + uint8_t count = 0; + LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) { + sensors_test &= PIOS_SENSORS_Test(sensor); + count++; } -#if defined(PIOS_INCLUDE_HMC5X83) - mag_test = PIOS_HMC5x83_Test(onboard_mag); -#else - mag_test = 0; -#endif - - if (accel_test < 0 || gyro_test < 0 || mag_test < 0) { + PIOS_Assert(count); + RELOAD_WDG(); + if (!sensors_test) { AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); while (1) { -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); -#endif vTaskDelay(10); } } - PERF_INIT_COUNTER(counterGyroSamples, 0x53000001); - PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002); + // 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) { -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); -#endif + RELOAD_WDG(); lastSysTime = xTaskGetTickCount(); - vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS); + vTaskDelayUntil(&lastSysTime, sensor_period_ticks); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); error = false; } else { @@ -267,219 +243,195 @@ static void SensorsTask(__attribute__((unused)) void *parameters) } - for (int i = 0; i < 3; i++) { - accel_accum[i] = 0; - gyro_accum[i] = 0; - } - accel_samples = 0; - gyro_samples = 0; + // reset the fetch context + clearContext(&sensor_context); + LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) { + // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate) + bool is_primary = (sensor->type && PIOS_SENSORS_TYPE_3AXIS_ACCEL); - AccelSensorData accelSensorData; - GyroSensorData gyroSensorData; - - 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_ms) ? true : error; + if (!sensor->driver->is_polled) { + const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor); + while (xQueueReceive(queue, + (void *)source_data, + (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) { + accumulateSamples(&sensor_context, source_data); } - 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 - accelSensorData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f; - } -#endif /* if defined(PIOS_INCLUDE_BMA180) */ -#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) { + if (sensor_context.count) { + processSamples(&sensor_context, sensor); + clearContext(&sensor_context); + } else if (is_primary) { 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 - gyroSensorData.temperature = gyro.temperature; - } -#endif /* if defined(PIOS_INCLUDE_L3GD20) */ - 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++; + } else { + if (PIOS_SENSORS_Poll(sensor)) { + PIOS_SENSOR_Fetch(sensor, (void *)&source_data, MAX_SENSORS_PER_INSTANCE); + accumulateSamples(&sensor_context, source_data); + processSamples(&sensor_context, sensor); + clearContext(&sensor_context); } - - PERF_MEASURE_PERIOD(counterSensorPeriod); - PERF_TRACK_VALUE(counterGyroSamples, gyro_samples); - - if (gyro_samples == 0) { - PIOS_MPU6000_ReadGyros(&mpu6000_data); - error = true; - continue; - } - - gyro_scaling = PIOS_MPU6000_GetScale(); - accel_scaling = PIOS_MPU6000_GetAccelScale(); - - gyroSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; - accelSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; - } -#endif /* PIOS_INCLUDE_MPU6000 */ - break; - default: - PIOS_DEBUG_Assert(0); - } - - if (isnan(accel_temperature)) { - accel_temperature = accelSensorData.temperature; - gyro_temperature = gyroSensorData.temperature; - } - - accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature; - gyro_temperature = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature; - - if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) { - temp_calibration_count = TEMP_CALIB_INTERVAL; - if (accel_temp_calibrated) { - float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min); - accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp; - accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp; - accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp; - } - - if (gyro_temp_calibrated) { - float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min); - - gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp; - gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp; - gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp; } } - temp_calibration_count--; - // 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 * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0], - accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1], - accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] }; - - - if (rotate) { - rot_mult(R, accels_out, accels); - accelSensorData.x = accels[0]; - accelSensorData.y = accels[1]; - accelSensorData.z = accels[2]; - } else { - accelSensorData.x = accels_out[0]; - accelSensorData.y = accels_out[1]; - accelSensorData.z = accels_out[2]; - } - AccelSensorSet(&accelSensorData); - - // 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 * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0], - gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1], - gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] }; - - if (rotate) { - rot_mult(R, gyros_out, gyros); - gyroSensorData.x = gyros[0]; - gyroSensorData.y = gyros[1]; - gyroSensorData.z = gyros[2]; - } else { - gyroSensorData.x = gyros_out[0]; - gyroSensorData.y = gyros_out[1]; - gyroSensorData.z = gyros_out[2]; - } - - GyroSensorSet(&gyroSensorData); - - // 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_HMC5X83) - MagSensorData mag; - if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { - int16_t values[3]; - PIOS_HMC5x83_ReadMag(onboard_mag, values); - float mags[3] = { (float)values[1] - mag_bias[0], - (float)values[0] - mag_bias[1], - -(float)values[2] - mag_bias[2] }; - - float mag_out[3]; - rot_mult(mag_transform, mags, mag_out); - - mag.x = mag_out[0]; - mag.y = mag_out[1]; - mag.z = mag_out[2]; - - MagSensorSet(&mag); - mag_update_time = PIOS_DELAY_GetRaw(); - } -#endif /* if defined(PIOS_INCLUDE_HMC5X83) */ - -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); -#endif - vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS); + RELOAD_WDG(); + vTaskDelayUntil(&lastSysTime, sensor_period_ticks); } } +static void clearContext(sensor_fetch_context *sensor_context) +{ + // clear the context once it has finished + for (uint32_t i = 0; i < MAX_SENSORS_PER_INSTANCE; i++) { + sensor_context->accum[i].x = 0; + sensor_context->accum[i].y = 0; + sensor_context->accum[i].z = 0; + } + sensor_context->temperature = 0; + sensor_context->count = 0; +} + +static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample) +{ + for (uint32_t i = 0; (i < MAX_SENSORS_PER_INSTANCE) && (i < sample->sensorSample3Axis.count); i++) { + sensor_context->accum[i].x += sample->sensorSample3Axis.sample[i].x; + sensor_context->accum[i].y += sample->sensorSample3Axis.sample[i].y; + sensor_context->accum[i].z += sample->sensorSample3Axis.sample[i].z; + } + sensor_context->temperature += sample->sensorSample3Axis.temperature; + sensor_context->count++; +} + +static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor) +{ + float samples[3]; + float temperature; + float scales[MAX_SENSORS_PER_INSTANCE]; + + PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE); + float inv_count = 1.0f / (float)sensor_context->count; + if ((sensor->type && PIOS_SENSORS_TYPE_3AXIS_ACCEL) || + (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)) { + float t = inv_count * scales[0]; + samples[0] = ((float)sensor_context->accum[0].x * t); + samples[1] = ((float)sensor_context->accum[0].y * t); + samples[2] = ((float)sensor_context->accum[0].z * t); + temperature = (float)sensor_context->temperature * inv_count * 0.01f; + if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) { + handleMag(samples, temperature); + return; + } else { + handleAccel(samples, temperature); + } + } + + if (sensor->type && PIOS_SENSORS_TYPE_3AXIS_GYRO) { + uint8_t index = 0; + if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) { + index = 1; + } + float t = inv_count * scales[index]; + samples[0] = ((float)sensor_context->accum[index].x * t); + samples[1] = ((float)sensor_context->accum[index].y * t); + samples[2] = ((float)sensor_context->accum[index].z * t); + temperature = (float)sensor_context->temperature * inv_count * 0.01f; + handleGyro(samples, temperature); + return; + } + + if (sensor->type == PIOS_SENSORS_TYPE_1AXIS_BARO) { + PIOS_Assert(0); // not yet implemented + } +} + +void handleAccel(float *samples, float temperature) +{ + AccelSensorData accelSensorData; + + updateAccelTempBias(temperature); + float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0], + samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1], + samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] }; + + rot_mult(R, accels_out, samples); + accelSensorData.x = samples[0]; + accelSensorData.y = samples[1]; + accelSensorData.z = samples[2]; + + AccelSensorSet(&accelSensorData); +} +void handleGyro(float *samples, float temperature) +{ + GyroSensorData gyroSensorData; + + updateGyroTempBias(temperature); + float gyros_out[3] = { samples[0] * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0], + samples[1] * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1], + samples[2] * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] }; + + rot_mult(R, gyros_out, samples); + gyroSensorData.temperature = temperature; + gyroSensorData.x = samples[0]; + gyroSensorData.y = samples[1]; + gyroSensorData.z = samples[2]; + + GyroSensorSet(&gyroSensorData); +} + +void handleMag(float *samples, float temperature) +{ + MagSensorData mag; + float mags[3] = { (float)samples[1] - mag_bias[0], + (float)samples[0] - mag_bias[1], + (float)samples[2] - mag_bias[2] }; + + rot_mult(mag_transform, mags, samples); + + mag.x = samples[0]; + mag.y = samples[1]; + mag.z = samples[2]; + mag.temperature = temperature; + + MagSensorSet(&mag); +} + +static void updateAccelTempBias(float temperature) +{ + if (isnan(accel_temperature)) { + accel_temperature = temperature; + } + accel_temperature = temp_alpha * (temperature - accel_temperature) + accel_temperature; + + if ((accel_temp_calibrated) && !accel_temp_calibration_count) { + accel_temp_calibration_count = TEMP_CALIB_INTERVAL; + if (accel_temp_calibrated) { + float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min); + accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp; + accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp; + accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp; + } + } + accel_temp_calibration_count--; +} +static void updateGyroTempBias(float temperature) +{ + if (isnan(gyro_temperature)) { + gyro_temperature = temperature; + } + + gyro_temperature = temp_alpha * (temperature - gyro_temperature) + gyro_temperature; + + if (gyro_temp_calibrated && !gyro_temp_calibration_count) { + gyro_temp_calibration_count = TEMP_CALIB_INTERVAL; + + if (gyro_temp_calibrated) { + float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min); + gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp; + gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp; + gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp; + } + } + gyro_temp_calibration_count--; +} + /** * Locally cache some variables from the AtttitudeSettings object */ diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index bdddd13ae..8cc57cad5 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -73,7 +73,7 @@ #define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d)) // helper macros for LPFs -#define LPF_ALPHA(dt,fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc))) +#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc))) // Useful math macros #define MAX(a, b) ((a) > (b) ? (a) : (b)) diff --git a/shared/uavobjectdefinition/magsensor.xml b/shared/uavobjectdefinition/magsensor.xml index 8622bddd9..9b11393e9 100644 --- a/shared/uavobjectdefinition/magsensor.xml +++ b/shared/uavobjectdefinition/magsensor.xml @@ -1,9 +1,10 @@ Calibrated sensor data from 3 axis magnetometer in MilliGauss. - - - + + + +