mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-19 04:52:12 +01:00
Merged in TheOtherCliff/librepilot/theothercliff/LP-340_AutoTune_fix_some_time_measurement_issues_in_original_code (pull request #282)
LP-340 Autotune fix some time measurement issues in original code
This commit is contained in:
Alessio Morale
commit
6b1f4e52be
@ -67,7 +67,8 @@
|
||||
|
||||
// Private constants
|
||||
#undef STACK_SIZE_BYTES
|
||||
// Pull Request version tested on Nano. 120 bytes of stack left when configured with 1340
|
||||
// Pull Request version tested on Sparky2. 292 bytes of stack left when configured with 1340
|
||||
// Beware that Nano needs 156 bytes more stack than Sparky2
|
||||
#define STACK_SIZE_BYTES 1340
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
|
||||
|
||||
@ -78,6 +79,7 @@
|
||||
#define AT_QUEUE_NUMELEM 18
|
||||
#endif
|
||||
|
||||
#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */
|
||||
#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */
|
||||
#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */
|
||||
#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */
|
||||
@ -90,11 +92,14 @@
|
||||
#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */
|
||||
|
||||
// CheckSettings() returned error bits
|
||||
#define ROLL_BETA_LOW 1
|
||||
#define PITCH_BETA_LOW 2
|
||||
#define YAW_BETA_LOW 4
|
||||
#define TAU_TOO_LONG 8
|
||||
#define TAU_TOO_SHORT 16
|
||||
#define TAU_NAN 1
|
||||
#define BETA_NAN 2
|
||||
#define ROLL_BETA_LOW 4
|
||||
#define PITCH_BETA_LOW 8
|
||||
#define YAW_BETA_LOW 16
|
||||
#define TAU_TOO_LONG 32
|
||||
#define TAU_TOO_SHORT 64
|
||||
#define CPU_TOO_SLOW 128
|
||||
|
||||
// smooth-quick modes
|
||||
#define SMOOTH_QUICK_DISABLED 0
|
||||
@ -104,47 +109,54 @@
|
||||
|
||||
// Private types
|
||||
enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING };
|
||||
|
||||
struct at_queued_data {
|
||||
float y[3]; /* Gyro measurements */
|
||||
float u[3]; /* Actuator desired */
|
||||
float throttle; /* Throttle desired */
|
||||
|
||||
uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */
|
||||
float y[3]; /* Gyro measurements */
|
||||
float u[3]; /* Actuator desired */
|
||||
float throttle; /* Throttle desired */
|
||||
uint32_t gyroStateCallbackTimestamp; /* PIOS_DELAY_GetRaw() time of GyroState callback */
|
||||
uint32_t sensorReadTimestamp; /* PIOS_DELAY_GetRaw() time of sensor read */
|
||||
};
|
||||
|
||||
|
||||
// Private variables
|
||||
static SystemIdentSettingsData systemIdentSettings;
|
||||
// save memory because metadata is only briefly accessed, when normal data struct is not being used
|
||||
// unnamed union issues a warning
|
||||
static union {
|
||||
SystemIdentStateData systemIdentState;
|
||||
UAVObjMetadata systemIdentStateMetaData;
|
||||
} u;
|
||||
static StabilizationBankManualRateData manualRate;
|
||||
static xTaskHandle taskHandle;
|
||||
static bool moduleEnabled;
|
||||
static xQueueHandle atQueue;
|
||||
static float gX[AF_NUMX] = { 0 };
|
||||
static float gP[AF_NUMP] = { 0 };
|
||||
static float gyroReadTimeAverage;
|
||||
static float gyroReadTimeAverageAlpha;
|
||||
static float gyroReadTimeAverageAlphaAlpha;
|
||||
static float alpha;
|
||||
static float smoothQuickValue;
|
||||
static volatile uint32_t atPointsSpilled;
|
||||
static uint32_t throttleAccumulator;
|
||||
static uint8_t rollMax, pitchMax;
|
||||
static StabilizationBankManualRateData manualRate;
|
||||
static float gX[AF_NUMX] = { 0 };
|
||||
static float gP[AF_NUMP] = { 0 };
|
||||
static SystemIdentSettingsData systemIdentSettings;
|
||||
static SystemIdentStateData systemIdentState;
|
||||
static int8_t accessoryToUse;
|
||||
static int8_t flightModeSwitchTogglePosition;
|
||||
static bool moduleEnabled;
|
||||
|
||||
|
||||
// Private functions
|
||||
static void AutoTuneTask(void *parameters);
|
||||
static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in);
|
||||
static void AfInit(float X[AF_NUMX], float P[AF_NUMP]);
|
||||
static uint8_t CheckSettingsRaw();
|
||||
static uint8_t CheckSettings();
|
||||
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise);
|
||||
static void ComputeStabilizationAndSetPids();
|
||||
static void ProportionPidsSmoothToQuick(float min, float val, float max);
|
||||
static void AtNewGyroData(UAVObjEvent *ev);
|
||||
static void AutoTuneTask(void *parameters);
|
||||
static void AfInit(float X[AF_NUMX], float P[AF_NUMP]);
|
||||
static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in);
|
||||
static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
|
||||
static uint8_t CheckSettings();
|
||||
static uint8_t CheckSettingsRaw();
|
||||
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise);
|
||||
static void InitSystemIdent(bool loadDefaults);
|
||||
static void ProportionPidsSmoothToQuick();
|
||||
static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle);
|
||||
static void UpdateStabilizationDesired(bool doingIdent);
|
||||
static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
|
||||
static void InitSystemIdent(bool loadDefaults);
|
||||
static void InitSmoothQuick(bool loadToggle);
|
||||
|
||||
|
||||
/**
|
||||
@ -202,9 +214,6 @@ int32_t AutoTuneStart(void)
|
||||
{
|
||||
// Start main task if it is enabled
|
||||
if (moduleEnabled) {
|
||||
// taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
|
||||
// TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
|
||||
// PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
|
||||
GyroStateConnectCallback(AtNewGyroData);
|
||||
xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
|
||||
@ -221,14 +230,20 @@ MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart);
|
||||
*/
|
||||
static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
enum AUTOTUNE_STATE state = AT_INIT;
|
||||
float noise[3] = { 0 };
|
||||
float dT_s = 0.0f;
|
||||
uint32_t lastUpdateTime = 0; // initialization is only for compiler warning
|
||||
float noise[3] = { 0 };
|
||||
uint32_t lastTime = 0.0f;
|
||||
uint32_t measureTime = 0;
|
||||
uint32_t updateCounter = 0;
|
||||
bool saveSiNeeded = false;
|
||||
bool savePidNeeded = false;
|
||||
uint32_t lastTime = 0;
|
||||
uint32_t measureTime = 0;
|
||||
uint32_t updateCounter = 0;
|
||||
enum AUTOTUNE_STATE state = AT_INIT;
|
||||
bool saveSiNeeded = false;
|
||||
bool savePidNeeded = false;
|
||||
|
||||
// wait for the accessory values to stabilize
|
||||
// otherwise they come up as zero, then change to their real value
|
||||
// and that causes the PIDs to be re-exported (if smoothquick is active), which the user may not want
|
||||
vTaskDelay(TASK_STARTUP_DELAY_MS / portTICK_RATE_MS);
|
||||
|
||||
// get max attitude / max rate
|
||||
// for use in generating Attitude mode commands from this module
|
||||
@ -238,9 +253,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
StabilizationBankManualRateGet(&manualRate);
|
||||
// correctly set accessoryToUse and flightModeSwitchTogglePosition
|
||||
// based on what is in SystemIdent
|
||||
// so that the user can use the PID smooth->quick slider in following flights
|
||||
// so that the user can use the PID smooth->quick slider in flights following the autotune flight
|
||||
InitSystemIdent(false);
|
||||
InitSmoothQuick(true);
|
||||
smoothQuickValue = systemIdentSettings.SmoothQuickValue;
|
||||
|
||||
while (1) {
|
||||
uint32_t diffTime;
|
||||
@ -249,9 +264,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
// I have never seen this module misbehave in a way that requires a watchdog, so not bothering making one
|
||||
// PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
|
||||
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
|
||||
if (saveSiNeeded) {
|
||||
saveSiNeeded = false;
|
||||
@ -284,21 +296,23 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
&& systemIdentSettings.Complete && !CheckSettings()) {
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
|
||||
// if user toggled while armed set PID's to next in sequence
|
||||
// 2,3,4,0,1 (5 positions) or 1,2,0 (3 positions) or 3,4,5,6,0,1,2 (7 positions)
|
||||
// if you assume that smoothest is -100 and quickest is +100
|
||||
// this corresponds to 0,+50,+100,-100,-50 (for 5 position toggle)
|
||||
++flightModeSwitchTogglePosition;
|
||||
if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) {
|
||||
flightModeSwitchTogglePosition = 0;
|
||||
// if you assume that smoothest is -1 and quickest is +1
|
||||
// this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle)
|
||||
smoothQuickValue += 1.0f / (float)flightModeSwitchTogglePosition;
|
||||
if (smoothQuickValue > 1.001f) {
|
||||
smoothQuickValue = -1.0f;
|
||||
}
|
||||
} else {
|
||||
// if they did it disarmed, then set PID's back to AutoTune default
|
||||
flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
|
||||
// if they did the 3x FMS toggle while disarmed, set PID's back to the middle of smoothquick
|
||||
smoothQuickValue = 0.0f;
|
||||
}
|
||||
ProportionPidsSmoothToQuick(0.0f,
|
||||
(float)flightModeSwitchTogglePosition,
|
||||
(float)(systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE));
|
||||
// calculate PIDs based on new smoothQuickValue and save to the PID bank
|
||||
ProportionPidsSmoothToQuick();
|
||||
// save new PIDs permanently when / if disarmed
|
||||
savePidNeeded = true;
|
||||
// we also save the new knob/toggle value for startup next time
|
||||
// this keeps the PIDs in sync with the toggle position
|
||||
saveSiNeeded = true;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////
|
||||
@ -321,43 +335,35 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
// - the state machine needs to be reset
|
||||
// - the local version of Attitude mode gets skipped
|
||||
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
|
||||
static bool savePidActive = false;
|
||||
// if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range
|
||||
// and FC is not currently running autotune
|
||||
// and accessory0-3 changed by at least 1/160 of full range (2)
|
||||
// and accessory0-3 changed by at least 1/85 of full range (2)
|
||||
// (don't bother checking to see if the requested accessory# is configured properly
|
||||
// if it isn't, the value will be 0 which is the center of [-1,1] anyway)
|
||||
if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) {
|
||||
static AccessoryDesiredData accessoryValueOld = { 0.0f };
|
||||
AccessoryDesiredData accessoryValue;
|
||||
AccessoryDesiredInstGet(accessoryToUse, &accessoryValue);
|
||||
// if the accessory changed more than 1/80
|
||||
// (this test is intended to remove one unit jitter)
|
||||
// some old PPM receivers use a low resolution chip which only allows about 180 steps
|
||||
// if the accessory changed more than some percent of total range
|
||||
// some old PPM receivers use a low resolution chip which only allows about 180 steps out of a range of 2.0
|
||||
// a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0
|
||||
// what we are doing here does not need any higher precision than that
|
||||
if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) {
|
||||
accessoryValueOld = accessoryValue;
|
||||
// this copies the PIDs to memory and makes them active
|
||||
// but does not write them to permanent storage
|
||||
ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f);
|
||||
// don't save PID to perm storage the first time
|
||||
// that usually means at power up
|
||||
//
|
||||
// that keeps it from writing the same value at each boot
|
||||
// but means that it won't be permanent if they move the slider before FC power on
|
||||
// (note that the PIDs will be changed, just not saved permanently)
|
||||
if (savePidActive) {
|
||||
// this schedules the first possible write of the PIDs to occur a fraction of a second or so from now
|
||||
// and moves the scheduled time if it is already scheduled
|
||||
savePidDelay = SMOOTH_QUICK_FLUSH_TICKS;
|
||||
} else {
|
||||
savePidActive = true;
|
||||
}
|
||||
// user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed
|
||||
if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) {
|
||||
smoothQuickValue = accessoryValue.AccessoryVal;
|
||||
// calculate PIDs based on new smoothQuickValue and save to the PID bank
|
||||
ProportionPidsSmoothToQuick();
|
||||
// this schedules the first possible write of the PIDs to occur a fraction of a second or so from now
|
||||
// and changes the scheduled time if it is already scheduled
|
||||
savePidDelay = SMOOTH_QUICK_FLUSH_TICKS;
|
||||
} else if (savePidDelay && --savePidDelay == 0) {
|
||||
// this flags that the PIDs can be written to permanent storage right now
|
||||
// but they will only be written when the FC is disarmed
|
||||
// so this means immediate (after NOT_AT_MODE_DELAY_MS) or wait till FC is disarmed
|
||||
savePidNeeded = true;
|
||||
// we also save the new knob/toggle value for startup next time
|
||||
// this avoids rewriting the PIDs at each startup
|
||||
// because knob is unknown / not where it is expected / looks like knob moved
|
||||
saveSiNeeded = true;
|
||||
}
|
||||
} else {
|
||||
savePidDelay = 0;
|
||||
@ -395,7 +401,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
// load SystemIdentSettings so that they can change it
|
||||
// and do smooth-quick on changed values
|
||||
InitSystemIdent(false);
|
||||
InitSmoothQuick(false);
|
||||
// wait for FC to arm in case they are doing this without a flight mode switch
|
||||
// that causes the 2+ second delay that follows to happen after arming
|
||||
// which gives them a chance to take off before the shakes start
|
||||
@ -416,36 +421,42 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
// after 2 seconds start systemident flight mode
|
||||
if (diffTime > SYSTEMIDENT_TIME_DELAY_MS) {
|
||||
// load default tune and clean up any NANs from previous tune
|
||||
InitSystemIdent(true);
|
||||
AfInit(gX, gP);
|
||||
// and write it out to the UAVO so innerloop can see the default values
|
||||
UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f);
|
||||
// before starting SystemIdent stabilization mode
|
||||
doingIdent = true;
|
||||
// after an additional .5 seconds start capturing data
|
||||
if (diffTime > INIT_TIME_DELAY2_MS) {
|
||||
// Reset save status
|
||||
// save SI data even if partial or bad, aids in diagnostics
|
||||
saveSiNeeded = true;
|
||||
// don't save PIDs until data gathering is complete
|
||||
// and the complete data has been sanity checked
|
||||
savePidNeeded = false;
|
||||
InitSystemIdent(true);
|
||||
InitSmoothQuick(true);
|
||||
AfInit(gX, gP);
|
||||
UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f);
|
||||
measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
|
||||
state = AT_START;
|
||||
}
|
||||
state = AT_START;
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_START:
|
||||
lastTime = PIOS_DELAY_GetRaw();
|
||||
diffTime = xTaskGetTickCount() - lastUpdateTime;
|
||||
doingIdent = true;
|
||||
/* Drain the queue of all current data */
|
||||
xQueueReset(atQueue);
|
||||
/* And reset the point spill counter */
|
||||
updateCounter = 0;
|
||||
atPointsSpilled = 0;
|
||||
throttleAccumulator = 0;
|
||||
state = AT_RUN;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
// after an additional short delay, start capturing data
|
||||
if (diffTime > INIT_TIME_DELAY2_MS) {
|
||||
// Reset save status
|
||||
// save SI data even if partial or bad, aids in diagnostics
|
||||
saveSiNeeded = true;
|
||||
// don't save PIDs until data gathering is complete
|
||||
// and the complete data has been sanity checked
|
||||
savePidNeeded = false;
|
||||
// get the tuning duration in case the user just changed it
|
||||
measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
|
||||
// init the "previous packet timestamp"
|
||||
lastTime = PIOS_DELAY_GetRaw();
|
||||
/* Drain the queue of all current data */
|
||||
xQueueReset(atQueue);
|
||||
/* And reset the point spill counter */
|
||||
updateCounter = 0;
|
||||
atPointsSpilled = 0;
|
||||
throttleAccumulator = 0;
|
||||
alpha = 0.0f;
|
||||
state = AT_RUN;
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
}
|
||||
break;
|
||||
|
||||
case AT_RUN:
|
||||
@ -465,13 +476,18 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
break;
|
||||
}
|
||||
/* calculate time between successive points */
|
||||
float dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.raw_time) * 1.0e-6f;
|
||||
dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.gyroStateCallbackTimestamp) * 1.0e-6f;
|
||||
/* This is for the first point, but
|
||||
* also if we have extended drops */
|
||||
if (dT_s > 0.010f) {
|
||||
dT_s = 0.010f;
|
||||
if (dT_s > 5.0f / PIOS_SENSOR_RATE) {
|
||||
dT_s = 5.0f / PIOS_SENSOR_RATE;
|
||||
}
|
||||
lastTime = pt.raw_time;
|
||||
lastTime = pt.gyroStateCallbackTimestamp;
|
||||
// original algorithm handles time from GyroStateGet() to detected motion
|
||||
// this algorithm also includes the time from raw gyro read to GyroStateGet()
|
||||
gyroReadTimeAverage = gyroReadTimeAverage * alpha
|
||||
+ PIOS_DELAY_DiffuS2(pt.sensorReadTimestamp, pt.gyroStateCallbackTimestamp) * 1.0e-6f * (1.0f - alpha);
|
||||
alpha = alpha * gyroReadTimeAverageAlphaAlpha + gyroReadTimeAverageAlpha * (1.0f - gyroReadTimeAverageAlphaAlpha);
|
||||
AfPredict(gX, gP, pt.u, pt.y, dT_s, pt.throttle);
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz
|
||||
@ -481,7 +497,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
throttleAccumulator += 10000 * pt.throttle;
|
||||
// Update uavo every 256 cycles to avoid
|
||||
// telemetry spam
|
||||
if (((updateCounter++) & 0xff) == 0) {
|
||||
if (((++updateCounter) & 0xff) == 0) {
|
||||
float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
|
||||
UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle);
|
||||
}
|
||||
@ -493,16 +509,23 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
break;
|
||||
|
||||
case AT_FINISHED:
|
||||
// update with info from the last few data points
|
||||
if ((updateCounter & 0xff) != 0) {
|
||||
float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
|
||||
UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle);
|
||||
}
|
||||
// data is automatically considered bad if FC was disarmed at the time AT completed
|
||||
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
|
||||
// always calculate and save PIDs if disabling sanity checks
|
||||
if (!CheckSettings()) {
|
||||
ComputeStabilizationAndSetPids();
|
||||
ProportionPidsSmoothToQuick();
|
||||
savePidNeeded = true;
|
||||
// mark these results as good in the permanent settings so they can be used next flight too
|
||||
systemIdentSettings.Complete = true;
|
||||
// mark these results as good in the log settings so they can be viewed in playback
|
||||
systemIdentState.Complete = true;
|
||||
u.systemIdentState.Complete = true;
|
||||
SystemIdentStateCompleteSet(&u.systemIdentState.Complete);
|
||||
// mark these results as good in the permanent settings so they can be used next flight too
|
||||
// this is written to the UAVO below, outside of the ARMED and CheckSettings() checks
|
||||
systemIdentSettings.Complete = true;
|
||||
}
|
||||
// always raise an alarm if sanity checks failed
|
||||
// even if disabling sanity checks
|
||||
@ -514,8 +537,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits);
|
||||
}
|
||||
}
|
||||
float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
|
||||
UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hoverThrottle);
|
||||
// need to save UAVO after .Complete gets potentially set
|
||||
// SystemIdentSettings needs the whole UAVO saved so it is saved outside the previous checks
|
||||
SystemIdentSettingsSet(&systemIdentSettings);
|
||||
state = AT_WAITING;
|
||||
break;
|
||||
@ -546,6 +569,7 @@ static void AtNewGyroData(UAVObjEvent *ev)
|
||||
static bool last_sample_unpushed = false;
|
||||
GyroStateData gyro;
|
||||
ActuatorDesiredData actuators;
|
||||
uint32_t timestamp;
|
||||
|
||||
if (!ev || !ev->obj || ev->instId != 0 || ev->event != EV_UPDATED) {
|
||||
return;
|
||||
@ -553,6 +577,7 @@ static void AtNewGyroData(UAVObjEvent *ev)
|
||||
|
||||
// object will at times change asynchronously so must copy data here, with locking
|
||||
// and do it as soon as possible
|
||||
timestamp = PIOS_DELAY_GetRaw();
|
||||
GyroStateGet(&gyro);
|
||||
ActuatorDesiredGet(&actuators);
|
||||
|
||||
@ -564,14 +589,15 @@ static void AtNewGyroData(UAVObjEvent *ev)
|
||||
}
|
||||
}
|
||||
|
||||
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.gyroStateCallbackTimestamp = timestamp;
|
||||
q_item.y[0] = q_item.y[0] * stabSettings.gyro_alpha + gyro.x * (1 - stabSettings.gyro_alpha);
|
||||
q_item.y[1] = q_item.y[1] * stabSettings.gyro_alpha + gyro.y * (1 - stabSettings.gyro_alpha);
|
||||
q_item.y[2] = q_item.y[2] * stabSettings.gyro_alpha + gyro.z * (1 - stabSettings.gyro_alpha);
|
||||
q_item.u[0] = actuators.Roll;
|
||||
q_item.u[1] = actuators.Pitch;
|
||||
q_item.u[2] = actuators.Yaw;
|
||||
q_item.throttle = actuators.Thrust;
|
||||
q_item.sensorReadTimestamp = gyro.SensorReadTimestamp;
|
||||
|
||||
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
|
||||
last_sample_unpushed = true;
|
||||
@ -620,68 +646,77 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode)
|
||||
// and set some flags based on the values
|
||||
// it is used two ways:
|
||||
// - on startup it reads settings so the user can reuse an old tune with smooth-quick
|
||||
// - at tune time, it inits the state in preparation for tuning
|
||||
// - at tune time, it inits the state to default values of uavo xml file, in preparation for tuning
|
||||
static void InitSystemIdent(bool loadDefaults)
|
||||
{
|
||||
SystemIdentSettingsGet(&systemIdentSettings);
|
||||
|
||||
if (loadDefaults) {
|
||||
// get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau)
|
||||
// so that if they are changed there (mainly for future code changes), they will be changed here too
|
||||
// save metadata from being changed by the following SetDefaults()
|
||||
SystemIdentStateGetMetadata(&u.systemIdentStateMetaData);
|
||||
SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0);
|
||||
SystemIdentStateGet(&systemIdentState);
|
||||
// Tau, Beta, and the Complete flag get default values
|
||||
SystemIdentStateSetMetadata(&u.systemIdentStateMetaData);
|
||||
SystemIdentStateGet(&u.systemIdentState);
|
||||
// Tau, GyroReadTimeAverage, Beta, and the Complete flag get default values
|
||||
// in preparation for running AutoTune
|
||||
systemIdentSettings.Tau = systemIdentState.Tau;
|
||||
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
|
||||
systemIdentSettings.Complete = systemIdentState.Complete;
|
||||
systemIdentSettings.Tau = u.systemIdentState.Tau;
|
||||
systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage;
|
||||
memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
|
||||
systemIdentSettings.Complete = u.systemIdentState.Complete;
|
||||
} else {
|
||||
// Tau, Beta, and the Complete flag get stored values
|
||||
// Tau, GyroReadTimeAverage, Beta, and the Complete flag get stored values
|
||||
// so the user can fly another battery to select and test PIDs with the slider/knob
|
||||
systemIdentState.Tau = systemIdentSettings.Tau;
|
||||
memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData));
|
||||
systemIdentState.Complete = systemIdentSettings.Complete;
|
||||
u.systemIdentState.Tau = systemIdentSettings.Tau;
|
||||
u.systemIdentState.GyroReadTimeAverage = systemIdentSettings.GyroReadTimeAverage;
|
||||
memcpy(&u.systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData));
|
||||
u.systemIdentState.Complete = systemIdentSettings.Complete;
|
||||
}
|
||||
}
|
||||
SystemIdentStateSet(&u.systemIdentState);
|
||||
|
||||
// (1.0f / PIOS_SENSOR_RATE) is gyro period
|
||||
// the -1/10 makes it converge nicely, the other values make it converge the same way if the configuration is changed
|
||||
// gyroReadTimeAverageAlphaAlpha is 0.9996 when the tuning duration is the default of 60 seconds
|
||||
gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / PIOS_SENSOR_RATE / ((float)systemIdentSettings.TuningDuration / 10.0f));
|
||||
if (!IS_REAL(gyroReadTimeAverageAlphaAlpha)) {
|
||||
gyroReadTimeAverageAlphaAlpha = expapprox(-1.0f / 500.0f / (60 / 10)); // basically 0.9996
|
||||
}
|
||||
// 0.99999988f is as close to 1.0f as possible to make final average as smooth as possible
|
||||
gyroReadTimeAverageAlpha = 0.99999988f;
|
||||
gyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage;
|
||||
|
||||
static void InitSmoothQuick(bool loadToggle)
|
||||
{
|
||||
uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource;
|
||||
|
||||
switch (SmoothQuickSource) {
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
|
||||
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
|
||||
// leave smoothQuickValue alone since it is always controlled by knob
|
||||
// disable PID changing with flight mode switch
|
||||
// ignore loadToggle if user is also switching to use knob as source
|
||||
flightModeSwitchTogglePosition = -1;
|
||||
// enable PID changing with accessory0-3
|
||||
accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
|
||||
systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
|
||||
break;
|
||||
|
||||
case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points
|
||||
case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points
|
||||
case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points
|
||||
// don't allow init of current toggle position in the middle of 3x fms toggle
|
||||
if (loadDefaults) {
|
||||
// set toggle to middle of range
|
||||
smoothQuickValue = 0.0f;
|
||||
}
|
||||
// enable PID changing with flight mode switch
|
||||
flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
|
||||
// disable PID changing with accessory0-3
|
||||
accessoryToUse = -1;
|
||||
// don't allow init of current toggle position in the middle of 3x fms toggle
|
||||
if (loadToggle) {
|
||||
// first test PID is in the middle of the smooth -> quick range
|
||||
flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
|
||||
}
|
||||
systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
|
||||
break;
|
||||
|
||||
case SMOOTH_QUICK_DISABLED:
|
||||
default:
|
||||
// leave smoothQuickValue alone so user can set it to a different value and have it stay that value
|
||||
// disable PID changing with flight mode switch
|
||||
// ignore loadToggle since user is disabling toggle
|
||||
flightModeSwitchTogglePosition = -1;
|
||||
// disable PID changing with accessory0-3
|
||||
accessoryToUse = -1;
|
||||
systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -693,34 +728,38 @@ static void InitSmoothQuick(bool loadToggle)
|
||||
static void UpdateSystemIdentState(const float *X, const float *noise,
|
||||
float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle)
|
||||
{
|
||||
systemIdentState.Beta.Roll = X[6];
|
||||
systemIdentState.Beta.Pitch = X[7];
|
||||
systemIdentState.Beta.Yaw = X[8];
|
||||
systemIdentState.Bias.Roll = X[10];
|
||||
systemIdentState.Bias.Pitch = X[11];
|
||||
systemIdentState.Bias.Yaw = X[12];
|
||||
systemIdentState.Tau = X[9];
|
||||
// 'settings' beta and tau have same value as 'state' versions
|
||||
u.systemIdentState.Beta.Roll = X[6];
|
||||
u.systemIdentState.Beta.Pitch = X[7];
|
||||
u.systemIdentState.Beta.Yaw = X[8];
|
||||
u.systemIdentState.Bias.Roll = X[10];
|
||||
u.systemIdentState.Bias.Pitch = X[11];
|
||||
u.systemIdentState.Bias.Yaw = X[12];
|
||||
u.systemIdentState.Tau = X[9];
|
||||
if (noise) {
|
||||
u.systemIdentState.Noise.Roll = noise[0];
|
||||
u.systemIdentState.Noise.Pitch = noise[1];
|
||||
u.systemIdentState.Noise.Yaw = noise[2];
|
||||
}
|
||||
u.systemIdentState.Period = dT_s * 1000.0f;
|
||||
u.systemIdentState.NumAfPredicts = predicts;
|
||||
u.systemIdentState.NumSpilledPts = spills;
|
||||
u.systemIdentState.HoverThrottle = hover_throttle;
|
||||
u.systemIdentState.GyroReadTimeAverage = gyroReadTimeAverage;
|
||||
|
||||
// 'settings' tau, beta, and GyroReadTimeAverage have same value as 'state' versions
|
||||
// the state version produces a GCS log
|
||||
// the settings version is remembered after power off/on
|
||||
systemIdentSettings.Tau = systemIdentState.Tau;
|
||||
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
|
||||
if (noise) {
|
||||
systemIdentState.Noise.Roll = noise[0];
|
||||
systemIdentState.Noise.Pitch = noise[1];
|
||||
systemIdentState.Noise.Yaw = noise[2];
|
||||
}
|
||||
systemIdentState.Period = dT_s * 1000.0f;
|
||||
systemIdentState.NumAfPredicts = predicts;
|
||||
systemIdentState.NumSpilledPts = spills;
|
||||
systemIdentState.HoverThrottle = hover_throttle;
|
||||
systemIdentSettings.Tau = u.systemIdentState.Tau;
|
||||
memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
|
||||
systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage;
|
||||
systemIdentSettings.SmoothQuickValue = smoothQuickValue;
|
||||
|
||||
SystemIdentStateSet(&systemIdentState);
|
||||
SystemIdentStateSet(&u.systemIdentState);
|
||||
}
|
||||
|
||||
|
||||
// when running AutoTune mode, this bypasses manualcontrol.c / stabilizedhandler.c
|
||||
// to control exactly when the multicopter should be in Attitude mode vs. SystemIdent mode
|
||||
// to control whether the multicopter should be in Attitude mode vs. SystemIdent mode
|
||||
static void UpdateStabilizationDesired(bool doingIdent)
|
||||
{
|
||||
StabilizationDesiredData stabDesired;
|
||||
@ -755,31 +794,49 @@ static uint8_t CheckSettingsRaw()
|
||||
{
|
||||
uint8_t retVal = 0;
|
||||
|
||||
// inverting the comparisons then negating the bool result should catch the nans but it doesn't
|
||||
// so explictly check for nans
|
||||
if (!IS_REAL(expapprox(u.systemIdentState.Tau))) {
|
||||
retVal |= TAU_NAN;
|
||||
}
|
||||
if (!IS_REAL(expapprox(u.systemIdentState.Beta.Roll))) {
|
||||
retVal |= BETA_NAN;
|
||||
}
|
||||
if (!IS_REAL(expapprox(u.systemIdentState.Beta.Pitch))) {
|
||||
retVal |= BETA_NAN;
|
||||
}
|
||||
if (!IS_REAL(expapprox(u.systemIdentState.Beta.Yaw))) {
|
||||
retVal |= BETA_NAN;
|
||||
}
|
||||
|
||||
// Check the axis gains
|
||||
// Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values.
|
||||
if (systemIdentState.Beta.Roll < 6) {
|
||||
if (u.systemIdentState.Beta.Roll < 6) {
|
||||
retVal |= ROLL_BETA_LOW;
|
||||
}
|
||||
if (systemIdentState.Beta.Pitch < 6) {
|
||||
if (u.systemIdentState.Beta.Pitch < 6) {
|
||||
retVal |= PITCH_BETA_LOW;
|
||||
}
|
||||
// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default
|
||||
#if 0
|
||||
// if (systemIdentState.Beta.Yaw < 4) {
|
||||
if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) {
|
||||
retVal |= YAW_BETA_LOW;
|
||||
}
|
||||
#endif
|
||||
// yaw gain is no longer checked, because the yaw options only include:
|
||||
// - not calculating yaw
|
||||
// - limiting yaw gain between two sane values (default)
|
||||
// - ignoring errors and accepting the calculated yaw
|
||||
|
||||
// Check the response speed
|
||||
// Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values.
|
||||
if (expf(systemIdentState.Tau) > 0.1f) {
|
||||
if (expapprox(u.systemIdentState.Tau) > 0.1f) {
|
||||
retVal |= TAU_TOO_LONG;
|
||||
}
|
||||
// Extreme values: Your estimated response speed (tau) is faster than normal. This will result in large PID values.
|
||||
else if (expf(systemIdentState.Tau) < 0.008f) {
|
||||
else if (expapprox(u.systemIdentState.Tau) < 0.008f) {
|
||||
retVal |= TAU_TOO_SHORT;
|
||||
}
|
||||
|
||||
// Sanity check: CPU is too slow compared to gyro rate
|
||||
if (gyroReadTimeAverage > (1.0f / PIOS_SENSOR_RATE)) {
|
||||
retVal |= CPU_TOO_SLOW;
|
||||
}
|
||||
|
||||
return retVal;
|
||||
}
|
||||
|
||||
@ -795,27 +852,16 @@ static uint8_t CheckSettings()
|
||||
if (systemIdentSettings.DisableSanityChecks) {
|
||||
retVal = 0;
|
||||
}
|
||||
// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default
|
||||
#if 0
|
||||
// if not calculating yaw, or if calculating yaw but ignoring errors
|
||||
else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) {
|
||||
// clear the yaw error bit
|
||||
retVal &= ~YAW_BETA_LOW;
|
||||
}
|
||||
#endif
|
||||
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
// given Tau(delay) and Beta(gain) from the tune (and user selection of smooth to quick) calculate the PIDs
|
||||
// this code came from dRonin GCS and uses double precision math
|
||||
// most of the doubles could be replaced with floats
|
||||
// given Tau"+"GyroReadTimeAverage(delay) and Beta(gain) from the tune (and user selection of smooth to quick) calculate the PIDs
|
||||
// this code came from dRonin GCS and has been converted from double precision math to single precision
|
||||
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate)
|
||||
{
|
||||
_Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)");
|
||||
StabilizationBankData stabSettingsBank;
|
||||
|
||||
StabilizationBankData volatile stabSettingsBank;
|
||||
switch (systemIdentSettings.DestinationPidBank) {
|
||||
case 1:
|
||||
StabilizationSettingsBank1Get((void *)&stabSettingsBank);
|
||||
@ -835,59 +881,65 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
|
||||
// make oscillations less likely
|
||||
// - ghf is the amount of high frequency gain and limits the influence
|
||||
// of noise
|
||||
const double ghf = (double)noiseRate / 1000.0d;
|
||||
const double damp = (double)dampRate / 100.0d;
|
||||
const float ghf = noiseRate / 1000.0f;
|
||||
const float damp = dampRate / 100.0f;
|
||||
|
||||
double tau = exp(systemIdentState.Tau);
|
||||
double exp_beta_roll_times_ghf = exp(systemIdentState.Beta.Roll) * ghf;
|
||||
double exp_beta_pitch_times_ghf = exp(systemIdentState.Beta.Pitch) * ghf;
|
||||
float tau = expapprox(u.systemIdentState.Tau) + systemIdentSettings.GyroReadTimeAverage;
|
||||
float exp_beta_roll_times_ghf = expapprox(u.systemIdentState.Beta.Roll) * ghf;
|
||||
float exp_beta_pitch_times_ghf = expapprox(u.systemIdentState.Beta.Pitch) * ghf;
|
||||
|
||||
double wn = 1.0d / tau;
|
||||
double tau_d = 0.0d;
|
||||
float wn = 1.0f / tau;
|
||||
float tau_d = 0.0f;
|
||||
for (int i = 0; i < 30; i++) {
|
||||
double tau_d_roll = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_roll_times_ghf);
|
||||
double tau_d_pitch = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_pitch_times_ghf);
|
||||
float tau_d_roll = (2.0f * damp * tau * wn - 1.0f) / (4.0f * tau * damp * damp * wn * wn - 2.0f * damp * wn - tau * wn * wn + exp_beta_roll_times_ghf);
|
||||
float tau_d_pitch = (2.0f * damp * tau * wn - 1.0f) / (4.0f * tau * damp * damp * wn * wn - 2.0f * damp * wn - tau * wn * wn + exp_beta_pitch_times_ghf);
|
||||
// Select the slowest filter property
|
||||
tau_d = (tau_d_roll > tau_d_pitch) ? tau_d_roll : tau_d_pitch;
|
||||
wn = (tau + tau_d) / (tau * tau_d) / (2.0d * damp + 2.0d);
|
||||
wn = (tau + tau_d) / (tau * tau_d) / (2.0f * damp + 2.0f);
|
||||
}
|
||||
|
||||
// Set the real pole position. The first pole is quite slow, which
|
||||
// prevents the integral being too snappy and driving too much
|
||||
// overshoot.
|
||||
const double a = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn) / 20.0d;
|
||||
const double b = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn - a);
|
||||
const float a = ((tau + tau_d) / tau / tau_d - 2.0f * damp * wn) / 20.0f;
|
||||
const float b = ((tau + tau_d) / tau / tau_d - 2.0f * damp * wn - a);
|
||||
|
||||
// Calculate the gain for the outer loop by approximating the
|
||||
// inner loop as a single order lpf. Set the outer loop to be
|
||||
// critically damped;
|
||||
const double zeta_o = 1.3d;
|
||||
const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d / wn);
|
||||
const double ki_o = 0.75d * kp_o / (2.0d * M_PI_D * tau * 10.0d);
|
||||
const float zeta_o = 1.3f;
|
||||
const float kp_o = 1.0f / 4.0f / (zeta_o * zeta_o) / (1.0f / wn);
|
||||
const float ki_o = 0.75f * kp_o / (2.0f * M_PI_F * tau * 10.0f);
|
||||
|
||||
float kpMax = 0.0f;
|
||||
double betaMinLn = 1000.0d;
|
||||
StabilizationBankRollRatePIDData *rollPitchPid = NULL; // satisfy compiler warning only
|
||||
float betaMinLn = 1000.0f;
|
||||
StabilizationBankRollRatePIDData volatile *rollPitchPid = NULL; // satisfy compiler warning only
|
||||
|
||||
for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) {
|
||||
double betaLn = SystemIdentStateBetaToArray(systemIdentState.Beta)[i];
|
||||
double beta = exp(betaLn);
|
||||
double ki;
|
||||
double kp;
|
||||
double kd;
|
||||
float betaLn = SystemIdentStateBetaToArray(u.systemIdentState.Beta)[i];
|
||||
float beta = expapprox(betaLn);
|
||||
float ki;
|
||||
float kp;
|
||||
float kd;
|
||||
|
||||
switch (i) {
|
||||
case 0: // Roll
|
||||
case 1: // Pitch
|
||||
ki = a * b * wn * wn * tau * tau_d / beta;
|
||||
kp = tau * tau_d * ((a + b) * wn * wn + 2.0d * a * b * damp * wn) / beta - ki * tau_d;
|
||||
kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0d * damp * wn) - 1.0d) / beta - kp * tau_d;
|
||||
kp = tau * tau_d * ((a + b) * wn * wn + 2.0f * a * b * damp * wn) / beta - ki * tau_d;
|
||||
kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0f * damp * wn) - 1.0f) / beta - kp * tau_d;
|
||||
if (betaMinLn > betaLn) {
|
||||
betaMinLn = betaLn;
|
||||
// RollRatePID PitchRatePID YawRatePID
|
||||
// form an array of structures
|
||||
// point to one
|
||||
rollPitchPid = &(&stabSettingsBank.RollRatePID)[i];
|
||||
// this pointer arithmetic no longer works as expected in a gcc 64 bit test program
|
||||
// rollPitchPid = &(&stabSettingsBank.RollRatePID)[i];
|
||||
if (i == 0) {
|
||||
rollPitchPid = &stabSettingsBank.RollRatePID;
|
||||
} else {
|
||||
rollPitchPid = (StabilizationBankRollRatePIDData *)&stabSettingsBank.PitchRatePID;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2: // Yaw
|
||||
@ -907,16 +959,17 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
|
||||
// which is xp / (betaMin * betaYaw^0.6 / betaMin^0.6)
|
||||
// which is xp / (betaMin^0.4 * betaYaw^0.6)
|
||||
// hence the new effective betaYaw for Yaw P is (betaMin^0.4)*(betaYaw^0.6)
|
||||
beta = exp(0.6d * (betaMinLn - (double)systemIdentState.Beta.Yaw));
|
||||
kp = (double)rollPitchPid->Kp * beta;
|
||||
ki = 0.8d * (double)rollPitchPid->Ki * beta;
|
||||
kd = 0.8d * (double)rollPitchPid->Kd * beta;
|
||||
beta = expapprox(0.6f * (betaMinLn - u.systemIdentState.Beta.Yaw));
|
||||
// this casting assumes that RollRatePID is the same as PitchRatePID
|
||||
kp = rollPitchPid->Kp * beta;
|
||||
ki = 0.8f * rollPitchPid->Ki * beta;
|
||||
kd = 0.8f * rollPitchPid->Kd * beta;
|
||||
break;
|
||||
}
|
||||
|
||||
if (i < 2) {
|
||||
if (kpMax < (float)kp) {
|
||||
kpMax = (float)kp;
|
||||
if (kpMax < kp) {
|
||||
kpMax = kp;
|
||||
}
|
||||
} else {
|
||||
// use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value
|
||||
@ -939,14 +992,53 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
|
||||
}
|
||||
|
||||
float ratio = 1.0f;
|
||||
if (min > 0.0f && (float)kp < min) {
|
||||
ratio = (float)kp / min;
|
||||
} else if (max > 0.0f && (float)kp > max) {
|
||||
ratio = (float)kp / max;
|
||||
if (min > 0.0f && kp < min) {
|
||||
ratio = kp / min;
|
||||
} else if (max > 0.0f && kp > max) {
|
||||
ratio = kp / max;
|
||||
}
|
||||
kp /= (double)ratio;
|
||||
ki /= (double)ratio;
|
||||
kd /= (double)ratio;
|
||||
kp /= ratio;
|
||||
ki /= ratio;
|
||||
kd /= ratio;
|
||||
}
|
||||
|
||||
// reduce kd if so configured
|
||||
// both of the quads tested for d term oscillation exhibit some degree of it with the stock autotune PIDs
|
||||
// if may be that adjusting stabSettingsBank.DerivativeCutoff would have a similar affect
|
||||
// reducing kd requires that kp and ki be reduced to avoid ringing
|
||||
// the amount to reduce kp and ki is taken from ZN tuning
|
||||
// specifically kp is parameterized based on the ratio between kp(PID) and kp(PI) as the D factor varies from 1 to 0
|
||||
// https://en.wikipedia.org/wiki/PID_controller
|
||||
// Kp Ki Kd
|
||||
// -----------------------------------
|
||||
// P 0.50*Ku - -
|
||||
// PI 0.45*Ku 1.2*Kp/Tu -
|
||||
// PID 0.60*Ku 2.0*Kp/Tu Kp*Tu/8
|
||||
//
|
||||
// so Kp is multiplied by (.45/.60) if Kd is reduced to 0
|
||||
// and Ki is multiplied by (1.2/2.0) if Kd is reduced to 0
|
||||
#define KP_REDUCTION (.45f / .60f)
|
||||
#define KI_REDUCTION (1.2f / 2.0f)
|
||||
|
||||
// this link gives some additional ratios that are different
|
||||
// the reduced overshoot ratios are invalid for this purpose
|
||||
// https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
|
||||
// Kp Ki Kd
|
||||
// ------------------------------------------------
|
||||
// P 0.50*Ku - -
|
||||
// PI 0.45*Ku Tu/1.2 -
|
||||
// PD 0.80*Ku - Tu/8
|
||||
// classic PID 0.60*Ku Tu/2.0 Tu/8 #define KP_REDUCTION (.45f/.60f) #define KI_REDUCTION (1.2f/2.0f)
|
||||
// Pessen Integral Rule 0.70*Ku Tu/2.5 3.0*Tu/20 #define KP_REDUCTION (.45f/.70f) #define KI_REDUCTION (1.2f/2.5f)
|
||||
// some overshoot 0.33*Ku Tu/2.0 Tu/3 #define KP_REDUCTION (.45f/.33f) #define KI_REDUCTION (1.2f/2.0f)
|
||||
// no overshoot 0.20*Ku Tu/2.0 Tu/3 #define KP_REDUCTION (.45f/.20f) #define KI_REDUCTION (1.2f/2.0f)
|
||||
|
||||
// reduce roll and pitch, but not yaw
|
||||
// yaw PID is entirely based on roll or pitch PIDs which have already been reduced
|
||||
if (i < 2) {
|
||||
kp = kp * KP_REDUCTION + kp * systemIdentSettings.DerivativeFactor * (1.0f - KP_REDUCTION);
|
||||
ki = ki * KI_REDUCTION + ki * systemIdentSettings.DerivativeFactor * (1.0f - KI_REDUCTION);
|
||||
kd *= systemIdentSettings.DerivativeFactor;
|
||||
}
|
||||
|
||||
switch (i) {
|
||||
@ -980,8 +1072,9 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
|
||||
}
|
||||
|
||||
// Librepilot might do something more with this some time
|
||||
// stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d);
|
||||
// stabSettingsBank.DerivativeCutoff = 1.0f / (2.0f*M_PI_F*tau_d);
|
||||
// SystemIdentSettingsDerivativeCutoffSet(&systemIdentSettings.DerivativeCutoff);
|
||||
// then something to schedule saving this permanently to flash when disarmed
|
||||
|
||||
// Save PIDs to UAVO RAM (not permanently yet)
|
||||
switch (systemIdentSettings.DestinationPidBank) {
|
||||
@ -998,13 +1091,6 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
|
||||
}
|
||||
|
||||
|
||||
// calculate PIDs using default smooth-quick settings
|
||||
static void ComputeStabilizationAndSetPids()
|
||||
{
|
||||
ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentSettings.DampRate, systemIdentSettings.NoiseRate);
|
||||
}
|
||||
|
||||
|
||||
// scale the damp and the noise to generate PIDs according to how a slider or other user specified ratio is set
|
||||
//
|
||||
// when val is half way between min and max, it generates the default PIDs
|
||||
@ -1017,9 +1103,12 @@ static void ComputeStabilizationAndSetPids()
|
||||
// this is done piecewise because we are not guaranteed that default-min == max-default
|
||||
// but we are given that [smoothDamp,smoothNoise] [defaultDamp,defaultNoise] [quickDamp,quickNoise] are all good parameterizations
|
||||
// this code guarantees that we will get those exact parameterizations at (val =) min, (max+min)/2, and max
|
||||
static void ProportionPidsSmoothToQuick(float min, float val, float max)
|
||||
static void ProportionPidsSmoothToQuick()
|
||||
{
|
||||
float ratio, damp, noise;
|
||||
float min = -1.0f;
|
||||
float val = smoothQuickValue;
|
||||
float max = 1.0f;
|
||||
|
||||
// translate from range [min, max] to range [0, max-min]
|
||||
// that takes care of min < 0 case too
|
||||
@ -1040,6 +1129,8 @@ static void ProportionPidsSmoothToQuick(float min, float val, float max)
|
||||
}
|
||||
|
||||
ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise);
|
||||
// save it to the system, but not yet written to flash
|
||||
SystemIdentSettingsSmoothQuickValueSet(&smoothQuickValue);
|
||||
}
|
||||
|
||||
|
||||
@ -1214,11 +1305,11 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
|
||||
P[1] = -D[1] * (D[1] / S[1] - 1);
|
||||
P[2] = -D[2] * (D[2] / S[2] - 1);
|
||||
P[3] = -D[3] * (D[0] / S[0] - 1);
|
||||
P[4] = D[4] - D[3] * D[3] / S[0];
|
||||
P[4] = D[4] - D[3] * (D[3] / S[0]);
|
||||
P[5] = -D[5] * (D[1] / S[1] - 1);
|
||||
P[6] = D[6] - D[5] * D[5] / S[1];
|
||||
P[6] = D[6] - D[5] * (D[5] / S[1]);
|
||||
P[7] = -D[7] * (D[2] / S[2] - 1);
|
||||
P[8] = D[8] - D[7] * D[7] / S[2];
|
||||
P[8] = D[8] - D[7] * (D[7] / S[2]);
|
||||
P[9] = -D[9] * (D[0] / S[0] - 1);
|
||||
P[10] = D[10] - D[3] * (D[9] / S[0]);
|
||||
P[11] = D[11] - D[9] * (D[9] / S[0]);
|
||||
@ -1302,8 +1393,8 @@ static void AfInit(float X[AF_NUMX], float P[AF_NUMP])
|
||||
memset(X, 0, AF_NUMX * sizeof(X[0]));
|
||||
// get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau)
|
||||
// so that if they are changed there (mainly for future code changes), they will be changed here too
|
||||
memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta));
|
||||
X[9] = systemIdentState.Tau;
|
||||
memcpy(&X[6], &u.systemIdentState.Beta, sizeof(u.systemIdentState.Beta));
|
||||
X[9] = u.systemIdentState.Tau;
|
||||
|
||||
// P initialization
|
||||
memset(P, 0, AF_NUMP * sizeof(P[0]));
|
||||
|
||||
@ -109,9 +109,11 @@ static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f
|
||||
// Private types
|
||||
typedef struct {
|
||||
// used to accumulate all samples in a task iteration
|
||||
Vector3i32 accum[2];
|
||||
int32_t temperature;
|
||||
uint32_t count;
|
||||
uint64_t timestamp; // sum of "PIOS_DELAY_GetRaw() times of sensor read" in this averaged set
|
||||
Vector3i32 accum[2]; // summed 16 bit sensor values in this averaged set
|
||||
int32_t temperature; // sum of 16 bit temperatures in this averaged set
|
||||
uint32_t prev_timestamp; // to detect timer wrap around
|
||||
uint16_t count; // number of sensor reads in this averaged set
|
||||
} sensor_fetch_context;
|
||||
|
||||
#define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16))
|
||||
@ -145,7 +147,7 @@ static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const P
|
||||
static void clearContext(sensor_fetch_context *sensor_context);
|
||||
|
||||
static void handleAccel(float *samples, float temperature);
|
||||
static void handleGyro(float *samples, float temperature);
|
||||
static void handleGyro(float *samples, float temperature, uint32_t timestamp);
|
||||
static void handleMag(float *samples, float temperature);
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
static void handleAuxMag(float *samples);
|
||||
@ -254,8 +256,6 @@ int32_t mag_test;
|
||||
* stabilization and to the attitude loop
|
||||
*
|
||||
*/
|
||||
|
||||
uint32_t sensor_dt_us;
|
||||
static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
portTickType lastSysTime;
|
||||
@ -359,8 +359,10 @@ static void clearContext(sensor_fetch_context *sensor_context)
|
||||
sensor_context->accum[i].y = 0;
|
||||
sensor_context->accum[i].z = 0;
|
||||
}
|
||||
sensor_context->temperature = 0;
|
||||
sensor_context->count = 0;
|
||||
sensor_context->temperature = 0;
|
||||
sensor_context->prev_timestamp = 0;
|
||||
sensor_context->timestamp = 0LL;
|
||||
sensor_context->count = 0;
|
||||
}
|
||||
|
||||
static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample)
|
||||
@ -371,6 +373,14 @@ static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data
|
||||
sensor_context->accum[i].z += sample->sensorSample3Axis.sample[i].z;
|
||||
}
|
||||
sensor_context->temperature += sample->sensorSample3Axis.temperature;
|
||||
sensor_context->timestamp += sample->sensorSample3Axis.timestamp;
|
||||
if (sensor_context->prev_timestamp > sample->sensorSample3Axis.timestamp) {
|
||||
// we've wrapped so add the dropped top bit
|
||||
// this makes the average come out correct instead of (0xfd+0x01)/2 = 0x7f or such
|
||||
sensor_context->timestamp += 0x100000000LL;
|
||||
} else {
|
||||
sensor_context->prev_timestamp = sample->sensorSample3Axis.timestamp;
|
||||
}
|
||||
sensor_context->count++;
|
||||
}
|
||||
|
||||
@ -416,6 +426,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE
|
||||
|
||||
if (sensor->type & PIOS_SENSORS_TYPE_3AXIS_GYRO) {
|
||||
uint8_t index = 0;
|
||||
uint32_t timestamp;
|
||||
if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) {
|
||||
index = 1;
|
||||
}
|
||||
@ -424,7 +435,8 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE
|
||||
samples[1] = ((float)sensor_context->accum[index].y * t);
|
||||
samples[2] = ((float)sensor_context->accum[index].z * t);
|
||||
temperature = (float)sensor_context->temperature * inv_count * 0.01f;
|
||||
handleGyro(samples, temperature);
|
||||
timestamp = (uint32_t)(sensor_context->timestamp / sensor_context->count);
|
||||
handleGyro(samples, temperature, timestamp);
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -456,10 +468,11 @@ static void handleAccel(float *samples, float temperature)
|
||||
accelSensorData.y = samples[1];
|
||||
accelSensorData.z = samples[2];
|
||||
accelSensorData.temperature = temperature;
|
||||
|
||||
AccelSensorSet(&accelSensorData);
|
||||
}
|
||||
|
||||
static void handleGyro(float *samples, float temperature)
|
||||
static void handleGyro(float *samples, float temperature, uint32_t timestamp)
|
||||
{
|
||||
GyroSensorData gyroSensorData;
|
||||
|
||||
@ -469,10 +482,11 @@ static void handleGyro(float *samples, float temperature)
|
||||
samples[2] * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
|
||||
|
||||
rot_mult(R, gyros_out, samples);
|
||||
gyroSensorData.temperature = temperature;
|
||||
gyroSensorData.x = samples[0];
|
||||
gyroSensorData.y = samples[1];
|
||||
gyroSensorData.z = samples[2];
|
||||
gyroSensorData.temperature = temperature;
|
||||
gyroSensorData.SensorReadTimestamp = timestamp;
|
||||
|
||||
GyroSensorSet(&gyroSensorData);
|
||||
}
|
||||
|
||||
@ -572,6 +572,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
|
||||
t.x = s.x + gyroDelta[0];
|
||||
t.y = s.y + gyroDelta[1];
|
||||
t.z = s.z + gyroDelta[2];
|
||||
t.SensorReadTimestamp = s.SensorReadTimestamp;
|
||||
GyroStateSet(&t);
|
||||
}
|
||||
|
||||
|
||||
@ -104,6 +104,7 @@ static mpu6000_data_t mpu6000_data;
|
||||
static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
|
||||
#define SENSOR_COUNT 2
|
||||
#define SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT)
|
||||
|
||||
// ! Private functions
|
||||
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
|
||||
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
|
||||
@ -111,7 +112,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg);
|
||||
static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
|
||||
static int32_t PIOS_MPU6000_GetReg(uint8_t address);
|
||||
static void PIOS_MPU6000_SetSpeed(const bool fast);
|
||||
static bool PIOS_MPU6000_HandleData();
|
||||
static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp);
|
||||
static bool PIOS_MPU6000_ReadSensor(bool *woken);
|
||||
|
||||
static int32_t PIOS_MPU6000_Test(void);
|
||||
@ -540,24 +541,21 @@ static int32_t PIOS_MPU6000_Test(void)
|
||||
|
||||
bool PIOS_MPU6000_IRQHandler(void)
|
||||
{
|
||||
uint32_t gyro_read_timestamp = PIOS_DELAY_GetRaw();
|
||||
bool woken = false;
|
||||
|
||||
if (!mpu6000_configured) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool read_ok = false;
|
||||
read_ok = PIOS_MPU6000_ReadSensor(&woken);
|
||||
|
||||
if (read_ok) {
|
||||
bool woken2 = PIOS_MPU6000_HandleData();
|
||||
woken |= woken2;
|
||||
if (PIOS_MPU6000_ReadSensor(&woken)) {
|
||||
woken |= PIOS_MPU6000_HandleData(gyro_read_timestamp);
|
||||
}
|
||||
|
||||
return woken;
|
||||
}
|
||||
|
||||
static bool PIOS_MPU6000_HandleData()
|
||||
static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp)
|
||||
{
|
||||
if (!queue_data) {
|
||||
return false;
|
||||
@ -600,6 +598,7 @@ static bool PIOS_MPU6000_HandleData()
|
||||
const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature);
|
||||
// Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53
|
||||
queue_data->temperature = 3653 + (temp * 100) / 340;
|
||||
queue_data->timestamp = gyro_read_timestamp;
|
||||
|
||||
BaseType_t higherPriorityTaskWoken;
|
||||
xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken);
|
||||
|
||||
@ -137,7 +137,7 @@ static void PIOS_MPU9250_Config(struct pios_mpu9250_cfg const *cfg);
|
||||
static int32_t PIOS_MPU9250_SetReg(uint8_t address, uint8_t buffer);
|
||||
static int32_t PIOS_MPU9250_GetReg(uint8_t address);
|
||||
static void PIOS_MPU9250_SetSpeed(const bool fast);
|
||||
static bool PIOS_MPU9250_HandleData();
|
||||
static bool PIOS_MPU9250_HandleData(uint32_t gyro_read_timestamp);
|
||||
static bool PIOS_MPU9250_ReadSensor(bool *woken);
|
||||
static int32_t PIOS_MPU9250_Test(void);
|
||||
#if defined(PIOS_MPU9250_MAG)
|
||||
@ -812,9 +812,9 @@ static bool PIOS_MPU9250_ReadMag(bool *woken)
|
||||
* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
|
||||
* higher priority task is now eligible to run
|
||||
*/
|
||||
|
||||
bool PIOS_MPU9250_IRQHandler(void)
|
||||
{
|
||||
uint32_t gyro_read_timestamp = PIOS_DELAY_GetRaw();
|
||||
bool woken = false;
|
||||
|
||||
if (!mpu9250_configured) {
|
||||
@ -825,18 +825,14 @@ bool PIOS_MPU9250_IRQHandler(void)
|
||||
PIOS_MPU9250_ReadMag(&woken);
|
||||
#endif
|
||||
|
||||
bool read_ok = false;
|
||||
read_ok = PIOS_MPU9250_ReadSensor(&woken);
|
||||
|
||||
if (read_ok) {
|
||||
bool woken2 = PIOS_MPU9250_HandleData();
|
||||
woken |= woken2;
|
||||
if (PIOS_MPU9250_ReadSensor(&woken)) {
|
||||
woken |= PIOS_MPU9250_HandleData(gyro_read_timestamp);
|
||||
}
|
||||
|
||||
return woken;
|
||||
}
|
||||
|
||||
static bool PIOS_MPU9250_HandleData()
|
||||
static bool PIOS_MPU9250_HandleData(uint32_t gyro_read_timestamp)
|
||||
{
|
||||
// Rotate the sensor to OP convention. The datasheet defines X as towards the right
|
||||
// and Y as forward. OP convention transposes this. Also the Z is defined negatively
|
||||
@ -916,6 +912,7 @@ static bool PIOS_MPU9250_HandleData()
|
||||
queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu9250_data, Gyro_Z));
|
||||
const int16_t temp = GET_SENSOR_DATA(mpu9250_data, Temperature);
|
||||
queue_data->temperature = 2100 + ((float)(temp - PIOS_MPU9250_TEMP_OFFSET)) * (100.0f / PIOS_MPU9250_TEMP_SENSITIVITY);
|
||||
queue_data->timestamp = gyro_read_timestamp;
|
||||
mag_data->temperature = queue_data->temperature;
|
||||
#ifdef PIOS_MPU9250_MAG
|
||||
if (mag_valid) {
|
||||
|
||||
@ -76,14 +76,15 @@ typedef struct PIOS_SENSORS_Instance {
|
||||
* A 3d Accel sample with temperature
|
||||
*/
|
||||
typedef struct PIOS_SENSORS_3Axis_SensorsWithTemp {
|
||||
uint16_t count; // number of sensor instances
|
||||
uint32_t timestamp; // PIOS_DELAY_GetRaw() time of sensor read
|
||||
uint16_t count; // number of sensor instances
|
||||
int16_t temperature; // Degrees Celsius * 100
|
||||
Vector3i16 sample[];
|
||||
} PIOS_SENSORS_3Axis_SensorsWithTemp;
|
||||
|
||||
typedef struct PIOS_SENSORS_1Axis_SensorsWithTemp {
|
||||
float temperature; // Degrees Celsius
|
||||
float sample; // sample
|
||||
float temperature; // Degrees Celsius
|
||||
} PIOS_SENSORS_1Axis_SensorsWithTemp;
|
||||
|
||||
/**
|
||||
|
||||
@ -1,10 +1,11 @@
|
||||
<xml>
|
||||
<object name="GyroSensor" singleinstance="true" settings="false" category="Sensors">
|
||||
<description>Calibrated sensor data from 3 axis gyroscope in deg/s.</description>
|
||||
<field name="x" units="deg/s" type="float" elements="1"/>
|
||||
<field name="y" units="deg/s" type="float" elements="1"/>
|
||||
<field name="z" units="deg/s" type="float" elements="1"/>
|
||||
<field name="x" units="deg/s" type="float" elements="1"/>
|
||||
<field name="y" units="deg/s" type="float" elements="1"/>
|
||||
<field name="z" units="deg/s" type="float" elements="1"/>
|
||||
<field name="temperature" units="deg C" type="float" elements="1"/>
|
||||
<field name="SensorReadTimestamp" units="tick" type="uint32" elements="1" description="STM32 CPU clock ticks"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -1,9 +1,10 @@
|
||||
<xml>
|
||||
<object name="GyroState" singleinstance="true" settings="false" category="State">
|
||||
<description>The filtered rotation sensor data.</description>
|
||||
<field name="x" units="deg/s" type="float" elements="1"/>
|
||||
<field name="y" units="deg/s" type="float" elements="1"/>
|
||||
<field name="z" units="deg/s" type="float" elements="1"/>
|
||||
<field name="x" units="deg/s" type="float" elements="1"/>
|
||||
<field name="y" units="deg/s" type="float" elements="1"/>
|
||||
<field name="z" units="deg/s" type="float" elements="1"/>
|
||||
<field name="SensorReadTimestamp" units="tick" type="uint32" elements="1" description="STM32 CPU clock ticks"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -1,9 +1,16 @@
|
||||
<xml>
|
||||
<object name="SystemIdentSettings" singleinstance="true" settings="true" category="Control">
|
||||
<description>The input to the PID tuning.</description>
|
||||
<field name="Tau" units="ln(sec)" type="float" elements="1" defaultvalue="-4.0"/>
|
||||
<!-- Beta default valuses 10.0 10.0 7.0 so that SystemIdent mode can be run without AutoTune -->
|
||||
<field name="Beta" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="10.0,10.0,7.0"/>
|
||||
<description>The input to and results of the PID tuning.</description>
|
||||
<field name="Tau" units="ln(sec)" type="float" elements="1" defaultvalue="-4.0" description="Measured delay between inner loop and detected gyro response"/>
|
||||
<!-- Beta default values 10.0 10.0 7.0 so that SystemIdent mode can be run without AutoTune -->
|
||||
<field name="Beta" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="10.0,10.0,7.0" description="(Natural log of) Measured control gain"/>
|
||||
<!-- The following comments come from / are extrapolated from various TauLabs / dRonin sources. -->
|
||||
<!-- TL / dRonin requires you to adjust two sliders, one called RateDamp and the other called RateNoise. -->
|
||||
<!-- They have documented that certain pairings of damp and noise go together well. -->
|
||||
<!-- The LP code uses these "known good pairs" to weld the damp and noise scales together into the SmoothQuick slider. -->
|
||||
<!-- When using the SmoothQuick slider you automatically get an interpolated matching pair of damp and noise. -->
|
||||
<!-- So be aware that these comments refer to the TL/dRronin slider names. -->
|
||||
<!-- -->
|
||||
<!-- Decrease damping to make your aircraft response more rapidly. Increase it for more stability. -->
|
||||
<!-- Increasing noise (sensitivity) will make your aircraft respond more rapidly, but will cause twitches due to noise. -->
|
||||
<!-- Use RateDamp 130 with RateNoise 08 for very smooth flight. -->
|
||||
@ -20,22 +27,36 @@
|
||||
<!-- Use RateDamp 150 with RateNoise 06 for very very smooth flight. -->
|
||||
<!-- Use RateDamp 90 with RateNoise 16 for very very snappy flight. -->
|
||||
<!-- use additive so the piecewise algorithm will give the exact recommended pairs at 25%, 50%, and 75% of slider -->
|
||||
<field name="DampMin" units="" type="uint8" elements="1" defaultvalue="90"/>
|
||||
<field name="DampRate" units="" type="uint8" elements="1" defaultvalue="110"/>
|
||||
<field name="DampMax" units="" type="uint8" elements="1" defaultvalue="150"/>
|
||||
<field name="NoiseMin" units="" type="uint8" elements="1" defaultvalue="6"/>
|
||||
<field name="NoiseRate" units="" type="uint8" elements="1" defaultvalue="10"/>
|
||||
<field name="NoiseMax" units="" type="uint8" elements="1" defaultvalue="16"/>
|
||||
<!-- <field name="CalculateYaw" units="" type="enum" elements="1" options="False,True,TrueIgnoreError" defaultvalue="True"/> -->
|
||||
<field name="CalculateYaw" units="" type="enum" elements="1" options="False,TrueLimitToRatio,TrueIgnoreLimit" defaultvalue="TrueLimitToRatio"/>
|
||||
<!-- <field name="YawBetaMin" units="" type="float" elements="1" defaultvalue="5.6"/> -->
|
||||
<!-- -->
|
||||
<!-- So the following 6 fields set up the SmoothQuick slider using 3 known good damp and noise pairings. -->
|
||||
<!-- You can adjust just one end of SmoothQuick by changing e.g. the DampMin and NoiseMax pairing. -->
|
||||
<!-- You can maintain the known good ratio to simply extend or shrink the SmoothQuick scale. -->
|
||||
<!-- You can change the known good ratio e.g. if you find that the quick end of SmoothQuick tends to oscillate. -->
|
||||
<!-- You can change the default PIDs by changing the "center" values DampRate and NoiseRate. -->
|
||||
<!-- You can change the whole SmoothQuick slider uniformly by e.g. multiplying all Damp* fields by .75 -->
|
||||
<!-- You can shift the whole SmoothQuick slider up or down -->
|
||||
<!-- etc! -->
|
||||
<!-- on the SmoothQuick value scale of [-1,+1]: -->
|
||||
<!-- the default SmoothQuick value of 0.0 gives recommended default damp and rate for normal flight -->
|
||||
<!-- a SmoothQuick value of -0.5 gives the recommended damp and rate for very smooth flight -->
|
||||
<!-- a SmoothQuick value of +0.5 gives the recommended damp and rate for very quick flight -->
|
||||
<!-- a SmoothQuick value of -1.0 is extrapolated to give very very smooth flight while still being inside the range of sane values-->
|
||||
<!-- a SmoothQuick value of +1.0 is extrapolated to give very very quick flight while still being inside the range of sane values-->
|
||||
<!-- other values of SmoothQuick are interpolated or extrapolated from these -->
|
||||
<field name="DampMin" units="" type="uint8" elements="1" defaultvalue="90" description="Setting: Part of smoothquick slider (expert)"/>
|
||||
<field name="DampRate" units="" type="uint8" elements="1" defaultvalue="110" description="Setting: Part of smoothquick slider (expert)"/>
|
||||
<field name="DampMax" units="" type="uint8" elements="1" defaultvalue="150" description="Setting: Part of smoothquick slider (expert)"/>
|
||||
<field name="NoiseMin" units="" type="uint8" elements="1" defaultvalue="6" description="Setting: Part of smoothquick slider (expert)"/>
|
||||
<field name="NoiseRate" units="" type="uint8" elements="1" defaultvalue="10" description="Setting: Part of smoothquick slider (expert)"/>
|
||||
<field name="NoiseMax" units="" type="uint8" elements="1" defaultvalue="16" description="Setting: Part of smoothquick slider (expert)"/>
|
||||
<field name="CalculateYaw" units="" type="enum" elements="1" options="False,TrueLimitToRatio,TrueIgnoreLimit" defaultvalue="TrueLimitToRatio" description="Setting: Whether to calculate yaw and whether to use YawToRollPitchPIDRatio"/>
|
||||
<!-- Mateuze quad needs yaw P to be at most 2.6 times roll/pitch P to avoid yaw oscillation -->
|
||||
<!-- Cliff sluggish 500 quad thinks that yaw P should be about 5.8 times roll/pitch P, but can easily (and better) live with 2.6 -->
|
||||
<field name="YawToRollPitchPIDRatioMin" units="" type="float" elements="1" defaultvalue="1.0"/>
|
||||
<field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5"/>
|
||||
<!-- <field name="YawPIDRatioFunction" units="" type="enum" elements="1" options="Disable,Limit" defaultvalue="Limit"/> -->
|
||||
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="2"/>
|
||||
<field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60"/>
|
||||
<field name="YawToRollPitchPIDRatioMin" units="" type="float" elements="1" defaultvalue="1.0" description="Setting: Yaw PID will be at least this times Pitch PID (if enabled)"/>
|
||||
<field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5" description="Setting: Yaw PID will be at most this times Pitch PID (if enabled)"/>
|
||||
<field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1" description="Setting: Reduce this toward zero if you have D term oscillations, it will reduce PID D terms"/>
|
||||
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="3" limits="%BE:1:3" description="Setting: Which bank the calculated PIDs will be stored in after tuning"/>
|
||||
<field name="TuningDuration" units="s" type="uint8" elements="1" defaultvalue="60" limits="%BI:0" description="Setting: Duration of the tuning motions (expert)"/>
|
||||
<!-- SmoothQuickSource: the smooth vs. quick PID selector -->
|
||||
<!-- 0 = disabled -->
|
||||
<!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs -->
|
||||
@ -48,9 +69,11 @@
|
||||
<!-- 25 (5 stops) means stops at 50, 75, 100, 0, 25 (repeat as you toggle) -->
|
||||
<!-- 27 (7 stops) means stops at 50, 67, 83, 100, 0, 17, 33 (repeat as you toggle) -->
|
||||
<!-- 25 is special in that the 3 middle values (25, 50, 75) are exactly those that are recommended for smooth, normal, and quick responses -->
|
||||
<field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/>
|
||||
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
<field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25" description="Setting: 10-13 to use one of accessory0-3, 23 25 27 for FMS 3x toggle method with 3 5 7 positions"/>
|
||||
<field name="SmoothQuickValue" units="" type="float" elements="1" defaultvalue="0.0" description="Remembers the value of the FMS 3x toggle"/>
|
||||
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False" description="Setting: Debugging tool (expert)"/>
|
||||
<field name="GyroReadTimeAverage" units="s" type="float" elements="1" defaultvalue="0.001" description="Measured delay from gyro read to inner loop"/>
|
||||
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False" description="Automatically set True for a good complete tune or False for incomplete or bad"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
@ -1,16 +1,17 @@
|
||||
<xml>
|
||||
<object name="SystemIdentState" singleinstance="true" settings="false" category="Control">
|
||||
<description>The input to the PID tuning.</description>
|
||||
<field name="Tau" units="ln(sec)" type="float" elements="1" defaultvalue="-4.0"/>
|
||||
<!-- Beta default valuses 10.0 10.0 7.0 so that SystemIdent mode can be run without AutoTune -->
|
||||
<field name="Beta" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="10.0,10.0,7.0"/>
|
||||
<field name="Bias" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Noise" units="(deg/s)^2" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0"/>
|
||||
<field name="Period" units="ms" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="NumAfPredicts" units="" type="uint32" elements="1" defaultvalue="0"/>
|
||||
<field name="NumSpilledPts" units="" type="uint32" elements="1" defaultvalue="0"/>
|
||||
<field name="HoverThrottle" units="%/100" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
|
||||
<object name="SystemIdentState" singleinstance="true" settings="false" category="State">
|
||||
<description>Used for logging PID tuning.</description>
|
||||
<field name="Tau" units="ln(sec)" type="float" elements="1" defaultvalue="-4.0" description="Measured delay between inner loop and detected gyro response"/>
|
||||
<!-- Beta default values 10.0 10.0 7.0 so that SystemIdent mode can be run without AutoTune -->
|
||||
<field name="Beta" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="10.0,10.0,7.0" description="(Natural log of) Measured control gain"/>
|
||||
<field name="Bias" units="" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0" description="Measured amount and direction of control stick that stabilization automatically adds for motionless hover"/>
|
||||
<field name="Noise" units="(deg/s)^2" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0" description="Measured vibration: Usually 10's to 1000's"/>
|
||||
<field name="Period" units="ms" type="float" elements="1" defaultvalue="0" description="Measured time between gyro samples"/>
|
||||
<field name="NumAfPredicts" units="" type="uint32" elements="1" defaultvalue="0" description="Number of gyro samples that were counted"/>
|
||||
<field name="NumSpilledPts" units="" type="uint32" elements="1" defaultvalue="0" description="Number of gyro samples that were dropped (should be zero)"/>
|
||||
<field name="HoverThrottle" units="%/100" type="float" elements="1" defaultvalue="0" description="Measured throttle stick position"/>
|
||||
<field name="GyroReadTimeAverage" units="s" type="float" elements="1" defaultvalue="0.001" description="Measured delay from gyro read to inner loop"/>
|
||||
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False" description="Automatically set True for a good complete tune or False for incomplete or bad"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user