rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1531 Filter temperatures for gyro and accel calibration. calculate new bias every 30 samples

Browse Source
This commit is contained in:
Alessio Morale 2014-10-07 11:23:26 +02:00
parent 9fd4fb26b9
commit 46fee43487
2 changed files with 90 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
flight/modules/Attitude/attitude.c
View File

@ -64,7 +64,7 @@
#include "CoordinateConversions.h"
#include <pios_notify.h>
#include <mathmisc.h>
#include <pios_instrumentation_helper.h>
@ -79,16 +79,20 @@ PERF_DEFINE_COUNTER(counterAtt);
// - 0xA7710004 number of accel samples read for each loop (cc only).
// Private constants
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 4
#define UPDATE_RATE 25.0f
#define SENSOR_PERIOD 4
#define UPDATE_RATE 25.0f
#define UPDATE_EXPECTED (1.0f / 500.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30
#define TEMP_ALPHA 0.9f
#define UPDATE_EXPECTED (1.0f / 500.0f)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
// Private types
@ -130,6 +134,10 @@ static bool apply_accel_temp = false;
static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
static float temperature = 0;
static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0;
// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
static AccelGyroSettingsgyro_scaleData gyro_scale;
@ -480,29 +488,44 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
}
float invcount = 1.0f / count;
PERF_TIMED_SECTION_START(counterUpd);
gyros[0] *= gyro_scale.X * invcount;
gyros[1] *= gyro_scale.Y * invcount;
gyros[2] *= gyro_scale.Z * invcount;
gyros[0] *= gyro_scale.X * invcount;
gyros[1] *= gyro_scale.Y * invcount;
gyros[2] *= gyro_scale.Z * invcount;
accels[0] *= accel_scale.X * invcount;
accels[1] *= accel_scale.Y * invcount;
accels[2] *= accel_scale.Z * invcount;
accels[0] *= accel_scale.X * invcount;
accels[1] *= accel_scale.Y * invcount;
accels[2] *= accel_scale.Z * invcount;
temp *= invcount;
float ctemp = temp > temp_calibrated_extent.max ? temp_calibrated_extent.max :
(temp < temp_calibrated_extent.min ? temp_calibrated_extent.min
: temp);
temperature = TEMP_ALPHA * temperature + (1 - TEMP_ALPHA) * temp;
if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
float ctemp = boundf(temp, temp_calibrated_extent.max, temp_calibrated_extent.min);
if (apply_gyro_temp) {
gyro_temp_bias[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
gyro_temp_bias[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyro_temp_bias[2] = (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (apply_accel_temp) {
accel_temp_bias[0] = accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = accel_temp_coeff.Z * ctemp;
}
}
temp_calibration_count--;
if (apply_gyro_temp) {
gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros[1] -= (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros[2] -= (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
gyros[0] -= gyro_temp_bias[0];
gyros[1] -= gyro_temp_bias[1];
gyros[2] -= gyro_temp_bias[2];
}
if (apply_accel_temp) {
accels[0] -= accel_temp_coeff.X * ctemp;
accels[1] -= accel_temp_coeff.Y * ctemp;
accels[2] -= accel_temp_coeff.Z * ctemp;
accels[0] -= accel_temp_bias[0];
accels[1] -= accel_temp_bias[1];
accels[2] -= accel_temp_bias[2];
}
// gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;

69
flight/modules/Sensors/sensors.c
View File

@ -64,11 +64,15 @@
#include <pios_board_info.h>
// Private constants
#define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 2
#define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 2
#define ZERO_ROT_ANGLE 0.00001f
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30
#define TEMP_ALPHA 0.9f
#define ZERO_ROT_ANGLE 0.00001f
// Private types
@ -96,6 +100,13 @@ static float mag_transform[3][3] = {
static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false;
static float accel_temperature = 0;
static float gyro_temperature = 0;
static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0;
static float R[3][3] = {
{ 0 }
};
@ -350,24 +361,38 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_DEBUG_Assert(0);
}
accel_temperature = TEMP_ALPHA * accelSensorData.temperature + (1 - TEMP_ALPHA) * accelSensorData.temperature;
gyro_temperature = TEMP_ALPHA * gyroSensorData.temperature + (1 - TEMP_ALPHA) * gyroSensorData.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,
accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y,
accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z };
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 (accel_temp_calibrated) {
float ctemp = accelSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(accelSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: accelSensorData.temperature);
accels_out[0] -= agcal.accel_temp_coeff.X * ctemp;
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
}
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
@ -385,18 +410,10 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
(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,
gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y,
gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z };
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 (gyro_temp_calibrated) {
float ctemp = gyroSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(gyroSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: gyroSensorData.temperature);
gyros_out[0] -= (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];