rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-12 20:08:48 +01:00
Code Issues Releases Activity
7553eb4f32
LibrePilot/flight/modules/AutoTune/autotune.c
Cliff Geerdes 7553eb4f32 LP-340 Quick code with both timing fixes
2016-07-12 14:48:10 -04:00

1355 lines
60 KiB
C
Raw Blame History

/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file AutoTune/autotune.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* dRonin, http://dRonin.org/, Copyright (C) 2015-2016
* Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Automatic PID tuning module.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <gyrostate.h>
#include <actuatordesired.h>
#include <stabilizationdesired.h>
#include <stabilizationsettings.h>
#include <systemidentsettings.h>
#include <systemidentstate.h>
#include <pios_board_info.h>
#include <systemsettings.h>
#include <taskinfo.h>
#include <stabilization.h>
#include <hwsettings.h>
#include <stabilizationsettingsbank1.h>
#include <stabilizationsettingsbank2.h>
#include <stabilizationsettingsbank3.h>
#include <accessorydesired.h>
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
#define powapprox fastpow
#define expapprox fastexp
#else
#define powapprox powf
#define expapprox expf
#endif /* defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// Private constants
#undef STACK_SIZE_BYTES
// Pull Request version tested on Nano. 120 bytes of stack left when configured with 1340
#define STACK_SIZE_BYTES 1340
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define AF_NUMX 13
#define AF_NUMP 43
#if !defined(AT_QUEUE_NUMELEM)
#define AT_QUEUE_NUMELEM 18
#endif
#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */
#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */
#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */
#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */
#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */
#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */
#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */
#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */
#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */
// CheckSettings() returned error bits
#define ROLL_BETA_LOW 1
#define PITCH_BETA_LOW 2
#define YAW_BETA_LOW 4
#define TAU_TOO_LONG 8
#define TAU_TOO_SHORT 16
// smooth-quick modes
#define SMOOTH_QUICK_DISABLED 0
#define SMOOTH_QUICK_ACCESSORY_BASE 10
#define SMOOTH_QUICK_TOGGLE_BASE 20
// Private types
enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING };
struct at_queued_data {
float y[3]; /* Gyro measurements */
float u[3]; /* Actuator desired */
float throttle; /* Throttle desired */
uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */
};
// Global variables
extern uint32_t gyroReadTime;
// Private variables
static xTaskHandle taskHandle;
static bool moduleEnabled;
static xQueueHandle atQueue;
static volatile uint32_t atPointsSpilled;
static uint32_t throttleAccumulator;
static uint8_t rollMax, pitchMax;
static StabilizationBankManualRateData manualRate;
static float gX[AF_NUMX] = { 0 };
static float gP[AF_NUMP] = { 0 };
static SystemIdentSettingsData systemIdentSettings;
static SystemIdentStateData systemIdentState;
static int8_t accessoryToUse;
static int8_t flightModeSwitchTogglePosition;
static float gyroReadTimeAverage;
static float gyroReadTimeAverageAlpha;
#define GYRO_READ_TIME_DECAY_TIME_CONSTANT 2.0f
// Private functions
static void AutoTuneTask(void *parameters);
static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in);
static void AfInit(float X[AF_NUMX], float P[AF_NUMP]);
static uint8_t CheckSettingsRaw();
static uint8_t CheckSettings();
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise);
static void ComputeStabilizationAndSetPids();
static void ProportionPidsSmoothToQuick(float min, float val, float max);
static void AtNewGyroData(UAVObjEvent *ev);
static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle);
static void UpdateStabilizationDesired(bool doingIdent);
static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
static void InitSystemIdent(bool loadDefaults);
static void InitSmoothQuick(bool loadToggle);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AutoTuneInitialize(void)
{
// Create a queue, connect to manual control command and flightstatus
#ifdef MODULE_AutoTune_BUILTIN
moduleEnabled = true;
#else
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) {
// even though the AutoTune module is automatically enabled
// (below, when the flight mode switch is configured to use autotune)
// there are use cases where the user may even want it enabled without being on the FMS
// that allows PIDs to be adjusted in flight
moduleEnabled = true;
} else {
// if the user did not enable the autotune module
// do it for them if they have autotune on their flight mode switch
FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
moduleEnabled = false;
FlightModeSettingsInitialize();
FlightModeSettingsFlightModePositionGet(fms);
for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) {
if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
moduleEnabled = true;
break;
}
}
}
#endif /* ifdef MODULE_AutoTune_BUILTIN */
if (moduleEnabled) {
SystemIdentSettingsInitialize();
SystemIdentStateInitialize();
atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data));
if (!atQueue) {
moduleEnabled = false;
}
// 0.002 is gyro period
// make the smoothing decay about 1 second
gyroReadTimeAverageAlpha = expf(-0.002f / GYRO_READ_TIME_DECAY_TIME_CONSTANT);
}
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AutoTuneStart(void)
{
// Start main task if it is enabled
if (moduleEnabled) {
// taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
// TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
// PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
GyroStateConnectCallback(AtNewGyroData);
xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
}
return 0;
}
MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart);
/**
* Module thread, should not return.
*/
static void AutoTuneTask(__attribute__((unused)) void *parameters)
{
enum AUTOTUNE_STATE state = AT_INIT;
uint32_t lastUpdateTime = 0; // initialization is only for compiler warning
float noise[3] = { 0 };
uint32_t lastTime = 0.0f;
uint32_t measureTime = 0;
uint32_t updateCounter = 0;
bool saveSiNeeded = false;
bool savePidNeeded = false;
// get max attitude / max rate
// for use in generating Attitude mode commands from this module
// note that the values could change when they change flight mode (and the associated bank)
StabilizationBankRollMaxGet(&rollMax);
StabilizationBankPitchMaxGet(&pitchMax);
StabilizationBankManualRateGet(&manualRate);
// correctly set accessoryToUse and flightModeSwitchTogglePosition
// based on what is in SystemIdent
// so that the user can use the PID smooth->quick slider in following flights
InitSystemIdent(false);
InitSmoothQuick(true);
while (1) {
uint32_t diffTime;
bool doingIdent = false;
bool canSleep = true;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// I have never seen this module misbehave in a way that requires a watchdog, so not bothering making one
// PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
if (saveSiNeeded) {
saveSiNeeded = false;
// Save SystemIdentSettings to permanent settings
UAVObjSave(SystemIdentSettingsHandle(), 0);
}
if (savePidNeeded) {
savePidNeeded = false;
// Save PIDs to permanent settings
switch (systemIdentSettings.DestinationPidBank) {
case 1:
UAVObjSave(StabilizationSettingsBank1Handle(), 0);
break;
case 2:
UAVObjSave(StabilizationSettingsBank2Handle(), 0);
break;
case 3:
UAVObjSave(StabilizationSettingsBank3Handle(), 0);
break;
}
}
}
// if using flight mode switch "quick toggle 3x" to "try smooth -> quick PIDs" is enabled
// and user toggled into and back out of AutoTune three times in the last two seconds
// and the autotune data gathering is complete
// and the autotune data gathered is good
// note: CheckFlightModeSwitchForPidRequest(mode) only returns true if current mode is not autotune
if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)
&& systemIdentSettings.Complete && !CheckSettings()) {
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
// if user toggled while armed set PID's to next in sequence
// 2,3,4,0,1 (5 positions) or 1,2,0 (3 positions) or 3,4,5,6,0,1,2 (7 positions)
// if you assume that smoothest is -100 and quickest is +100
// this corresponds to 0,+50,+100,-100,-50 (for 5 position toggle)
++flightModeSwitchTogglePosition;
if (flightModeSwitchTogglePosition > systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) {
flightModeSwitchTogglePosition = 0;
}
} else {
// if they did it disarmed, then set PID's back to AutoTune default
flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
}
ProportionPidsSmoothToQuick(0.0f,
(float)flightModeSwitchTogglePosition,
(float)(systemIdentSettings.SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE));
savePidNeeded = true;
}
//////////////////////////////////////////////////////////////////////////////////////
// if configured to use a slider for smooth-quick and the autotune module is running
// (note that the module can be automatically or manually enabled)
// then the smooth-quick slider is always active (when not actually in autotune mode)
//
// when the slider is active it will immediately change the PIDs
// and it will schedule the PIDs to be written to permanent storage
//
// if the FC is disarmed, the perm write will happen on next loop
// but if the FC is armed, the perm write will only occur when the FC goes disarmed
//////////////////////////////////////////////////////////////////////////////////////
// we don't want it saving to permanent storage many times
// while the user is moving the knob once, so wait till the knob stops moving
static uint8_t savePidDelay;
// any time we are not in AutoTune mode:
// - the user may be using the accessory0-3 knob/slider to request PID changes
// - the state machine needs to be reset
// - the local version of Attitude mode gets skipped
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
static bool savePidActive = false;
// if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range
// and FC is not currently running autotune
// and accessory0-3 changed by at least 1/160 of full range (2)
// (don't bother checking to see if the requested accessory# is configured properly
// if it isn't, the value will be 0 which is the center of [-1,1] anyway)
if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) {
static AccessoryDesiredData accessoryValueOld = { 0.0f };
AccessoryDesiredData accessoryValue;
AccessoryDesiredInstGet(accessoryToUse, &accessoryValue);
// if the accessory changed more than 1/80
// (this test is intended to remove one unit jitter)
// some old PPM receivers use a low resolution chip which only allows about 180 steps
// what we are doing here does not need any higher precision than that
if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) {
accessoryValueOld = accessoryValue;
// this copies the PIDs to memory and makes them active
// but does not write them to permanent storage
ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f);
// don't save PID to perm storage the first time
// that usually means at power up
//
// that keeps it from writing the same value at each boot
// but means that it won't be permanent if they move the slider before FC power on
// (note that the PIDs will be changed, just not saved permanently)
if (savePidActive) {
// this schedules the first possible write of the PIDs to occur a fraction of a second or so from now
// and moves the scheduled time if it is already scheduled
savePidDelay = SMOOTH_QUICK_FLUSH_TICKS;
} else {
savePidActive = true;
}
} else if (savePidDelay && --savePidDelay == 0) {
// this flags that the PIDs can be written to permanent storage right now
// but they will only be written when the FC is disarmed
// so this means immediate (after NOT_AT_MODE_DELAY_MS) or wait till FC is disarmed
savePidNeeded = true;
}
} else {
savePidDelay = 0;
}
state = AT_INIT;
vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS);
continue;
} else {
savePidDelay = 0;
}
switch (state) {
case AT_INIT:
// beware that control comes here every time the user toggles the flight mode switch into AutoTune
// and it isn't appropriate to reset the main state here
// init must wait until after a delay has passed:
// - to make sure they intended to stay in this mode
// - to wait for the stab bank to get populated with the new bank info
// This is a race. It is possible that flightStatus.FlightMode has been changed,
// but the stab bank hasn't been changed yet.
state = AT_INIT_DELAY;
lastUpdateTime = xTaskGetTickCount();
break;
case AT_INIT_DELAY:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// after a small delay, get the stab bank values and SystemIdentSettings in case they changed
// this is a very small delay (100ms), so "quick 3x fms toggle" gets in here
if (diffTime > INIT_TIME_DELAY_MS) {
// do these here so the user has at most a 1/10th second
// with controls that use the previous bank's rates
StabilizationBankRollMaxGet(&rollMax);
StabilizationBankPitchMaxGet(&pitchMax);
StabilizationBankManualRateGet(&manualRate);
// load SystemIdentSettings so that they can change it
// and do smooth-quick on changed values
InitSystemIdent(false);
InitSmoothQuick(false);
// wait for FC to arm in case they are doing this without a flight mode switch
// that causes the 2+ second delay that follows to happen after arming
// which gives them a chance to take off before the shakes start
// the FC must be armed and if we check here it also allows switchless setup to use autotune
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
state = AT_INIT_DELAY2;
lastUpdateTime = xTaskGetTickCount();
}
}
break;
case AT_INIT_DELAY2:
// delay for 2 seconds before actually starting the SystemIdent flight mode and AutoTune.
// that allows the user to get his fingers on the sticks
// and avoids starting the AutoTune if the user is toggling the flight mode switch
// to select other PIDs on the "simulated Smooth Quick slider".
// or simply "passing through" this flight mode to get to another flight mode
diffTime = xTaskGetTickCount() - lastUpdateTime;
// after 2 seconds start systemident flight mode
if (diffTime > SYSTEMIDENT_TIME_DELAY_MS) {
doingIdent = true;
// after an additional .5 seconds start capturing data
if (diffTime > INIT_TIME_DELAY2_MS) {
// Reset save status
// save SI data even if partial or bad, aids in diagnostics
saveSiNeeded = true;
// don't save PIDs until data gathering is complete
// and the complete data has been sanity checked
savePidNeeded = false;
InitSystemIdent(true);
InitSmoothQuick(true);
AfInit(gX, gP);
UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f);
measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
state = AT_START;
}
}
break;
case AT_START:
lastTime = PIOS_DELAY_GetRaw();
doingIdent = true;
/* Drain the queue of all current data */
xQueueReset(atQueue);
/* And reset the point spill counter */
updateCounter = 0;
atPointsSpilled = 0;
throttleAccumulator = 0;
state = AT_RUN;
lastUpdateTime = xTaskGetTickCount();
break;
case AT_RUN:
diffTime = xTaskGetTickCount() - lastUpdateTime;
doingIdent = true;
canSleep = false;
// 4 gyro samples per cycle
// 2ms cycle time
// that is 500 gyro samples per second if it sleeps each time
// actually less than 500 because it cycle time is processing time + 2ms
for (int i = 0; i < MAX_PTS_PER_CYCLE; i++) {
struct at_queued_data pt;
/* Grab an autotune point */
if (xQueueReceive(atQueue, &pt, 0) != pdTRUE) {
/* We've drained the buffer fully */
canSleep = true;
break;
}
/* calculate time between successive points */
float dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.raw_time) * 1.0e-6f;
/* This is for the first point, but
* also if we have extended drops */
if (dT_s > 0.010f) {
dT_s = 0.010f;
}
lastTime = pt.raw_time;
AfPredict(gX, gP, pt.u, pt.y, dT_s, pt.throttle);
for (int j = 0; j < 3; ++j) {
const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz
noise[j] = NOISE_ALPHA * noise[j] + (1 - NOISE_ALPHA) * (pt.y[j] - gX[j]) * (pt.y[j] - gX[j]);
}
// This will work up to 8kHz with an 89% throttle position before overflow
throttleAccumulator += 10000 * pt.throttle;
// Update uavo every 256 cycles to avoid
// telemetry spam
if (((updateCounter++) & 0xff) == 0) {
float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hoverThrottle);
}
}
if (diffTime > measureTime) { // Move on to next state
// permanent flag that AT is complete and PIDs can be calculated
state = AT_FINISHED;
}
break;
case AT_FINISHED:
// data is automatically considered bad if FC was disarmed at the time AT completed
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
// always calculate and save PIDs if disabling sanity checks
if (!CheckSettings()) {
ComputeStabilizationAndSetPids();
savePidNeeded = true;
// mark these results as good in the permanent settings so they can be used next flight too
systemIdentSettings.Complete = true;
// mark these results as good in the log settings so they can be viewed in playback
systemIdentState.Complete = true;
}
// always raise an alarm if sanity checks failed
// even if disabling sanity checks
// that way user can still see that they failed
uint8_t failureBits = CheckSettingsRaw();
if (failureBits) {
// raise a warning that includes failureBits to indicate what failed
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING,
SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits);
}
}
float hoverThrottle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hoverThrottle);
SystemIdentSettingsSet(&systemIdentSettings);
state = AT_WAITING;
break;
case AT_WAITING:
default:
// after tuning, wait here till user switches to another flight mode
// or disarms
break;
}
// fly in Attitude mode or in SystemIdent mode
UpdateStabilizationDesired(doingIdent);
if (canSleep) {
vTaskDelay(YIELD_MS / portTICK_RATE_MS);
}
}
}
// gyro sensor callback
// get gyro data and actuatordesired into a packet
// and put it in the queue for later processing
static void AtNewGyroData(UAVObjEvent *ev)
{
static struct at_queued_data q_item;
static bool last_sample_unpushed = false;
GyroStateData gyro;
ActuatorDesiredData actuators;
if (!ev || !ev->obj || ev->instId != 0 || ev->event != EV_UPDATED) {
return;
}
// object will at times change asynchronously so must copy data here, with locking
// and do it as soon as possible
GyroStateGet(&gyro);
ActuatorDesiredGet(&actuators);
gyroReadTimeAverage = gyroReadTimeAverage * gyroReadTimeAverageAlpha + PIOS_DELAY_DiffuS(gyroReadTime) * 1.0e-6f * (1 - gyroReadTimeAverageAlpha);
if (last_sample_unpushed) {
/* Last time we were unable to queue up the gyro data.
* Try again, last chance! */
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
atPointsSpilled++;
}
}
q_item.raw_time = PIOS_DELAY_GetRaw();
#if 0
gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha);
q_item.y[0] = gyro.x;
q_item.y[1] = gyro.y;
q_item.y[2] = gyro.z;
#endif
q_item.y[0] = q_item.y[0] * stabSettings.gyro_alpha + gyro.x * (1 - stabSettings.gyro_alpha);
q_item.y[1] = q_item.y[1] * stabSettings.gyro_alpha + gyro.y * (1 - stabSettings.gyro_alpha);
q_item.y[2] = q_item.y[2] * stabSettings.gyro_alpha + gyro.z * (1 - stabSettings.gyro_alpha);
q_item.u[0] = actuators.Roll;
q_item.u[1] = actuators.Pitch;
q_item.u[2] = actuators.Yaw;
q_item.throttle = actuators.Thrust;
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
last_sample_unpushed = true;
} else {
last_sample_unpushed = false;
}
}
// check for the user quickly toggling the flight mode switch
// into and out of AutoTune, 3 times
// that is a signal that the user wants to try the next PID settings
// on the scale from smooth to quick
// when it exceeds the quickest setting, it starts back at the smoothest setting
static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode)
{
static uint32_t lastUpdateTime;
static uint8_t flightModePrev;
static uint8_t counter;
uint32_t updateTime;
// only count transitions into and out of autotune
if ((flightModePrev == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ^ (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE)) {
flightModePrev = flightMode;
updateTime = xTaskGetTickCount();
// if it has been over 2 seconds, reset the counter
if (updateTime - lastUpdateTime > 2000) {
counter = 0;
}
// if the counter is reset, start a new time period
if (counter == 0) {
lastUpdateTime = updateTime;
}
// if flight mode has toggled into autotune 3 times but is currently not autotune
if (++counter >= 5 && flightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
counter = 0;
return true;
}
}
return false;
}
// read SystemIdent uavos, update the local structures
// and set some flags based on the values
// it is used two ways:
// - on startup it reads settings so the user can reuse an old tune with smooth-quick
// - at tune time, it inits the state in preparation for tuning
static void InitSystemIdent(bool loadDefaults)
{
SystemIdentSettingsGet(&systemIdentSettings);
if (loadDefaults) {
// get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau)
// so that if they are changed there (mainly for future code changes), they will be changed here too
SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0);
SystemIdentStateGet(&systemIdentState);
// Tau, Beta, and the Complete flag get default values
// in preparation for running AutoTune
systemIdentSettings.Tau = systemIdentState.Tau;
systemIdentSettings.GyroReadTimeAverage = systemIdentState.GyroReadTimeAverage;
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;
systemIdentState.GyroReadTimeAverage = systemIdentSettings.GyroReadTimeAverage;
memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData));
systemIdentState.Complete = systemIdentSettings.Complete;
}
}
static void InitSmoothQuick(bool loadToggle)
{
uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource;
switch (SmoothQuickSource) {
case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0
case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
// disable PID changing with flight mode switch
// ignore loadToggle if user is also switching to use knob as source
flightModeSwitchTogglePosition = -1;
accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
break;
case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points
case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points
case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points
// disable PID changing with accessory0-3
accessoryToUse = -1;
// don't allow init of current toggle position in the middle of 3x fms toggle
if (loadToggle) {
// first test PID is in the middle of the smooth -> quick range
flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
}
systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
break;
case SMOOTH_QUICK_DISABLED:
default:
// disable PID changing with flight mode switch
// ignore loadToggle since user is disabling toggle
flightModeSwitchTogglePosition = -1;
// disable PID changing with accessory0-3
accessoryToUse = -1;
systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED;
break;
}
}
// update the gain and delay with current calculated value
// these are stored in the settings for use with next battery
// and also in the state for logging purposes
static void UpdateSystemIdentState(const float *X, const float *noise,
float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle)
{
systemIdentState.Beta.Roll = X[6];
systemIdentState.Beta.Pitch = X[7];
systemIdentState.Beta.Yaw = X[8];
systemIdentState.Bias.Roll = X[10];
systemIdentState.Bias.Pitch = X[11];
systemIdentState.Bias.Yaw = X[12];
systemIdentState.Tau = X[9];
// 'settings' beta and tau have same value as 'state' versions
// the state version produces a GCS log
// the settings version is remembered after power off/on
systemIdentSettings.Tau = systemIdentState.Tau;
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
if (noise) {
systemIdentState.Noise.Roll = noise[0];
systemIdentState.Noise.Pitch = noise[1];
systemIdentState.Noise.Yaw = noise[2];
}
systemIdentState.Period = dT_s * 1000.0f;
systemIdentState.NumAfPredicts = predicts;
systemIdentState.NumSpilledPts = spills;
systemIdentState.HoverThrottle = hover_throttle;
systemIdentState.GyroReadTimeAverage = gyroReadTimeAverage;
systemIdentSettings.GyroReadTimeAverage = systemIdentState.GyroReadTimeAverage;
SystemIdentStateSet(&systemIdentState);
}
// when running AutoTune mode, this bypasses manualcontrol.c / stabilizedhandler.c
// to control exactly when the multicopter should be in Attitude mode vs. SystemIdent mode
static void UpdateStabilizationDesired(bool doingIdent)
{
StabilizationDesiredData stabDesired;
ManualControlCommandData manualControlCommand;
ManualControlCommandGet(&manualControlCommand);
stabDesired.Roll = manualControlCommand.Roll * rollMax;
stabDesired.Pitch = manualControlCommand.Pitch * pitchMax;
stabDesired.Yaw = manualControlCommand.Yaw * manualRate.Yaw;
stabDesired.Thrust = manualControlCommand.Thrust;
if (doingIdent) {
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
} else {
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
// check the completed autotune state (mainly gain and delay)
// to see if it is reasonable
// return a bit mask of errors detected
static uint8_t CheckSettingsRaw()
{
uint8_t retVal = 0;
// Check the axis gains
// Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values.
if (systemIdentState.Beta.Roll < 6) {
retVal |= ROLL_BETA_LOW;
}
if (systemIdentState.Beta.Pitch < 6) {
retVal |= PITCH_BETA_LOW;
}
// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default
#if 0
// if (systemIdentState.Beta.Yaw < 4) {
if (systemIdentState.Beta.Yaw < systemIdentSettings.YawBetaMin) {
retVal |= YAW_BETA_LOW;
}
#endif
// Check the response speed
// Extreme values: Your estimated response speed (tau) is slower than normal. This will result in large PID values.
if (expf(systemIdentState.Tau) > 0.1f) {
retVal |= TAU_TOO_LONG;
}
// Extreme values: Your estimated response speed (tau) is faster than normal. This will result in large PID values.
else if (expf(systemIdentState.Tau) < 0.008f) {
retVal |= TAU_TOO_SHORT;
}
return retVal;
}
// check the completed autotune state (mainly gain and delay)
// to see if it is reasonable
// override bad yaw values if configured that way
// return a bit mask of errors detected
static uint8_t CheckSettings()
{
uint8_t retVal = CheckSettingsRaw();
if (systemIdentSettings.DisableSanityChecks) {
retVal = 0;
}
// previously we didn't save PID if there was too little yaw beta (too big a yaw PID), now we correct it (limit yaw PID) by default
#if 0
// if not calculating yaw, or if calculating yaw but ignoring errors
else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) {
// clear the yaw error bit
retVal &= ~YAW_BETA_LOW;
}
#endif
return retVal;
}
// given Tau(delay) and Beta(gain) from the tune (and user selection of smooth to quick) calculate the PIDs
// this code came from dRonin GCS and uses double precision math
// most of the doubles could be replaced with floats
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate)
{
_Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)");
StabilizationBankData stabSettingsBank;
switch (systemIdentSettings.DestinationPidBank) {
case 1:
StabilizationSettingsBank1Get((void *)&stabSettingsBank);
break;
case 2:
StabilizationSettingsBank2Get((void *)&stabSettingsBank);
break;
case 3:
StabilizationSettingsBank3Get((void *)&stabSettingsBank);
break;
}
// These three parameters define the desired response properties
// - rate scale in the fraction of the natural speed of the system
// to strive for.
// - damp is the amount of damping in the system. higher values
// make oscillations less likely
// - ghf is the amount of high frequency gain and limits the influence
// of noise
const double ghf = (double)noiseRate / 1000.0d;
const double damp = (double)dampRate / 100.0d;
double tau = exp(systemIdentState.Tau) + (double) systemIdentSettings.GyroReadTimeAverage;
double exp_beta_roll_times_ghf = exp(systemIdentState.Beta.Roll) * ghf;
double exp_beta_pitch_times_ghf = exp(systemIdentState.Beta.Pitch) * ghf;
double wn = 1.0d / tau;
double tau_d = 0.0d;
for (int i = 0; i < 30; i++) {
double tau_d_roll = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_roll_times_ghf);
double tau_d_pitch = (2.0d * damp * tau * wn - 1.0d) / (4.0d * tau * damp * damp * wn * wn - 2.0d * damp * wn - tau * wn * wn + exp_beta_pitch_times_ghf);
// Select the slowest filter property
tau_d = (tau_d_roll > tau_d_pitch) ? tau_d_roll : tau_d_pitch;
wn = (tau + tau_d) / (tau * tau_d) / (2.0d * damp + 2.0d);
}
// Set the real pole position. The first pole is quite slow, which
// prevents the integral being too snappy and driving too much
// overshoot.
const double a = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn) / 20.0d;
const double b = ((tau + tau_d) / tau / tau_d - 2.0d * damp * wn - a);
// Calculate the gain for the outer loop by approximating the
// inner loop as a single order lpf. Set the outer loop to be
// critically damped;
const double zeta_o = 1.3d;
const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d / wn);
const double ki_o = 0.75d * kp_o / (2.0d * M_PI_D * tau * 10.0d);
float kpMax = 0.0f;
double betaMinLn = 1000.0d;
StabilizationBankRollRatePIDData *rollPitchPid = NULL; // satisfy compiler warning only
for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) {
double betaLn = SystemIdentStateBetaToArray(systemIdentState.Beta)[i];
double beta = exp(betaLn);
double ki;
double kp;
double kd;
switch (i) {
case 0: // Roll
case 1: // Pitch
ki = a * b * wn * wn * tau * tau_d / beta;
kp = tau * tau_d * ((a + b) * wn * wn + 2.0d * a * b * damp * wn) / beta - ki * tau_d;
kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0d * damp * wn) - 1.0d) / beta - kp * tau_d;
if (betaMinLn > betaLn) {
betaMinLn = betaLn;
// RollRatePID PitchRatePID YawRatePID
// form an array of structures
// point to one
rollPitchPid = &(&stabSettingsBank.RollRatePID)[i];
}
break;
case 2: // Yaw
// yaw uses a mixture of yaw and the slowest axis (pitch) for it's beta and thus PID calculation
// calculate the ratio to use when converting from the slowest axis (pitch) to the yaw axis
// as (e^(betaMinLn-betaYawLn))^0.6
// which is (e^betaMinLn / e^betaYawLn)^0.6
// which is (betaMin / betaYaw)^0.6
// which is betaMin^0.6 / betaYaw^0.6
// now given that kp for each axis can be written as kpaxis = xp / betaaxis
// for xp that is constant across all axes
// then kpmin (probably kppitch) was xp / betamin (probably betapitch)
// which we multiply by betaMin^0.6 / betaYaw^0.6 to get the new Yaw kp
// so the new kpyaw is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6)
// which is (xp / betaMin) * (betaMin^0.6 / betaYaw^0.6)
// which is (xp * betaMin^0.6) / (betaMin * betaYaw^0.6)
// which is xp / (betaMin * betaYaw^0.6 / betaMin^0.6)
// which is xp / (betaMin^0.4 * betaYaw^0.6)
// hence the new effective betaYaw for Yaw P is (betaMin^0.4)*(betaYaw^0.6)
beta = exp(0.6d * (betaMinLn - (double)systemIdentState.Beta.Yaw));
kp = (double)rollPitchPid->Kp * beta;
ki = 0.8d * (double)rollPitchPid->Ki * beta;
kd = 0.8d * (double)rollPitchPid->Kd * beta;
break;
}
if (i < 2) {
if (kpMax < (float)kp) {
kpMax = (float)kp;
}
} else {
// use the ratio with the largest roll/pitch kp to limit yaw kp to a reasonable value
// use largest roll/pitch kp because it is the axis most slowed by rotational inertia
// and yaw is also slowed maximally by rotational inertia
// note that kp, ki, kd are all proportional in beta
// so reducing them all proportionally is the same as changing beta
float min = 0.0f;
float max = 0.0f;
switch (systemIdentSettings.CalculateYaw) {
case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUELIMITTORATIO:
max = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMax;
min = kpMax * systemIdentSettings.YawToRollPitchPIDRatioMin;
break;
case SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUEIGNORELIMIT:
default:
max = 1000.0f;
min = 0.0f;
break;
}
float ratio = 1.0f;
if (min > 0.0f && (float)kp < min) {
ratio = (float)kp / min;
} else if (max > 0.0f && (float)kp > max) {
ratio = (float)kp / max;
}
kp /= (double)ratio;
ki /= (double)ratio;
kd /= (double)ratio;
}
switch (i) {
case 0: // Roll
stabSettingsBank.RollRatePID.Kp = kp;
stabSettingsBank.RollRatePID.Ki = ki;
stabSettingsBank.RollRatePID.Kd = kd;
stabSettingsBank.RollPI.Kp = kp_o;
stabSettingsBank.RollPI.Ki = ki_o;
break;
case 1: // Pitch
stabSettingsBank.PitchRatePID.Kp = kp;
stabSettingsBank.PitchRatePID.Ki = ki;
stabSettingsBank.PitchRatePID.Kd = kd;
stabSettingsBank.PitchPI.Kp = kp_o;
stabSettingsBank.PitchPI.Ki = ki_o;
break;
case 2: // Yaw
stabSettingsBank.YawRatePID.Kp = kp;
stabSettingsBank.YawRatePID.Ki = ki;
stabSettingsBank.YawRatePID.Kd = kd;
#if 0
// if we ever choose to use these
// (e.g. mag yaw attitude)
// here they are
stabSettingsBank.YawPI.Kp = kp_o;
stabSettingsBank.YawPI.Ki = ki_o;
#endif
break;
}
}
// Librepilot might do something more with this some time
// stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d);
// SystemIdentSettingsDerivativeCutoffSet(&systemIdentSettings.DerivativeCutoff);
// Save PIDs to UAVO RAM (not permanently yet)
switch (systemIdentSettings.DestinationPidBank) {
case 1:
StabilizationSettingsBank1Set((void *)&stabSettingsBank);
break;
case 2:
StabilizationSettingsBank2Set((void *)&stabSettingsBank);
break;
case 3:
StabilizationSettingsBank3Set((void *)&stabSettingsBank);
break;
}
}
// calculate PIDs using default smooth-quick settings
static void ComputeStabilizationAndSetPids()
{
ComputeStabilizationAndSetPidsFromDampAndNoise(systemIdentSettings.DampRate, systemIdentSettings.NoiseRate);
}
// scale the damp and the noise to generate PIDs according to how a slider or other user specified ratio is set
//
// when val is half way between min and max, it generates the default PIDs
// when val is min, it generates the smoothest configured PIDs
// when val is max, it generates the quickest configured PIDs
//
// when val is between min and (min+max)/2, it scales val over the range [min, (min+max)/2] to generate PIDs between smoothest and default
// when val is between (min+max)/2 and max, it scales val over the range [(min+max)/2, max] to generate PIDs between default and quickest
//
// this is done piecewise because we are not guaranteed that default-min == max-default
// but we are given that [smoothDamp,smoothNoise] [defaultDamp,defaultNoise] [quickDamp,quickNoise] are all good parameterizations
// this code guarantees that we will get those exact parameterizations at (val =) min, (max+min)/2, and max
static void ProportionPidsSmoothToQuick(float min, float val, float max)
{
float ratio, damp, noise;
// translate from range [min, max] to range [0, max-min]
// that takes care of min < 0 case too
val -= min;
max -= min;
ratio = val / max;
if (ratio <= 0.5f) {
// scale ratio in [0,0.5] to produce PIDs in [smoothest,default]
ratio *= 2.0f;
damp = (systemIdentSettings.DampMax * (1.0f - ratio)) + (systemIdentSettings.DampRate * ratio);
noise = (systemIdentSettings.NoiseMin * (1.0f - ratio)) + (systemIdentSettings.NoiseRate * ratio);
} else {
// scale ratio in [0.5,1.0] to produce PIDs in [default,quickest]
ratio = (ratio - 0.5f) * 2.0f;
damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio);
noise = (systemIdentSettings.NoiseRate * (1.0f - ratio)) + (systemIdentSettings.NoiseMax * ratio);
}
ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise);
}
/**
* Prediction step for EKF on control inputs to quad that
* learns the system properties
* @param X the current state estimate which is updated in place
* @param P the current covariance matrix, updated in place
* @param[in] the current control inputs (roll, pitch, yaw)
* @param[in] the gyro measurements
*/
__attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in)
{
const float Ts = dT_s;
const float Tsq = Ts * Ts;
const float Tsq3 = Tsq * Ts;
const float Tsq4 = Tsq * Tsq;
// for convenience and clarity code below uses the named versions of
// the state variables
float w1 = X[0]; // roll rate estimate
float w2 = X[1]; // pitch rate estimate
float w3 = X[2]; // yaw rate estimate
float u1 = X[3]; // scaled roll torque
float u2 = X[4]; // scaled pitch torque
float u3 = X[5]; // scaled yaw torque
const float e_b1 = expapprox(X[6]); // roll torque scale
const float b1 = X[6];
const float e_b2 = expapprox(X[7]); // pitch torque scale
const float b2 = X[7];
const float e_b3 = expapprox(X[8]); // yaw torque scale
const float b3 = X[8];
const float e_tau = expapprox(X[9]); // time response of the motors
const float tau = X[9];
const float bias1 = X[10]; // bias in the roll torque
const float bias2 = X[11]; // bias in the pitch torque
const float bias3 = X[12]; // bias in the yaw torque
// inputs to the system (roll, pitch, yaw)
const float u1_in = 4 * t_in * u_in[0];
const float u2_in = 4 * t_in * u_in[1];
const float u3_in = 4 * t_in * u_in[2];
// measurements from gyro
const float gyro_x = gyro[0];
const float gyro_y = gyro[1];
const float gyro_z = gyro[2];
// update named variables because we want to use predicted
// values below
w1 = X[0] = w1 - Ts * bias1 * e_b1 + Ts * u1 * e_b1;
w2 = X[1] = w2 - Ts * bias2 * e_b2 + Ts * u2 * e_b2;
w3 = X[2] = w3 - Ts * bias3 * e_b3 + Ts * u3 * e_b3;
u1 = X[3] = (Ts * u1_in) / (Ts + e_tau) + (u1 * e_tau) / (Ts + e_tau);
u2 = X[4] = (Ts * u2_in) / (Ts + e_tau) + (u2 * e_tau) / (Ts + e_tau);
u3 = X[5] = (Ts * u3_in) / (Ts + e_tau) + (u3 * e_tau) / (Ts + e_tau);
// X[6] to X[12] unchanged
/**** filter parameters ****/
const float q_w = 1e-3f;
const float q_ud = 1e-3f;
const float q_B = 1e-6f;
const float q_tau = 1e-6f;
const float q_bias = 1e-19f;
const float s_a = 150.0f; // expected gyro measurment noise
const float Q[AF_NUMX] = { q_w, q_w, q_w, q_ud, q_ud, q_ud, q_B, q_B, q_B, q_tau, q_bias, q_bias, q_bias };
float D[AF_NUMP];
for (uint32_t i = 0; i < AF_NUMP; i++) {
D[i] = P[i];
}
const float e_tau2 = e_tau * e_tau;
const float e_tau3 = e_tau * e_tau2;
const float e_tau4 = e_tau2 * e_tau2;
const float Ts_e_tau2 = (Ts + e_tau) * (Ts + e_tau);
const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2;
// covariance propagation - D is stored copy of covariance
P[0] = D[0] + Q[0] + 2 * Ts * e_b1 * (D[3] - D[28] - D[9] * bias1 + D[9] * u1)
+ Tsq * (e_b1 * e_b1) * (D[4] - 2 * D[29] + D[32] - 2 * D[10] * bias1 + 2 * D[30] * bias1 + 2 * D[10] * u1 - 2 * D[30] * u1
+ D[11] * (bias1 * bias1) + D[11] * (u1 * u1) - 2 * D[11] * bias1 * u1);
P[1] = D[1] + Q[1] + 2 * Ts * e_b2 * (D[5] - D[33] - D[12] * bias2 + D[12] * u2)
+ Tsq * (e_b2 * e_b2) * (D[6] - 2 * D[34] + D[37] - 2 * D[13] * bias2 + 2 * D[35] * bias2 + 2 * D[13] * u2 - 2 * D[35] * u2
+ D[14] * (bias2 * bias2) + D[14] * (u2 * u2) - 2 * D[14] * bias2 * u2);
P[2] = D[2] + Q[2] + 2 * Ts * e_b3 * (D[7] - D[38] - D[15] * bias3 + D[15] * u3)
+ Tsq * (e_b3 * e_b3) * (D[8] - 2 * D[39] + D[42] - 2 * D[16] * bias3 + 2 * D[40] * bias3 + 2 * D[16] * u3 - 2 * D[40] * u3
+ D[17] * (bias3 * bias3) + D[17] * (u3 * u3) - 2 * D[17] * bias3 * u3);
P[3] = (D[3] * (e_tau2 + Ts * e_tau) + Ts * e_b1 * e_tau2 * (D[4] - D[29]) + Tsq * e_b1 * e_tau * (D[4] - D[29])
+ D[18] * Ts * e_tau * (u1 - u1_in) + D[10] * e_b1 * (u1 * (Ts * e_tau2 + Tsq * e_tau) - bias1 * (Ts * e_tau2 + Tsq * e_tau))
+ D[21] * Tsq * e_b1 * e_tau * (u1 - u1_in) + D[31] * Tsq * e_b1 * e_tau * (u1_in - u1)
+ D[24] * Tsq * e_b1 * e_tau * (u1 * (u1 - bias1) + u1_in * (bias1 - u1))) / Ts_e_tau2;
P[4] = (Q[3] * Tsq4 + e_tau4 * (D[4] + Q[3]) + 2 * Ts * e_tau3 * (D[4] + 2 * Q[3]) + 4 * Q[3] * Tsq3 * e_tau
+ Tsq * e_tau2 * (D[4] + 6 * Q[3] + u1 * (D[27] * u1 + 2 * D[21]) + u1_in * (D[27] * u1_in - 2 * D[21]))
+ 2 * D[21] * Ts * e_tau3 * (u1 - u1_in) - 2 * D[27] * Tsq * u1 * u1_in * e_tau2) / Ts_e_tau4;
P[5] = (D[5] * (e_tau2 + Ts * e_tau) + Ts * e_b2 * e_tau2 * (D[6] - D[34])
+ Tsq * e_b2 * e_tau * (D[6] - D[34]) + D[19] * Ts * e_tau * (u2 - u2_in)
+ D[13] * e_b2 * (u2 * (Ts * e_tau2 + Tsq * e_tau) - bias2 * (Ts * e_tau2 + Tsq * e_tau))
+ D[22] * Tsq * e_b2 * e_tau * (u2 - u2_in) + D[36] * Tsq * e_b2 * e_tau * (u2_in - u2)
+ D[25] * Tsq * e_b2 * e_tau * (u2 * (u2 - bias2) + u2_in * (bias2 - u2))) / Ts_e_tau2;
P[6] = (Q[4] * Tsq4 + e_tau4 * (D[6] + Q[4]) + 2 * Ts * e_tau3 * (D[6] + 2 * Q[4]) + 4 * Q[4] * Tsq3 * e_tau
+ Tsq * e_tau2 * (D[6] + 6 * Q[4] + u2 * (D[27] * u2 + 2 * D[22]) + u2_in * (D[27] * u2_in - 2 * D[22]))
+ 2 * D[22] * Ts * e_tau3 * (u2 - u2_in) - 2 * D[27] * Tsq * u2 * u2_in * e_tau2) / Ts_e_tau4;
P[7] = (D[7] * (e_tau2 + Ts * e_tau) + Ts * e_b3 * e_tau2 * (D[8] - D[39])
+ Tsq * e_b3 * e_tau * (D[8] - D[39]) + D[20] * Ts * e_tau * (u3 - u3_in)
+ D[16] * e_b3 * (u3 * (Ts * e_tau2 + Tsq * e_tau) - bias3 * (Ts * e_tau2 + Tsq * e_tau))
+ D[23] * Tsq * e_b3 * e_tau * (u3 - u3_in) + D[41] * Tsq * e_b3 * e_tau * (u3_in - u3)
+ D[26] * Tsq * e_b3 * e_tau * (u3 * (u3 - bias3) + u3_in * (bias3 - u3))) / Ts_e_tau2;
P[8] = (Q[5] * Tsq4 + e_tau4 * (D[8] + Q[5]) + 2 * Ts * e_tau3 * (D[8] + 2 * Q[5]) + 4 * Q[5] * Tsq3 * e_tau
+ Tsq * e_tau2 * (D[8] + 6 * Q[5] + u3 * (D[27] * u3 + 2 * D[23]) + u3_in * (D[27] * u3_in - 2 * D[23]))
+ 2 * D[23] * Ts * e_tau3 * (u3 - u3_in) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2) / Ts_e_tau4;
P[9] = D[9] - Ts * e_b1 * (D[30] - D[10] + D[11] * (bias1 - u1));
P[10] = (D[10] * (Ts + e_tau) + D[24] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2);
P[11] = D[11] + Q[6];
P[12] = D[12] - Ts * e_b2 * (D[35] - D[13] + D[14] * (bias2 - u2));
P[13] = (D[13] * (Ts + e_tau) + D[25] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2);
P[14] = D[14] + Q[7];
P[15] = D[15] - Ts * e_b3 * (D[40] - D[16] + D[17] * (bias3 - u3));
P[16] = (D[16] * (Ts + e_tau) + D[26] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2);
P[17] = D[17] + Q[8];
P[18] = D[18] - Ts * e_b1 * (D[31] - D[21] + D[24] * (bias1 - u1));
P[19] = D[19] - Ts * e_b2 * (D[36] - D[22] + D[25] * (bias2 - u2));
P[20] = D[20] - Ts * e_b3 * (D[41] - D[23] + D[26] * (bias3 - u3));
P[21] = (D[21] * (Ts + e_tau) + D[27] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2);
P[22] = (D[22] * (Ts + e_tau) + D[27] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2);
P[23] = (D[23] * (Ts + e_tau) + D[27] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2);
P[24] = D[24];
P[25] = D[25];
P[26] = D[26];
P[27] = D[27] + Q[9];
P[28] = D[28] - Ts * e_b1 * (D[32] - D[29] + D[30] * (bias1 - u1));
P[29] = (D[29] * (Ts + e_tau) + D[31] * Ts * (u1 - u1_in)) * (e_tau / Ts_e_tau2);
P[30] = D[30];
P[31] = D[31];
P[32] = D[32] + Q[10];
P[33] = D[33] - Ts * e_b2 * (D[37] - D[34] + D[35] * (bias2 - u2));
P[34] = (D[34] * (Ts + e_tau) + D[36] * Ts * (u2 - u2_in)) * (e_tau / Ts_e_tau2);
P[35] = D[35];
P[36] = D[36];
P[37] = D[37] + Q[11];
P[38] = D[38] - Ts * e_b3 * (D[42] - D[39] + D[40] * (bias3 - u3));
P[39] = (D[39] * (Ts + e_tau) + D[41] * Ts * (u3 - u3_in)) * (e_tau / Ts_e_tau2);
P[40] = D[40];
P[41] = D[41];
P[42] = D[42] + Q[12];
/********* this is the update part of the equation ***********/
float S[3] = { P[0] + s_a, P[1] + s_a, P[2] + s_a };
X[0] = w1 + P[0] * ((gyro_x - w1) / S[0]);
X[1] = w2 + P[1] * ((gyro_y - w2) / S[1]);
X[2] = w3 + P[2] * ((gyro_z - w3) / S[2]);
X[3] = u1 + P[3] * ((gyro_x - w1) / S[0]);
X[4] = u2 + P[5] * ((gyro_y - w2) / S[1]);
X[5] = u3 + P[7] * ((gyro_z - w3) / S[2]);
X[6] = b1 + P[9] * ((gyro_x - w1) / S[0]);
X[7] = b2 + P[12] * ((gyro_y - w2) / S[1]);
X[8] = b3 + P[15] * ((gyro_z - w3) / S[2]);
X[9] = tau + P[18] * ((gyro_x - w1) / S[0]) + P[19] * ((gyro_y - w2) / S[1]) + P[20] * ((gyro_z - w3) / S[2]);
X[10] = bias1 + P[28] * ((gyro_x - w1) / S[0]);
X[11] = bias2 + P[33] * ((gyro_y - w2) / S[1]);
X[12] = bias3 + P[38] * ((gyro_z - w3) / S[2]);
// update the duplicate cache
for (uint32_t i = 0; i < AF_NUMP; i++) {
D[i] = P[i];
}
// This is an approximation that removes some cross axis uncertainty but
// substantially reduces the number of calculations
P[0] = -D[0] * (D[0] / S[0] - 1);
P[1] = -D[1] * (D[1] / S[1] - 1);
P[2] = -D[2] * (D[2] / S[2] - 1);
P[3] = -D[3] * (D[0] / S[0] - 1);
P[4] = D[4] - D[3] * D[3] / S[0];
P[5] = -D[5] * (D[1] / S[1] - 1);
P[6] = D[6] - D[5] * D[5] / S[1];
P[7] = -D[7] * (D[2] / S[2] - 1);
P[8] = D[8] - D[7] * D[7] / S[2];
P[9] = -D[9] * (D[0] / S[0] - 1);
P[10] = D[10] - D[3] * (D[9] / S[0]);
P[11] = D[11] - D[9] * (D[9] / S[0]);
P[12] = -D[12] * (D[1] / S[1] - 1);
P[13] = D[13] - D[5] * (D[12] / S[1]);
P[14] = D[14] - D[12] * (D[12] / S[1]);
P[15] = -D[15] * (D[2] / S[2] - 1);
P[16] = D[16] - D[7] * (D[15] / S[2]);
P[17] = D[17] - D[15] * (D[15] / S[2]);
P[18] = -D[18] * (D[0] / S[0] - 1);
P[19] = -D[19] * (D[1] / S[1] - 1);
P[20] = -D[20] * (D[2] / S[2] - 1);
P[21] = D[21] - D[3] * (D[18] / S[0]);
P[22] = D[22] - D[5] * (D[19] / S[1]);
P[23] = D[23] - D[7] * (D[20] / S[2]);
P[24] = D[24] - D[9] * (D[18] / S[0]);
P[25] = D[25] - D[12] * (D[19] / S[1]);
P[26] = D[26] - D[15] * (D[20] / S[2]);
P[27] = D[27] - D[18] * (D[18] / S[0]) - D[19] * (D[19] / S[1]) - D[20] * (D[20] / S[2]);
P[28] = -D[28] * (D[0] / S[0] - 1);
P[29] = D[29] - D[3] * (D[28] / S[0]);
P[30] = D[30] - D[9] * (D[28] / S[0]);
P[31] = D[31] - D[18] * (D[28] / S[0]);
P[32] = D[32] - D[28] * (D[28] / S[0]);
P[33] = -D[33] * (D[1] / S[1] - 1);
P[34] = D[34] - D[5] * (D[33] / S[1]);
P[35] = D[35] - D[12] * (D[33] / S[1]);
P[36] = D[36] - D[19] * (D[33] / S[1]);
P[37] = D[37] - D[33] * (D[33] / S[1]);
P[38] = -D[38] * (D[2] / S[2] - 1);
P[39] = D[39] - D[7] * (D[38] / S[2]);
P[40] = D[40] - D[15] * (D[38] / S[2]);
P[41] = D[41] - D[20] * (D[38] / S[2]);
P[42] = D[42] - D[38] * (D[38] / S[2]);
// apply limits to some of the state variables
if (X[9] > -1.5f) {
X[9] = -1.5f;
} else if (X[9] < -5.5f) { /* 4ms */
X[9] = -5.5f;
}
if (X[10] > 0.5f) {
X[10] = 0.5f;
} else if (X[10] < -0.5f) {
X[10] = -0.5f;
}
if (X[11] > 0.5f) {
X[11] = 0.5f;
} else if (X[11] < -0.5f) {
X[11] = -0.5f;
}
if (X[12] > 0.5f) {
X[12] = 0.5f;
} else if (X[12] < -0.5f) {
X[12] = -0.5f;
}
}
/**
* Initialize the state variable and covariance matrix
* for the system identification EKF
*/
static void AfInit(float X[AF_NUMX], float P[AF_NUMP])
{
static const float qInit[AF_NUMX] = {
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
0.05f, 0.05f, 0.005f,
0.05f,
0.05f, 0.05f, 0.05f
};
// X[0] = X[1] = X[2] = 0.0f; // assume no rotation
// X[3] = X[4] = X[5] = 0.0f; // and no net torque
// X[6] = X[7] = 10.0f; // roll and pitch medium amount of strength
// X[8] = 7.0f; // yaw strength
// X[9] = -4.0f; // and 50 (18?) ms time scale
// X[10] = X[11] = X[12] = 0.0f; // zero bias
memset(X, 0, AF_NUMX * sizeof(X[0]));
// get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau)
// so that if they are changed there (mainly for future code changes), they will be changed here too
memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta));
X[9] = systemIdentState.Tau;
// P initialization
memset(P, 0, AF_NUMP * sizeof(P[0]));
P[0] = qInit[0];
P[1] = qInit[1];
P[2] = qInit[2];
P[4] = qInit[3];
P[6] = qInit[4];
P[8] = qInit[5];
P[11] = qInit[6];
P[14] = qInit[7];
P[17] = qInit[8];
P[27] = qInit[9];
P[32] = qInit[10];
P[37] = qInit[11];
P[42] = qInit[12];
}
/**
* @}
* @}
*/
Reference in New Issue View Git Blame Copy Permalink