diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c
index a48eb646c..d9c8d9206 100644
--- a/flight/modules/AutoTune/autotune.c
+++ b/flight/modules/AutoTune/autotune.c
@@ -114,6 +114,10 @@ struct at_queued_data {
};
+// Global variables
+extern uint32_t gyroReadTime;
+
+
// Private variables
static xTaskHandle taskHandle;
static bool moduleEnabled;
@@ -129,6 +133,10 @@ static SystemIdentStateData systemIdentState;
static int8_t accessoryToUse;
static int8_t flightModeSwitchTogglePosition;
+static float gyroReadTimeAverage;
+static float gyroReadTimeAverageAlpha;
+#define GYRO_READ_TIME_DECAY_TIME_CONSTANT 2.0f
+
// Private functions
static void AutoTuneTask(void *parameters);
@@ -188,6 +196,9 @@ int32_t AutoTuneInitialize(void)
if (!atQueue) {
moduleEnabled = false;
}
+ // 0.002 is gyro period
+ // make the smoothing decay about 1 second
+ gyroReadTimeAverageAlpha = expf(-0.002f / GYRO_READ_TIME_DECAY_TIME_CONSTANT);
}
return 0;
@@ -556,6 +567,8 @@ static void AtNewGyroData(UAVObjEvent *ev)
GyroStateGet(&gyro);
ActuatorDesiredGet(&actuators);
+ gyroReadTimeAverage = gyroReadTimeAverage * gyroReadTimeAverageAlpha + PIOS_DELAY_DiffuS(gyroReadTime) * 1.0e-6f * (1 - gyroReadTimeAverageAlpha);
+
if (last_sample_unpushed) {
/* Last time we were unable to queue up the gyro data.
* Try again, last chance! */
@@ -565,9 +578,17 @@ static void AtNewGyroData(UAVObjEvent *ev)
}
q_item.raw_time = PIOS_DELAY_GetRaw();
+#if 0
+ gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha);
+ gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha);
+ gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha);
q_item.y[0] = gyro.x;
q_item.y[1] = gyro.y;
q_item.y[2] = gyro.z;
+#endif
+ q_item.y[0] = q_item.y[0] * stabSettings.gyro_alpha + gyro.x * (1 - stabSettings.gyro_alpha);
+ q_item.y[1] = q_item.y[1] * stabSettings.gyro_alpha + gyro.y * (1 - stabSettings.gyro_alpha);
+ q_item.y[2] = q_item.y[2] * stabSettings.gyro_alpha + gyro.z * (1 - stabSettings.gyro_alpha);
q_item.u[0] = actuators.Roll;
q_item.u[1] = actuators.Pitch;
q_item.u[2] = actuators.Yaw;
@@ -633,12 +654,14 @@ static void InitSystemIdent(bool loadDefaults)
// Tau, Beta, and the Complete flag get default values
// in preparation for running AutoTune
systemIdentSettings.Tau = systemIdentState.Tau;
+systemIdentSettings.GyroReadTimeAverage = systemIdentState.GyroReadTimeAverage;
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
systemIdentSettings.Complete = systemIdentState.Complete;
} else {
// Tau, Beta, and the Complete flag get stored values
// so the user can fly another battery to select and test PIDs with the slider/knob
systemIdentState.Tau = systemIdentSettings.Tau;
+systemIdentState.GyroReadTimeAverage = systemIdentSettings.GyroReadTimeAverage;
memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData));
systemIdentState.Complete = systemIdentSettings.Complete;
}
@@ -715,6 +738,9 @@ static void UpdateSystemIdentState(const float *X, const float *noise,
systemIdentState.NumSpilledPts = spills;
systemIdentState.HoverThrottle = hover_throttle;
+ systemIdentState.GyroReadTimeAverage = gyroReadTimeAverage;
+ systemIdentSettings.GyroReadTimeAverage = systemIdentState.GyroReadTimeAverage;
+
SystemIdentStateSet(&systemIdentState);
}
@@ -838,7 +864,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
const double ghf = (double)noiseRate / 1000.0d;
const double damp = (double)dampRate / 100.0d;
- double tau = exp(systemIdentState.Tau);
+ double tau = exp(systemIdentState.Tau) + (double) systemIdentSettings.GyroReadTimeAverage;
double exp_beta_roll_times_ghf = exp(systemIdentState.Beta.Roll) * ghf;
double exp_beta_pitch_times_ghf = exp(systemIdentState.Beta.Pitch) * ghf;
diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c
index 92a4054b0..7295b5ac3 100644
--- a/flight/pios/common/pios_mpu6000.c
+++ b/flight/pios/common/pios_mpu6000.c
@@ -104,6 +104,8 @@ static mpu6000_data_t mpu6000_data;
static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
#define SENSOR_COUNT 2
#define SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT)
+uint32_t gyroReadTime;
+
// ! Private functions
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
@@ -550,6 +552,7 @@ bool PIOS_MPU6000_IRQHandler(void)
read_ok = PIOS_MPU6000_ReadSensor(&woken);
if (read_ok) {
+ gyroReadTime = PIOS_DELAY_GetRaw();
bool woken2 = PIOS_MPU6000_HandleData();
woken |= woken2;
}
diff --git a/flight/pios/common/pios_mpu9250.c b/flight/pios/common/pios_mpu9250.c
index 6fd4ba824..42ff55c18 100644
--- a/flight/pios/common/pios_mpu9250.c
+++ b/flight/pios/common/pios_mpu9250.c
@@ -129,6 +129,7 @@ static volatile bool mag_ready = false;
static struct mpu9250_dev *dev;
volatile bool mpu9250_configured = false;
static mpu9250_data_t mpu9250_data;
+uint32_t gyroReadTime;
// ! Private functions
static struct mpu9250_dev *PIOS_MPU9250_alloc(const struct pios_mpu9250_cfg *cfg);
@@ -829,6 +830,7 @@ bool PIOS_MPU9250_IRQHandler(void)
read_ok = PIOS_MPU9250_ReadSensor(&woken);
if (read_ok) {
+ gyroReadTime = PIOS_DELAY_GetRaw();
bool woken2 = PIOS_MPU9250_HandleData();
woken |= woken2;
}
diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml
index 9a2d2f7be..0fef6efe3 100644
--- a/shared/uavobjectdefinition/systemidentsettings.xml
+++ b/shared/uavobjectdefinition/systemidentsettings.xml
@@ -51,6 +51,7 @@
+
diff --git a/shared/uavobjectdefinition/systemidentstate.xml b/shared/uavobjectdefinition/systemidentstate.xml
index 457ef3515..9741b9853 100644
--- a/shared/uavobjectdefinition/systemidentstate.xml
+++ b/shared/uavobjectdefinition/systemidentstate.xml
@@ -11,6 +11,7 @@
+