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

OP-1513 - Fix time constants, set initial temperature as first sample value

Browse Source
This commit is contained in:
Alessio Morale 2014-10-11 23:54:21 +02:00
parent 823a1afad2
commit f2fc5f5616
4 changed files with 27 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/modules/Altitude/revolution/altitude.c
View File

@ -53,7 +53,8 @@
// Interval in number of sample to recalculate temp bias // Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 10 #define TEMP_CALIB_INTERVAL 10
#define TEMP_ALPHA 0.9f // 5Hz @120Hz update
#define TEMP_ALPHA 0.9979f
// Private types // Private types
@ -64,7 +65,7 @@ static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled; static volatile bool tempCorrectionEnabled;
static float baro_temp_bias = 0; static float baro_temp_bias = 0;
static float baro_temperature = 0; static float baro_temperature = NAN;
static uint8_t temp_calibration_count = 0; static uint8_t temp_calibration_count = 0;
// Private functions // Private functions
@ -176,6 +177,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
temp = PIOS_MS5611_GetTemperature(); temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure(); press = PIOS_MS5611_GetPressure();
if (isnan(baro_temperature)) {
baro_temperature = temp;
}
baro_temperature = TEMP_ALPHA * baro_temperature + (1 - TEMP_ALPHA) * temp; baro_temperature = TEMP_ALPHA * baro_temperature + (1 - TEMP_ALPHA) * temp;
if (tempCorrectionEnabled && !temp_calibration_count) { if (tempCorrectionEnabled && !temp_calibration_count) {

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

@ -89,7 +89,9 @@ PERF_DEFINE_COUNTER(counterAtt);
// Interval in number of sample to recalculate temp bias // Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30 #define TEMP_CALIB_INTERVAL 30
#define TEMP_ALPHA 0.9f
// LPF 5Hz at 500Hz rate
#define TEMP_ALPHA 0.999504f
#define UPDATE_EXPECTED (1.0f / 500.0f) #define UPDATE_EXPECTED (1.0f / 500.0f)
#define UPDATE_MIN 1.0e-6f #define UPDATE_MIN 1.0e-6f
@ -136,7 +138,7 @@ static bool apply_accel_temp = false;
static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;; static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff; static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent; static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
static float temperature = 0; static float temperature = NAN;
static float accel_temp_bias[3] = { 0 }; static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 }; static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0; static uint8_t temp_calibration_count = 0;
@ -499,11 +501,14 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
accels[2] *= accel_scale.Z * invcount; accels[2] *= accel_scale.Z * invcount;
temp *= invcount; temp *= invcount;
if(isnan(temperature)){
temperature = temp;
}
temperature = TEMP_ALPHA * temperature + (1 - TEMP_ALPHA) * temp; temperature = TEMP_ALPHA * temperature + (1 - TEMP_ALPHA) * temp;
if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) { if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL; temp_calibration_count = TEMP_CALIB_INTERVAL;
float ctemp = boundf(temp, temp_calibrated_extent.max, temp_calibrated_extent.min); float ctemp = boundf(temperature, temp_calibrated_extent.max, temp_calibrated_extent.min);
if (apply_gyro_temp) { if (apply_gyro_temp) {
gyro_temp_bias[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp; 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[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;

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

@ -70,7 +70,8 @@
// Interval in number of sample to recalculate temp bias // Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30 #define TEMP_CALIB_INTERVAL 30
#define TEMP_ALPHA 0.9f // LPF 5Hz at 500Hz rate
#define TEMP_ALPHA 0.999504f
#define ZERO_ROT_ANGLE 0.00001f #define ZERO_ROT_ANGLE 0.00001f
// Private types // Private types
@ -108,8 +109,8 @@ static float mag_transform[3][3] = {
static volatile bool gyro_temp_calibrated = false; static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false; static volatile bool accel_temp_calibrated = false;
static float accel_temperature = 0; static float accel_temperature = NAN;
static float gyro_temperature = 0; static float gyro_temperature = NAN;
static float accel_temp_bias[3] = { 0 }; static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 }; static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0; static uint8_t temp_calibration_count = 0;
@ -373,9 +374,14 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }
accel_temperature = TEMP_ALPHA * accelSensorData.temperature + (1 - TEMP_ALPHA) * accelSensorData.temperature; if (isnan(accel_temperature)) {
gyro_temperature = TEMP_ALPHA * gyroSensorData.temperature + (1 - TEMP_ALPHA) * gyroSensorData.temperature; accel_temperature = accelSensorData.temperature;
gyro_temperature = gyroSensorData.temperature;
}
accel_temperature = TEMP_ALPHA * accel_temperature + (1 - TEMP_ALPHA) * accelSensorData.temperature;
gyro_temperature = TEMP_ALPHA * gyro_temperature + (1 - TEMP_ALPHA) * gyroSensorData.temperature;
gyroSensorData.temperature = gyro_temperature;
if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) { if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL; temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) { if (accel_temp_calibrated) {
@ -467,7 +473,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif #endif
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
lastSysTime = xTaskGetTickCount();
} }
} }

2
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -121,7 +121,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 8 for 1000 Hz // Clock at 8 khz, downsampled by 8 for 1000 Hz
.Smpl_rate_div_no_dlp = 7, .Smpl_rate_div_no_dlp = 7,
// Clock at 1 khz, downsampled by 2 for 1000 Hz // Clock at 1 khz, downsampled by 1 for 1000 Hz
.Smpl_rate_div_dlp = 0, .Smpl_rate_div_dlp = 0,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,