From 7553eb4f32efc4604d96c25a11d3650063530e5e Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 19 Jun 2016 01:43:26 -0400 Subject: [PATCH 01/12] 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 @@ + From f2f8d8e712b627904446a47e1ef6beb450954aed Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 12 Jul 2016 13:27:56 -0400 Subject: [PATCH 02/12] LP-340 code changes --- flight/modules/AutoTune/autotune.c | 461 +++++++++--------- flight/modules/Sensors/sensors.c | 32 +- .../modules/StateEstimation/stateestimation.c | 1 + flight/pios/common/pios_mpu6000.c | 16 +- flight/pios/common/pios_mpu9250.c | 17 +- flight/pios/inc/pios_sensors.h | 5 +- shared/uavobjectdefinition/gyrosensor.xml | 1 + shared/uavobjectdefinition/gyrostate.xml | 1 + .../systemidentsettings.xml | 5 +- .../uavobjectdefinition/systemidentstate.xml | 4 +- 10 files changed, 277 insertions(+), 266 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index d9c8d9206..1593a1b5e 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -78,6 +78,7 @@ #define AT_QUEUE_NUMELEM 18 #endif +#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */ #define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */ #define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */ #define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */ @@ -95,6 +96,7 @@ #define YAW_BETA_LOW 4 #define TAU_TOO_LONG 8 #define TAU_TOO_SHORT 16 +#define CPU_TOO_SLOW 32 // smooth-quick modes #define SMOOTH_QUICK_DISABLED 0 @@ -104,55 +106,54 @@ // Private types enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING }; - struct at_queued_data { - float y[3]; /* Gyro measurements */ - float u[3]; /* Actuator desired */ - float throttle; /* Throttle desired */ - - uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */ + float y[3]; /* Gyro measurements */ + float u[3]; /* Actuator desired */ + float throttle; /* Throttle desired */ + uint32_t gyroStateCallbackTimestamp; /* PIOS_DELAY_GetRaw() time of GyroState callback */ + uint32_t sensorReadTimestamp; /* PIOS_DELAY_GetRaw() time of sensor read */ }; -// Global variables -extern uint32_t gyroReadTime; - - // Private variables +static SystemIdentSettingsData systemIdentSettings; +// save memory because metadata is only briefly accessed, when normal data struct is not being used +// unnamed union issues a warning +static union { + SystemIdentStateData systemIdentState; + UAVObjMetadata systemIdentStateMetaData; +} u; +static StabilizationBankManualRateData manualRate; static xTaskHandle taskHandle; -static bool moduleEnabled; static xQueueHandle atQueue; +static float gX[AF_NUMX] = { 0 }; +static float gP[AF_NUMP] = { 0 }; +static float gyroReadTimeAverage; +static float gyroReadTimeAverageAlpha; +static float gyroReadTimeAverageAlphaAlpha; +static float alpha; +static float smoothQuickSetting; static volatile uint32_t atPointsSpilled; static uint32_t throttleAccumulator; static uint8_t rollMax, pitchMax; -static StabilizationBankManualRateData manualRate; -static float gX[AF_NUMX] = { 0 }; -static float gP[AF_NUMP] = { 0 }; -static SystemIdentSettingsData systemIdentSettings; -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 +static bool moduleEnabled; // Private functions -static void AutoTuneTask(void *parameters); -static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in); -static void AfInit(float X[AF_NUMX], float P[AF_NUMP]); -static uint8_t CheckSettingsRaw(); -static uint8_t CheckSettings(); -static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise); -static void ComputeStabilizationAndSetPids(); -static void ProportionPidsSmoothToQuick(float min, float val, float max); static void AtNewGyroData(UAVObjEvent *ev); +static void AutoTuneTask(void *parameters); +static void AfInit(float X[AF_NUMX], float P[AF_NUMP]); +static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in); +static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); +static uint8_t CheckSettings(); +static uint8_t CheckSettingsRaw(); +static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise); +static void InitSystemIdent(bool loadDefaults); +static void ProportionPidsSmoothToQuick(); static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle); static void UpdateStabilizationDesired(bool doingIdent); -static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); -static void InitSystemIdent(bool loadDefaults); -static void InitSmoothQuick(bool loadToggle); /** @@ -196,9 +197,6 @@ 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; @@ -213,9 +211,6 @@ int32_t AutoTuneStart(void) { // Start main task if it is enabled if (moduleEnabled) { - // taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY); - // TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle); - // PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); GyroStateConnectCallback(AtNewGyroData); xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); @@ -232,15 +227,21 @@ MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart); */ static void AutoTuneTask(__attribute__((unused)) void *parameters) { - enum AUTOTUNE_STATE state = AT_INIT; - uint32_t lastUpdateTime = 0; // initialization is only for compiler warning float noise[3] = { 0 }; + float dT_s = 0.0f; + uint32_t lastUpdateTime = 0; // initialization is only for compiler warning uint32_t lastTime = 0.0f; uint32_t measureTime = 0; uint32_t updateCounter = 0; + enum AUTOTUNE_STATE state = AT_INIT; bool saveSiNeeded = false; bool savePidNeeded = false; + // wait for the accessory values to stabilize + // otherwise they come up as zero, then change to their real value + // and that causes the PIDs to be re-exported (if smoothquick is active), which the user may not want + vTaskDelay(TASK_STARTUP_DELAY_MS / portTICK_RATE_MS); + // get max attitude / max rate // for use in generating Attitude mode commands from this module // note that the values could change when they change flight mode (and the associated bank) @@ -249,9 +250,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) StabilizationBankManualRateGet(&manualRate); // correctly set accessoryToUse and flightModeSwitchTogglePosition // based on what is in SystemIdent - // so that the user can use the PID smooth->quick slider in following flights + // so that the user can use the PID smooth->quick slider in flights following the autotune flight InitSystemIdent(false); - InitSmoothQuick(true); + smoothQuickSetting = systemIdentSettings.SmoothQuickSetting; while (1) { uint32_t diffTime; @@ -260,9 +261,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) FlightStatusData flightStatus; FlightStatusGet(&flightStatus); - // I have never seen this module misbehave in a way that requires a watchdog, so not bothering making one - // PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (saveSiNeeded) { saveSiNeeded = false; @@ -295,21 +293,23 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) && systemIdentSettings.Complete && !CheckSettings()) { if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // if user toggled while armed set PID's to next in sequence - // 2,3,4,0,1 (5 positions) or 1,2,0 (3 positions) or 3,4,5,6,0,1,2 (7 positions) - // if you assume that smoothest is -100 and quickest is +100 - // this corresponds to 0,+50,+100,-100,-50 (for 5 position toggle) - ++flightModeSwitchTogglePosition; - if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) { - flightModeSwitchTogglePosition = 0; + // if you assume that smoothest is -1 and quickest is +1 + // this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle) + smoothQuickSetting += 1.0f / (float)flightModeSwitchTogglePosition; + if (smoothQuickSetting > 1.001f) { + smoothQuickSetting = -1.0f; } } else { - // if they did it disarmed, then set PID's back to AutoTune default - flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; + // if they did the 3x FMS toggle while disarmed, set PID's back to the middle of smoothquick + smoothQuickSetting = 0.0f; } - ProportionPidsSmoothToQuick(0.0f, - (float)flightModeSwitchTogglePosition, - (float)(systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE)); + // calculate PIDs based on new smoothQuickSetting and save to the PID bank + ProportionPidsSmoothToQuick(); + // save new PIDs permanently when / if disarmed savePidNeeded = true; + // we also save the new knob/toggle value for startup next time + // this keeps the PIDs in sync with the toggle position + saveSiNeeded = true; } ////////////////////////////////////////////////////////////////////////////////////// @@ -332,43 +332,35 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // - the state machine needs to be reset // - the local version of Attitude mode gets skipped if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { - static bool savePidActive = false; // if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range // and FC is not currently running autotune - // and accessory0-3 changed by at least 1/160 of full range (2) + // and accessory0-3 changed by at least 1/85 of full range (2) // (don't bother checking to see if the requested accessory# is configured properly // if it isn't, the value will be 0 which is the center of [-1,1] anyway) if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) { - static AccessoryDesiredData accessoryValueOld = { 0.0f }; AccessoryDesiredData accessoryValue; AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); - // if the accessory changed more than 1/80 - // (this test is intended to remove one unit jitter) - // some old PPM receivers use a low resolution chip which only allows about 180 steps + // if the accessory changed more than some percent of total range + // some old PPM receivers use a low resolution chip which only allows about 180 steps out of a range of 2.0 + // a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0 // what we are doing here does not need any higher precision than that - if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) { - accessoryValueOld = accessoryValue; - // this copies the PIDs to memory and makes them active - // but does not write them to permanent storage - ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f); - // don't save PID to perm storage the first time - // that usually means at power up - // - // that keeps it from writing the same value at each boot - // but means that it won't be permanent if they move the slider before FC power on - // (note that the PIDs will be changed, just not saved permanently) - if (savePidActive) { - // this schedules the first possible write of the PIDs to occur a fraction of a second or so from now - // and moves the scheduled time if it is already scheduled - savePidDelay = SMOOTH_QUICK_FLUSH_TICKS; - } else { - savePidActive = true; - } + // user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed + if (fabsf(smoothQuickSetting - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) { + smoothQuickSetting = accessoryValue.AccessoryVal; + // calculate PIDs based on new smoothQuickSetting and save to the PID bank + ProportionPidsSmoothToQuick(); + // this schedules the first possible write of the PIDs to occur a fraction of a second or so from now + // and changes the scheduled time if it is already scheduled + savePidDelay = SMOOTH_QUICK_FLUSH_TICKS; } else if (savePidDelay && --savePidDelay == 0) { // this flags that the PIDs can be written to permanent storage right now // but they will only be written when the FC is disarmed // so this means immediate (after NOT_AT_MODE_DELAY_MS) or wait till FC is disarmed savePidNeeded = true; + // we also save the new knob/toggle value for startup next time + // this avoids rewriting the PIDs at each startup + // because knob is unknown / not where it is expected / looks like knob moved + saveSiNeeded = true; } } else { savePidDelay = 0; @@ -406,7 +398,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // load SystemIdentSettings so that they can change it // and do smooth-quick on changed values InitSystemIdent(false); - InitSmoothQuick(false); // wait for FC to arm in case they are doing this without a flight mode switch // that causes the 2+ second delay that follows to happen after arming // which gives them a chance to take off before the shakes start @@ -437,7 +428,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // and the complete data has been sanity checked savePidNeeded = false; InitSystemIdent(true); - InitSmoothQuick(true); AfInit(gX, gP); UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; @@ -455,6 +445,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) updateCounter = 0; atPointsSpilled = 0; throttleAccumulator = 0; + alpha = 0.0f; state = AT_RUN; lastUpdateTime = xTaskGetTickCount(); break; @@ -476,13 +467,18 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) break; } /* calculate time between successive points */ - float dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.raw_time) * 1.0e-6f; + dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.gyroStateCallbackTimestamp) * 1.0e-6f; /* This is for the first point, but * also if we have extended drops */ if (dT_s > 0.010f) { dT_s = 0.010f; } - lastTime = pt.raw_time; + lastTime = pt.gyroStateCallbackTimestamp; + // original algorithm handles time from GyroStateGet() to detected motion + // this algorithm also includes the time from raw gyro read to GyroStateGet() + gyroReadTimeAverage = gyroReadTimeAverage * alpha + + PIOS_DELAY_DiffuS2(pt.sensorReadTimestamp, pt.gyroStateCallbackTimestamp) * 1.0e-6f * (1.0f - alpha); + alpha = alpha * gyroReadTimeAverageAlphaAlpha + gyroReadTimeAverageAlpha * (1.0f - gyroReadTimeAverageAlphaAlpha); AfPredict(gX, gP, pt.u, pt.y, dT_s, pt.throttle); for (int j = 0; j < 3; ++j) { const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz @@ -492,7 +488,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) throttleAccumulator += 10000 * pt.throttle; // Update uavo every 256 cycles to avoid // telemetry spam - if (((updateCounter++) & 0xff) == 0) { + if (((++updateCounter) & 0xff) == 0) { float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle); } @@ -504,16 +500,23 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) break; case AT_FINISHED: + // update with info from the last few data points + if ((updateCounter & 0xff) != 0) { + float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; + UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle); + } // data is automatically considered bad if FC was disarmed at the time AT completed if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // always calculate and save PIDs if disabling sanity checks if (!CheckSettings()) { - ComputeStabilizationAndSetPids(); + ProportionPidsSmoothToQuick(); savePidNeeded = true; - // mark these results as good in the permanent settings so they can be used next flight too - systemIdentSettings.Complete = true; // mark these results as good in the log settings so they can be viewed in playback - systemIdentState.Complete = true; + u.systemIdentState.Complete = true; + SystemIdentStateCompleteSet(&u.systemIdentState.Complete); + // mark these results as good in the permanent settings so they can be used next flight too + // this is written to the UAVO below, outside of the ARMED and CheckSettings() checks + systemIdentSettings.Complete = true; } // always raise an alarm if sanity checks failed // even if disabling sanity checks @@ -525,8 +528,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); } } - float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; - UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hoverThrottle); + // need to save UAVO after .Complete gets potentially set + // SystemIdentSettings needs the whole UAVO saved so it is saved outside the previous checks SystemIdentSettingsSet(&systemIdentSettings); state = AT_WAITING; break; @@ -557,6 +560,7 @@ static void AtNewGyroData(UAVObjEvent *ev) static bool last_sample_unpushed = false; GyroStateData gyro; ActuatorDesiredData actuators; + uint32_t timestamp; if (!ev || !ev->obj || ev->instId != 0 || ev->event != EV_UPDATED) { return; @@ -564,11 +568,10 @@ static void AtNewGyroData(UAVObjEvent *ev) // object will at times change asynchronously so must copy data here, with locking // and do it as soon as possible + timestamp = PIOS_DELAY_GetRaw(); 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! */ @@ -577,22 +580,15 @@ 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; - q_item.throttle = actuators.Thrust; + q_item.gyroStateCallbackTimestamp = timestamp; + 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; + q_item.throttle = actuators.Thrust; + q_item.sensorReadTimestamp = gyro.SensorReadTimestamp; if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) { last_sample_unpushed = true; @@ -641,7 +637,7 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) // and set some flags based on the values // it is used two ways: // - on startup it reads settings so the user can reuse an old tune with smooth-quick -// - at tune time, it inits the state in preparation for tuning +// - at tune time, it inits the state to default values of uavo xml file, in preparation for tuning static void InitSystemIdent(bool loadDefaults) { SystemIdentSettingsGet(&systemIdentSettings); @@ -649,27 +645,37 @@ static void InitSystemIdent(bool loadDefaults) if (loadDefaults) { // get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau) // so that if they are changed there (mainly for future code changes), they will be changed here too + // save metadata from being changed by the following SetDefaults() + SystemIdentStateGetMetadata(&u.systemIdentStateMetaData); SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0); - SystemIdentStateGet(&systemIdentState); - // Tau, Beta, and the Complete flag get default values + SystemIdentStateSetMetadata(&u.systemIdentStateMetaData); + SystemIdentStateGet(&u.systemIdentState); + // Tau, GyroReadTimeAverage, 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; + systemIdentSettings.Tau = u.systemIdentState.Tau; + systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; + memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); + systemIdentSettings.Complete = u.systemIdentState.Complete; } else { - // Tau, Beta, and the Complete flag get stored values + // Tau, GyroReadTimeAverage, 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; + u.systemIdentState.Tau = systemIdentSettings.Tau; + u.systemIdentState.GyroReadTimeAverage = systemIdentSettings.GyroReadTimeAverage; + memcpy(&u.systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData)); + u.systemIdentState.Complete = systemIdentSettings.Complete; } -} + // (1.0f / PIOS_SENSOR_RATE) is gyro period + // the -1/10 makes it converge nicely, the other values make it converge the same way if the configuration is changed + // gyroReadTimeAverageAlphaAlpha is 0.9996 when the tuning duration is the default of 60 seconds + gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / PIOS_SENSOR_RATE / ((float)systemIdentSettings.TuningDuration/10.0f)); + if (!IS_REAL(gyroReadTimeAverageAlphaAlpha)) { + gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / 500.0f / (60/10)); // basically 0.9996 + } + // 0.99999988f is as close to 1.0f as possible to make final average as smooth as possible + gyroReadTimeAverageAlpha = 0.99999988f; + gyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; -static void InitSmoothQuick(bool loadToggle) -{ uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; switch (SmoothQuickSource) { @@ -677,34 +683,34 @@ static void InitSmoothQuick(bool loadToggle) case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3 + // leave smoothQuickSetting alone since it is always controlled by knob // disable PID changing with flight mode switch - // ignore loadToggle if user is also switching to use knob as source flightModeSwitchTogglePosition = -1; + // enable PID changing with accessory0-3 accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; - systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points + // don't allow init of current toggle position in the middle of 3x fms toggle + if (loadDefaults) { + // set toggle to middle of range + smoothQuickSetting = 0.0f; + } + // enable PID changing with flight mode switch + flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; // disable PID changing with accessory0-3 accessoryToUse = -1; - // don't allow init of current toggle position in the middle of 3x fms toggle - if (loadToggle) { - // first test PID is in the middle of the smooth -> quick range - flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; - } - systemIdentSettings.SmoothQuickSource = SmoothQuickSource; break; case SMOOTH_QUICK_DISABLED: default: + // leave smoothQuickSetting alone so user can set it to a different value and have it stay that value // disable PID changing with flight mode switch - // ignore loadToggle since user is disabling toggle flightModeSwitchTogglePosition = -1; // disable PID changing with accessory0-3 accessoryToUse = -1; - systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED; break; } } @@ -716,37 +722,38 @@ static void InitSmoothQuick(bool loadToggle) static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle) { - systemIdentState.Beta.Roll = X[6]; - systemIdentState.Beta.Pitch = X[7]; - systemIdentState.Beta.Yaw = X[8]; - systemIdentState.Bias.Roll = X[10]; - systemIdentState.Bias.Pitch = X[11]; - systemIdentState.Bias.Yaw = X[12]; - systemIdentState.Tau = X[9]; - // 'settings' beta and tau have same value as 'state' versions + u.systemIdentState.Beta.Roll = X[6]; + u.systemIdentState.Beta.Pitch = X[7]; + u.systemIdentState.Beta.Yaw = X[8]; + u.systemIdentState.Bias.Roll = X[10]; + u.systemIdentState.Bias.Pitch = X[11]; + u.systemIdentState.Bias.Yaw = X[12]; + u.systemIdentState.Tau = X[9]; + if (noise) { + u.systemIdentState.Noise.Roll = noise[0]; + u.systemIdentState.Noise.Pitch = noise[1]; + u.systemIdentState.Noise.Yaw = noise[2]; + } + u.systemIdentState.Period = dT_s * 1000.0f; + u.systemIdentState.NumAfPredicts = predicts; + u.systemIdentState.NumSpilledPts = spills; + u.systemIdentState.HoverThrottle = hover_throttle; + u.systemIdentState.GyroReadTimeAverage = gyroReadTimeAverage; + + // 'settings' tau, beta, and GyroReadTimeAverage have same value as 'state' versions // the state version produces a GCS log // the settings version is remembered after power off/on - systemIdentSettings.Tau = systemIdentState.Tau; - memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); - if (noise) { - systemIdentState.Noise.Roll = noise[0]; - systemIdentState.Noise.Pitch = noise[1]; - systemIdentState.Noise.Yaw = noise[2]; - } - systemIdentState.Period = dT_s * 1000.0f; - systemIdentState.NumAfPredicts = predicts; - systemIdentState.NumSpilledPts = spills; - systemIdentState.HoverThrottle = hover_throttle; + systemIdentSettings.Tau = u.systemIdentState.Tau; + memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); + systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; + systemIdentSettings.SmoothQuickSetting = smoothQuickSetting; - systemIdentState.GyroReadTimeAverage = gyroReadTimeAverage; - systemIdentSettings.GyroReadTimeAverage = systemIdentState.GyroReadTimeAverage; - - SystemIdentStateSet(&systemIdentState); + SystemIdentStateSet(&u.systemIdentState); } // when running AutoTune mode, this bypasses manualcontrol.c / stabilizedhandler.c -// to control exactly when the multicopter should be in Attitude mode vs. SystemIdent mode +// to control whether the multicopter should be in Attitude mode vs. SystemIdent mode static void UpdateStabilizationDesired(bool doingIdent) { StabilizationDesiredData stabDesired; @@ -783,28 +790,26 @@ static uint8_t CheckSettingsRaw() // Check the axis gains // Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values. - if (systemIdentState.Beta.Roll < 6) { + if (u.systemIdentState.Beta.Roll < 6) { retVal |= ROLL_BETA_LOW; } - if (systemIdentState.Beta.Pitch < 6) { + if (u.systemIdentState.Beta.Pitch < 6) { retVal |= PITCH_BETA_LOW; } -// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default -#if 0 - // if (systemIdentState.Beta.Yaw < 4) { - if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) { - retVal |= YAW_BETA_LOW; - } -#endif // Check the response speed // Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values. - if (expf(systemIdentState.Tau) > 0.1f) { + if (expapprox(u.systemIdentState.Tau) > 0.1f) { retVal |= TAU_TOO_LONG; } // Extreme values: Your estimated response speed (tau) is faster than normal. This will result in large PID values. - else if (expf(systemIdentState.Tau) < 0.008f) { + else if (expapprox(u.systemIdentState.Tau) < 0.008f) { retVal |= TAU_TOO_SHORT; } + // Check gyroReadTimeAverage + // Sanity check: CPU is too slow compared to gyro rate + if (gyroReadTimeAverage > (1.0f/PIOS_SENSOR_RATE)) { + retVal |= CPU_TOO_SLOW; + } return retVal; } @@ -821,27 +826,17 @@ static uint8_t CheckSettings() if (systemIdentSettings.DisableSanityChecks) { retVal = 0; } -// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default -#if 0 - // if not calculating yaw, or if calculating yaw but ignoring errors - else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) { - // clear the yaw error bit - retVal &= ~YAW_BETA_LOW; - } -#endif return retVal; } -// given Tau(delay) and Beta(gain) from the tune (and user selection of smooth to quick) calculate the PIDs -// this code came from dRonin GCS and uses double precision math -// most of the doubles could be replaced with floats +// given Tau"+"GyroReadTimeAverage(delay) and Beta(gain) from the tune (and user selection of smooth to quick) calculate the PIDs +// this code came from dRonin GCS and has been converted from double precision math to single precision static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) { _Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)"); - StabilizationBankData stabSettingsBank; - + StabilizationBankData volatile stabSettingsBank; switch (systemIdentSettings.DestinationPidBank) { case 1: StabilizationSettingsBank1Get((void *)&stabSettingsBank); @@ -861,59 +856,65 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // make oscillations less likely // - ghf is the amount of high frequency gain and limits the influence // of noise - const double ghf = (double)noiseRate / 1000.0d; - const double damp = (double)dampRate / 100.0d; + const float ghf = noiseRate / 1000.0f; + const float damp = dampRate / 100.0f; - 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; + float tau = expapprox(u.systemIdentState.Tau) + systemIdentSettings.GyroReadTimeAverage; + float exp_beta_roll_times_ghf = expapprox(u.systemIdentState.Beta.Roll) * ghf; + float exp_beta_pitch_times_ghf = expapprox(u.systemIdentState.Beta.Pitch) * ghf; - double wn = 1.0d / tau; - double tau_d = 0.0d; + float wn = 1.0f / tau; + float tau_d = 0.0f; for (int i = 0; i < 30; i++) { - double tau_d_roll = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_roll_times_ghf); - double tau_d_pitch = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_pitch_times_ghf); + float tau_d_roll = (2.0f * damp * tau * wn - 1.0f) / (4.0f * tau * damp * damp * wn * wn - 2.0f * damp * wn - tau * wn * wn + exp_beta_roll_times_ghf); + float tau_d_pitch = (2.0f * damp * tau * wn - 1.0f) / (4.0f * tau * damp * damp * wn * wn - 2.0f * damp * wn - tau * wn * wn + exp_beta_pitch_times_ghf); // Select the slowest filter property tau_d = (tau_d_roll > tau_d_pitch) ? tau_d_roll : tau_d_pitch; - wn = (tau + tau_d) / (tau * tau_d) / (2.0d * damp + 2.0d); + wn = (tau + tau_d) / (tau * tau_d) / (2.0f * damp + 2.0f); } // Set the real pole position. The first pole is quite slow, which // prevents the integral being too snappy and driving too much // overshoot. - const double a = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn) / 20.0d; - const double b = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn - a); + const float a = ((tau + tau_d) / tau / tau_d - 2.0f * damp * wn) / 20.0f; + const float b = ((tau + tau_d) / tau / tau_d - 2.0f * damp * wn - a); // Calculate the gain for the outer loop by approximating the // inner loop as a single order lpf. Set the outer loop to be // critically damped; - const double zeta_o = 1.3d; - const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d / wn); - const double ki_o = 0.75d * kp_o / (2.0d * M_PI_D * tau * 10.0d); + const float zeta_o = 1.3f; + const float kp_o = 1.0f / 4.0f / (zeta_o * zeta_o) / (1.0f / wn); + const float ki_o = 0.75f * kp_o / (2.0f * M_PI_F * tau * 10.0f); float kpMax = 0.0f; - double betaMinLn = 1000.0d; - StabilizationBankRollRatePIDData *rollPitchPid = NULL; // satisfy compiler warning only + float betaMinLn = 1000.0f; + StabilizationBankRollRatePIDData volatile * rollPitchPid = NULL; // satisfy compiler warning only for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) { - double betaLn = SystemIdentStateBetaToArray(systemIdentState.Beta)[i]; - double beta = exp(betaLn); - double ki; - double kp; - double kd; + float betaLn = SystemIdentStateBetaToArray(u.systemIdentState.Beta)[i]; + float beta = expapprox(betaLn); + float ki; + float kp; + float kd; switch (i) { case 0: // Roll case 1: // Pitch ki = a * b * wn * wn * tau * tau_d / beta; - kp = tau * tau_d * ((a + b) * wn * wn + 2.0d * a * b * damp * wn) / beta - ki * tau_d; - kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0d * damp * wn) - 1.0d) / beta - kp * tau_d; + kp = tau * tau_d * ((a + b) * wn * wn + 2.0f * a * b * damp * wn) / beta - ki * tau_d; + kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0f * damp * wn) - 1.0f) / beta - kp * tau_d; if (betaMinLn > betaLn) { betaMinLn = betaLn; // RollRatePID PitchRatePID YawRatePID // form an array of structures // point to one - rollPitchPid = &(&stabSettingsBank.RollRatePID)[i]; + // this pointer arithmetic no longer works as expected in a gcc 64 bit test program + // rollPitchPid = &(&stabSettingsBank.RollRatePID)[i]; + if (i==0) { + rollPitchPid = &stabSettingsBank.RollRatePID; + } else { + rollPitchPid = (StabilizationBankRollRatePIDData *) &stabSettingsBank.PitchRatePID; + } } break; case 2: // Yaw @@ -933,16 +934,17 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // which is xp / (betaMin * betaYaw^0.6 / betaMin^0.6) // which is xp / (betaMin^0.4 * betaYaw^0.6) // hence the new effective betaYaw for Yaw P is (betaMin^0.4)*(betaYaw^0.6) - beta = exp(0.6d * (betaMinLn - (double)systemIdentState.Beta.Yaw)); - kp = (double)rollPitchPid->Kp * beta; - ki = 0.8d * (double)rollPitchPid->Ki * beta; - kd = 0.8d * (double)rollPitchPid->Kd * beta; + beta = expapprox(0.6f * (betaMinLn - u.systemIdentState.Beta.Yaw)); + // this casting assumes that RollRatePID is the same as PitchRatePID + kp = rollPitchPid->Kp * beta; + ki = 0.8f * rollPitchPid->Ki * beta; + kd = 0.8f * rollPitchPid->Kd * beta; break; } if (i < 2) { - if (kpMax < (float)kp) { - kpMax = (float)kp; + if (kpMax < kp) { + kpMax = kp; } } else { // use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value @@ -965,14 +967,14 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float } float ratio = 1.0f; - if (min > 0.0f && (float)kp < min) { - ratio = (float)kp / min; - } else if (max > 0.0f && (float)kp > max) { - ratio = (float)kp / max; + if (min > 0.0f && kp < min) { + ratio = kp / min; + } else if (max > 0.0f && kp > max) { + ratio = kp / max; } - kp /= (double)ratio; - ki /= (double)ratio; - kd /= (double)ratio; + kp /= ratio; + ki /= ratio; + kd /= ratio; } switch (i) { @@ -1006,8 +1008,9 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float } // Librepilot might do something more with this some time - // stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d); + // stabSettingsBank.DerivativeCutoff = 1.0f / (2.0f*M_PI_F*tau_d); // SystemIdentSettingsDerivativeCutoffSet(&systemIdentSettings.DerivativeCutoff); + // then something to schedule saving this permanently to flash when disarmed // Save PIDs to UAVO RAM (not permanently yet) switch (systemIdentSettings.DestinationPidBank) { @@ -1024,13 +1027,6 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float } -// calculate PIDs using default smooth-quick settings -static void ComputeStabilizationAndSetPids() -{ - ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentSettings.DampRate, systemIdentSettings.NoiseRate); -} - - // scale the damp and the noise to generate PIDs according to how a slider or other user specified ratio is set // // when val is half way between min and max, it generates the default PIDs @@ -1043,9 +1039,12 @@ static void ComputeStabilizationAndSetPids() // this is done piecewise because we are not guaranteed that default-min == max-default // but we are given that [smoothDamp,smoothNoise] [defaultDamp,defaultNoise] [quickDamp,quickNoise] are all good parameterizations // this code guarantees that we will get those exact parameterizations at (val =) min, (max+min)/2, and max -static void ProportionPidsSmoothToQuick(float min, float val, float max) +static void ProportionPidsSmoothToQuick() { float ratio, damp, noise; + float min = -1.0f; + float val = smoothQuickSetting; + float max = 1.0f; // translate from range [min, max] to range [0, max-min] // that takes care of min < 0 case too @@ -1066,6 +1065,8 @@ static void ProportionPidsSmoothToQuick(float min, float val, float max) } ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise); + // save it to the system, but not yet written to flash + SystemIdentSettingsSmoothQuickSettingSet(&smoothQuickSetting); } @@ -1328,8 +1329,8 @@ static void AfInit(float X[AF_NUMX], float P[AF_NUMP]) memset(X, 0, AF_NUMX * sizeof(X[0])); // get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau) // so that if they are changed there (mainly for future code changes), they will be changed here too - memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta)); - X[9] = systemIdentState.Tau; + memcpy(&X[6], &u.systemIdentState.Beta, sizeof(u.systemIdentState.Beta)); + X[9] = u.systemIdentState.Tau; // P initialization memset(P, 0, AF_NUMP * sizeof(P[0])); diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index c13b96e5f..6f418788a 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -109,9 +109,11 @@ static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f // Private types typedef struct { // used to accumulate all samples in a task iteration - Vector3i32 accum[2]; - int32_t temperature; - uint32_t count; + uint64_t timestamp; // sum of "PIOS_DELAY_GetRaw() times of sensor read" in this averaged set + Vector3i32 accum[2]; // summed 16 bit sensor values in this averaged set + int32_t temperature; // sum of 16 bit temperatures in this averaged set + uint32_t prev_timestamp; // to detect timer wrap around + uint16_t count; // number of sensor reads in this averaged set } sensor_fetch_context; #define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16)) @@ -145,7 +147,7 @@ static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const P static void clearContext(sensor_fetch_context *sensor_context); static void handleAccel(float *samples, float temperature); -static void handleGyro(float *samples, float temperature); +static void handleGyro(float *samples, float temperature, uint32_t timestamp); static void handleMag(float *samples, float temperature); #if defined(PIOS_INCLUDE_HMC5X83) static void handleAuxMag(float *samples); @@ -254,8 +256,6 @@ int32_t mag_test; * stabilization and to the attitude loop * */ - -uint32_t sensor_dt_us; static void SensorsTask(__attribute__((unused)) void *parameters) { portTickType lastSysTime; @@ -360,6 +360,8 @@ static void clearContext(sensor_fetch_context *sensor_context) sensor_context->accum[i].z = 0; } sensor_context->temperature = 0; + sensor_context->prev_timestamp = 0; + sensor_context->timestamp = 0LL; sensor_context->count = 0; } @@ -371,6 +373,14 @@ static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data sensor_context->accum[i].z += sample->sensorSample3Axis.sample[i].z; } sensor_context->temperature += sample->sensorSample3Axis.temperature; + sensor_context->timestamp += sample->sensorSample3Axis.timestamp; + if (sensor_context->prev_timestamp > sample->sensorSample3Axis.timestamp) { + // we've wrapped so add the dropped top bit + // this makes the average come out correct instead of (0xfd+0x01)/2 = 0x7f or such + sensor_context->timestamp += 0x100000000LL; + } else { + sensor_context->prev_timestamp = sample->sensorSample3Axis.timestamp; + } sensor_context->count++; } @@ -416,6 +426,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE if (sensor->type & PIOS_SENSORS_TYPE_3AXIS_GYRO) { uint8_t index = 0; + uint32_t timestamp; if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) { index = 1; } @@ -424,7 +435,8 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE samples[1] = ((float)sensor_context->accum[index].y * t); samples[2] = ((float)sensor_context->accum[index].z * t); temperature = (float)sensor_context->temperature * inv_count * 0.01f; - handleGyro(samples, temperature); + timestamp = (uint32_t)(sensor_context->timestamp / sensor_context->count); + handleGyro(samples, temperature, timestamp); return; } } @@ -456,10 +468,11 @@ static void handleAccel(float *samples, float temperature) accelSensorData.y = samples[1]; accelSensorData.z = samples[2]; accelSensorData.temperature = temperature; + AccelSensorSet(&accelSensorData); } -static void handleGyro(float *samples, float temperature) +static void handleGyro(float *samples, float temperature, uint32_t timestamp) { GyroSensorData gyroSensorData; @@ -469,10 +482,11 @@ static void handleGyro(float *samples, float temperature) samples[2] * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] }; rot_mult(R, gyros_out, samples); - gyroSensorData.temperature = temperature; gyroSensorData.x = samples[0]; gyroSensorData.y = samples[1]; gyroSensorData.z = samples[2]; + gyroSensorData.temperature = temperature; + gyroSensorData.SensorReadTimestamp = timestamp; GyroSensorSet(&gyroSensorData); } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index bde280636..72d28fb00 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -572,6 +572,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev) t.x = s.x + gyroDelta[0]; t.y = s.y + gyroDelta[1]; t.z = s.z + gyroDelta[2]; + t.SensorReadTimestamp = s.SensorReadTimestamp; GyroStateSet(&t); } diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index 7295b5ac3..ccf328da7 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -104,7 +104,6 @@ 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); @@ -113,7 +112,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg); static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU6000_GetReg(uint8_t address); static void PIOS_MPU6000_SetSpeed(const bool fast); -static bool PIOS_MPU6000_HandleData(); +static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp); static bool PIOS_MPU6000_ReadSensor(bool *woken); static int32_t PIOS_MPU6000_Test(void); @@ -542,25 +541,21 @@ static int32_t PIOS_MPU6000_Test(void) bool PIOS_MPU6000_IRQHandler(void) { + uint32_t gyro_read_timestamp = PIOS_DELAY_GetRaw(); bool woken = false; if (!mpu6000_configured) { return false; } - bool read_ok = false; - read_ok = PIOS_MPU6000_ReadSensor(&woken); - - if (read_ok) { - gyroReadTime = PIOS_DELAY_GetRaw(); - bool woken2 = PIOS_MPU6000_HandleData(); - woken |= woken2; + if (PIOS_MPU6000_ReadSensor(&woken)) { + woken |= PIOS_MPU6000_HandleData(gyro_read_timestamp); } return woken; } -static bool PIOS_MPU6000_HandleData() +static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp) { if (!queue_data) { return false; @@ -603,6 +598,7 @@ static bool PIOS_MPU6000_HandleData() const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature); // Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 queue_data->temperature = 3653 + (temp * 100) / 340; + queue_data->timestamp = gyro_read_timestamp; BaseType_t higherPriorityTaskWoken; xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken); diff --git a/flight/pios/common/pios_mpu9250.c b/flight/pios/common/pios_mpu9250.c index 42ff55c18..cdf11c259 100644 --- a/flight/pios/common/pios_mpu9250.c +++ b/flight/pios/common/pios_mpu9250.c @@ -129,7 +129,6 @@ 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); @@ -138,7 +137,7 @@ static void PIOS_MPU9250_Config(struct pios_mpu9250_cfg const *cfg); static int32_t PIOS_MPU9250_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU9250_GetReg(uint8_t address); static void PIOS_MPU9250_SetSpeed(const bool fast); -static bool PIOS_MPU9250_HandleData(); +static bool PIOS_MPU9250_HandleData(uint32_t gyro_read_timestamp); static bool PIOS_MPU9250_ReadSensor(bool *woken); static int32_t PIOS_MPU9250_Test(void); #if defined(PIOS_MPU9250_MAG) @@ -813,9 +812,9 @@ static bool PIOS_MPU9250_ReadMag(bool *woken) * @return a boolean to the EXTI IRQ Handler wrapper indicating if a * higher priority task is now eligible to run */ - bool PIOS_MPU9250_IRQHandler(void) { + uint32_t gyro_read_timestamp = PIOS_DELAY_GetRaw(); bool woken = false; if (!mpu9250_configured) { @@ -826,19 +825,14 @@ bool PIOS_MPU9250_IRQHandler(void) PIOS_MPU9250_ReadMag(&woken); #endif - bool read_ok = false; - read_ok = PIOS_MPU9250_ReadSensor(&woken); - - if (read_ok) { - gyroReadTime = PIOS_DELAY_GetRaw(); - bool woken2 = PIOS_MPU9250_HandleData(); - woken |= woken2; + if (PIOS_MPU9250_ReadSensor(&woken)) { + woken |= PIOS_MPU9250_HandleData(gyro_read_timestamp); } return woken; } -static bool PIOS_MPU9250_HandleData() +static bool PIOS_MPU9250_HandleData(uint32_t gyro_read_timestamp) { // Rotate the sensor to OP convention. The datasheet defines X as towards the right // and Y as forward. OP convention transposes this. Also the Z is defined negatively @@ -918,6 +912,7 @@ static bool PIOS_MPU9250_HandleData() queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu9250_data, Gyro_Z)); const int16_t temp = GET_SENSOR_DATA(mpu9250_data, Temperature); queue_data->temperature = 2100 + ((float)(temp - PIOS_MPU9250_TEMP_OFFSET)) * (100.0f / PIOS_MPU9250_TEMP_SENSITIVITY); + queue_data->timestamp = gyro_read_timestamp; mag_data->temperature = queue_data->temperature; #ifdef PIOS_MPU9250_MAG if (mag_valid) { diff --git a/flight/pios/inc/pios_sensors.h b/flight/pios/inc/pios_sensors.h index 085fe96f2..4397bc844 100644 --- a/flight/pios/inc/pios_sensors.h +++ b/flight/pios/inc/pios_sensors.h @@ -76,14 +76,15 @@ typedef struct PIOS_SENSORS_Instance { * A 3d Accel sample with temperature */ typedef struct PIOS_SENSORS_3Axis_SensorsWithTemp { - uint16_t count; // number of sensor instances + uint32_t timestamp; // PIOS_DELAY_GetRaw() time of sensor read + uint16_t count; // number of sensor instances int16_t temperature; // Degrees Celsius * 100 Vector3i16 sample[]; } PIOS_SENSORS_3Axis_SensorsWithTemp; typedef struct PIOS_SENSORS_1Axis_SensorsWithTemp { + float sample; // sample float temperature; // Degrees Celsius - float sample; // sample } PIOS_SENSORS_1Axis_SensorsWithTemp; /** diff --git a/shared/uavobjectdefinition/gyrosensor.xml b/shared/uavobjectdefinition/gyrosensor.xml index 3107f952e..80b8894b5 100644 --- a/shared/uavobjectdefinition/gyrosensor.xml +++ b/shared/uavobjectdefinition/gyrosensor.xml @@ -5,6 +5,7 @@ + diff --git a/shared/uavobjectdefinition/gyrostate.xml b/shared/uavobjectdefinition/gyrostate.xml index a4d497578..cf0386d8b 100644 --- a/shared/uavobjectdefinition/gyrostate.xml +++ b/shared/uavobjectdefinition/gyrostate.xml @@ -4,6 +4,7 @@ + diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 0fef6efe3..5f0e4cc22 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -35,7 +35,7 @@ - + @@ -49,9 +49,10 @@ + + - diff --git a/shared/uavobjectdefinition/systemidentstate.xml b/shared/uavobjectdefinition/systemidentstate.xml index 9741b9853..f09eb41c9 100644 --- a/shared/uavobjectdefinition/systemidentstate.xml +++ b/shared/uavobjectdefinition/systemidentstate.xml @@ -1,5 +1,5 @@ - + The input to the PID tuning. @@ -10,8 +10,8 @@ + - From 69f1964b12b18c2d72ccaa073f78cac2eac57279 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 12 Jul 2016 14:47:29 -0400 Subject: [PATCH 03/12] LP-340 uncrustify --- flight/modules/AutoTune/autotune.c | 50 +++++++++++++++--------------- flight/modules/Sensors/sensors.c | 8 ++--- flight/pios/common/pios_mpu6000.c | 2 +- flight/pios/inc/pios_sensors.h | 2 +- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 1593a1b5e..1a3f1c794 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -111,7 +111,7 @@ struct at_queued_data { float u[3]; /* Actuator desired */ float throttle; /* Throttle desired */ uint32_t gyroStateCallbackTimestamp; /* PIOS_DELAY_GetRaw() time of GyroState callback */ - uint32_t sensorReadTimestamp; /* PIOS_DELAY_GetRaw() time of sensor read */ + uint32_t sensorReadTimestamp; /* PIOS_DELAY_GetRaw() time of sensor read */ }; @@ -227,15 +227,15 @@ MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart); */ static void AutoTuneTask(__attribute__((unused)) void *parameters) { - float noise[3] = { 0 }; - float dT_s = 0.0f; + float noise[3] = { 0 }; + float dT_s = 0.0f; uint32_t lastUpdateTime = 0; // initialization is only for compiler warning - uint32_t lastTime = 0.0f; - uint32_t measureTime = 0; - uint32_t updateCounter = 0; + uint32_t lastTime = 0.0f; + uint32_t measureTime = 0; + uint32_t updateCounter = 0; enum AUTOTUNE_STATE state = AT_INIT; - bool saveSiNeeded = false; - bool savePidNeeded = false; + bool saveSiNeeded = false; + bool savePidNeeded = false; // wait for the accessory values to stabilize // otherwise they come up as zero, then change to their real value @@ -360,7 +360,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // we also save the new knob/toggle value for startup next time // this avoids rewriting the PIDs at each startup // because knob is unknown / not where it is expected / looks like knob moved - saveSiNeeded = true; + saveSiNeeded = true; } } else { savePidDelay = 0; @@ -512,7 +512,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) ProportionPidsSmoothToQuick(); savePidNeeded = true; // mark these results as good in the log settings so they can be viewed in playback - u.systemIdentState.Complete = true; + u.systemIdentState.Complete = true; SystemIdentStateCompleteSet(&u.systemIdentState.Complete); // mark these results as good in the permanent settings so they can be used next flight too // this is written to the UAVO below, outside of the ARMED and CheckSettings() checks @@ -580,14 +580,14 @@ static void AtNewGyroData(UAVObjEvent *ev) } } - q_item.gyroStateCallbackTimestamp = timestamp; - 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; - q_item.throttle = actuators.Thrust; + q_item.gyroStateCallbackTimestamp = timestamp; + 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; + q_item.throttle = actuators.Thrust; q_item.sensorReadTimestamp = gyro.SensorReadTimestamp; if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) { @@ -668,9 +668,9 @@ static void InitSystemIdent(bool loadDefaults) // (1.0f / PIOS_SENSOR_RATE) is gyro period // the -1/10 makes it converge nicely, the other values make it converge the same way if the configuration is changed // gyroReadTimeAverageAlphaAlpha is 0.9996 when the tuning duration is the default of 60 seconds - gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / PIOS_SENSOR_RATE / ((float)systemIdentSettings.TuningDuration/10.0f)); + gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / PIOS_SENSOR_RATE / ((float)systemIdentSettings.TuningDuration / 10.0f)); if (!IS_REAL(gyroReadTimeAverageAlphaAlpha)) { - gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / 500.0f / (60/10)); // basically 0.9996 + gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / 500.0f / (60 / 10)); // basically 0.9996 } // 0.99999988f is as close to 1.0f as possible to make final average as smooth as possible gyroReadTimeAverageAlpha = 0.99999988f; @@ -746,7 +746,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise, systemIdentSettings.Tau = u.systemIdentState.Tau; memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; - systemIdentSettings.SmoothQuickSetting = smoothQuickSetting; + systemIdentSettings.SmoothQuickSetting = smoothQuickSetting; SystemIdentStateSet(&u.systemIdentState); } @@ -807,7 +807,7 @@ static uint8_t CheckSettingsRaw() } // Check gyroReadTimeAverage // Sanity check: CPU is too slow compared to gyro rate - if (gyroReadTimeAverage > (1.0f/PIOS_SENSOR_RATE)) { + if (gyroReadTimeAverage > (1.0f / PIOS_SENSOR_RATE)) { retVal |= CPU_TOO_SLOW; } @@ -888,7 +888,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float float kpMax = 0.0f; float betaMinLn = 1000.0f; - StabilizationBankRollRatePIDData volatile * rollPitchPid = NULL; // satisfy compiler warning only + StabilizationBankRollRatePIDData volatile *rollPitchPid = NULL; // satisfy compiler warning only for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) { float betaLn = SystemIdentStateBetaToArray(u.systemIdentState.Beta)[i]; @@ -910,10 +910,10 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // point to one // this pointer arithmetic no longer works as expected in a gcc 64 bit test program // rollPitchPid = &(&stabSettingsBank.RollRatePID)[i]; - if (i==0) { + if (i == 0) { rollPitchPid = &stabSettingsBank.RollRatePID; } else { - rollPitchPid = (StabilizationBankRollRatePIDData *) &stabSettingsBank.PitchRatePID; + rollPitchPid = (StabilizationBankRollRatePIDData *)&stabSettingsBank.PitchRatePID; } } break; diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 6f418788a..a08e74b64 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -110,7 +110,7 @@ static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f typedef struct { // used to accumulate all samples in a task iteration uint64_t timestamp; // sum of "PIOS_DELAY_GetRaw() times of sensor read" in this averaged set - Vector3i32 accum[2]; // summed 16 bit sensor values in this averaged set + Vector3i32 accum[2]; // summed 16 bit sensor values in this averaged set int32_t temperature; // sum of 16 bit temperatures in this averaged set uint32_t prev_timestamp; // to detect timer wrap around uint16_t count; // number of sensor reads in this averaged set @@ -359,10 +359,10 @@ static void clearContext(sensor_fetch_context *sensor_context) sensor_context->accum[i].y = 0; sensor_context->accum[i].z = 0; } - sensor_context->temperature = 0; + sensor_context->temperature = 0; sensor_context->prev_timestamp = 0; - sensor_context->timestamp = 0LL; - sensor_context->count = 0; + sensor_context->timestamp = 0LL; + sensor_context->count = 0; } static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample) diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index ccf328da7..97daddd68 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -598,7 +598,7 @@ static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp) const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature); // Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 queue_data->temperature = 3653 + (temp * 100) / 340; - queue_data->timestamp = gyro_read_timestamp; + queue_data->timestamp = gyro_read_timestamp; BaseType_t higherPriorityTaskWoken; xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken); diff --git a/flight/pios/inc/pios_sensors.h b/flight/pios/inc/pios_sensors.h index 4397bc844..46789cfa5 100644 --- a/flight/pios/inc/pios_sensors.h +++ b/flight/pios/inc/pios_sensors.h @@ -83,7 +83,7 @@ typedef struct PIOS_SENSORS_3Axis_SensorsWithTemp { } PIOS_SENSORS_3Axis_SensorsWithTemp; typedef struct PIOS_SENSORS_1Axis_SensorsWithTemp { - float sample; // sample + float sample; // sample float temperature; // Degrees Celsius } PIOS_SENSORS_1Axis_SensorsWithTemp; From 06193c60ac53b2f3a59020d2fe77d82a49f15b64 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 12 Jul 2016 18:39:44 -0400 Subject: [PATCH 04/12] LP-340 reduce stack allocation --- flight/modules/AutoTune/autotune.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 1a3f1c794..d7a7a0978 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -67,8 +67,8 @@ // Private constants #undef STACK_SIZE_BYTES -// Pull Request version tested on Nano. 120 bytes of stack left when configured with 1340 -#define STACK_SIZE_BYTES 1340 +// Pull Request version tested on Sparky2. 292 bytes of stack left when configured with 1340 +#define STACK_SIZE_BYTES 1240 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define AF_NUMX 13 From 8b812fb8dca250c50d9210ee3593f2aba0b8d66d Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 16 Jul 2016 00:37:06 -0400 Subject: [PATCH 05/12] LP-340 fix ancient tabs and remove commented out xml --- shared/uavobjectdefinition/gyrosensor.xml | 6 +++--- shared/uavobjectdefinition/gyrostate.xml | 6 +++--- shared/uavobjectdefinition/systemidentsettings.xml | 6 ++---- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/shared/uavobjectdefinition/gyrosensor.xml b/shared/uavobjectdefinition/gyrosensor.xml index 80b8894b5..eddc4e943 100644 --- a/shared/uavobjectdefinition/gyrosensor.xml +++ b/shared/uavobjectdefinition/gyrosensor.xml @@ -1,9 +1,9 @@ Calibrated sensor data from 3 axis gyroscope in deg/s. - - - + + + diff --git a/shared/uavobjectdefinition/gyrostate.xml b/shared/uavobjectdefinition/gyrostate.xml index cf0386d8b..a90373fa7 100644 --- a/shared/uavobjectdefinition/gyrostate.xml +++ b/shared/uavobjectdefinition/gyrostate.xml @@ -1,9 +1,9 @@ The filtered rotation sensor data. - - - + + + diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 5f0e4cc22..9095668a3 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -26,16 +26,14 @@ - - - + - + From a3655783853a37374bc0614dadc179d647c3f242 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 16 Jul 2016 00:42:20 -0400 Subject: [PATCH 06/12] LP-340 dont export nan pids and new setting for d-term oscillation --- flight/modules/AutoTune/autotune.c | 92 ++++++++++++++++++++++++------ 1 file changed, 74 insertions(+), 18 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index d7a7a0978..2ce177c28 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -91,12 +91,14 @@ #define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */ // CheckSettings() returned error bits -#define ROLL_BETA_LOW 1 -#define PITCH_BETA_LOW 2 -#define YAW_BETA_LOW 4 -#define TAU_TOO_LONG 8 -#define TAU_TOO_SHORT 16 -#define CPU_TOO_SLOW 32 +#define TAU_NAN 1 +#define BETA_NAN 2 +#define ROLL_BETA_LOW 4 +#define PITCH_BETA_LOW 8 +#define YAW_BETA_LOW 16 +#define TAU_TOO_LONG 32 +#define TAU_TOO_SHORT 64 +#define CPU_TOO_SLOW 128 // smooth-quick modes #define SMOOTH_QUICK_DISABLED 0 @@ -470,8 +472,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.gyroStateCallbackTimestamp) * 1.0e-6f; /* This is for the first point, but * also if we have extended drops */ - if (dT_s > 0.010f) { - dT_s = 0.010f; + if (dT_s > 5.0f/PIOS_SENSOR_RATE) { + dT_s = 5.0f/PIOS_SENSOR_RATE; } lastTime = pt.gyroStateCallbackTimestamp; // original algorithm handles time from GyroStateGet() to detected motion @@ -641,7 +643,6 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) static void InitSystemIdent(bool loadDefaults) { SystemIdentSettingsGet(&systemIdentSettings); - if (loadDefaults) { // get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau) // so that if they are changed there (mainly for future code changes), they will be changed here too @@ -664,6 +665,7 @@ static void InitSystemIdent(bool loadDefaults) memcpy(&u.systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData)); u.systemIdentState.Complete = systemIdentSettings.Complete; } + SystemIdentStateSet(&u.systemIdentState); // (1.0f / PIOS_SENSOR_RATE) is gyro period // the -1/10 makes it converge nicely, the other values make it converge the same way if the configuration is changed @@ -677,7 +679,6 @@ static void InitSystemIdent(bool loadDefaults) gyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; - switch (SmoothQuickSource) { case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0 case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 @@ -689,7 +690,6 @@ static void InitSystemIdent(bool loadDefaults) // enable PID changing with accessory0-3 accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; break; - case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points @@ -703,7 +703,6 @@ static void InitSystemIdent(bool loadDefaults) // disable PID changing with accessory0-3 accessoryToUse = -1; break; - case SMOOTH_QUICK_DISABLED: default: // leave smoothQuickSetting alone so user can set it to a different value and have it stay that value @@ -788,6 +787,21 @@ static uint8_t CheckSettingsRaw() { uint8_t retVal = 0; + // inverting the comparisons then negating the bool result should catch the nans but it doesn't + // so explictly check for nans + if (!IS_REAL(expapprox(u.systemIdentState.Tau))) { + retVal |= TAU_NAN; + } + if (!IS_REAL(expapprox(u.systemIdentState.Beta.Roll))) { + retVal |= BETA_NAN; + } + if (!IS_REAL(expapprox(u.systemIdentState.Beta.Pitch))) { + retVal |= BETA_NAN; + } + if (!IS_REAL(expapprox(u.systemIdentState.Beta.Yaw))) { + retVal |= BETA_NAN; + } + // Check the axis gains // Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values. if (u.systemIdentState.Beta.Roll < 6) { @@ -796,6 +810,11 @@ static uint8_t CheckSettingsRaw() if (u.systemIdentState.Beta.Pitch < 6) { retVal |= PITCH_BETA_LOW; } + // yaw gain is no longer checked, because the yaw options only include: + // - not calculating yaw + // - limiting yaw gain between two sane values (default) + // - ignoring errors and accepting the calculated yaw + // Check the response speed // Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values. if (expapprox(u.systemIdentState.Tau) > 0.1f) { @@ -805,7 +824,7 @@ static uint8_t CheckSettingsRaw() else if (expapprox(u.systemIdentState.Tau) < 0.008f) { retVal |= TAU_TOO_SHORT; } - // Check gyroReadTimeAverage + // Sanity check: CPU is too slow compared to gyro rate if (gyroReadTimeAverage > (1.0f / PIOS_SENSOR_RATE)) { retVal |= CPU_TOO_SLOW; @@ -822,11 +841,9 @@ static uint8_t CheckSettingsRaw() static uint8_t CheckSettings() { uint8_t retVal = CheckSettingsRaw(); - if (systemIdentSettings.DisableSanityChecks) { retVal = 0; } - return retVal; } @@ -977,6 +994,45 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float kd /= ratio; } + // reduce kd if so configured + // both of the quads tested for d term oscillation exhibit some degree of it with the stock autotune PIDs + // if may be that adjusting stabSettingsBank.DerivativeCutoff would have a similar affect + // reducing kd requires that kp and ki be reduced to avoid ringing + // the amount to reduce kp and ki is taken from ZN tuning + // specifically kp is parameterized based on the ratio between kp(PID) and kp(PI) as the D factor varies from 1 to 0 + // https://en.wikipedia.org/wiki/PID_controller + // Kp Ki Kd + // ----------------------------------- + // P 0.50*Ku - - + // PI 0.45*Ku 1.2*Kp/Tu - + // PID 0.60*Ku 2.0*Kp/Tu Kp*Tu/8 + // + // so Kp is multiplied by (.45/.60) if Kd is reduced to 0 + // and Ki is multiplied by (1.2/2.0) if Kd is reduced to 0 + #define KP_REDUCTION (.45f/.60f) + #define KI_REDUCTION (1.2f/2.0f) + + // this link gives some additional ratios that are different + // the reduced overshoot ratios are invalid for this purpose + // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method + // Kp Ki Kd + // ------------------------------------------------ + // P 0.50*Ku - - + // PI 0.45*Ku Tu/1.2 - + // PD 0.80*Ku - Tu/8 + // classic PID 0.60*Ku Tu/2.0 Tu/8 #define KP_REDUCTION (.45f/.60f) #define KI_REDUCTION (1.2f/2.0f) + // Pessen Integral Rule 0.70*Ku Tu/2.5 3.0*Tu/20 #define KP_REDUCTION (.45f/.70f) #define KI_REDUCTION (1.2f/2.5f) + // some overshoot 0.33*Ku Tu/2.0 Tu/3 #define KP_REDUCTION (.45f/.33f) #define KI_REDUCTION (1.2f/2.0f) + // no overshoot 0.20*Ku Tu/2.0 Tu/3 #define KP_REDUCTION (.45f/.20f) #define KI_REDUCTION (1.2f/2.0f) + + // reduce roll and pitch, but not yaw + // yaw PID is entirely based on roll or pitch PIDs which have already been reduced + if (i < 2) { + kp = kp * KP_REDUCTION + kp * systemIdentSettings.DerivativeFactor * (1.0f-KP_REDUCTION); + ki = ki * KI_REDUCTION + ki * systemIdentSettings.DerivativeFactor * (1.0f-KI_REDUCTION); + kd *= systemIdentSettings.DerivativeFactor; + } + switch (i) { case 0: // Roll stabSettingsBank.RollRatePID.Kp = kp; @@ -1241,11 +1297,11 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl P[1] = -D[1] * (D[1] / S[1] - 1); P[2] = -D[2] * (D[2] / S[2] - 1); P[3] = -D[3] * (D[0] / S[0] - 1); - P[4] = D[4] - D[3] * D[3] / S[0]; + P[4] = D[4] - D[3] * (D[3] / S[0]); P[5] = -D[5] * (D[1] / S[1] - 1); - P[6] = D[6] - D[5] * D[5] / S[1]; + P[6] = D[6] - D[5] * (D[5] / S[1]); P[7] = -D[7] * (D[2] / S[2] - 1); - P[8] = D[8] - D[7] * D[7] / S[2]; + P[8] = D[8] - D[7] * (D[7] / S[2]); P[9] = -D[9] * (D[0] / S[0] - 1); P[10] = D[10] - D[3] * (D[9] / S[0]); P[11] = D[11] - D[9] * (D[9] / S[0]); From 792aae6f9b516e906787acfeebbe3eec91f8a96d Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 19 Jul 2016 17:41:58 -0400 Subject: [PATCH 07/12] LP-340 stop NAN propagation at the start of tuning --- flight/modules/AutoTune/autotune.c | 54 +++++++++++++++++------------- 1 file changed, 30 insertions(+), 24 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 2ce177c28..6d5f34536 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -420,36 +420,42 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) diffTime = xTaskGetTickCount() - lastUpdateTime; // after 2 seconds start systemident flight mode if (diffTime > SYSTEMIDENT_TIME_DELAY_MS) { + // load default tune and clean up any NANs from previous tune + InitSystemIdent(true); + AfInit(gX, gP); + // and write it out to the UAVO so innerloop can see the default values + UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); + // before starting SystemIdent stabilization mode doingIdent = true; - // after an additional .5 seconds start capturing data - if (diffTime > INIT_TIME_DELAY2_MS) { - // Reset save status - // save SI data even if partial or bad, aids in diagnostics - saveSiNeeded = true; - // don't save PIDs until data gathering is complete - // and the complete data has been sanity checked - savePidNeeded = false; - InitSystemIdent(true); - AfInit(gX, gP); - UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); - measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; - state = AT_START; - } + state = AT_START; } break; case AT_START: - lastTime = PIOS_DELAY_GetRaw(); + diffTime = xTaskGetTickCount() - lastUpdateTime; doingIdent = true; - /* Drain the queue of all current data */ - xQueueReset(atQueue); - /* And reset the point spill counter */ - updateCounter = 0; - atPointsSpilled = 0; - throttleAccumulator = 0; - alpha = 0.0f; - state = AT_RUN; - lastUpdateTime = xTaskGetTickCount(); + // after an additional short delay, start capturing data + if (diffTime > INIT_TIME_DELAY2_MS) { + // Reset save status + // save SI data even if partial or bad, aids in diagnostics + saveSiNeeded = true; + // don't save PIDs until data gathering is complete + // and the complete data has been sanity checked + savePidNeeded = false; + // get the tuning duration in case the user just changed it + measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; + // init the "previous packet timestamp" + lastTime = PIOS_DELAY_GetRaw(); + /* Drain the queue of all current data */ + xQueueReset(atQueue); + /* And reset the point spill counter */ + updateCounter = 0; + atPointsSpilled = 0; + throttleAccumulator = 0; + alpha = 0.0f; + state = AT_RUN; + lastUpdateTime = xTaskGetTickCount(); + } break; case AT_RUN: From d4f3e7e60287ab9e62416979a3f02ad095873504 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 19 Jul 2016 18:26:38 -0400 Subject: [PATCH 08/12] LP-340 change destPIDbank to 3 for ease of use by users that already use 1 and 2 --- shared/uavobjectdefinition/systemidentsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 9095668a3..017d00417 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -32,7 +32,7 @@ - + From 447dff6fc9573b2b3f3cb94d273e02de182f20f8 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 19 Jul 2016 19:04:40 -0400 Subject: [PATCH 09/12] LP-340 cosmetic type of issues requested in PR --- flight/modules/AutoTune/autotune.c | 57 ++++++++++--------- shared/uavobjectdefinition/gyrosensor.xml | 2 +- shared/uavobjectdefinition/gyrostate.xml | 2 +- .../systemidentsettings.xml | 4 +- 4 files changed, 33 insertions(+), 32 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 6d5f34536..cd2b6dd7a 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -134,7 +134,7 @@ static float gyroReadTimeAverage; static float gyroReadTimeAverageAlpha; static float gyroReadTimeAverageAlphaAlpha; static float alpha; -static float smoothQuickSetting; +static float smoothQuickValue; static volatile uint32_t atPointsSpilled; static uint32_t throttleAccumulator; static uint8_t rollMax, pitchMax; @@ -232,7 +232,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) float noise[3] = { 0 }; float dT_s = 0.0f; uint32_t lastUpdateTime = 0; // initialization is only for compiler warning - uint32_t lastTime = 0.0f; + uint32_t lastTime = 0; uint32_t measureTime = 0; uint32_t updateCounter = 0; enum AUTOTUNE_STATE state = AT_INIT; @@ -254,7 +254,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // based on what is in SystemIdent // so that the user can use the PID smooth->quick slider in flights following the autotune flight InitSystemIdent(false); - smoothQuickSetting = systemIdentSettings.SmoothQuickSetting; + smoothQuickValue = systemIdentSettings.SmoothQuickValue; while (1) { uint32_t diffTime; @@ -297,15 +297,15 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // if user toggled while armed set PID's to next in sequence // if you assume that smoothest is -1 and quickest is +1 // this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle) - smoothQuickSetting += 1.0f / (float)flightModeSwitchTogglePosition; - if (smoothQuickSetting > 1.001f) { - smoothQuickSetting = -1.0f; + smoothQuickValue += 1.0f / (float)flightModeSwitchTogglePosition; + if (smoothQuickValue > 1.001f) { + smoothQuickValue = -1.0f; } } else { // if they did the 3x FMS toggle while disarmed, set PID's back to the middle of smoothquick - smoothQuickSetting = 0.0f; + smoothQuickValue = 0.0f; } - // calculate PIDs based on new smoothQuickSetting and save to the PID bank + // calculate PIDs based on new smoothQuickValue and save to the PID bank ProportionPidsSmoothToQuick(); // save new PIDs permanently when / if disarmed savePidNeeded = true; @@ -347,9 +347,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0 // what we are doing here does not need any higher precision than that // user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed - if (fabsf(smoothQuickSetting - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) { - smoothQuickSetting = accessoryValue.AccessoryVal; - // calculate PIDs based on new smoothQuickSetting and save to the PID bank + if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) { + smoothQuickValue = accessoryValue.AccessoryVal; + // calculate PIDs based on new smoothQuickValue and save to the PID bank ProportionPidsSmoothToQuick(); // this schedules the first possible write of the PIDs to occur a fraction of a second or so from now // and changes the scheduled time if it is already scheduled @@ -432,7 +432,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) break; case AT_START: - diffTime = xTaskGetTickCount() - lastUpdateTime; + diffTime = xTaskGetTickCount() - lastUpdateTime; doingIdent = true; // after an additional short delay, start capturing data if (diffTime > INIT_TIME_DELAY2_MS) { @@ -443,9 +443,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // and the complete data has been sanity checked savePidNeeded = false; // get the tuning duration in case the user just changed it - measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; + measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; // init the "previous packet timestamp" - lastTime = PIOS_DELAY_GetRaw(); + lastTime = PIOS_DELAY_GetRaw(); /* Drain the queue of all current data */ xQueueReset(atQueue); /* And reset the point spill counter */ @@ -478,8 +478,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.gyroStateCallbackTimestamp) * 1.0e-6f; /* This is for the first point, but * also if we have extended drops */ - if (dT_s > 5.0f/PIOS_SENSOR_RATE) { - dT_s = 5.0f/PIOS_SENSOR_RATE; + if (dT_s > 5.0f / PIOS_SENSOR_RATE) { + dT_s = 5.0f / PIOS_SENSOR_RATE; } lastTime = pt.gyroStateCallbackTimestamp; // original algorithm handles time from GyroStateGet() to detected motion @@ -690,7 +690,7 @@ static void InitSystemIdent(bool loadDefaults) case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3 - // leave smoothQuickSetting alone since it is always controlled by knob + // leave smoothQuickValue alone since it is always controlled by knob // disable PID changing with flight mode switch flightModeSwitchTogglePosition = -1; // enable PID changing with accessory0-3 @@ -702,7 +702,7 @@ static void InitSystemIdent(bool loadDefaults) // don't allow init of current toggle position in the middle of 3x fms toggle if (loadDefaults) { // set toggle to middle of range - smoothQuickSetting = 0.0f; + smoothQuickValue = 0.0f; } // enable PID changing with flight mode switch flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; @@ -711,7 +711,7 @@ static void InitSystemIdent(bool loadDefaults) break; case SMOOTH_QUICK_DISABLED: default: - // leave smoothQuickSetting alone so user can set it to a different value and have it stay that value + // leave smoothQuickValue alone so user can set it to a different value and have it stay that value // disable PID changing with flight mode switch flightModeSwitchTogglePosition = -1; // disable PID changing with accessory0-3 @@ -751,7 +751,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise, systemIdentSettings.Tau = u.systemIdentState.Tau; memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; - systemIdentSettings.SmoothQuickSetting = smoothQuickSetting; + systemIdentSettings.SmoothQuickValue = smoothQuickValue; SystemIdentStateSet(&u.systemIdentState); } @@ -847,6 +847,7 @@ static uint8_t CheckSettingsRaw() static uint8_t CheckSettings() { uint8_t retVal = CheckSettingsRaw(); + if (systemIdentSettings.DisableSanityChecks) { retVal = 0; } @@ -1007,7 +1008,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // the amount to reduce kp and ki is taken from ZN tuning // specifically kp is parameterized based on the ratio between kp(PID) and kp(PI) as the D factor varies from 1 to 0 // https://en.wikipedia.org/wiki/PID_controller - // Kp Ki Kd + // Kp Ki Kd // ----------------------------------- // P 0.50*Ku - - // PI 0.45*Ku 1.2*Kp/Tu - @@ -1015,13 +1016,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // // so Kp is multiplied by (.45/.60) if Kd is reduced to 0 // and Ki is multiplied by (1.2/2.0) if Kd is reduced to 0 - #define KP_REDUCTION (.45f/.60f) - #define KI_REDUCTION (1.2f/2.0f) + #define KP_REDUCTION (.45f / .60f) + #define KI_REDUCTION (1.2f / 2.0f) // this link gives some additional ratios that are different // the reduced overshoot ratios are invalid for this purpose // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method - // Kp Ki Kd + // Kp Ki Kd // ------------------------------------------------ // P 0.50*Ku - - // PI 0.45*Ku Tu/1.2 - @@ -1034,8 +1035,8 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float // reduce roll and pitch, but not yaw // yaw PID is entirely based on roll or pitch PIDs which have already been reduced if (i < 2) { - kp = kp * KP_REDUCTION + kp * systemIdentSettings.DerivativeFactor * (1.0f-KP_REDUCTION); - ki = ki * KI_REDUCTION + ki * systemIdentSettings.DerivativeFactor * (1.0f-KI_REDUCTION); + kp = kp * KP_REDUCTION + kp * systemIdentSettings.DerivativeFactor * (1.0f - KP_REDUCTION); + ki = ki * KI_REDUCTION + ki * systemIdentSettings.DerivativeFactor * (1.0f - KI_REDUCTION); kd *= systemIdentSettings.DerivativeFactor; } @@ -1105,7 +1106,7 @@ static void ProportionPidsSmoothToQuick() { float ratio, damp, noise; float min = -1.0f; - float val = smoothQuickSetting; + float val = smoothQuickValue; float max = 1.0f; // translate from range [min, max] to range [0, max-min] @@ -1128,7 +1129,7 @@ static void ProportionPidsSmoothToQuick() ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise); // save it to the system, but not yet written to flash - SystemIdentSettingsSmoothQuickSettingSet(&smoothQuickSetting); + SystemIdentSettingsSmoothQuickValueSet(&smoothQuickValue); } diff --git a/shared/uavobjectdefinition/gyrosensor.xml b/shared/uavobjectdefinition/gyrosensor.xml index eddc4e943..d51efd1a1 100644 --- a/shared/uavobjectdefinition/gyrosensor.xml +++ b/shared/uavobjectdefinition/gyrosensor.xml @@ -5,7 +5,7 @@ - + diff --git a/shared/uavobjectdefinition/gyrostate.xml b/shared/uavobjectdefinition/gyrostate.xml index a90373fa7..35e7c1da8 100644 --- a/shared/uavobjectdefinition/gyrostate.xml +++ b/shared/uavobjectdefinition/gyrostate.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 017d00417..02c9b1924 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -33,7 +33,7 @@ - + @@ -47,7 +47,7 @@ - + From a13de1e62b22f9e87d0dc862e98f6aab7d94842c Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 20 Jul 2016 02:56:10 -0400 Subject: [PATCH 10/12] LP-340 Add 100 bytes of stack because Nano needs 156 more than Sparky2 --- flight/modules/AutoTune/autotune.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index cd2b6dd7a..df93d4a81 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -68,7 +68,8 @@ // Private constants #undef STACK_SIZE_BYTES // Pull Request version tested on Sparky2. 292 bytes of stack left when configured with 1340 -#define STACK_SIZE_BYTES 1240 +// Beware that Nano needs 156 bytes more stack than Sparky2 +#define STACK_SIZE_BYTES 1340 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define AF_NUMX 13 From d12847d7727075a5fdd165a794eb0ce7e8ac7683 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 20 Jul 2016 02:57:27 -0400 Subject: [PATCH 11/12] LP-340 Add descriptions in xml --- .../systemidentsettings.xml | 40 +++++++++---------- .../uavobjectdefinition/systemidentstate.xml | 22 +++++----- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 02c9b1924..5f457b63d 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -1,9 +1,9 @@ - The input to the PID tuning. - + The input to and results of the PID tuning. + - + @@ -20,20 +20,20 @@ - - - - - - - + + + + + + + - - - - - + + + + + @@ -46,11 +46,11 @@ - - - - - + + + + + diff --git a/shared/uavobjectdefinition/systemidentstate.xml b/shared/uavobjectdefinition/systemidentstate.xml index f09eb41c9..a9dca4b7b 100644 --- a/shared/uavobjectdefinition/systemidentstate.xml +++ b/shared/uavobjectdefinition/systemidentstate.xml @@ -1,17 +1,17 @@ - The input to the PID tuning. - + Used for logging PID tuning. + - - - - - - - - - + + + + + + + + + From b29780a7b9e09e5e8501c80e48eaba3389879db6 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 20 Jul 2016 14:05:47 -0400 Subject: [PATCH 12/12] LP-340 fix typos and clarify xml comments --- .../systemidentsettings.xml | 25 ++++++++++++++++++- .../uavobjectdefinition/systemidentstate.xml | 2 +- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml index 5f457b63d..b589e8b5b 100644 --- a/shared/uavobjectdefinition/systemidentsettings.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -2,8 +2,15 @@ The input to and results of the PID tuning. - + + + + + + + + @@ -20,6 +27,22 @@ + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/systemidentstate.xml b/shared/uavobjectdefinition/systemidentstate.xml index a9dca4b7b..b0269c201 100644 --- a/shared/uavobjectdefinition/systemidentstate.xml +++ b/shared/uavobjectdefinition/systemidentstate.xml @@ -2,7 +2,7 @@ Used for logging PID tuning. - +