diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index a7cd0c5bf..8c019e38e 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -164,6 +164,14 @@ int32_t configuration_check() ADDSEVERITY(!coptercontrol); ADDSEVERITY(navCapableFusion); break; +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: + ADDSEVERITY(!gps_assisted); + // it would be fun to try autotune on a fixed wing + // but that should only be attempted by devs at first + ADDSEVERITY(multirotor); + break; +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ default: // Uncovered modes are automatically an error ADDSEVERITY(false); diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c new file mode 100644 index 000000000..a48eb646c --- /dev/null +++ b/flight/modules/AutoTune/autotune.c @@ -0,0 +1,1328 @@ +/** + ****************************************************************************** + * @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 Nano. 120 bytes of stack left when configured with 1340 +#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 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 ROLL_BETA_LOW 1 +#define PITCH_BETA_LOW 2 +#define YAW_BETA_LOW 4 +#define TAU_TOO_LONG 8 +#define TAU_TOO_SHORT 16 + +// 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 raw_time; /* From PIOS_DELAY_GetRaw() */ +}; + + +// Private variables +static xTaskHandle taskHandle; +static bool moduleEnabled; +static xQueueHandle atQueue; +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; + + +// 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 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); + + +/** + * 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; + } + } + + 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) { + // 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); + } + return 0; +} + + +MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart); + + +/** + * Module thread, should not return. + */ +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 }; + uint32_t lastTime = 0.0f; + uint32_t measureTime = 0; + uint32_t updateCounter = 0; + bool saveSiNeeded = false; + bool savePidNeeded = false; + + // 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 following flights + InitSystemIdent(false); + InitSmoothQuick(true); + + while (1) { + uint32_t diffTime; + bool doingIdent = false; + bool canSleep = true; + 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; + // 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 + // 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; + } + } else { + // if they did it disarmed, then set PID's back to AutoTune default + flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; + } + ProportionPidsSmoothToQuick(0.0f, + (float)flightModeSwitchTogglePosition, + (float)(systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE)); + savePidNeeded = 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) { + 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) + // (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 + // 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; + } + } 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; + } + } 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); + 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 + // 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) { + doingIdent = true; + // after an additional .5 seconds start capturing data + if (diffTime > INIT_TIME_DELAY2_MS) { + // Reset save status + // save SI data even if partial or bad, aids in diagnostics + saveSiNeeded = true; + // don't save PIDs until data gathering is complete + // and the complete data has been sanity checked + savePidNeeded = false; + InitSystemIdent(true); + InitSmoothQuick(true); + AfInit(gX, gP); + UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); + measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; + state = AT_START; + } + } + break; + + case AT_START: + lastTime = PIOS_DELAY_GetRaw(); + doingIdent = true; + /* Drain the queue of all current data */ + xQueueReset(atQueue); + /* And reset the point spill counter */ + updateCounter = 0; + atPointsSpilled = 0; + throttleAccumulator = 0; + 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 */ + float dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.raw_time) * 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; + 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: + // 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(); + 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; + } + // 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); + } + } + float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f; + UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hoverThrottle); + 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; + + 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 + 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.raw_time = PIOS_DELAY_GetRaw(); + q_item.y[0] = gyro.x; + q_item.y[1] = gyro.y; + q_item.y[2] = gyro.z; + q_item.u[0] = actuators.Roll; + q_item.u[1] = actuators.Pitch; + q_item.u[2] = actuators.Yaw; + q_item.throttle = actuators.Thrust; + + 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 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 + SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0); + SystemIdentStateGet(&systemIdentState); + // Tau, Beta, and the Complete flag get default values + // in preparation for running AutoTune + systemIdentSettings.Tau = systemIdentState.Tau; + memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); + systemIdentSettings.Complete = systemIdentState.Complete; + } else { + // Tau, Beta, and the Complete flag get stored values + // so the user can fly another battery to select and test PIDs with the slider/knob + systemIdentState.Tau = systemIdentSettings.Tau; + memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData)); + systemIdentState.Complete = systemIdentSettings.Complete; + } +} + + +static void InitSmoothQuick(bool loadToggle) +{ + 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 + // disable PID changing with flight mode switch + // ignore loadToggle if user is also switching to use knob as source + flightModeSwitchTogglePosition = -1; + 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 + // 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: + // 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; + } +} + + +// 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) +{ + 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 + // 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; + + SystemIdentStateSet(&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 +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; + + // 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) { + retVal |= ROLL_BETA_LOW; + } + if (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) { + 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) { + retVal |= TAU_TOO_SHORT; + } + + 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; + } +// 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 +static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) +{ + _Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)"); + StabilizationBankData 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 double ghf = (double)noiseRate / 1000.0d; + const double damp = (double)dampRate / 100.0d; + + double tau = exp(systemIdentState.Tau); + double exp_beta_roll_times_ghf = exp(systemIdentState.Beta.Roll) * ghf; + double exp_beta_pitch_times_ghf = exp(systemIdentState.Beta.Pitch) * ghf; + + double wn = 1.0d / tau; + double tau_d = 0.0d; + 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); + // 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); + } + + // 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); + + // 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); + + float kpMax = 0.0f; + double betaMinLn = 1000.0d; + StabilizationBankRollRatePIDData *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; + + 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; + if (betaMinLn > betaLn) { + betaMinLn = betaLn; + // RollRatePID PitchRatePID YawRatePID + // form an array of structures + // point to one + rollPitchPid = &(&stabSettingsBank.RollRatePID)[i]; + } + 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 = 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; + break; + } + + if (i < 2) { + if (kpMax < (float)kp) { + kpMax = (float)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 && (float)kp < min) { + ratio = (float)kp / min; + } else if (max > 0.0f && (float)kp > max) { + ratio = (float)kp / max; + } + kp /= (double)ratio; + ki /= (double)ratio; + kd /= (double)ratio; + } + + 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.0d / (2.0d*M_PI*tau_d); + // SystemIdentSettingsDerivativeCutoffSet(&systemIdentSettings.DerivativeCutoff); + + // 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; + } +} + + +// 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 +// 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 min, float val, float max) +{ + float ratio, damp, noise; + + // 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); +} + + +/** + * 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], &systemIdentState.Beta, sizeof(systemIdentState.Beta)); + X[9] = 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]; +} + +/** + * @} + * @} + */ diff --git a/flight/modules/Autotune/autotune.c b/flight/modules/Autotune/autotune.c deleted file mode 100644 index 60166dede..000000000 --- a/flight/modules/Autotune/autotune.c +++ /dev/null @@ -1,337 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Autotuning module - * @brief Reads from @ref ManualControlCommand and fakes a rate mode while - * toggling the channels to relay mode - * @{ - * - * @file autotune.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Module to handle all comms to the AHRS on a periodic basis. - * - * @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 - */ - -/** - * Input objects: None, takes sensor data via pios - * Output objects: @ref AttitudeRaw @ref AttitudeActual - * - * This module computes an attitude estimate from the sensor data - * - * The module executes in its own thread. - * - * UAVObjects are automatically generated by the UAVObjectGenerator from - * the object definition XML file. - * - * Modules have no API, all communication to other modules is done through UAVObjects. - * However modules may use the API exposed by shared libraries. - * See the OpenPilot wiki for more details. - * http://www.openpilot.org/OpenPilot_Application_Architecture - * - */ - -#include - -#include "flightstatus.h" -#include "hwsettings.h" -#include "manualcontrolcommand.h" -#include "manualcontrolsettings.h" -#include "relaytuning.h" -#include "relaytuningsettings.h" -#include "stabilizationdesired.h" -#include "stabilizationsettings.h" -#include "taskinfo.h" - -// Private constants -#define STACK_SIZE_BYTES 1024 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) - -// Private types -enum AUTOTUNE_STATE { AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET }; - -// Private variables -static xTaskHandle taskHandle; -static bool autotuneEnabled; - -// Private functions -static void AutotuneTask(void *parameters); -static void update_stabilization_settings(); - -/** - * 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 - autotuneEnabled = true; -#else - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - - HwSettingsOptionalModulesGet(optionalModules); - - if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - autotuneEnabled = true; - } else { - autotuneEnabled = false; - } -#endif - - 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 (autotuneEnabled) { - xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE); - } - return 0; -} - -MODULE_INITCALL(AutotuneInitialize, AutotuneStart); - -/** - * Module thread, should not return. - */ -static void AutotuneTask(__attribute__((unused)) void *parameters) -{ - // AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - enum AUTOTUNE_STATE state = AT_INIT; - - portTickType lastUpdateTime = xTaskGetTickCount(); - - while (1) { - PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); - // TODO: - // 1. get from queue - // 2. based on whether it is flightstatus or manualcontrol - - portTickType diffTime; - - const uint32_t PREPARE_TIME = 2000; - const uint32_t MEAURE_TIME = 30000; - - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - // Only allow this module to run when autotuning - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { - state = AT_INIT; - vTaskDelay(50); - continue; - } - - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); - - ManualControlSettingsData manualSettings; - ManualControlSettingsGet(&manualSettings); - - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); - - bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; - - if (rate) { // rate mode - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - - stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; - stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; - } else { - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - - stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; - stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; - } - - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_THRUST] = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - stabDesired.Thrust = manualControl.Thrust; - - switch (state) { - case AT_INIT: - - lastUpdateTime = xTaskGetTickCount(); - - // Only start when armed and flying - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Thrust > 0) { - state = AT_START; - } - break; - - case AT_START: - - diffTime = xTaskGetTickCount() - lastUpdateTime; - - // Spend the first block of time in normal rate mode to get airborne - if (diffTime > PREPARE_TIME) { - state = AT_ROLL; - lastUpdateTime = xTaskGetTickCount(); - } - break; - - case AT_ROLL: - - diffTime = xTaskGetTickCount() - lastUpdateTime; - - // Run relay mode on the roll axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : - STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; - if (diffTime > MEAURE_TIME) { // Move on to next state - state = AT_PITCH; - lastUpdateTime = xTaskGetTickCount(); - } - break; - - case AT_PITCH: - - diffTime = xTaskGetTickCount() - lastUpdateTime; - - // Run relay mode on the pitch axis for the measurement time - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : - STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; - if (diffTime > MEAURE_TIME) { // Move on to next state - state = AT_FINISHED; - lastUpdateTime = xTaskGetTickCount(); - } - break; - - case AT_FINISHED: - - // Wait until disarmed and landed before updating the settings - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Thrust <= 0) { - state = AT_SET; - } - - break; - - case AT_SET: - update_stabilization_settings(); - state = AT_INIT; - break; - - default: - // Set an alarm or some shit like that - break; - } - - StabilizationDesiredSet(&stabDesired); - - vTaskDelay(10); - } -} - -/** - * Called after measuring roll and pitch to update the - * stabilization settings - * - * takes in @ref RelayTuning and outputs @ref StabilizationSettings - */ -static void update_stabilization_settings() -{ - RelayTuningData relayTuning; - - RelayTuningGet(&relayTuning); - - RelayTuningSettingsData relaySettings; - RelayTuningSettingsGet(&relaySettings); - - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); - - // Eventually get these settings from RelayTuningSettings - const float gain_ratio_r = 1.0f / 3.0f; - const float zero_ratio_r = 1.0f / 3.0f; - const float gain_ratio_p = 1.0f / 5.0f; - const float zero_ratio_p = 1.0f / 5.0f; - - // For now just run over roll and pitch - for (uint i = 0; i < 2; i++) { - float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s) - - float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s) - float zc = wc * zero_ratio_r; // controller zero location (rad/s) - float kpu = 4.0f / M_PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity - float kp = kpu * gain_ratio_r; // proportional gain - float ki = zc * kp; // integral gain - - // Now calculate gains for the next loop out knowing it is the integral of - // the inner loop -- the plant is position/velocity = scale*1/s - float wc2 = wc * gain_ratio_p; // crossover of the attitude loop - float kp2 = wc2; // kp of attitude - float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude - - switch (i) { - case 0: // roll - stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; - stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; - stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; - stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; - break; - case 1: // Pitch - stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp; - stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki; - stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2; - stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2; - break; - default: - // Oh shit oh shit oh shit - break; - } - } - switch (relaySettings.Behavior) { - case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE: - // Just measure, don't update the stab settings - break; - case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE: - StabilizationSettingsSet(&stabSettings); - break; - case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE: - StabilizationSettingsSet(&stabSettings); - UAVObjSave(StabilizationSettingsHandle(), 0); - break; - } -} - -/** - * @} - * @} - */ diff --git a/flight/modules/Autotune/inc/autotune.h b/flight/modules/Autotune/inc/autotune.h deleted file mode 100644 index 64c556a8e..000000000 --- a/flight/modules/Autotune/inc/autotune.h +++ /dev/null @@ -1,37 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Attitude Attitude Module - * @{ - * - * @file attitude.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @brief Acquires sensor data and fuses it into attitude estimate for CC - * - * @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 - */ -#ifndef ATTITUDE_H -#define ATTITUDE_H - -#include "openpilot.h" - -int32_t AttitudeInitialize(void); - -#endif // ATTITUDE_H diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index c44e8f3fe..2ccfcc7a6 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -7,7 +7,8 @@ * @{ * * @file armhandler.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * * @see The GNU Public License (GPL) Version 3 * @@ -342,6 +343,9 @@ static bool okToArm(void) break; case FLIGHTSTATUS_FLIGHTMODE_LAND: +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ return false; case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index af5195bbf..4ad431355 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -11,7 +11,8 @@ * AttitudeDesired object (stabilized mode) * * @file manualcontrol.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief ManualControl module. Handles safety R/C link and flight mode. * * @see The GNU Public License (GPL) Version 3 @@ -47,7 +48,7 @@ #include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) @@ -118,7 +119,7 @@ static void commandUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings); -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ static void SettingsUpdatedCb(UAVObjEvent *ev); #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) @@ -146,8 +147,8 @@ int32_t ManualControlStart() #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES takeOffLocationHandlerInit(); +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ -#endif // Start main task PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); @@ -174,7 +175,7 @@ int32_t ManualControlInitialize() VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); SystemSettingsConnectCallback(&SettingsUpdatedCb); -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES); return 0; @@ -202,7 +203,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) break; } } -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ } /** @@ -214,7 +215,7 @@ static void manualControlTask(void) armHandler(false, frameType); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES takeOffLocationHandler(); -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ // Process flight mode FlightStatusData flightStatus; @@ -225,7 +226,7 @@ static void manualControlTask(void) #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES VtolPathFollowerSettingsThrustLimitsData thrustLimits; VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits); -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ FlightModeSettingsData modeSettings; FlightModeSettingsGet(&modeSettings); @@ -273,6 +274,9 @@ static void manualControlTask(void) case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ handler = &handler_STABILIZED; #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES @@ -286,7 +290,6 @@ static void manualControlTask(void) newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; } - if (newFlightModeAssist) { // assess roll/pitch state bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f); @@ -539,22 +542,21 @@ static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) } +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) /** * Check and set modes for gps assisted stablised flight modes */ -#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings) { - uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE; - uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE; StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM]; StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap); - if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) { - flightModeAssistOption = FlightModeAssistMap[position]; + if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE + || position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) { + return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE; } - switch (flightModeAssistOption) { + switch (FlightModeAssistMap[position]) { case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE: break; case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST: @@ -601,22 +603,22 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD: case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO: // this is only for use with stabi mods with althold/vario. - isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST; - break; + return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST; + case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL: case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL: default: // this is the default for non stabi modes also - isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST; - break; + return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST; } } break; } - return isAssistedFlag; + // return isAssistedFlag; + return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE; } -#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ /** * @} diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 8cb6d3156..f57e665f5 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -7,7 +7,8 @@ * @{ * * @file stabilizedhandler.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * * @see The GNU Public License (GPL) Version 3 * @@ -35,6 +36,7 @@ #include #include #include +#include // Private constants @@ -48,6 +50,7 @@ static uint8_t currentFpvTiltAngle = 0; static float cosAngle = 0.0f; static float sinAngle = 0.0f; + static float applyExpo(float value, float expo) { // note: fastPow makes a small error, therefore result needs to be bound @@ -74,12 +77,16 @@ static float applyExpo(float value, float expo) * @input: ManualControlCommand * @output: StabilizationDesired */ -void stabilizedHandler(bool newinit) +void stabilizedHandler(__attribute__((unused)) bool newinit) { - if (newinit) { + static bool inited = false; + + if (!inited) { + inited = true; StabilizationDesiredInitialize(); StabilizationBankInitialize(); } + ManualControlCommandData cmd; ManualControlCommandGet(&cmd); @@ -116,7 +123,6 @@ void stabilizedHandler(bool newinit) uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); - switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); @@ -136,6 +142,13 @@ void stabilizedHandler(bool newinit) case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings); break; +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: + // let autotune.c handle it + // because it must switch to Attitude after seconds + return; + +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ default: // Major error, this should not occur because only enter this block when one of these is true AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); @@ -153,6 +166,9 @@ void stabilizedHandler(bool newinit) (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax : +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Roll * stabSettings.RollMax : +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ 0; // this is an invalid mode stabilization.Pitch = @@ -165,6 +181,9 @@ void stabilizedHandler(bool newinit) (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Pitch * stabSettings.PitchMax : +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ 0; // this is an invalid mode // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order @@ -187,6 +206,9 @@ void stabilizedHandler(bool newinit) (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax : +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Yaw * stabSettings.ManualRate.Yaw : +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ 0; // this is an invalid mode } diff --git a/flight/modules/Notify/inc/sequences.h b/flight/modules/Notify/inc/sequences.h index 3cf46d832..3298d5ff0 100644 --- a/flight/modules/Notify/inc/sequences.h +++ b/flight/modules/Notify/inc/sequences.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file sequences.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Notify module, sequences configuration. * * @see The GNU Public License (GPL) Version 3 @@ -184,6 +185,9 @@ const LedSequence_t *flightModeMap[] = { [FLIGHTSTATUS_FLIGHTMODE_POI] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND], +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + [FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL], +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ }; // List of alarms to show with attached sequences for each status diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 629e87c20..a50e6e6ec 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -9,7 +9,7 @@ * @{ * * @file innerloop.c - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief Attitude stabilization module. * @@ -51,14 +51,28 @@ #include #include #include +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) +#include +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ + // Private constants -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL -#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) -#define UPDATE_MIN 1.0e-6f -#define UPDATE_MAX 1.0f -#define UPDATE_ALPHA 1.0e-2f +#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f + +#define SYSTEM_IDENT_PERIOD ((uint32_t)75) + +#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 variables static DelayedCallbackInfo *callbackHandle; @@ -69,6 +83,10 @@ static PiOSDeltatimeConfig timeval; static float speedScaleFactor = 1.0f; static bool frame_is_multirotor; static bool measuredDterm_enabled; +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) +static uint32_t systemIdentTimeVal = 0; +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ + // Private functions static void stabilizationInnerloopTask(); static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); @@ -86,6 +104,9 @@ void stabilizationInnerloopInit() ManualControlCommandInitialize(); StabilizationDesiredInitialize(); ActuatorDesiredInitialize(); +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + SystemIdentStateInitialize(); +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ #ifdef REVOLUTION AirspeedStateInitialize(); AirspeedStateConnectCallback(AirSpeedUpdatedCb); @@ -100,6 +121,10 @@ void stabilizationInnerloopInit() frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + // Settings for system identification + systemIdentTimeVal = PIOS_DELAY_GetRaw(); +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ } static float get_pid_scale_source_value() @@ -230,7 +255,6 @@ static void stabilizationInnerloopTask() } } - RateDesiredData rateDesired; ActuatorDesiredData actuator; StabilizationStatusInnerLoopData enabled; @@ -286,6 +310,7 @@ static void stabilizationInnerloopTask() // IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication! // keep order as it is, RATE must follow! case STABILIZATIONSTATUS_INNERLOOP_RATE: + { // limit rate to maximum configured limits (once here instead of 5 times in outer loop) rate[t] = boundf(rate[t], -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t], @@ -293,7 +318,8 @@ static void stabilizationInnerloopTask() ); pid_scaler scaler = create_pid_scaler(t); actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled); - break; + } + break; case STABILIZATIONSTATUS_INNERLOOP_ACRO: { float stickinput[3]; @@ -312,6 +338,67 @@ static void stabilizationInnerloopTask() actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate; } break; + +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT: + { + static int8_t identIteration = 0; + static float identOffsets[3] = { 0 }; + + if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) { + const float SCALE_BIAS = 7.1f; + SystemIdentStateBetaData systemIdentBeta; + + SystemIdentStateBetaGet(&systemIdentBeta); + systemIdentTimeVal = PIOS_DELAY_GetRaw(); + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration + 1) & 7; + // why does yaw change twice a cycle and roll/pitch change only once? + uint8_t index = ((uint8_t[]) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } + )[identIteration]; + float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentBeta)[index]); + // if roll or pitch limit to 25% of range + if (identIteration & 1) { + if (scale > 0.25f) { + scale = 0.25f; + } + } + // else it is yaw that can be a little more radical + else { + if (scale > 0.45f) { + scale = 0.45f; + } + } + if (identIteration & 2) { + scale = -scale; + } + identOffsets[index] = scale; + // this results in: + // when identIteration==0: identOffsets[2] = yaw_scale; + // when identIteration==1: identOffsets[0] = roll_scale; + // when identIteration==2: identOffsets[2] = -yaw_scale; + // when identIteration==3: identOffsets[0] = -roll_scale; + // when identIteration==4: identOffsets[2] = yaw_scale; + // when identIteration==5: identOffsets[1] = pitch_scale; + // when identIteration==6: identOffsets[2] = -yaw_scale; + // when identIteration==7: identOffsets[1] = -pitch_scale; + // each change has one axis with an offset + // and another axis coming back to zero from having an offset + } + + rate[t] = boundf(rate[t], + -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t], + StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] + ); + pid_scaler scaler = create_pid_scaler(t); + actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled); + actuatorDesiredAxis[t] += identOffsets[t]; + } + break; +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ + case STABILIZATIONSTATUS_INNERLOOP_DIRECT: default: actuatorDesiredAxis[t] = rate[t]; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index d9c3c01cb..24f257d12 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -9,7 +9,8 @@ * @{ * * @file stabilization.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Attitude stabilization module. * * @see The GNU Public License (GPL) Version 3 @@ -141,6 +142,24 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT: +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) + // roll or pitch + if (t <= 1) { + StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; + } + // else yaw (other modes don't worry about invalid thrust mode either) + else { + StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; + } + StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT; + break; +#else /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ + // no break, do not reorder this code + // for low power FCs just fall through to Attitude mode + // that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ +// do not reorder this code case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; diff --git a/flight/pios/inc/pios_delay.h b/flight/pios/inc/pios_delay.h index 4a2da6347..63546d75d 100644 --- a/flight/pios/inc/pios_delay.h +++ b/flight/pios/inc/pios_delay.h @@ -7,7 +7,8 @@ * @{ * * @file pios_settings.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Settings functions header * @see The GNU Public License (GPL) Version 3 * @@ -39,6 +40,7 @@ extern uint32_t PIOS_DELAY_GetuS(); extern uint32_t PIOS_DELAY_GetuSSince(uint32_t t); extern uint32_t PIOS_DELAY_GetRaw(); extern uint32_t PIOS_DELAY_DiffuS(uint32_t raw); +extern uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later); #endif /* PIOS_DELAY_H */ diff --git a/flight/pios/stm32f4xx/pios_delay.c b/flight/pios/stm32f4xx/pios_delay.c index 55970d203..f9c5d62b1 100644 --- a/flight/pios/stm32f4xx/pios_delay.c +++ b/flight/pios/stm32f4xx/pios_delay.c @@ -7,7 +7,8 @@ * @{ * * @file pios_delay.c - * @author Michael Smith Copyright (C) 2012 + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * Michael Smith Copyright (C) 2012 * @brief Delay Functions * - Provides a micro-second granular delay using the CPU * cycle counter. @@ -168,6 +169,17 @@ uint32_t PIOS_DELAY_DiffuS(uint32_t raw) return diff / us_ticks; } +#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) +/** + * @brief Subtract two raw times and convert to us. + * @return Interval between raw times in microseconds + */ +uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) +{ + return (later - raw) / us_ticks; +} +#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ + #endif /* PIOS_INCLUDE_DELAY */ /** diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 1f46a4f1a..ee1113e44 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # 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 @@ -55,6 +55,7 @@ MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index afc1c82d1..b704f4ab0 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -1,5 +1,6 @@ # -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # 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 @@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 2327c2d71..ec760e0a4 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # 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 @@ -52,6 +52,7 @@ MODULES += Logging MODULES += Telemetry MODULES += Notify +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 0b751c1d0..6100c4750 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -1,5 +1,6 @@ # -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # 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 @@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile index 7e3701a2f..3a20a18d4 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -1,6 +1,6 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2014, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2014, The OpenPilot Team, http://www.openpilot.org # # 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 @@ -51,6 +51,7 @@ MODULES += PathFollower MODULES += Telemetry MODULES += Notify +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge diff --git a/flight/targets/boards/revonano/firmware/UAVObjects.inc b/flight/targets/boards/revonano/firmware/UAVObjects.inc index 0b751c1d0..6100c4750 100644 --- a/flight/targets/boards/revonano/firmware/UAVObjects.inc +++ b/flight/targets/boards/revonano/firmware/UAVObjects.inc @@ -1,5 +1,6 @@ # -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # 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 @@ -125,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 67ad81a59..736978d5e 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # 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 @@ -52,6 +52,7 @@ MODULES += Telemetry SRC += $(FLIGHTLIB)/notification.c +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index c98bc205e..cba723f6b 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -1,5 +1,6 @@ # -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # 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 @@ -124,6 +125,9 @@ UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation +# UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 7aa0f3511..1849b9740 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -2,7 +2,7 @@ # # TODO: This file should be reworked. It will be done as a part of sim target refactoring. # -# Copyright (c) 2015 The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2015-2016 The LibrePilot Project, http://www.librepilot.org # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009. # # @@ -50,6 +50,8 @@ MODULES += Airspeed SRC += $(FLIGHTLIB)/notification.c +OPTMODULES += AutoTune + # Paths OPSYSTEM = . BOARDINC = .. diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 928a1c0dc..52093b2e9 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -3,6 +3,7 @@ # # Makefile for OpenPilot UAVObject code # +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011. # # This program is free software; you can redistribute it and/or modify @@ -119,6 +120,9 @@ UAVOBJSRCFILENAMES += altitudeholdstatus UAVOBJSRCFILENAMES += ekfconfiguration UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += takeofflocation +# UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/sparky2/firmware/Makefile b/flight/targets/boards/sparky2/firmware/Makefile index 160081566..e84a4adb4 100644 --- a/flight/targets/boards/sparky2/firmware/Makefile +++ b/flight/targets/boards/sparky2/firmware/Makefile @@ -1,7 +1,7 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org -# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot # # 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 @@ -52,6 +52,7 @@ MODULES += Logging MODULES += Telemetry MODULES += Notify +OPTMODULES += AutoTune OPTMODULES += ComUsbBridge OPTMODULES += UAVOMSPBridge diff --git a/flight/targets/boards/sparky2/firmware/UAVObjects.inc b/flight/targets/boards/sparky2/firmware/UAVObjects.inc index 7e7a70f8b..f9ff52a9f 100644 --- a/flight/targets/boards/sparky2/firmware/UAVObjects.inc +++ b/flight/targets/boards/sparky2/firmware/UAVObjects.inc @@ -1,6 +1,6 @@ # -# Copyright (c) 2016, The LibrePilot Project, http://www.librepilot.org -# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org # # 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 @@ -126,6 +126,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter - +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate + UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/ground/gcs/src/plugins/uavobjects/uavobjects.pro b/ground/gcs/src/plugins/uavobjects/uavobjects.pro index 25372e372..92be03832 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -137,6 +137,8 @@ UAVOBJS = \ $${UAVOBJ_XML_DIR}/statusvtolautotakeoff.xml \ $${UAVOBJ_XML_DIR}/statusvtolland.xml \ $${UAVOBJ_XML_DIR}/systemalarms.xml \ + $${UAVOBJ_XML_DIR}/systemidentsettings.xml \ + $${UAVOBJ_XML_DIR}/systemidentstate.xml \ $${UAVOBJ_XML_DIR}/systemsettings.xml \ $${UAVOBJ_XML_DIR}/systemstats.xml \ $${UAVOBJ_XML_DIR}/takeofflocation.xml \ diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 71f2b6453..5ee84a544 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -7,57 +7,57 @@ @@ -66,7 +66,7 @@ units="" type="enum" elements="6" - options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff" + options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff,AutoTune" defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6" limits="%NE:POI:AutoCruise; \ %NE:POI:AutoCruise; \ diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 6e3797e35..c758d84ff 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index e51a01934..56e6b2bf0 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -29,7 +29,7 @@ - + - + diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index 5126ed9c1..4f78fc8f3 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -3,7 +3,7 @@ Contains status information to control submodules for stabilization. - + Roll Pitch @@ -11,7 +11,7 @@ Thrust - + Roll Pitch diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml index ba4cd24de..7cf9d55ed 100644 --- a/shared/uavobjectdefinition/systemalarms.xml +++ b/shared/uavobjectdefinition/systemalarms.xml @@ -37,6 +37,7 @@ + diff --git a/shared/uavobjectdefinition/systemidentsettings.xml b/shared/uavobjectdefinition/systemidentsettings.xml new file mode 100644 index 000000000..9a2d2f7be --- /dev/null +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -0,0 +1,59 @@ + + + The input to the PID tuning. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/systemidentstate.xml b/shared/uavobjectdefinition/systemidentstate.xml new file mode 100644 index 000000000..457ef3515 --- /dev/null +++ b/shared/uavobjectdefinition/systemidentstate.xml @@ -0,0 +1,19 @@ + + + The input to the PID tuning. + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index b9facf626..4518b9250 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -32,6 +32,7 @@ GPS OSDGen UAVOMSPBridge + AutoTune @@ -65,6 +66,7 @@ GPS OSDGen UAVOMSPBridge + AutoTune @@ -102,6 +104,7 @@ GPS OSDGen UAVOMSPBridge + AutoTune