From 7553eb4f32efc4604d96c25a11d3650063530e5e Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 19 Jun 2016 01:43:26 -0400 Subject: [PATCH] LP-340 Quick code with both timing fixes --- flight/modules/AutoTune/autotune.c | 28 ++++++++++++++++++- flight/pios/common/pios_mpu6000.c | 3 ++ flight/pios/common/pios_mpu9250.c | 2 ++ .../systemidentsettings.xml | 1 + .../uavobjectdefinition/systemidentstate.xml | 1 + 5 files changed, 34 insertions(+), 1 deletion(-) 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 @@ +