From f2f8d8e712b627904446a47e1ef6beb450954aed Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 12 Jul 2016 13:27:56 -0400 Subject: [PATCH] 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 @@ + -