rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-340 Quick code with both timing fixes

Browse Source
This commit is contained in:
Cliff Geerdes 2016-06-19 01:43:26 -04:00
parent 49bb804b00
commit 7553eb4f32
5 changed files with 34 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/modules/AutoTune/autotune.c
View File

@ -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;

3
flight/pios/common/pios_mpu6000.c
View File

@ -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;
}

2
flight/pios/common/pios_mpu9250.c
View File

@ -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;
}

1
shared/uavobjectdefinition/systemidentsettings.xml
View File

@ -51,6 +51,7 @@
<field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/>
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<field name="GyroReadTimeAverage" units="s" type="float" elements="1" defaultvalue="0.0005"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

1
shared/uavobjectdefinition/systemidentstate.xml
View File

@ -11,6 +11,7 @@
<field name="NumSpilledPts" units="" type="uint32" elements="1" defaultvalue="0"/>
<field name="HoverThrottle" units="%/100" type="float" elements="1" defaultvalue="0"/>
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<field name="GyroReadTimeAverage" units="s" type="float" elements="1" defaultvalue="0.0005"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>