diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index fe6f2eba6..d78dadf69 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -43,11 +43,11 @@ #include "actuatordesired.h" #include "stabilizationdesired.h" #include "stabilizationsettings.h" -#include "systemident.h" +#include "systemidentsettings.h" +#include "systemidentstate.h" #include #include "systemsettings.h" #include "taskinfo.h" - #include "stabilization.h" #include "hwsettings.h" #include "stabilizationsettingsbank1.h" @@ -55,9 +55,6 @@ #include "stabilizationsettingsbank3.h" #include "accessorydesired.h" - -#define PIOS_malloc pios_malloc - #if defined(PIOS_EXCLUDE_ADVANCED_FEATURES) #define powapprox fastpow #define expapprox fastexp @@ -66,180 +63,6 @@ #define expapprox expf #endif /* defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ -//#define USE_CIRC_QUEUE - - -#if defined(USE_CIRC_QUEUE) -/** - ****************************************************************************** - * @file circqueue.h - * @author dRonin, http://dRonin.org/, Copyright (C) 2015 - * @brief Public header for 1 reader, 1 writer circular queue - *****************************************************************************/ - -typedef struct circ_queue *circ_queue_t; - -circ_queue_t circ_queue_new(uint16_t elem_size, uint16_t num_elem); - -void *circ_queue_cur_write_pos(circ_queue_t q); - -int circ_queue_advance_write(circ_queue_t q); - -void *circ_queue_read_pos(circ_queue_t q); - -void circ_queue_read_completed(circ_queue_t q); - -/** - ****************************************************************************** - * @file circqueue.c - * @author dRonin, http://dRonin.org/, Copyright (C) 2015 - * @brief Implements a 1 reader, 1 writer nonblocking circular queue - *****************************************************************************/ -/* - * 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 - -struct circ_queue { - uint16_t elem_size; /**< Element size in octets */ - uint16_t num_elem; /**< Number of elements in circqueue (capacity+1) */ - volatile uint16_t write_head; /**< Element position writer is at */ - volatile uint16_t read_tail; /**< Element position reader is at */ - - /* head == tail: empty. - * head == tail-1: full. - */ - - /* This is declared as a uint32_t for alignment reasons. */ - uint32_t contents[]; /**< Contents of the circular queue */ -}; - -/** Allocate a new circular queue. - * @param[in] elem_size The size of each element, as obtained from sizeof(). - * @param[in] num_elem The number of elements in the queue. The capacity is - * one less than this (it may not be completely filled). - * @returns The handle to the circular queue. - */ -circ_queue_t circ_queue_new(uint16_t elem_size, uint16_t num_elem) { - PIOS_Assert(elem_size > 0); - PIOS_Assert(num_elem > 2); - - uint32_t size = elem_size * num_elem; - - /* PIOS_malloc_no_dma may not be safe for some later uses.. hmmm */ - struct circ_queue *ret = PIOS_malloc(sizeof(*ret) + size); - - memset(ret, 0, sizeof(*ret) + size); -CSS pixel - ret->elem_size = elem_size; - ret->num_elem = num_elem; - - return ret; -} - -/** Get a pointer to the current queue write position. - * This position is unavailable to any present readers and may be filled in - * with the desired data without respect to any synchronization. - * - * @param[in] q Handle to circular queue. - * @returns The position for new data to be written to (of size elem_size). - */ -void *circ_queue_cur_write_pos(circ_queue_t q) { - void *contents = q->contents; - - return contents + q->write_head * q->elem_size; -} - -static inline uint16_t next_pos(uint16_t num_pos, uint16_t current_pos) { - PIOS_Assert(current_pos < num_pos); - - current_pos++; - /* Also save on uint16_t wrap */ - - if (current_pos >= num_pos) { - current_pos = 0; - } - - return current_pos; -} - -/** Makes the current block of data available to readers and advances write pos. - * This may fail if the queue contain num_elems -1 elements, in which case the - * advance may be retried in the future. In this case, data already written to - * write_pos is preserved and the advance may be retried (or overwritten with - * new data). - * - * @param[in] q Handle to circular queue. - * @returns 0 if the write succeeded, nonzero on error. - */ -int circ_queue_advance_write(circ_queue_t q) { - uint16_t new_write_head = next_pos(q->num_elem, q->write_head); - - /* the head is not allowed to advance to meet the tail */ - if (new_write_head == q->read_tail) { - return -1; /* Full */ - - /* Caller can either let the data go away, or try again to - * advance later */ - } - - q->write_head = new_write_head; - return 0; -} - -/** Returns a block of data to the reader. - * The block is "claimed" until released with circ_queue_read_completed. - * No new data is available until that call is made (instead the same - * block-in-progress will be returned). - * - * @param[in] q Handle to circular queue. - * @returns pointer to the data, or NULL if the queue is empty. - */ -void *circ_queue_read_pos(circ_queue_t q) { - uint16_t read_tail = q->read_tail; - void *contents = q->contents; - - if (q->write_head == read_tail) { - /* There is nothing new to read. */ - return NULL; - } - - return contents + q->read_tail * q->elem_size; -} - -/** Releases a block of read data obtained by circ_queue_read_pos. - * Behavior is undefined if circ_queue_read_pos did not previously return - * a block of data. - * - * @param[in] q Handle to the circular queue. - */ -void circ_queue_read_completed(circ_queue_t q) { - /* Avoid multiple accesses to a volatile */ - uint16_t read_tail = q->read_tail; - - /* If this is being called, the queue had better not be empty-- - * we're supposed to finish consuming this element after a prior call - * to circ_queue_read_pos. - */ - PIOS_Assert(read_tail != q->write_head); - - q->read_tail = next_pos(q->num_elem, read_tail); -} -#endif - // Private constants #undef STACK_SIZE_BYTES @@ -256,24 +79,25 @@ void circ_queue_read_completed(circ_queue_t q) { #define AT_QUEUE_NUMELEM 18 #endif -#define MAX_PTS_PER_CYCLE 4 -//#define START_TIME_DELAY_MS 2000 -#define INIT_TIME_DELAY_MS 2000 -#define YIELD_MS 2 +#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 */ -#if defined(USE_CIRC_QUEUE) -#define DEREFERENCE(str, ele) (str->ele) -#else -#define DEREFERENCE(str, ele) (str.ele) -#endif +#define ROLL_BETA_LOW 1 +#define PITCH_BETA_LOW 2 +#define YAW_BETA_LOW 4 +#define TAU_TOO_LONG 8 +#define TAU_TOO_SHORT 16 -#define SMOOTH_QUICK_DISABLED 0 -#define SMOOTH_QUICK_AUX_BASE 10 -#define SMOOTH_QUICK_TOGGLE_BASE 21 +#define SMOOTH_QUICK_DISABLED 0 +#define SMOOTH_QUICK_ACCESSORY_BASE 10 +#define SMOOTH_QUICK_TOGGLE_BASE 21 // Private types -enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_START, AT_RUN, AT_FINISHED, AT_WAITING }; +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 */ @@ -285,22 +109,17 @@ struct at_queued_data { // Private variables -//static struct pios_thread *taskHandle; static xTaskHandle taskHandle; static bool moduleEnabled; -#if defined(USE_CIRC_QUEUE) -static circ_queue_t atQueue; -#else static xQueueHandle atQueue; -#endif static volatile uint32_t atPointsSpilled; static uint32_t throttleAccumulator; static uint8_t rollMax, pitchMax; static StabilizationBankManualRateData manualRate; -//static SystemSettingsAirframeTypeOptions airframeType; static float gX[AF_NUMX] = {0}; static float gP[AF_NUMP] = {0}; -SystemIdentData systemIdentData; +SystemIdentSettingsData systemIdentSettings; +SystemIdentStateData systemIdentState; int8_t accessoryToUse; int8_t flightModeSwitchTogglePosition; @@ -309,12 +128,13 @@ int8_t flightModeSwitchTogglePosition; 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 UpdateSystemIdent(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle); +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); @@ -340,12 +160,9 @@ int32_t AutoTuneInitialize(void) #endif if (moduleEnabled) { - SystemIdentInitialize(); -#if defined(USE_CIRC_QUEUE) - atQueue = circ_queue_new(sizeof(struct at_queued_data), AT_QUEUE_NUMELEM); -#else + SystemIdentSettingsInitialize(); + SystemIdentStateInitialize(); atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data)); -#endif if (!atQueue) { moduleEnabled = false; } @@ -385,18 +202,19 @@ 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}; -// is this needed? -// AfInit(gX,gP); uint32_t lastTime = 0.0f; bool saveSiNeeded = false; bool savePidNeeded = false; -// should this be put in Init()? -// GyroSensorConnectCallback(AtNewGyroData); - + // 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 a following flight + // so that the user can use the PID smooth->quick slider in following flights InitSystemIdent(false); while (1) { @@ -405,25 +223,22 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) uint32_t measureTime = 60000; bool doingIdent = false; bool canSleep = true; -//PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); FlightStatusData flightStatus; FlightStatusGet(&flightStatus); -// can't restart till after you save that's OK I guess -// but you should be able to stop in mid tune and restart from beginning -// maybe reset state in that fn that gets called on mode change + // I have never seen this module misbehave so not bothering making a watchdog + //PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (saveSiNeeded) { saveSiNeeded = false; - // Save SystemIdent to permanent settings - UAVObjSave(SystemIdentHandle(), 0); -//so how to restart if it failed and both saves are false + // Save SystemIdentSettings to permanent settings + UAVObjSave(SystemIdentSettingsHandle(), 0); } if (savePidNeeded) { savePidNeeded = false; // Save PIDs to permanent settings - switch (systemIdentData.DestinationPidBank) { + switch (systemIdentSettings.DestinationPidBank) { case 1: UAVObjSave(StabilizationSettingsBank1Handle(), 0); break; @@ -435,42 +250,49 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) break; } } -// can't set to AT_INIT because when we land and disarm it will jump to init and clear things out after 2 seconds -//state = AT_INIT; } // if using flight mode switch quick toggle to "try smooth -> quick PIDs" is enabled // and user toggled into and back out of AutoTune // three times in the last two seconds - // CheckFlightModeSwitchForPidRequest(mode) only returns true if mode is not autotune - if (flightModeSwitchTogglePosition!=-1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentData.Complete && !CheckSettings()) { + // and the data gathering is complete + // and the data gathered is good + // note: CheckFlightModeSwitchForPidRequest(mode) only returns true if 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 3,4,5,1,2 or 2,3,1 + // if user toggled while armed set PID's to next in sequence 2,3,4,0,1... or 1,2,0... + // if smoothest is -100 and quickest is +100 this corresponds to 0,+50,+100,-100,-50... or 0,+100,-100 ++flightModeSwitchTogglePosition; - if (flightModeSwitchTogglePosition > systemIdentData.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) { + if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) { flightModeSwitchTogglePosition = 0; } } else { - // if they did it disarmed, then set PID's back to AT default - flightModeSwitchTogglePosition = (systemIdentData.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2; + // if they did it disarmed, then set PID's back to AutoTune default + flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2; } - ProportionPidsSmoothToQuick(0.0f, (float) flightModeSwitchTogglePosition, (float) (systemIdentData.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE)); + ProportionPidsSmoothToQuick(0.0f, + (float) flightModeSwitchTogglePosition, + (float) (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE)); savePidNeeded = true; } + // any time we are not in AutoTune mode: + // - the user may be using the accessory0-3 knob/slider to request PID changes + // - the state machine needs to be reset + // - the local version of Attitude mode gets skipped if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { - // if accessory0-3 is configured as a PID vario over the smooth to quick range + // 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/900 of full range - // 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 && CheckAccessoryForPidRequest(accessoryToUse)) { - if (accessoryToUse != -1 && systemIdentData.Complete && !CheckSettings()) { + // (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 }; -// static float accessoryValueOld = 0.0f; AccessoryDesiredData accessoryValue; -// float accessoryValue; AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); + // if the accessory changed more than 1/900 + // (this test is intended to remove one unit jitter) if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (1.0f/900.0f)) { accessoryValueOld = accessoryValue; ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f); @@ -486,155 +308,157 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) 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 - // that must wait until after a delay has passed to make sure they intended to stay in this mode - // is this a race? is it possible that flightStatus.FlightMode has been changed, but the stab bank hasn't been changed yet? - StabilizationBankRollMaxGet(&rollMax); - StabilizationBankPitchMaxGet(&pitchMax); - StabilizationBankManualRateGet(&manualRate); + // 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; - // Spend the first block of time in normal rate mode to get stabilized + // after a small delay, get the stab bank values and SystemIdentSettings in case they changed + // this is a very small delay, so fms toggle gets in here if (diffTime > INIT_TIME_DELAY_MS) { - // Only start when armed and flying - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { -// SystemSettingsAirframeTypeGet(&airframeType); - // Reset save status; only save if this tune completes. - saveSiNeeded = false; - savePidNeeded = false; -// systemIdentData.Complete = false; -//// lastUpdateTime = xTaskGetTickCount(); - InitSystemIdent(true); - AfInit(gX, gP); - UpdateSystemIdent(gX, NULL, 0.0f, 0, 0, 0.0f); - measureTime = (uint32_t)systemIdentData.TuningDuration * (uint32_t)1000; - state = AT_START; -#if 0 - lastUpdateTime = xTaskGetTickCount(); -#endif + // 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); + 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". + 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) { + // Only start when armed and flying + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { + // Reset save status + // save SI data even if partial or bad, aids in diagnostics + saveSiNeeded = true; + // don't save PIDs until data gathering is complete + // and the complete data has been sanity checked + savePidNeeded = false; + InitSystemIdent(true); + AfInit(gX, gP); + UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); + measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; + state = AT_START; + } } -//// lastUpdateTime = xTaskGetTickCount(); } break; case AT_START: -#if 0 - diffTime = xTaskGetTickCount() - lastUpdateTime; - // Spend the first block of time in normal rate mode to get stabilized - if (diffTime > START_TIME_DELAY_MS) { -#else - { -#endif - lastTime = PIOS_DELAY_GetRaw(); - /* Drain the queue of all current data */ -#if defined(USE_CIRC_QUEUE) - while (circ_queue_read_pos(atQueue)) { - circ_queue_read_completed(atQueue); - } -#else - xQueueReset(atQueue); -#endif - /* And reset the point spill counter */ - updateCounter = 0; - atPointsSpilled = 0; - throttleAccumulator = 0; - state = AT_RUN; - lastUpdateTime = xTaskGetTickCount(); - } + 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 0.010f) { dT_s = 0.010f; } - lastTime = DEREFERENCE(pt,raw_time); - AfPredict(gX, gP, DEREFERENCE(pt,u), DEREFERENCE(pt,y), dT_s, DEREFERENCE(pt,throttle)); + 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) * (DEREFERENCE(pt,y[j]) - gX[j]) * (DEREFERENCE(pt,y[j]) - gX[j]); + 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 * DEREFERENCE(pt,throttle); + // 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)) { + if (((updateCounter++) & 0xff) == 0) { float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f; - UpdateSystemIdent(gX, noise, dT_s, updateCounter, atPointsSpilled, hover_throttle); + UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hover_throttle); } -#if defined(USE_CIRC_QUEUE) - /* Free the buffer containing an AT point */ - circ_queue_read_completed(atQueue); -#endif } if (diffTime > measureTime) { // Move on to next state // permanent flag that AT is complete and PIDs can be calculated state = AT_FINISHED; -// lastUpdateTime = xTaskGetTickCount(); } break; case AT_FINISHED: - ; - float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f; - uint8_t failureBits; - UpdateSystemIdent(gX, noise, 0, updateCounter, atPointsSpilled, hover_throttle); - saveSiNeeded = true; - // data is bad if FC was disarmed at the time AT completed + // data is automatically considered bad if FC was disarmed at the time AT completed if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { - failureBits = CheckSettings(); - if (!failureBits) { + // always calculate and save PIDs if disabling sanity checks + if (!CheckSettings()) { ComputeStabilizationAndSetPids(); savePidNeeded = true; - systemIdentData.Complete = true; - } else { -//is this right - // default to disable PID changing with flight mode switch and accessory0-3 - accessoryToUse = -1; - flightModeSwitchTogglePosition = -1; -// systemIdentData.Complete = false; - // raise a warning + // 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 hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f; + UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hover_throttle); + SystemIdentSettingsSet(&systemIdentSettings); state = AT_WAITING; break; case AT_WAITING: default: - // maybe set an alarm to notify user that tuning is done + // after tuning, wait here till user switches to another flight mode + // or disarms break; } - // Update based on ManualControlCommand + // fly in Attitude mode or in SystemIdent mode UpdateStabilizationDesired(doingIdent); + if (canSleep) { vTaskDelay(YIELD_MS / portTICK_RATE_MS); } @@ -642,12 +466,11 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) } +// 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) { -#if defined(USE_CIRC_QUEUE) - struct at_queued_data *q_item; -#else static struct at_queued_data q_item; -#endif static bool last_sample_unpushed = false; GyroSensorData gyro; ActuatorDesiredData actuators; @@ -656,40 +479,29 @@ static void AtNewGyroData(UAVObjEvent * ev) { return; } - // object can and possibly will at times change asynchronously so must copy data here, with locking + // object will at times change asynchronously so must copy data here, with locking // and do it as soon as possible GyroSensorGet(&gyro); ActuatorDesiredGet(&actuators); if (last_sample_unpushed) { - /* Last time we were unable to advance the write pointer. + /* Last time we were unable to queue up the gyro data. * Try again, last chance! */ -#if defined(USE_CIRC_QUEUE) - if (circ_queue_advance_write(atQueue)) { -#else if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) { -#endif atPointsSpilled++; } } -#if defined(USE_CIRC_QUEUE) - q_item = circ_queue_cur_write_pos(atQueue); -#endif - DEREFERENCE(q_item,raw_time) = PIOS_DELAY_GetRaw(); - DEREFERENCE(q_item,y[0]) = gyro.x; - DEREFERENCE(q_item,y[1]) = gyro.y; - DEREFERENCE(q_item,y[2]) = gyro.z; - DEREFERENCE(q_item,u[0]) = actuators.Roll; - DEREFERENCE(q_item,u[1]) = actuators.Pitch; - DEREFERENCE(q_item,u[2]) = actuators.Yaw; - DEREFERENCE(q_item,throttle) = actuators.Thrust; + 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 defined(USE_CIRC_QUEUE) - if (circ_queue_advance_write(atQueue) != 0) { -#else if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) { -#endif last_sample_unpushed = true; } else { last_sample_unpushed = false; @@ -697,6 +509,11 @@ static void AtNewGyroData(UAVObjEvent * ev) { } +// 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; @@ -726,103 +543,100 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) { } +// 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) { - SystemIdentGet(&systemIdentData); - uint8_t smoothQuick = systemIdentData.SmoothQuick; + SystemIdentSettingsGet(&systemIdentSettings); + uint8_t smoothQuick = systemIdentSettings.SmoothQuick; if (loadDefaults) { - // these are values that could be changed by the user - // save them through the following xSetDefaults() call - uint8_t dampMin = systemIdentData.DampMin; - uint8_t dampRate = systemIdentData.DampRate; - uint8_t dampMax = systemIdentData.DampMax; - uint8_t noiseMin = systemIdentData.NoiseMin; - uint8_t noiseRate = systemIdentData.NoiseRate; - uint8_t noiseMax = systemIdentData.NoiseMax; - bool calcYaw = systemIdentData.CalculateYaw; - uint8_t destBank = systemIdentData.DestinationPidBank; - uint8_t tuningDuration = systemIdentData.TuningDuration; -// bool complete = systemIdentData.Complete; - // 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 - SystemIdentSetDefaults(SystemIdentHandle(), 0); - SystemIdentGet(&systemIdentData); - - // restore the user changeable values - systemIdentData.DampMin = dampMin; - systemIdentData.DampRate = dampRate; - systemIdentData.DampMax = dampMax; - systemIdentData.NoiseMin = noiseMin; - systemIdentData.NoiseRate = noiseRate; - systemIdentData.NoiseMax = noiseMax; - systemIdentData.CalculateYaw = calcYaw; - systemIdentData.DestinationPidBank = destBank; - systemIdentData.TuningDuration = tuningDuration; -// systemIdentData.Complete = complete; + 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; } // default to disable PID changing with flight mode switch and accessory0-3 accessoryToUse = -1; flightModeSwitchTogglePosition = -1; - systemIdentData.SmoothQuick = 0; + systemIdentSettings.SmoothQuick = SMOOTH_QUICK_DISABLED; switch (smoothQuick) { - case 10: // use accessory0 - case 11: // use accessory1 - case 12: // use accessory2 - case 13: // use accessory3 - accessoryToUse = smoothQuick - 10; - systemIdentData.SmoothQuick = smoothQuick; + 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 + accessoryToUse = smoothQuick - SMOOTH_QUICK_ACCESSORY_BASE; + systemIdentSettings.SmoothQuick = smoothQuick; break; - case 23: // use flight mode switch toggle with 3 points - case 25: // use flight mode switch toggle with 5 points + case SMOOTH_QUICK_TOGGLE_BASE+2: // use flight mode switch toggle with 3 points + case SMOOTH_QUICK_TOGGLE_BASE+4: // use flight mode switch toggle with 5 points // first test PID is in the middle of the smooth -> quick range flightModeSwitchTogglePosition = (smoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2; - systemIdentData.SmoothQuick = smoothQuick; + systemIdentSettings.SmoothQuick = smoothQuick; break; } } -static void UpdateSystemIdent(const float *X, const float *noise, +// 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) { - systemIdentData.Beta.Roll = X[6]; - systemIdentData.Beta.Pitch = X[7]; - systemIdentData.Beta.Yaw = X[8]; - systemIdentData.Bias.Roll = X[10]; - systemIdentData.Bias.Pitch = X[11]; - systemIdentData.Bias.Yaw = X[12]; - systemIdentData.Tau = X[9]; + 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) { - systemIdentData.Noise.Roll = noise[0]; - systemIdentData.Noise.Pitch = noise[1]; - systemIdentData.Noise.Yaw = noise[2]; + systemIdentState.Noise.Roll = noise[0]; + systemIdentState.Noise.Pitch = noise[1]; + systemIdentState.Noise.Yaw = noise[2]; } - systemIdentData.Period = dT_s * 1000.0f; + systemIdentState.Period = dT_s * 1000.0f; - systemIdentData.NumAfPredicts = predicts; - systemIdentData.NumSpilledPts = spills; + systemIdentState.NumAfPredicts = predicts; + systemIdentState.NumSpilledPts = spills; - systemIdentData.HoverThrottle = hover_throttle; + systemIdentState.HoverThrottle = hover_throttle; - SystemIdentSet(&systemIdentData); + 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; - // unneeded since we are setting everything in this uavo - //StabilizationDesiredGet(&stabDesired); + ManualControlCommandData manualControlCommand; + ManualControlCommandGet(&manualControlCommand); - ManualControlCommandData manual_control_command; - ManualControlCommandGet(&manual_control_command); - - stabDesired.Roll = manual_control_command.Roll * rollMax; - stabDesired.Pitch = manual_control_command.Pitch * pitchMax; - stabDesired.Yaw = manual_control_command.Yaw * manualRate.Yaw; - //stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle; - stabDesired.Thrust = manual_control_command.Thrust; + 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; @@ -835,47 +649,67 @@ static void UpdateStabilizationDesired(bool doingIdent) { } stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; -// is this a race -// control feels very sluggish too StabilizationDesiredSet(&stabDesired); } -static uint8_t CheckSettings() +// 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; -#if 1 // Check the axis gains // Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values. - if (systemIdentData.Beta.Roll < 6) { - retVal |= 1; + if (systemIdentState.Beta.Roll < 6) { + retVal |= ROLL_BETA_LOW; } - if (systemIdentData.Beta.Pitch < 6) { - retVal |= 2; + if (systemIdentState.Beta.Pitch < 6) { + retVal |= PITCH_BETA_LOW; + } + if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) { + retVal |= YAW_BETA_LOW; } - // Check the response speed // Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values. - if (expf(systemIdentData.Tau) > 0.1f) { - retVal |= 4; + 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(systemIdentData.Tau) < 0.008f) { - retVal |= 8; + else if (expf(systemIdentState.Tau) < 0.008f) { + retVal |= TAU_TOO_SHORT; } -#endif return retVal; } -#if 0 +// 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 == YAW_BETA_LOW) && (!systemIdentSettings.CalculateYaw || systemIdentSettings.OverrideYawBeta))) { + retVal = 0; + } + + 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) { StabilizationSettingsBank1Data stabSettingsBank; - switch (systemIdentData.DestinationPidBank) { + switch (systemIdentSettings.DestinationPidBank) { case 1: StabilizationSettingsBank1Get((void *)&stabSettingsBank); break; @@ -897,26 +731,15 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float const double ghf = (double) noiseRate / 1000.0d; const double damp = (double) dampRate / 100.0d; - double tau = exp(systemIdentData.Tau); -#if 0 - double beta_roll = systemIdentData.Beta.Roll; - double beta_pitch = systemIdentData.Beta.Pitch; - - 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)*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)*ghf); -#else - double exp_beta_roll_times_ghf = exp(systemIdentData.Beta.Roll)*ghf; - double exp_beta_pitch_times_ghf = exp(systemIdentData.Beta.Pitch)*ghf; + 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); -#endif // 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); @@ -934,10 +757,15 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float const double zeta_o = 1.3d; const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d/wn); - // For now just run over roll and pitch - int axes = ((systemIdentData.CalculateYaw) ? 3 : 2); - for (int i = 0; i < axes; i++) { - double beta = exp(SystemIdentBetaToArray(systemIdentData.Beta)[i]); + // dRonin simply uses default PID settings for yaw + for (int i = 0; i < ((systemIdentSettings.CalculateYaw) ? 3 : 2); i++) { + double beta; + // if yaw axis and yaw beta is too low and user wants to override it if too low + if (i == 2 && systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin && systemIdentSettings.OverrideYawBeta) { + beta = exp(systemIdentSettings.YawBetaMin); + } else { + beta = exp(SystemIdentStateBetaToArray(systemIdentState.Beta)[i]); + } double ki = a * b * wn * wn * tau * tau_d / beta; double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d; @@ -958,7 +786,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float stabSettingsBank.PitchPI.Kp = kp_o; stabSettingsBank.PitchPI.Ki = 0; break; - case 2: // optional Yaw + case 2: // Yaw stabSettingsBank.YawRatePID.Kp = kp; stabSettingsBank.YawRatePID.Ki = ki; stabSettingsBank.YawRatePID.Kd = kd; @@ -968,10 +796,11 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float } } - //stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d); + // Librepilot might do something with this some time + // stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d); - // Save PIDs to permanent settings - switch (systemIdentData.DestinationPidBank) { + // Save PIDs to UAVO RAM (not permanently yet) + switch (systemIdentSettings.DestinationPidBank) { case 1: StabilizationSettingsBank1Set((void *)&stabSettingsBank); break; @@ -983,154 +812,16 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float break; } } -#else -static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) -{ - StabilizationSettingsBank1Data stabSettingsBank; - -#if 0 -systemIdentData.Bias.Roll = dampRate; -systemIdentData.Bias.Pitch = noiseRate; -SystemIdentSet(&systemIdentData); -#endif - - switch (systemIdentData.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(systemIdentData.Tau); - { - double exp_beta_roll_times_ghf = exp(systemIdentData.Beta.Roll)*ghf; - double exp_beta_pitch_times_ghf = exp(systemIdentData.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); - - for (int i = 0; i < 2; i++) { - double beta = exp(SystemIdentBetaToArray(systemIdentData.Beta)[i]); - - double ki = a * b * wn * wn * tau * tau_d / beta; - double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d; - double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d; - - 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 = 0; - break; - case 1: // Pitch - stabSettingsBank.PitchRatePID.Kp = kp; - stabSettingsBank.PitchRatePID.Ki = ki; - stabSettingsBank.PitchRatePID.Kd = kd; - stabSettingsBank.PitchPI.Kp = kp_o; - stabSettingsBank.PitchPI.Ki = 0; - break; - } - } - } - - // do yaw if requested - if (systemIdentData.CalculateYaw) { - double exp_beta_yaw_times_ghf = exp(systemIdentData.Beta.Yaw)*ghf; - - double wn = 1.0d/tau; - double tau_d = 0.0d; - for (int i = 0; i < 30; i++) { - tau_d = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp_beta_yaw_times_ghf); - 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); - - double beta = exp(systemIdentData.Beta.Yaw); - - double ki = a * b * wn * wn * tau * tau_d / beta; - double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d; - double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d; - - // optional Yaw - stabSettingsBank.YawRatePID.Kp = kp; - stabSettingsBank.YawRatePID.Ki = ki; - stabSettingsBank.YawRatePID.Kd = kd; - stabSettingsBank.YawPI.Kp = kp_o; - stabSettingsBank.YawPI.Ki = 0; - } - - //stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d); - - // Save PIDs to permanent settings - switch (systemIdentData.DestinationPidBank) { - case 1: - StabilizationSettingsBank1Set((void *)&stabSettingsBank); - break; - case 2: - StabilizationSettingsBank2Set((void *)&stabSettingsBank); - break; - case 3: - StabilizationSettingsBank3Set((void *)&stabSettingsBank); - break; - } -} -#endif +// calculate PIDs using default smooth-quick settings static void ComputeStabilizationAndSetPids() { - ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentData.DampRate, systemIdentData.NoiseRate); + ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentSettings.DampRate, systemIdentSettings.NoiseRate); } -// scale damp and noise to generate PIDs according to how a slider or other ratio is set +// 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 @@ -1141,12 +832,6 @@ static void ComputeStabilizationAndSetPids() // // this is done piecewise because we are not guaranteed that default-min == max-default // but we are given that [smoothDamp,smoothNoise] [defaultDamp,defaultNoise] [quickDamp,quickNoise] are all good parameterizations -#define dampMin systemIdentData.DampMin -#define dampDefault systemIdentData.DampRate -#define dampMax systemIdentData.DampMax -#define noiseMin systemIdentData.NoiseMin -#define noiseDefault systemIdentData.NoiseRate -#define noiseMax systemIdentData.NoiseMax static void ProportionPidsSmoothToQuick(float min, float val, float max) { float ratio, damp, noise; @@ -1160,13 +845,13 @@ static void ProportionPidsSmoothToQuick(float min, float val, float max) if (ratio <= 0.5f) { // scale ratio in [0,0.5] to produce PIDs in [smoothest,default] ratio *= 2.0f; - damp = (dampMax * (1.0f - ratio)) + (dampDefault * ratio); - noise = (noiseMin * (1.0f - ratio)) + (noiseDefault * ratio); + 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 = (dampDefault * (1.0f - ratio)) + (dampMin * ratio); - noise = (noiseDefault * (1.0f - ratio)) + (noiseMax * ratio); + damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio); + noise = (systemIdentSettings.NoiseRate * (1.0f - ratio)) + (systemIdentSettings.NoiseMax * ratio); } ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise); @@ -1204,7 +889,7 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl 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 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 @@ -1248,16 +933,186 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl const float Ts_e_tau2 = (Ts + e_tau) * (Ts + e_tau); const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2; +#if 0 // 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[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; +#endif + // 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]; @@ -1396,45 +1251,20 @@ static void AfInit(float X[AF_NUMX], float P[AF_NUMP]) // 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; // medium amount of strength - // X[8] = 7.0f; // yaw + // 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 -// lets not set SystemIdent to defaults at all -// that way the user can run it a second time, with initial values from the first run -#if 0 - // these are values that could be changed by the user - // save them through the following xSetDefaults() call - uint8_t damp = systemIdentData.DampRate; - uint8_t noise = systemIdentData.NoiseRate; - bool yaw = systemIdentData.CalculateYaw; - uint8_t bank = systemIdentData.DestinationPidBank; - + 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 - memset(X, 0, AF_NUMX*sizeof(X[0])); - SystemIdentSetDefaults(SystemIdentHandle(), 0); - SystemIdentBetaArrayGet(&X[6]); - SystemIdentTauGet(&X[9]); - - // restore the user changeable values - systemIdentData.DampRate = damp; - systemIdentData.NoiseRate = noise; - systemIdentData.CalculateYaw = yaw; - systemIdentData.DestinationPidBank = bank; -#else - // 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 - memset(X, 0, AF_NUMX*sizeof(X[0])); //SystemIdentSetDefaults(SystemIdentHandle(), 0); //SystemIdentBetaArrayGet(&X[6]); - memcpy(&X[6], &systemIdentData.Beta, sizeof(systemIdentData.Beta)); + memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta)); //SystemIdentTauGet(&X[9]); - X[9] = systemIdentData.Tau; -#endif + X[9] = systemIdentState.Tau; // P initialization - // Could zero this like: *P = *((float [AF_NUMP]){}); memset(P, 0, AF_NUMP*sizeof(P[0])); P[0] = qInit[0]; P[1] = qInit[1]; 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/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index ce94846d5..7f77eedf6 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -47,7 +47,6 @@ #include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include -#include #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ // Private constants @@ -119,9 +118,6 @@ 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); -#if 0 -static bool isSystemIdentFlightMode(uint8_t flightMode, FlightModeSettingsData *modeSettings); -#endif #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ static void SettingsUpdatedCb(UAVObjEvent *ev); @@ -255,13 +251,6 @@ static void manualControlTask(void) // check the flightmodeassist state. newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY; newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; -#if 0 -#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) - if (isSystemIdentFlightMode(newMode, &modeSettings)) { - SystemIdentInitData(); - } -#endif -#endif } // Depending on the mode update the Stabilization or Actuator objects @@ -623,52 +612,6 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight //return isAssistedFlag; return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE; } - -#if 0 -/** - * Check if this flight mode uses SystemIdent stabilization mode - */ -static bool isSystemIdentFlightMode(uint8_t flightMode, FlightModeSettingsData *modeSettings) -{ -#if 0 - if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { - return true; - } - if (flightMode >= FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 && flightMode <= FLIGHTSTATUS_FLIGHTMODE_STABILIZED6) { - if (modeSettings->Stabilization1Settings.Roll == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT - || modeSettings->Stabilization1Settings.Pitch == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT - || modeSettings->Stabilization1Settings.Yaw == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) { - return true; - } - } - return false; -#else - if ( - flightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE && ( - ( - flightMode < FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 || flightMode > FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 - ) || ( - modeSettings->Stabilization1Settings.Roll != FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT - && modeSettings->Stabilization1Settings.Pitch != FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT - && modeSettings->Stabilization1Settings.Yaw != FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT - ) - ) - ) { - return false; - } -#if 1 - static bool inited=false; - if (!inited) { - inited = true; -#else - if (!SystemIdentHandle()) { -#endif /* 1 */ - SystemIdentInitialize(); - } - return true; -#endif /* 0 */ -} -#endif /* 0 */ #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ /** diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index f7e4132f3..560c0b827 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -36,7 +36,6 @@ #include #include #include -#include // Private constants @@ -49,24 +48,7 @@ static float applyExpo(float value, float expo); static uint8_t currentFpvTiltAngle = 0; static float cosAngle = 0.0f; static float sinAngle = 0.0f; -#if 0 -#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) -static FlightModeSettingsStabilization1SettingsData autotuneSettings = { - .Roll = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT, - .Pitch = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT, - .Yaw = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT, - .Thrust = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL -}; -#if 0 -static FlightModeSettingsStabilization1SettingsData attitudeSettings = { - .Roll = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE, - .Pitch = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE, - .Yaw = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE, - .Thrust = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL -}; -#endif -#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ -#endif + static float applyExpo(float value, float expo) { @@ -160,18 +142,9 @@ void stabilizedHandler(__attribute__((unused)) bool newinit) break; #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: -#if 0 -// if (SystemIdentHandle()) { - stab_settings = (uint8_t *)&autotuneSettings; -// } else { -// stab_settings = (uint8_t *)&attitudeSettings; -// } - break; -#else // let autotune.c handle it // because it must switch to Attitude after seconds return; -#endif #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ default: // Major error, this should not occur because only enter this block when one of these is true diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index f396bf551..2cd2385f6 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -52,7 +52,7 @@ #include #include #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) -#include +#include #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ // Private constants @@ -84,7 +84,7 @@ static float speedScaleFactor = 1.0f; static bool frame_is_multirotor; static bool measuredDterm_enabled; #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) -static uint32_t system_ident_timeval = 0; +static uint32_t systemIdentTimeVal = 0; #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ // Private functions @@ -105,7 +105,7 @@ void stabilizationInnerloopInit() StabilizationDesiredInitialize(); ActuatorDesiredInitialize(); #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) - SystemIdentInitialize(); + SystemIdentStateInitialize(); #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ #ifdef REVOLUTION AirspeedStateInitialize(); @@ -123,7 +123,7 @@ void stabilizationInnerloopInit() measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) // Settings for system identification - system_ident_timeval = PIOS_DELAY_GetRaw(); + systemIdentTimeVal = PIOS_DELAY_GetRaw(); #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ } @@ -342,22 +342,21 @@ static void stabilizationInnerloopTask() #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT: { - static uint32_t ident_iteration = 0; - static float ident_offsets[3] = {0}; + static int8_t identIteration = 0; + static float identOffsets[3] = {0}; -/////////////// if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) { - if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD) { - system_ident_timeval = PIOS_DELAY_GetRaw(); + if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) { + systemIdentTimeVal = PIOS_DELAY_GetRaw(); - SystemIdentData systemIdent; - SystemIdentGet(&systemIdent); -// original code -#if 1 - const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */ - float roll_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Roll); - float pitch_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Pitch); - float yaw_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Yaw); - ident_iteration++; + SystemIdentStateData systemIdentState; + SystemIdentStateGet(&systemIdentState); +// original code used 32 bit identIteration +#if 0 + const float SCALE_BIAS = 7.1f; + float roll_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Roll); + float pitch_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch); + float yaw_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Yaw); + identIteration++; if (roll_scale > 0.25f) roll_scale = 0.25f; @@ -366,96 +365,47 @@ static void stabilizationInnerloopTask() if (yaw_scale > 0.25f) yaw_scale = 0.25f; -//why did he do this fsm? -//with yaw changing twice a cycle and roll/pitch changing once? - switch(ident_iteration & 0x07) { + // yaw changes twice a cycle and roll/pitch changes once ? + switch(identIteration & 0x07) { case 0: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = yaw_scale; + identOffsets[0] = 0; + identOffsets[1] = 0; + identOffsets[2] = yaw_scale; break; case 1: - ident_offsets[0] = roll_scale; - ident_offsets[1] = 0; - ident_offsets[2] = 0; + identOffsets[0] = roll_scale; + identOffsets[1] = 0; + identOffsets[2] = 0; break; case 2: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = -yaw_scale; + identOffsets[0] = 0; + identOffsets[1] = 0; + identOffsets[2] = -yaw_scale; break; case 3: - ident_offsets[0] = -roll_scale; - ident_offsets[1] = 0; - ident_offsets[2] = 0; + identOffsets[0] = -roll_scale; + identOffsets[1] = 0; + identOffsets[2] = 0; break; case 4: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = yaw_scale; + identOffsets[0] = 0; + identOffsets[1] = 0; + identOffsets[2] = yaw_scale; break; case 5: - ident_offsets[0] = 0; - ident_offsets[1] = pitch_scale; - ident_offsets[2] = 0; + identOffsets[0] = 0; + identOffsets[1] = pitch_scale; + identOffsets[2] = 0; break; case 6: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = -yaw_scale; + identOffsets[0] = 0; + identOffsets[1] = 0; + identOffsets[2] = -yaw_scale; break; case 7: - ident_offsets[0] = 0; - ident_offsets[1] = -pitch_scale; - ident_offsets[2] = 0; - break; - } -#endif - -// old partially completed good stuff -#if 0 - const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */ - float scale[3] = { expapprox(SCALE_BIAS - systemIdent.Beta.Roll), - expapprox(SCALE_BIAS - systemIdent.Beta.Pitch), - expapprox(SCALE_BIAS - systemIdent.Beta.Yaw) }; - ident_iteration++; - - if (scale[0] > 0.25f) - scale[0] = 0.25f; - if (scale[1] > 0.25f) - scale[1] = 0.25f; - if (scale[2] > 0.25f) - scale[2] = 0.25f; - -//why did he do this fsm? -//with yaw changing twice a cycle and roll/pitch changing once? - ident_offsets[0] = 0.0f; - ident_offsets[1] = 0.0f; - ident_offsets[2] = 0.0f; - switch(ident_iteration & 7) { - case 0: - ident_offsets[2] = scale[2]; - break; - case 1: - ident_offsets[0] = scale[0]; - break; - case 2: - ident_offsets[2] = -scale[2]; - break; - case 3: - ident_offsets[0] = -scale[0]; - break; - case 4: - ident_offsets[2] = scale[2]; - break; - case 5: - ident_offsets[1] = scale[1]; - break; - case 6: - ident_offsets[2] = -scale[2]; - break; - case 7: - ident_offsets[1] = -scale[1]; + identOffsets[0] = 0; + identOffsets[1] = -pitch_scale; + identOffsets[2] = 0; break; } #endif @@ -463,9 +413,9 @@ static void stabilizationInnerloopTask() //good stuff here #if 0 const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */ - float scale[3] = { expapprox(SCALE_BIAS - systemIdent.Beta.Roll), - expapprox(SCALE_BIAS - systemIdent.Beta.Pitch), - expapprox(SCALE_BIAS - systemIdent.Beta.Yaw) }; + float scale[3] = { expapprox(SCALE_BIAS - systemIdentState.Beta.Roll), + expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch), + expapprox(SCALE_BIAS - systemIdentState.Beta.Yaw) }; if (scale[0] > 0.25f) scale[0] = 0.25f; @@ -476,110 +426,214 @@ static void stabilizationInnerloopTask() //why did he do this fsm? //with yaw changing twice a cycle and roll/pitch changing once? - ident_offsets[0] = 0.0f; - ident_offsets[1] = 0.0f; - ident_offsets[2] = 0.0f; - ident_iteration = (ident_iteration+1) & 7; - uint8_t index = ((uint8_t *) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [ident_iteration]; - // if (ident_iteration & 2) scale[index] = -scale[index]; - ((uint8_t *)(&scale[index]))[3] ^= (ident_iteration & 2) << 6; - ident_offsets[index] = scale[index]; -#if 0 - switch(ident_iteration) { - case 0: - ident_offsets[2] = scale[2]; - break; - case 1: - ident_offsets[0] = scale[0]; - break; - case 2: - ident_offsets[2] = -scale[2]; - break; - case 3: - ident_offsets[0] = -scale[0]; - break; - case 4: - ident_offsets[2] = scale[2]; - break; - case 5: - ident_offsets[1] = scale[1]; - break; - case 6: - ident_offsets[2] = -scale[2]; - break; - case 7: - ident_offsets[1] = -scale[1]; - break; - } -#endif + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration+1) & 7; + uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration]; + // if (identIteration & 2) scale[index] = -scale[index]; + ((uint8_t *)(&scale[index]))[3] ^= (identIteration & 2) << 6; + identOffsets[index] = scale[index]; #endif -//aborted mod 9 +// same as stock +#if 1 + const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */ + // why does yaw change twice a cycle and roll/pitch change only once? + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration+1) & 7; + uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration]; + float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]); + if (scale > 0.25f) { + scale = 0.25f; + } + 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 +#endif + +// since we are not calculating yaw, remove it and test roll/pitch more frequently +// perhaps this will converge faster #if 0 const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */ - float roll_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Roll); - float pitch_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Pitch); - float yaw_scale = expapprox(SCALE_BIAS - systemIdent.Beta.Yaw); - ident_iteration++; - - if (roll_scale > 0.25f) - roll_scale = 0.25f; - if (pitch_scale > 0.25f) - pitch_scale = 0.25f; - if (yaw_scale > 0.25f) - yaw_scale = 0.25f; - -//why did he do this fsm? -//with yaw changing twice a cycle and roll/pitch changing once? - switch(ident_iteration % 9) { - case 0: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = yaw_scale; - break; - case 1: - ident_offsets[0] = roll_scale; - ident_offsets[1] = 0; - ident_offsets[2] = 0; - break; - case 2: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = -yaw_scale; - break; - case 3: - ident_offsets[0] = -roll_scale; - ident_offsets[1] = 0; - ident_offsets[2] = 0; - break; - case 4: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = yaw_scale; - break; - case 5: - ident_offsets[0] = 0; - ident_offsets[1] = pitch_scale; - ident_offsets[2] = 0; - break; - case 6: - ident_offsets[0] = 0; - ident_offsets[1] = 0; - ident_offsets[2] = -yaw_scale; - break; - case 7: - ident_offsets[0] = 0; - ident_offsets[1] = -pitch_scale; - ident_offsets[2] = 0; - break; - case 8: - ident_offsets[0] = 0; - ident_offsets[1] = -pitch_scale; - ident_offsets[2] = 0; - break; + // why does yaw change twice a cycle and roll/pitch change only once? + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration+1) & 3; + uint8_t index = ((uint8_t []) { '\0', '\0', '\1', '\1' } ) [identIteration]; + float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]); + if (scale > 0.25f) { + scale = 0.25f; } + if (identIteration & 2) { + scale = -scale; + } + identOffsets[index] = scale; + // this results in: + // when identIteration==0: identOffsets[0] = roll_scale; + // when identIteration==1: identOffsets[1] = pitch_scale; + // when identIteration==2: identOffsets[0] = -roll_scale; + // when identIteration==3: identOffsets[1] = -pitch_scale; + // each change has one axis with an offset + // and another axis coming back to zero from having an offset #endif +// since we are not calculating yaw, remove it +// for a cleaner roll / pitch signal +#if 0 + const float SCALE_BIAS = 7.1f; + // why does yaw change twice a cycle and roll/pitch change only once? + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration+1) & 7; + uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration]; +//recode to this uint8_t index = identIteration >> 2; + if (identIteration & 1) { + float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]); + if (scale > 0.25f) { + scale = 0.25f; + } + if (identIteration & 2) { + scale = -scale; + } + identOffsets[index] = scale; + } + // this results in: + // when identIteration==0: no offset + // when identIteration==1: identOffsets[0] = roll_scale; + // when identIteration==2: no offset + // when identIteration==3: identOffsets[0] = -roll_scale; + // when identIteration==4: no offset + // when identIteration==5: identOffsets[1] = pitch_scale; + // when identIteration==6: no offset + // when identIteration==7: identOffsets[1] = -pitch_scale; + // each change is either one axis with an offset + // or one axis coming back to zero from having an offset +#endif + +// since we are not calculating yaw, remove it +// for a cleaner roll / pitch signal +#if 0 + const float SCALE_BIAS = 7.1f; + // why does yaw change twice a cycle and roll/pitch change only once? + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration+1) % 12; +// uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration]; +//recode to this uint8_t index = identIteration >> 2; +#if 0 + if (identIteration < 5) { + index = 0; + } else { + index = 1; + } +#endif + uint8_t index = identIteration % 6 / 3; + uint8_t identIterationMod3 = identIteration % 3; + if (identIterationMod3 <= 1) { + float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]); + if (scale > 0.25f) { + scale = 0.25f; + } + if ((identIterationMod3 == 1) ^ (identIteration >= 6)) { + scale = -scale; + } + identOffsets[index] = scale; + } + // this results in: + // when identIteration== 0: identOffsets[0] = roll_scale; + // when identIteration== 1: identOffsets[0] = -roll_scale; + // when identIteration== 2: no offset + // when identIteration== 3: identOffsets[1] = pitch_scale; + // when identIteration== 4: identOffsets[1] = -pitch_scale; + // when identIteration== 5: no offset + // when identIteration== 6: identOffsets[0] = -roll_scale; + // when identIteration== 7: identOffsets[0] = roll_scale; + // when identIteration== 8: no offset + // when identIteration== 9: identOffsets[1] = -pitch_scale; + // when identIteration==10: identOffsets[1] = pitch_scale; + // when identIteration==11: no offset + // + // each change is either an axis going from zero to +-scale + // or going from +-scale to -+scale + // there is a delay when changing axes + // + // it's not clear whether AfPredict() is designed to handle double scale perturbances on a particular axis + // resulting from -offset to +offset and needs -offset to zero to +offset + // as an EKF it should handle it +#endif + +// one axis at a time +// full stroke with delay between axes +// for a cleaner signal +// a little more difficult to fly? +// makes slightly lower PIDs +// yaw pids seem way high and incorrect +#if 0 + const float SCALE_BIAS = 7.1f; + // why does yaw change twice a cycle and roll/pitch change only once? + identOffsets[0] = 0.0f; + identOffsets[1] = 0.0f; + identOffsets[2] = 0.0f; + identIteration = (identIteration+1) % 18; + uint8_t index = identIteration % 9 / 3; + uint8_t identIterationMod3 = identIteration % 3; +// if (identIterationMod3 <= 1) { + { + float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]); + if (scale > 0.25f) { + scale = 0.25f; + } + if ((identIterationMod3 == 1) ^ (identIteration >= 9)) { + scale = -scale; + } + identOffsets[index] = scale; + } + // this results in: + // when identIteration== 0: identOffsets[0] = roll_scale; + // when identIteration== 1: identOffsets[0] = -roll_scale; + // when identIteration== 2: no offset + // when identIteration== 3: identOffsets[1] = pitch_scale; + // when identIteration== 4: identOffsets[1] = -pitch_scale; + // when identIteration== 5: no offset + // when identIteration== 6: identOffsets[2] = yaw_scale; + // when identIteration== 7: identOffsets[2] = -yaw_scale; + // when identIteration== 8: no offset + // when identIteration== 9: identOffsets[0] = -roll_scale; + // when identIteration==10: identOffsets[0] = roll_scale; + // when identIteration==11: no offset + // when identIteration==12: identOffsets[1] = -pitch_scale; + // when identIteration==13: identOffsets[1] = pitch_scale; + // when identIteration==14: no offset + // when identIteration==15: identOffsets[2] = -yaw_scale; + // when identIteration==16: identOffsets[2] = yaw_scale; + // when identIteration==17: no offset + // + // each change is either an axis going from zero to +-scale + // or going from +-scale to -+scale + // there is a delay when changing axes + // + // it's not clear whether AfPredict() is designed to handle 2x scale perturbations on a particular axis + // resulting from -offset to +offset and instead needs -offset to zero to +offset + // ... as an EKF it should handle it +#endif } rate[t] = boundf(rate[t], @@ -588,7 +642,7 @@ 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); - actuatorDesiredAxis[t] += ident_offsets[t]; + actuatorDesiredAxis[t] += identOffsets[t]; // we shouldn't do any clamping until after the motors are calculated and scaled? //actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f); } diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index ae6cb1663..d94da6385 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -157,8 +157,8 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e // 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 -// default: #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/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 02940b780..4780f4453 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -125,7 +125,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter -UAVOBJSRCFILENAMES += systemident +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/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index a10cbd5ec..191f50460 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -125,7 +125,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter -UAVOBJSRCFILENAMES += systemident +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/UAVObjects.inc b/flight/targets/boards/revonano/firmware/UAVObjects.inc index a10cbd5ec..191f50460 100644 --- a/flight/targets/boards/revonano/firmware/UAVObjects.inc +++ b/flight/targets/boards/revonano/firmware/UAVObjects.inc @@ -125,7 +125,8 @@ UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter -UAVOBJSRCFILENAMES += systemident +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/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 6a98d3bd9..af257283d 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -126,7 +126,8 @@ UAVOBJSRCFILENAMES += txpidstatus UAVOBJSRCFILENAMES += takeofflocation # this was missing when systemident was added # UAVOBJSRCFILENAMES += perfcounter -UAVOBJSRCFILENAMES += systemident +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/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index a9e2b9872..e9d21d659 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -121,7 +121,8 @@ UAVOBJSRCFILENAMES += ekfstatevariance UAVOBJSRCFILENAMES += takeofflocation # this was missing when systemident was added # UAVOBJSRCFILENAMES += perfcounter -UAVOBJSRCFILENAMES += systemident +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 1553dfb1f..92be03832 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -137,7 +137,8 @@ UAVOBJS = \ $${UAVOBJ_XML_DIR}/statusvtolautotakeoff.xml \ $${UAVOBJ_XML_DIR}/statusvtolland.xml \ $${UAVOBJ_XML_DIR}/systemalarms.xml \ - $${UAVOBJ_XML_DIR}/systemident.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/systemident.xml b/shared/uavobjectdefinition/systemidentsettings.xml similarity index 69% rename from shared/uavobjectdefinition/systemident.xml rename to shared/uavobjectdefinition/systemidentsettings.xml index 5e05b9da3..884606fec 100644 --- a/shared/uavobjectdefinition/systemident.xml +++ b/shared/uavobjectdefinition/systemidentsettings.xml @@ -1,15 +1,9 @@ - + The input to the PID tuning. - - - - - - @@ -23,20 +17,24 @@ - + + + - - - + + + + + - + 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. + + + + + + + + + + + + + + + +