/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup StabilizationModule Stabilization Module * @brief Stabilization PID loops in an airframe type independent manner * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" * @{ * * @file AutoTune/autotune.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. * dRonin, http://dRonin.org/, Copyright (C) 2015-2016 * Tau Labs, http://taulabs.org, Copyright (C) 2013-2014 * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Automatic PID tuning module. * * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #if defined(PIOS_EXCLUDE_ADVANCED_FEATURES) #define powapprox fastpow #define expapprox fastexp #else #define powapprox powf #define expapprox expf #endif /* defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ // Private constants #undef STACK_SIZE_BYTES // Pull Request version tested on Sparky2. 292 bytes of stack left when configured with 1340 // 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 #define AF_NUMP 43 #if !defined(AT_QUEUE_NUMELEM) #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 */ #define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */ #define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */ #define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */ #define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */ #define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */ #define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */ // CheckSettings() returned error bits #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 #define SMOOTH_QUICK_ACCESSORY_BASE 10 #define SMOOTH_QUICK_TOGGLE_BASE 20 // 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 gyroStateCallbackTimestamp; /* PIOS_DELAY_GetRaw() time of GyroState callback */ uint32_t sensorReadTimestamp; /* PIOS_DELAY_GetRaw() time of sensor read */ }; // 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 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 smoothQuickValue; static volatile uint32_t atPointsSpilled; static uint32_t throttleAccumulator; static uint8_t rollMax, pitchMax; static int8_t accessoryToUse; static int8_t flightModeSwitchTogglePosition; static bool moduleEnabled; // Private functions 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 void flightModeSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; FlightModeSettingsFlightModePositionGet(fms); for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) { if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) { ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); break; } } } /** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AutoTuneInitialize(void) { // Create a queue, connect to manual control command and flightstatus #ifdef MODULE_AutoTune_BUILTIN moduleEnabled = true; #else HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesGet(&optionalModules); if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) { // even though the AutoTune module is automatically enabled // (below, when the flight mode switch is configured to use autotune) // there are use cases where the user may even want it enabled without being on the FMS // that allows PIDs to be adjusted in flight moduleEnabled = true; } else { // if the user did not enable the autotune module // do it for them if they have autotune on their flight mode switch FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; moduleEnabled = false; FlightModeSettingsInitialize(); FlightModeSettingsFlightModePositionGet(fms); for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) { if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) { moduleEnabled = true; break; } } } #endif /* ifdef MODULE_AutoTune_BUILTIN */ if (moduleEnabled) { SystemIdentSettingsInitialize(); SystemIdentStateInitialize(); atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data)); if (!atQueue) { moduleEnabled = false; } } if (!moduleEnabled) { FlightModeSettingsConnectCallback(flightModeSettingsUpdatedCb); } return 0; } /** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AutoTuneStart(void) { // Start main task if it is enabled if (moduleEnabled) { GyroStateConnectCallback(AtNewGyroData); xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); } return 0; } MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart); /** * Module thread, should not return. */ 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; 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) StabilizationBankRollMaxGet(&rollMax); StabilizationBankPitchMaxGet(&pitchMax); 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 flights following the autotune flight InitSystemIdent(false); smoothQuickValue = systemIdentSettings.SmoothQuickValue; while (1) { uint32_t diffTime; bool doingIdent = false; bool canSleep = true; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (saveSiNeeded) { saveSiNeeded = false; // Save SystemIdentSettings to permanent settings UAVObjSave(SystemIdentSettingsHandle(), 0); } if (savePidNeeded) { savePidNeeded = false; // Save PIDs to permanent settings switch (systemIdentSettings.DestinationPidBank) { case 1: UAVObjSave(StabilizationSettingsBank1Handle(), 0); break; case 2: UAVObjSave(StabilizationSettingsBank2Handle(), 0); break; case 3: UAVObjSave(StabilizationSettingsBank3Handle(), 0); break; } } } // if using flight mode switch "quick toggle 3x" to "try smooth -> quick PIDs" is enabled // and user toggled into and back out of AutoTune three times in the last two seconds // and the autotune data gathering is complete // and the autotune data gathered is good // note: CheckFlightModeSwitchForPidRequest(mode) only returns true if current mode is not autotune if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentSettings.Complete && !CheckSettings()) { if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // 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) 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 smoothQuickValue = 0.0f; } // calculate PIDs based on new smoothQuickValue 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; } ////////////////////////////////////////////////////////////////////////////////////// // if configured to use a slider for smooth-quick and the autotune module is running // (note that the module can be automatically or manually enabled) // then the smooth-quick slider is always active (when not actually in autotune mode) // // when the slider is active it will immediately change the PIDs // and it will schedule the PIDs to be written to permanent storage // // if the FC is disarmed, the perm write will happen on next loop // but if the FC is armed, the perm write will only occur when the FC goes disarmed ////////////////////////////////////////////////////////////////////////////////////// // we don't want it saving to permanent storage many times // while the user is moving the knob once, so wait till the knob stops moving static uint8_t savePidDelay; // any time we are not in AutoTune mode: // - the user may be using the accessory0-3 knob/slider to request PID changes // - the state machine needs to be reset // - the local version of Attitude mode gets skipped if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { // 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/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()) { AccessoryDesiredData accessoryValue; AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); // 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 // user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed 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 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; } state = AT_INIT; vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS); continue; } else { savePidDelay = 0; } switch (state) { case AT_INIT: // beware that control comes here every time the user toggles the flight mode switch into AutoTune // and it isn't appropriate to reset the main state here // init must wait until after a delay has passed: // - to make sure they intended to stay in this mode // - to wait for the stab bank to get populated with the new bank info // This is a race. It is possible that flightStatus.FlightMode has been changed, // but the stab bank hasn't been changed yet. state = AT_INIT_DELAY; lastUpdateTime = xTaskGetTickCount(); break; case AT_INIT_DELAY: diffTime = xTaskGetTickCount() - lastUpdateTime; // after a small delay, get the stab bank values and SystemIdentSettings in case they changed // this is a very small delay (100ms), so "quick 3x fms toggle" gets in here if (diffTime > INIT_TIME_DELAY_MS) { // do these here so the user has at most a 1/10th second // with controls that use the previous bank's rates StabilizationBankRollMaxGet(&rollMax); StabilizationBankPitchMaxGet(&pitchMax); StabilizationBankManualRateGet(&manualRate); // load SystemIdentSettings so that they can change it // and do smooth-quick on changed values InitSystemIdent(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 // the FC must be armed and if we check here it also allows switchless setup to use autotune if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { state = AT_INIT_DELAY2; lastUpdateTime = xTaskGetTickCount(); } } break; case AT_INIT_DELAY2: // delay for 2 seconds before actually starting the SystemIdent flight mode and AutoTune. // that allows the user to get his fingers on the sticks // and avoids starting the AutoTune if the user is toggling the flight mode switch // to select other PIDs on the "simulated Smooth Quick slider". // or simply "passing through" this flight mode to get to another flight mode 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; state = AT_START; } break; case AT_START: diffTime = xTaskGetTickCount() - lastUpdateTime; doingIdent = true; // 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: diffTime = xTaskGetTickCount() - lastUpdateTime; doingIdent = true; canSleep = false; // 4 gyro samples per cycle // 2ms cycle time // that is 500 gyro samples per second if it sleeps each time // actually less than 500 because it cycle time is processing time + 2ms for (int i = 0; i < MAX_PTS_PER_CYCLE; i++) { struct at_queued_data pt; /* Grab an autotune point */ if (xQueueReceive(atQueue, &pt, 0) != pdTRUE) { /* We've drained the buffer fully */ canSleep = true; break; } /* calculate time between successive points */ 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; } 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 noise[j] = NOISE_ALPHA * noise[j] + (1 - NOISE_ALPHA) * (pt.y[j] - gX[j]) * (pt.y[j] - gX[j]); } // This will work up to 8kHz with an 89% throttle position before overflow throttleAccumulator += 10000 * pt.throttle; // Update uavo every 256 cycles to avoid // telemetry spam if (((++updateCounter) & 0xff) == 0) { float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle); } } if (diffTime > measureTime) { // Move on to next state // permanent flag that AT is complete and PIDs can be calculated state = AT_FINISHED; } 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()) { ProportionPidsSmoothToQuick(); savePidNeeded = true; // mark these results as good in the log settings so they can be viewed in playback 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 // that way user can still see that they failed uint8_t failureBits = CheckSettingsRaw(); if (failureBits) { // raise a warning that includes failureBits to indicate what failed ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING, SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); } } // 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; case AT_WAITING: default: // after tuning, wait here till user switches to another flight mode // or disarms break; } // fly in Attitude mode or in SystemIdent mode UpdateStabilizationDesired(doingIdent); if (canSleep) { vTaskDelay(YIELD_MS / portTICK_RATE_MS); } } } // gyro sensor callback // get gyro data and actuatordesired into a packet // and put it in the queue for later processing static void AtNewGyroData(UAVObjEvent *ev) { static struct at_queued_data q_item; 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; } // 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); if (last_sample_unpushed) { /* Last time we were unable to queue up the gyro data. * Try again, last chance! */ if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) { atPointsSpilled++; } } 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; } else { last_sample_unpushed = false; } } // check for the user quickly toggling the flight mode switch // into and out of AutoTune, 3 times // that is a signal that the user wants to try the next PID settings // on the scale from smooth to quick // when it exceeds the quickest setting, it starts back at the smoothest setting static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) { static uint32_t lastUpdateTime; static uint8_t flightModePrev; static uint8_t counter; uint32_t updateTime; // only count transitions into and out of autotune if ((flightModePrev == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ^ (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE)) { flightModePrev = flightMode; updateTime = xTaskGetTickCount(); // if it has been over 2 seconds, reset the counter if (updateTime - lastUpdateTime > 2000) { counter = 0; } // if the counter is reset, start a new time period if (counter == 0) { lastUpdateTime = updateTime; } // if flight mode has toggled into autotune 3 times but is currently not autotune if (++counter >= 5 && flightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { counter = 0; return true; } } return false; } // read SystemIdent uavos, update the local structures // 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 to default values of uavo xml file, in preparation for tuning 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 // save metadata from being changed by the following SetDefaults() SystemIdentStateGetMetadata(&u.systemIdentStateMetaData); SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0); SystemIdentStateSetMetadata(&u.systemIdentStateMetaData); SystemIdentStateGet(&u.systemIdentState); // Tau, GyroReadTimeAverage, Beta, and the Complete flag get default values // in preparation for running AutoTune 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, 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 u.systemIdentState.Tau = systemIdentSettings.Tau; u.systemIdentState.GyroReadTimeAverage = systemIdentSettings.GyroReadTimeAverage; 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 // 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; uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; switch (SmoothQuickSource) { case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0 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 smoothQuickValue alone since it is always controlled by knob // disable PID changing with flight mode switch flightModeSwitchTogglePosition = -1; // 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 // don't allow init of current toggle position in the middle of 3x fms toggle if (loadDefaults) { // set toggle to middle of range smoothQuickValue = 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; break; case SMOOTH_QUICK_DISABLED: default: // 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 accessoryToUse = -1; break; } } // update the gain and delay with current calculated value // these are stored in the settings for use with next battery // and also in the state for logging purposes static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle) { 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 = u.systemIdentState.Tau; memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; systemIdentSettings.SmoothQuickValue = smoothQuickValue; SystemIdentStateSet(&u.systemIdentState); } // when running AutoTune mode, this bypasses manualcontrol.c / stabilizedhandler.c // to control whether the multicopter should be in Attitude mode vs. SystemIdent mode static void UpdateStabilizationDesired(bool doingIdent) { StabilizationDesiredData stabDesired; ManualControlCommandData manualControlCommand; ManualControlCommandGet(&manualControlCommand); stabDesired.Roll = manualControlCommand.Roll * rollMax; stabDesired.Pitch = manualControlCommand.Pitch * pitchMax; stabDesired.Yaw = manualControlCommand.Yaw * manualRate.Yaw; stabDesired.Thrust = manualControlCommand.Thrust; if (doingIdent) { stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; } else { stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; StabilizationDesiredSet(&stabDesired); } // check the completed autotune state (mainly gain and delay) // to see if it is reasonable // return a bit mask of errors detected 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) { retVal |= ROLL_BETA_LOW; } 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) { retVal |= TAU_TOO_LONG; } // Extreme values: Your estimated response speed (tau) is faster than normal. This will result in large PID values. else if (expapprox(u.systemIdentState.Tau) < 0.008f) { retVal |= TAU_TOO_SHORT; } // Sanity check: CPU is too slow compared to gyro rate if (gyroReadTimeAverage > (1.0f / PIOS_SENSOR_RATE)) { retVal |= CPU_TOO_SLOW; } return retVal; } // check the completed autotune state (mainly gain and delay) // to see if it is reasonable // override bad yaw values if configured that way // return a bit mask of errors detected static uint8_t CheckSettings() { uint8_t retVal = CheckSettingsRaw(); if (systemIdentSettings.DisableSanityChecks) { retVal = 0; } return retVal; } // 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 volatile stabSettingsBank; switch (systemIdentSettings.DestinationPidBank) { case 1: StabilizationSettingsBank1Get((void *)&stabSettingsBank); break; case 2: StabilizationSettingsBank2Get((void *)&stabSettingsBank); break; case 3: StabilizationSettingsBank3Get((void *)&stabSettingsBank); break; } // These three parameters define the desired response properties // - rate scale in the fraction of the natural speed of the system // to strive for. // - damp is the amount of damping in the system. higher values // make oscillations less likely // - ghf is the amount of high frequency gain and limits the influence // of noise const float ghf = noiseRate / 1000.0f; const float damp = dampRate / 100.0f; 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; float wn = 1.0f / tau; float tau_d = 0.0f; for (int i = 0; i < 30; i++) { 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.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 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 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; 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++) { 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.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 // 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 // yaw uses a mixture of yaw and the slowest axis (pitch) for it's beta and thus PID calculation // calculate the ratio to use when converting from the slowest axis (pitch) to the yaw axis // as (e^(betaMinLn-betaYawLn))^0.6 // which is (e^betaMinLn / e^betaYawLn)^0.6 // which is (betaMin / betaYaw)^0.6 // which is betaMin^0.6 / betaYaw^0.6 // now given that kp for each axis can be written as kpaxis = xp / betaaxis // for xp that is constant across all axes // then kpmin (probably kppitch) was xp / betamin (probably betapitch) // which we multiply by betaMin^0.6 / betaYaw^0.6 to get the new Yaw kp // so the new kpyaw is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6) // which is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6) // which is (xp * betaMin^0.6) / (betaMin * betaYaw^0.6) // 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 = 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 < kp) { kpMax = kp; } } else { // use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value // use largest roll/pitch kp because it is the axis most slowed by rotational inertia // and yaw is also slowed maximally by rotational inertia // note that kp, ki, kd are all proportional in beta // so reducing them all proportionally is the same as changing beta float min = 0.0f; float max = 0.0f; switch (systemIdentSettings.CalculateYaw) { case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUELIMITTORATIO: max = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMax; min = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMin; break; case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUEIGNORELIMIT: default: max = 1000.0f; min = 0.0f; break; } float ratio = 1.0f; if (min > 0.0f && kp < min) { ratio = kp / min; } else if (max > 0.0f && kp > max) { ratio = kp / max; } kp /= ratio; ki /= ratio; 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; stabSettingsBank.RollRatePID.Ki = ki; stabSettingsBank.RollRatePID.Kd = kd; stabSettingsBank.RollPI.Kp = kp_o; stabSettingsBank.RollPI.Ki = ki_o; break; case 1: // Pitch stabSettingsBank.PitchRatePID.Kp = kp; stabSettingsBank.PitchRatePID.Ki = ki; stabSettingsBank.PitchRatePID.Kd = kd; stabSettingsBank.PitchPI.Kp = kp_o; stabSettingsBank.PitchPI.Ki = ki_o; break; case 2: // Yaw stabSettingsBank.YawRatePID.Kp = kp; stabSettingsBank.YawRatePID.Ki = ki; stabSettingsBank.YawRatePID.Kd = kd; #if 0 // if we ever choose to use these // (e.g. mag yaw attitude) // here they are stabSettingsBank.YawPI.Kp = kp_o; stabSettingsBank.YawPI.Ki = ki_o; #endif break; } } // Librepilot might do something more with this some time // 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) { case 1: StabilizationSettingsBank1Set((void *)&stabSettingsBank); break; case 2: StabilizationSettingsBank2Set((void *)&stabSettingsBank); break; case 3: StabilizationSettingsBank3Set((void *)&stabSettingsBank); break; } } // 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 // when val is min, it generates the smoothest configured PIDs // when val is max, it generates the quickest configured PIDs // // when val is between min and (min+max)/2, it scales val over the range [min, (min+max)/2] to generate PIDs between smoothest and default // when val is between (min+max)/2 and max, it scales val over the range [(min+max)/2, max] to generate PIDs between default and quickest // // 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 ratio, damp, noise; float min = -1.0f; float val = smoothQuickValue; float max = 1.0f; // translate from range [min, max] to range [0, max-min] // that takes care of min < 0 case too val -= min; max -= min; ratio = val / max; if (ratio <= 0.5f) { // scale ratio in [0,0.5] to produce PIDs in [smoothest,default] ratio *= 2.0f; damp = (systemIdentSettings.DampMax * (1.0f - ratio)) + (systemIdentSettings.DampRate * ratio); noise = (systemIdentSettings.NoiseMin * (1.0f - ratio)) + (systemIdentSettings.NoiseRate * ratio); } else { // scale ratio in [0.5,1.0] to produce PIDs in [default,quickest] ratio = (ratio - 0.5f) * 2.0f; damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio); noise = (systemIdentSettings.NoiseRate * (1.0f - ratio)) + (systemIdentSettings.NoiseMax * ratio); } ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise); // save it to the system, but not yet written to flash SystemIdentSettingsSmoothQuickValueSet(&smoothQuickValue); } /** * Prediction step for EKF on control inputs to quad that * learns the system properties * @param X the current state estimate which is updated in place * @param P the current covariance matrix, updated in place * @param[in] the current control inputs (roll, pitch, yaw) * @param[in] the gyro measurements */ __attribute__((always_inline)) static inline 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) { const float Ts = dT_s; const float Tsq = Ts * Ts; const float Tsq3 = Tsq * Ts; const float Tsq4 = Tsq * Tsq; // for convenience and clarity code below uses the named versions of // the state variables float w1 = X[0]; // roll rate estimate float w2 = X[1]; // pitch rate estimate float w3 = X[2]; // yaw rate estimate float u1 = X[3]; // scaled roll torque float u2 = X[4]; // scaled pitch torque float u3 = X[5]; // scaled yaw torque const float e_b1 = expapprox(X[6]); // roll torque scale const float b1 = X[6]; const float e_b2 = expapprox(X[7]); // pitch torque scale const float b2 = X[7]; const float e_b3 = expapprox(X[8]); // yaw torque scale const float b3 = X[8]; const float e_tau = expapprox(X[9]); // time response of the motors const float tau = X[9]; const float bias1 = X[10]; // bias in the roll torque const float bias2 = X[11]; // bias in the pitch torque const float bias3 = X[12]; // bias in the yaw torque // inputs to the system (roll, pitch, yaw) const float u1_in = 4 * t_in * u_in[0]; const float u2_in = 4 * t_in * u_in[1]; const float u3_in = 4 * t_in * u_in[2]; // measurements from gyro const float gyro_x = gyro[0]; const float gyro_y = gyro[1]; const float gyro_z = gyro[2]; // update named variables because we want to use predicted // values below w1 = X[0] = w1 - Ts * bias1 * e_b1 + Ts * u1 * e_b1; w2 = X[1] = w2 - Ts * bias2 * e_b2 + Ts * u2 * e_b2; w3 = X[2] = w3 - Ts * bias3 * e_b3 + Ts * u3 * e_b3; u1 = X[3] = (Ts * u1_in) / (Ts + e_tau) + (u1 * e_tau) / (Ts + e_tau); u2 = X[4] = (Ts * u2_in) / (Ts + e_tau) + (u2 * e_tau) / (Ts + e_tau); u3 = X[5] = (Ts * u3_in) / (Ts + e_tau) + (u3 * e_tau) / (Ts + e_tau); // X[6] to X[12] unchanged /**** filter parameters ****/ const float q_w = 1e-3f; const float q_ud = 1e-3f; const float q_B = 1e-6f; const float q_tau = 1e-6f; const float q_bias = 1e-19f; const float s_a = 150.0f; // expected gyro measurment noise const float Q[AF_NUMX] = { q_w, q_w, q_w, q_ud, q_ud, q_ud, q_B, q_B, q_B, q_tau, q_bias, q_bias, q_bias }; float D[AF_NUMP]; for (uint32_t i = 0; i < AF_NUMP; i++) { D[i] = P[i]; } const float e_tau2 = e_tau * e_tau; const float e_tau3 = e_tau * e_tau2; const float e_tau4 = e_tau2 * e_tau2; const float Ts_e_tau2 = (Ts + e_tau) * (Ts + e_tau); const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2; // covariance propagation - D is stored copy of covariance P[0] = D[0] + Q[0] + 2 * Ts * e_b1 * (D[3] - D[28] - D[9] * bias1 + D[9] * u1) + Tsq * (e_b1 * e_b1) * (D[4] - 2 * D[29] + D[32] - 2 * D[10] * bias1 + 2 * D[30] * bias1 + 2 * D[10] * u1 - 2 * D[30] * u1 + D[11] * (bias1 * bias1) + D[11] * (u1 * u1) - 2 * D[11] * bias1 * u1); P[1] = D[1] + Q[1] + 2 * Ts * e_b2 * (D[5] - D[33] - D[12] * bias2 + D[12] * u2) + Tsq * (e_b2 * e_b2) * (D[6] - 2 * D[34] + D[37] - 2 * D[13] * bias2 + 2 * D[35] * bias2 + 2 * D[13] * u2 - 2 * D[35] * u2 + D[14] * (bias2 * bias2) + D[14] * (u2 * u2) - 2 * D[14] * bias2 * u2); P[2] = D[2] + Q[2] + 2 * Ts * e_b3 * (D[7] - D[38] - D[15] * bias3 + D[15] * u3) + Tsq * (e_b3 * e_b3) * (D[8] - 2 * D[39] + D[42] - 2 * D[16] * bias3 + 2 * D[40] * bias3 + 2 * D[16] * u3 - 2 * D[40] * u3 + D[17] * (bias3 * bias3) + D[17] * (u3 * u3) - 2 * D[17] * bias3 * u3); P[3] = (D[3] * (e_tau2 + Ts * e_tau) + Ts * e_b1 * e_tau2 * (D[4] - D[29]) + Tsq * e_b1 * e_tau * (D[4] - D[29]) + D[18] * Ts * e_tau * (u1 - u1_in) + D[10] * e_b1 * (u1 * (Ts * e_tau2 + Tsq * e_tau) - bias1 * (Ts * e_tau2 + Tsq * e_tau)) + D[21] * Tsq * e_b1 * e_tau * (u1 - u1_in) + D[31] * Tsq * e_b1 * e_tau * (u1_in - u1) + D[24] * Tsq * e_b1 * e_tau * (u1 * (u1 - bias1) + u1_in * (bias1 - u1))) / Ts_e_tau2; P[4] = (Q[3] * Tsq4 + e_tau4 * (D[4] + Q[3]) + 2 * Ts * e_tau3 * (D[4] + 2 * Q[3]) + 4 * Q[3] * Tsq3 * e_tau + Tsq * e_tau2 * (D[4] + 6 * Q[3] + u1 * (D[27] * u1 + 2 * D[21]) + u1_in * (D[27] * u1_in - 2 * D[21])) + 2 * D[21] * Ts * e_tau3 * (u1 - u1_in) - 2 * D[27] * Tsq * u1 * u1_in * e_tau2) / Ts_e_tau4; P[5] = (D[5] * (e_tau2 + Ts * e_tau) + Ts * e_b2 * e_tau2 * (D[6] - D[34]) + Tsq * e_b2 * e_tau * (D[6] - D[34]) + D[19] * Ts * e_tau * (u2 - u2_in) + D[13] * e_b2 * (u2 * (Ts * e_tau2 + Tsq * e_tau) - bias2 * (Ts * e_tau2 + Tsq * e_tau)) + D[22] * Tsq * e_b2 * e_tau * (u2 - u2_in) + D[36] * Tsq * e_b2 * e_tau * (u2_in - u2) + D[25] * Tsq * e_b2 * e_tau * (u2 * (u2 - bias2) + u2_in * (bias2 - u2))) / Ts_e_tau2; P[6] = (Q[4] * Tsq4 + e_tau4 * (D[6] + Q[4]) + 2 * Ts * e_tau3 * (D[6] + 2 * Q[4]) + 4 * Q[4] * Tsq3 * e_tau + Tsq * e_tau2 * (D[6] + 6 * Q[4] + u2 * (D[27] * u2 + 2 * D[22]) + u2_in * (D[27] * u2_in - 2 * D[22])) + 2 * D[22] * Ts * e_tau3 * (u2 - u2_in) - 2 * D[27] * Tsq * u2 * u2_in * e_tau2) / Ts_e_tau4; P[7] = (D[7] * (e_tau2 + Ts * e_tau) + Ts * e_b3 * e_tau2 * (D[8] - D[39]) + Tsq * e_b3 * e_tau * (D[8] - D[39]) + D[20] * Ts * e_tau * (u3 - u3_in) + D[16] * e_b3 * (u3 * (Ts * e_tau2 + Tsq * e_tau) - bias3 * (Ts * e_tau2 + Tsq * e_tau)) + D[23] * Tsq * e_b3 * e_tau * (u3 - u3_in) + D[41] * Tsq * e_b3 * e_tau * (u3_in - u3) + D[26] * Tsq * e_b3 * e_tau * (u3 * (u3 - bias3) + u3_in * (bias3 - u3))) / Ts_e_tau2; P[8] = (Q[5] * Tsq4 + e_tau4 * (D[8] + Q[5]) + 2 * Ts * e_tau3 * (D[8] + 2 * Q[5]) + 4 * Q[5] * Tsq3 * e_tau + Tsq * e_tau2 * (D[8] + 6 * Q[5] + u3 * (D[27] * u3 + 2 * D[23]) + u3_in * (D[27] * u3_in - 2 * D[23])) + 2 * D[23] * Ts * e_tau3 * (u3 - u3_in) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2) / Ts_e_tau4; P[9] = D[9] - Ts * e_b1 * (D[30] - D[10] + D[11] * (bias1 - u1)); P[10] = (D[10] * (Ts + e_tau) + D[24] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2); P[11] = D[11] + Q[6]; P[12] = D[12] - Ts * e_b2 * (D[35] - D[13] + D[14] * (bias2 - u2)); P[13] = (D[13] * (Ts + e_tau) + D[25] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2); P[14] = D[14] + Q[7]; P[15] = D[15] - Ts * e_b3 * (D[40] - D[16] + D[17] * (bias3 - u3)); P[16] = (D[16] * (Ts + e_tau) + D[26] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2); P[17] = D[17] + Q[8]; P[18] = D[18] - Ts * e_b1 * (D[31] - D[21] + D[24] * (bias1 - u1)); P[19] = D[19] - Ts * e_b2 * (D[36] - D[22] + D[25] * (bias2 - u2)); P[20] = D[20] - Ts * e_b3 * (D[41] - D[23] + D[26] * (bias3 - u3)); P[21] = (D[21] * (Ts + e_tau) + D[27] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2); P[22] = (D[22] * (Ts + e_tau) + D[27] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2); P[23] = (D[23] * (Ts + e_tau) + D[27] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2); P[24] = D[24]; P[25] = D[25]; P[26] = D[26]; P[27] = D[27] + Q[9]; P[28] = D[28] - Ts * e_b1 * (D[32] - D[29] + D[30] * (bias1 - u1)); P[29] = (D[29] * (Ts + e_tau) + D[31] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2); P[30] = D[30]; P[31] = D[31]; P[32] = D[32] + Q[10]; P[33] = D[33] - Ts * e_b2 * (D[37] - D[34] + D[35] * (bias2 - u2)); P[34] = (D[34] * (Ts + e_tau) + D[36] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2); P[35] = D[35]; P[36] = D[36]; P[37] = D[37] + Q[11]; P[38] = D[38] - Ts * e_b3 * (D[42] - D[39] + D[40] * (bias3 - u3)); P[39] = (D[39] * (Ts + e_tau) + D[41] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2); P[40] = D[40]; P[41] = D[41]; P[42] = D[42] + Q[12]; /********* this is the update part of the equation ***********/ float S[3] = { P[0] + s_a, P[1] + s_a, P[2] + s_a }; X[0] = w1 + P[0] * ((gyro_x - w1) / S[0]); X[1] = w2 + P[1] * ((gyro_y - w2) / S[1]); X[2] = w3 + P[2] * ((gyro_z - w3) / S[2]); X[3] = u1 + P[3] * ((gyro_x - w1) / S[0]); X[4] = u2 + P[5] * ((gyro_y - w2) / S[1]); X[5] = u3 + P[7] * ((gyro_z - w3) / S[2]); X[6] = b1 + P[9] * ((gyro_x - w1) / S[0]); X[7] = b2 + P[12] * ((gyro_y - w2) / S[1]); X[8] = b3 + P[15] * ((gyro_z - w3) / S[2]); X[9] = tau + P[18] * ((gyro_x - w1) / S[0]) + P[19] * ((gyro_y - w2) / S[1]) + P[20] * ((gyro_z - w3) / S[2]); X[10] = bias1 + P[28] * ((gyro_x - w1) / S[0]); X[11] = bias2 + P[33] * ((gyro_y - w2) / S[1]); X[12] = bias3 + P[38] * ((gyro_z - w3) / S[2]); // update the duplicate cache for (uint32_t i = 0; i < AF_NUMP; i++) { D[i] = P[i]; } // This is an approximation that removes some cross axis uncertainty but // substantially reduces the number of calculations P[0] = -D[0] * (D[0] / S[0] - 1); 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[5] = -D[5] * (D[1] / S[1] - 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[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]); P[12] = -D[12] * (D[1] / S[1] - 1); P[13] = D[13] - D[5] * (D[12] / S[1]); P[14] = D[14] - D[12] * (D[12] / S[1]); P[15] = -D[15] * (D[2] / S[2] - 1); P[16] = D[16] - D[7] * (D[15] / S[2]); P[17] = D[17] - D[15] * (D[15] / S[2]); P[18] = -D[18] * (D[0] / S[0] - 1); P[19] = -D[19] * (D[1] / S[1] - 1); P[20] = -D[20] * (D[2] / S[2] - 1); P[21] = D[21] - D[3] * (D[18] / S[0]); P[22] = D[22] - D[5] * (D[19] / S[1]); P[23] = D[23] - D[7] * (D[20] / S[2]); P[24] = D[24] - D[9] * (D[18] / S[0]); P[25] = D[25] - D[12] * (D[19] / S[1]); P[26] = D[26] - D[15] * (D[20] / S[2]); P[27] = D[27] - D[18] * (D[18] / S[0]) - D[19] * (D[19] / S[1]) - D[20] * (D[20] / S[2]); P[28] = -D[28] * (D[0] / S[0] - 1); P[29] = D[29] - D[3] * (D[28] / S[0]); P[30] = D[30] - D[9] * (D[28] / S[0]); P[31] = D[31] - D[18] * (D[28] / S[0]); P[32] = D[32] - D[28] * (D[28] / S[0]); P[33] = -D[33] * (D[1] / S[1] - 1); P[34] = D[34] - D[5] * (D[33] / S[1]); P[35] = D[35] - D[12] * (D[33] / S[1]); P[36] = D[36] - D[19] * (D[33] / S[1]); P[37] = D[37] - D[33] * (D[33] / S[1]); P[38] = -D[38] * (D[2] / S[2] - 1); P[39] = D[39] - D[7] * (D[38] / S[2]); P[40] = D[40] - D[15] * (D[38] / S[2]); P[41] = D[41] - D[20] * (D[38] / S[2]); P[42] = D[42] - D[38] * (D[38] / S[2]); // apply limits to some of the state variables if (X[9] > -1.5f) { X[9] = -1.5f; } else if (X[9] < -5.5f) { /* 4ms */ X[9] = -5.5f; } if (X[10] > 0.5f) { X[10] = 0.5f; } else if (X[10] < -0.5f) { X[10] = -0.5f; } if (X[11] > 0.5f) { X[11] = 0.5f; } else if (X[11] < -0.5f) { X[11] = -0.5f; } if (X[12] > 0.5f) { X[12] = 0.5f; } else if (X[12] < -0.5f) { X[12] = -0.5f; } } /** * Initialize the state variable and covariance matrix * for the system identification EKF */ static void AfInit(float X[AF_NUMX], float P[AF_NUMP]) { static const float qInit[AF_NUMX] = { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.05f, 0.05f, 0.005f, 0.05f, 0.05f, 0.05f, 0.05f }; // X[0] = X[1] = X[2] = 0.0f; // assume no rotation // X[3] = X[4] = X[5] = 0.0f; // and no net torque // X[6] = X[7] = 10.0f; // roll and pitch medium amount of strength // X[8] = 7.0f; // yaw strength // X[9] = -4.0f; // and 50 (18?) ms time scale // X[10] = X[11] = X[12] = 0.0f; // zero bias 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], &u.systemIdentState.Beta, sizeof(u.systemIdentState.Beta)); X[9] = u.systemIdentState.Tau; // P initialization memset(P, 0, AF_NUMP * sizeof(P[0])); P[0] = qInit[0]; P[1] = qInit[1]; P[2] = qInit[2]; P[4] = qInit[3]; P[6] = qInit[4]; P[8] = qInit[5]; P[11] = qInit[6]; P[14] = qInit[7]; P[17] = qInit[8]; P[27] = qInit[9]; P[32] = qInit[10]; P[37] = qInit[11]; P[42] = qInit[12]; } /** * @} * @} */