rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

LP-76 get ready for PR uncrustify and comments

Browse Source
This commit is contained in:
Cliff Geerdes 2016-03-14 16:58:06 -04:00
parent b48ed29fc5
commit fd22011bfa
8 changed files with 476 additions and 725 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/libraries/sanitycheck.c
View File

@ -274,12 +274,12 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
// we want to be able to use systemident with or without autotune
// If this axis allows enabling an autotune behavior without the module
// running then set an alarm now that aututune module initializes the
// running then set an alarm now that autotune module initializes the
// appropriate objects
//if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) &&
// (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))) {
// return false;
//}
// if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) &&
// (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))) {
// return false;
// }
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
}
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
@ -294,7 +294,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
// For multirotors verify that roll/pitch are either attitude or rattitude
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
return false;
}
}

857
flight/modules/AutoTune/autotune.c
View File

@ -34,26 +34,26 @@
* 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 "gyrosensor.h"
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "systemidentsettings.h"
#include "systemidentstate.h"
#include <openpilot.h>
#include <pios.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <manualcontrolsettings.h>
#include <gyrosensor.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"
#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
@ -68,43 +68,43 @@
#undef STACK_SIZE_BYTES
// Nano locks up it seems in UAVObjSav() with 1340
// why did it lock up? 1540 now works (after a long initial delay) with 360 bytes left
#define STACK_SIZE_BYTES 1340
//#define TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define STACK_SIZE_BYTES 1340
// #define TASK_PRIORITY PIOS_THREAD_PRIO_NORMAL
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define AF_NUMX 13
#define AF_NUMP 43
#define AF_NUMX 13
#define AF_NUMP 43
#if !defined(AT_QUEUE_NUMELEM)
#define AT_QUEUE_NUMELEM 18
#define AT_QUEUE_NUMELEM 18
#endif
#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 */
#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 */
#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 ROLL_BETA_LOW 1
#define PITCH_BETA_LOW 2
#define YAW_BETA_LOW 4
#define TAU_TOO_LONG 8
#define TAU_TOO_SHORT 16
#define SMOOTH_QUICK_DISABLED 0
#define SMOOTH_QUICK_DISABLED 0
#define SMOOTH_QUICK_ACCESSORY_BASE 10
#define SMOOTH_QUICK_TOGGLE_BASE 21
// Private types <access gcs="readwrite" flight="readwrite"/>
// 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 */
float y[3]; /* Gyro measurements */
float u[3]; /* Actuator desired */
float throttle; /* Throttle desired */
uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */
uint32_t raw_time; /* From PIOS_DELAY_GetRaw() */
};
@ -116,8 +116,8 @@ 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 float gX[AF_NUMX] = { 0 };
static float gP[AF_NUMP] = { 0 };
SystemIdentSettingsData systemIdentSettings;
SystemIdentStateData systemIdentState;
int8_t accessoryToUse;
@ -133,7 +133,7 @@ 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 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);
@ -180,9 +180,9 @@ 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);
// taskHandle = PIOS_Thread_Create(AutoTuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
// TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
// PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
GyroSensorConnectCallback(AtNewGyroData);
xTaskCreate(AutoTuneTask, "AutoTune", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
@ -200,10 +200,10 @@ MODULE_INITCALL(AutoTuneInitialize, AutoTuneStart);
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;
bool saveSiNeeded = false;
uint32_t lastUpdateTime = 0; // initialization is only for compiler warning
float noise[3] = { 0 };
uint32_t lastTime = 0.0f;
bool saveSiNeeded = false;
bool savePidNeeded = false;
// get max attitude / max rate
@ -222,12 +222,12 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
uint32_t diffTime;
uint32_t measureTime = 60000;
bool doingIdent = false;
bool canSleep = true;
bool canSleep = true;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// I have never seen this module misbehave so not bothering making a watchdog
//PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
// PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
if (saveSiNeeded) {
@ -258,7 +258,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// and the data gathering is complete
// and the data gathered is good
// note: CheckFlightModeSwitchForPidRequest(mode) only returns true if mode is not autotune
if (flightModeSwitchTogglePosition!=-1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)
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... or 1,2,0...
@ -272,8 +272,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
flightModeSwitchTogglePosition = (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2;
}
ProportionPidsSmoothToQuick(0.0f,
(float) flightModeSwitchTogglePosition,
(float) (systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE));
(float)flightModeSwitchTogglePosition,
(float)(systemIdentSettings.SmoothQuick - SMOOTH_QUICK_TOGGLE_BASE));
savePidNeeded = true;
}
@ -293,10 +293,10 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
AccessoryDesiredInstGet(accessoryToUse, &accessoryValue);
// if the accessory changed more than 1/900
// (this test is intended to remove one unit jitter)
if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (1.0f/900.0f)) {
if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (1.0f / 900.0f)) {
accessoryValueOld = accessoryValue;
ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f);
savePidNeeded = true;
savePidNeeded = true;
}
}
state = AT_INIT;
@ -304,7 +304,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
continue;
}
switch(state) {
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
@ -350,7 +350,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
// Reset save status
// save SI data even if partial or bad, aids in diagnostics
saveSiNeeded = true;
saveSiNeeded = true;
// don't save PIDs until data gathering is complete
// and the complete data has been sanity checked
savePidNeeded = false;
@ -365,27 +365,27 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
break;
case AT_START:
lastTime = PIOS_DELAY_GetRaw();
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;
updateCounter = 0;
atPointsSpilled = 0;
throttleAccumulator = 0;
state = AT_RUN;
lastUpdateTime = xTaskGetTickCount();
lastUpdateTime = xTaskGetTickCount();
break;
case AT_RUN:
diffTime = xTaskGetTickCount() - lastUpdateTime;
diffTime = xTaskGetTickCount() - lastUpdateTime;
doingIdent = true;
canSleep = false;
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++) {
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) {
@ -396,22 +396,22 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
/* 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 */
* 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]);
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 hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f;
float hover_throttle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
UpdateSystemIdentState(gX, noise, dT_s, updateCounter, atPointsSpilled, hover_throttle);
}
}
@ -431,7 +431,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// 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;
systemIdentState.Complete = true;
}
// always raise an alarm if sanity checks failed
// even if disabling sanity checks
@ -443,7 +443,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits);
}
}
float hover_throttle = ((float)(throttleAccumulator/updateCounter))/10000.0f;
float hover_throttle = ((float)(throttleAccumulator / updateCounter)) / 10000.0f;
UpdateSystemIdentState(gX, noise, 0, updateCounter, atPointsSpilled, hover_throttle);
SystemIdentSettingsSet(&systemIdentSettings);
state = AT_WAITING;
@ -469,13 +469,14 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// gyro sensor callback
// get gyro data and actuatordesired into a packet
// and put it in the queue for later processing
static void AtNewGyroData(UAVObjEvent * ev) {
static void AtNewGyroData(UAVObjEvent *ev)
{
static struct at_queued_data q_item;
static bool last_sample_unpushed = false;
GyroSensorData gyro;
ActuatorDesiredData actuators;
if (!ev || !ev->obj || ev->instId!=0 || ev->event!=EV_UPDATED) {
if (!ev || !ev->obj || ev->instId != 0 || ev->event != EV_UPDATED) {
return;
}
@ -493,12 +494,12 @@ 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.u[0] = actuators.Roll;
q_item.u[1] = actuators.Pitch;
q_item.u[2] = actuators.Yaw;
q_item.y[0] = gyro.x;
q_item.y[1] = gyro.y;
q_item.y[2] = gyro.z;
q_item.u[0] = actuators.Roll;
q_item.u[1] = actuators.Pitch;
q_item.u[2] = actuators.Yaw;
q_item.throttle = actuators.Thrust;
if (xQueueSend(atQueue, &q_item, 0) != pdTRUE) {
@ -514,7 +515,8 @@ static void AtNewGyroData(UAVObjEvent * ev) {
// 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 bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode)
{
static uint32_t lastUpdateTime;
static uint8_t flightModePrev;
static uint8_t counter;
@ -523,7 +525,7 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) {
// only count transitions into and out of autotune
if ((flightModePrev == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ^ (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE)) {
flightModePrev = flightMode;
updateTime = xTaskGetTickCount();
updateTime = xTaskGetTickCount();
// if it has been over 2 seconds, reset the counter
if (updateTime - lastUpdateTime > 2000) {
counter = 0;
@ -548,7 +550,8 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) {
// 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) {
static void InitSystemIdent(bool loadDefaults)
{
SystemIdentSettingsGet(&systemIdentSettings);
uint8_t smoothQuick = systemIdentSettings.SmoothQuick;
@ -572,20 +575,20 @@ static void InitSystemIdent(bool loadDefaults) {
// default to disable PID changing with flight mode switch and accessory0-3
accessoryToUse = -1;
flightModeSwitchTogglePosition = -1;
flightModeSwitchTogglePosition = -1;
systemIdentSettings.SmoothQuick = SMOOTH_QUICK_DISABLED;
switch (smoothQuick) {
case SMOOTH_QUICK_ACCESSORY_BASE+0: // use accessory0
case SMOOTH_QUICK_ACCESSORY_BASE+1: // use accessory1
case SMOOTH_QUICK_ACCESSORY_BASE+2: // use accessory2
case SMOOTH_QUICK_ACCESSORY_BASE+3: // use accessory3
case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0
case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
accessoryToUse = smoothQuick - SMOOTH_QUICK_ACCESSORY_BASE;
systemIdentSettings.SmoothQuick = smoothQuick;
break;
case SMOOTH_QUICK_TOGGLE_BASE+2: // use flight mode switch toggle with 3 points
case SMOOTH_QUICK_TOGGLE_BASE+4: // use flight mode switch toggle with 5 points
case SMOOTH_QUICK_TOGGLE_BASE + 2: // use flight mode switch toggle with 3 points
case SMOOTH_QUICK_TOGGLE_BASE + 4: // use flight mode switch toggle with 5 points
// first test PID is in the middle of the smooth -> quick range
flightModeSwitchTogglePosition = (smoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2;
flightModeSwitchTogglePosition = (smoothQuick - SMOOTH_QUICK_TOGGLE_BASE) / 2;
systemIdentSettings.SmoothQuick = smoothQuick;
break;
}
@ -596,30 +599,28 @@ static void InitSystemIdent(bool loadDefaults) {
// 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];
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;
systemIdentSettings.Tau = systemIdentState.Tau;
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
if (noise) {
systemIdentState.Noise.Roll = noise[0];
systemIdentState.Noise.Pitch = noise[1];
systemIdentState.Noise.Yaw = noise[2];
}
systemIdentState.Period = dT_s * 1000.0f;
systemIdentState.NumAfPredicts = predicts;
systemIdentState.NumSpilledPts = spills;
systemIdentState.HoverThrottle = hover_throttle;
SystemIdentStateSet(&systemIdentState);
@ -628,14 +629,16 @@ static void UpdateSystemIdentState(const float *X, const float *noise,
// 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) {
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.Roll = manualControlCommand.Roll * rollMax;
stabDesired.Pitch = manualControlCommand.Pitch * pitchMax;
stabDesired.Yaw = manualControlCommand.Yaw * manualRate.Yaw;
stabDesired.Thrust = manualControlCommand.Thrust;
if (doingIdent) {
@ -647,7 +650,7 @@ static void UpdateStabilizationDesired(bool doingIdent) {
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}
@ -694,8 +697,11 @@ static uint8_t CheckSettings()
uint8_t retVal = CheckSettingsRaw();
if (systemIdentSettings.DisableSanityChecks) {
retVal = 0;
} else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) {
retVal = 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;
}
@ -724,51 +730,51 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
// These three parameters define the desired response properties
// - rate scale in the fraction of the natural speed of the system
// to strive for.
// to strive for.
// - damp is the amount of damping in the system. higher values
// make oscillations less likely
// 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;
// of noise
const double ghf = (double)noiseRate / 1000.0d;
const double damp = (double)dampRate / 100.0d;
double tau = exp(systemIdentState.Tau);
double exp_beta_roll_times_ghf = exp(systemIdentState.Beta.Roll)*ghf;
double exp_beta_pitch_times_ghf = exp(systemIdentState.Beta.Pitch)*ghf;
double 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 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);
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);
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);
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 kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d / wn);
// dRonin simply uses default PID settings for yaw
float kpMax = 0.0f;
for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) {
double beta = exp(SystemIdentStateBetaToArray(systemIdentState.Beta)[i]);
double ki = a * b * wn * wn * tau * tau_d / beta;
double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d;
double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d;
double ki = a * b * wn * wn * tau * tau_d / beta;
double kp = tau * tau_d * ((a + b) * wn * wn + 2.0d * a * b * damp * wn) / beta - ki * tau_d;
double kd = (tau * tau_d * (a * b + wn * wn + (a + b) * 2.0d * damp * wn) - 1.0d) / beta - kp * tau_d;
if (i<2) {
if (kpMax < (float) kp) {
kpMax = (float) kp;
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
@ -789,17 +795,17 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
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;
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;
kp /= (double)ratio;
ki /= (double)ratio;
kd /= (double)ratio;
}
switch(i) {
switch (i) {
case 0: // Roll
stabSettingsBank.RollRatePID.Kp = kp;
stabSettingsBank.RollRatePID.Ki = ki;
@ -811,8 +817,8 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
stabSettingsBank.PitchRatePID.Kp = kp;
stabSettingsBank.PitchRatePID.Ki = ki;
stabSettingsBank.PitchRatePID.Kd = kd;
stabSettingsBank.PitchPI.Kp = kp_o;
stabSettingsBank.PitchPI.Ki = 0;
stabSettingsBank.PitchPI.Kp = kp_o;
stabSettingsBank.PitchPI.Ki = 0;
break;
case 2: // Yaw
stabSettingsBank.YawRatePID.Kp = kp;
@ -860,25 +866,26 @@ 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)
{
float ratio, damp, noise;
// translate from range [min, max] to range [0, max-min]
// takes care of min < 0 too
val -= min;
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);
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);
damp = (systemIdentSettings.DampRate * (1.0f - ratio)) + (systemIdentSettings.DampMin * ratio);
noise = (systemIdentSettings.NoiseRate * (1.0f - ratio)) + (systemIdentSettings.NoiseMax * ratio);
}
@ -896,35 +903,35 @@ static void ProportionPidsSmoothToQuick(float min, float val, float max)
*/
__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 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
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];
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];
@ -933,27 +940,28 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
// 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);
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_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};
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++)
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;
@ -961,304 +969,310 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
const float Ts_e_tau2 = (Ts + e_tau) * (Ts + e_tau);
const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2;
// expanded with indentation for easier reading but uncrustify even damages comments
#if 0
// covariance propagation - D is stored copy of covariance
P[0] = D[0] + Q[0] + 2*Ts*e_b1*(
D[3] - D[28] - D[9]*bias1 + D[9]*u1
) + Tsq*(e_b1*e_b1)*(
D[4] - 2*D[29] + D[32] - 2*D[10]*bias1 + 2*D[30]*bias1 + 2*D[10]*u1
- 2*D[30]*u1 + D[11]*(bias1*bias1) + D[11]*(u1*u1) - 2*D[11]*bias1*u1
);
P[1] = D[1] + Q[1] + 2*Ts*e_b2*(
D[5] - D[33] - D[12]*bias2 + D[12]*u2
) + Tsq*(e_b2*e_b2)*(
D[6] - 2*D[34] + D[37] - 2*D[13]*bias2 + 2*D[35]*bias2 + 2*D[13]*u2
- 2*D[35]*u2 + D[14]*(bias2*bias2) + D[14]*(u2*u2) - 2*D[14]*bias2*u2
);
P[2] = D[2] + Q[2] + 2*Ts*e_b3*(
D[7] - D[38] - D[15]*bias3 + D[15]*u3
) + Tsq*(e_b3*e_b3)*(
D[8] - 2*D[39] + D[42] - 2*D[16]*bias3 + 2*D[40]*bias3 + 2*D[16]*u3
- 2*D[40]*u3 + D[17]*(bias3*bias3) + D[17]*(u3*u3) - 2*D[17]*bias3*u3
);
P[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[3] * (
e_tau2 + Ts * e_tau
) + Ts * e_b1 * e_tau2 * (
D[4] - D[29]
) + Tsq*e_b1*e_tau*(
) + Tsq * e_b1 * e_tau * (
D[4] - D[29]
) + D[18]*Ts*e_tau*(
) + 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*(
) + 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*(
) + D[31] * Tsq * e_b1 * e_tau * (
u1_in - u1
) + D[24]*Tsq*e_b1*e_tau*(
u1*(
) + D[24] * Tsq * e_b1 * e_tau * (
u1 * (
u1 - bias1
) + u1_in*(
) + u1_in * (
bias1 - u1
)
)
)
) / Ts_e_tau2;
) / Ts_e_tau2;
P[4] = (
Q[3]*Tsq4 + e_tau4*(
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*(
) + 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;
) - 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[5] * (
e_tau2 + Ts * e_tau
) + Ts * e_b2 * e_tau2 * (
D[6] - D[34]
) + Tsq*e_b2*e_tau*(
) + Tsq * e_b2 * e_tau * (
D[6] - D[34]
) + D[19]*Ts*e_tau*(
) + 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*(
) + 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*(
) + D[36] * Tsq * e_b2 * e_tau * (
u2_in - u2
) + D[25]*Tsq*e_b2*e_tau*(
u2*(
) + D[25] * Tsq * e_b2 * e_tau * (
u2 * (
u2 - bias2
) + u2_in*(
) + u2_in * (
bias2 - u2
)
)
)
) / Ts_e_tau2;
) / Ts_e_tau2;
P[6] = (
Q[4]*Tsq4 + e_tau4*(
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*(
) + 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;
) - 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[7] * (
e_tau2 + Ts * e_tau
) + Ts * e_b3 * e_tau2 * (
D[8] - D[39]
) + Tsq*e_b3*e_tau*(
) + Tsq * e_b3 * e_tau * (
D[8] - D[39]
) + D[20]*Ts*e_tau*(
) + 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*(
) + 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*(
) + D[41] * Tsq * e_b3 * e_tau * (
u3_in - u3
) + D[26]*Tsq*e_b3*e_tau*(
u3*(
) + D[26] * Tsq * e_b3 * e_tau * (
u3 * (
u3 - bias3
) + u3_in*(
) + u3_in * (
bias3 - u3
)
)
)
) / Ts_e_tau2;
) / Ts_e_tau2;
P[8] = (
Q[5]*Tsq4 + e_tau4*(
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*(
) + 2 * Ts * e_tau3 * (
D[8] + 2 * Q[5]
) + 4 * Q[5] * Tsq3 * e_tau + Tsq * e_tau2 * (
D[8] + 6 * Q[5] + u3 * (
D[27] * u3 + 2 * D[23]
) + u3_in * (
D[27] * u3_in - 2 * D[23]
)
) + 2 * D[23] * Ts * e_tau3 * (
u3 - u3_in
) - 2*D[27]*Tsq*u3*u3_in*e_tau2
) / Ts_e_tau4;
#endif
) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2
) / Ts_e_tau4;
#endif /* if 0 */
// covariance propagation - D is stored copy of covariance
P[0] = D[0] + Q[0] + 2*Ts*e_b1*(D[3] - D[28] - D[9]*bias1 + D[9]*u1)
+ Tsq*(e_b1*e_b1)*(D[4] - 2*D[29] + D[32] - 2*D[10]*bias1 + 2*D[30]*bias1 + 2*D[10]*u1 - 2*D[30]*u1
+ D[11]*(bias1*bias1) + D[11]*(u1*u1) - 2*D[11]*bias1*u1);
P[1] = D[1] + Q[1] + 2*Ts*e_b2*(D[5] - D[33] - D[12]*bias2 + D[12]*u2)
+ Tsq*(e_b2*e_b2)*(D[6] - 2*D[34] + D[37] - 2*D[13]*bias2 + 2*D[35]*bias2 + 2*D[13]*u2 - 2*D[35]*u2
+ D[14]*(bias2*bias2) + D[14]*(u2*u2) - 2*D[14]*bias2*u2);
P[2] = D[2] + Q[2] + 2*Ts*e_b3*(D[7] - D[38] - D[15]*bias3 + D[15]*u3)
+ Tsq*(e_b3*e_b3)*(D[8] - 2*D[39] + D[42] - 2*D[16]*bias3 + 2*D[40]*bias3 + 2*D[16]*u3 - 2*D[40]*u3
+ D[17]*(bias3*bias3) + D[17]*(u3*u3) - 2*D[17]*bias3*u3);
P[3] = (D[3]*(e_tau2 + Ts*e_tau) + Ts*e_b1*e_tau2*(D[4] - D[29]) + Tsq*e_b1*e_tau*(D[4] - D[29])
+ D[18]*Ts*e_tau*(u1 - u1_in) + D[10]*e_b1*(u1*(Ts*e_tau2 + Tsq*e_tau) - bias1*(Ts*e_tau2 + Tsq*e_tau))
+ D[21]*Tsq*e_b1*e_tau*(u1 - u1_in) + D[31]*Tsq*e_b1*e_tau*(u1_in - u1)
+ D[24]*Tsq*e_b1*e_tau*(u1*(u1 - bias1) + u1_in*(bias1 - u1)))/Ts_e_tau2;
P[4] = (Q[3]*Tsq4 + e_tau4*(D[4] + Q[3]) + 2*Ts*e_tau3*(D[4] + 2*Q[3]) + 4*Q[3]*Tsq3*e_tau
+ Tsq*e_tau2*(D[4] + 6*Q[3] + u1*(D[27]*u1 + 2*D[21]) + u1_in*(D[27]*u1_in - 2*D[21]))
+ 2*D[21]*Ts*e_tau3*(u1 - u1_in) - 2*D[27]*Tsq*u1*u1_in*e_tau2)/Ts_e_tau4;
P[5] = (D[5]*(e_tau2 + Ts*e_tau) + Ts*e_b2*e_tau2*(D[6] - D[34])
+ Tsq*e_b2*e_tau*(D[6] - D[34]) + D[19]*Ts*e_tau*(u2 - u2_in)
+ D[13]*e_b2*(u2*(Ts*e_tau2 + Tsq*e_tau) - bias2*(Ts*e_tau2 + Tsq*e_tau))
+ D[22]*Tsq*e_b2*e_tau*(u2 - u2_in) + D[36]*Tsq*e_b2*e_tau*(u2_in - u2)
+ D[25]*Tsq*e_b2*e_tau*(u2*(u2 - bias2) + u2_in*(bias2 - u2)))/Ts_e_tau2;
P[6] = (Q[4]*Tsq4 + e_tau4*(D[6] + Q[4]) + 2*Ts*e_tau3*(D[6] + 2*Q[4]) + 4*Q[4]*Tsq3*e_tau
+ Tsq*e_tau2*(D[6] + 6*Q[4] + u2*(D[27]*u2 + 2*D[22]) + u2_in*(D[27]*u2_in - 2*D[22]))
+ 2*D[22]*Ts*e_tau3*(u2 - u2_in) - 2*D[27]*Tsq*u2*u2_in*e_tau2)/Ts_e_tau4;
P[7] = (D[7]*(e_tau2 + Ts*e_tau) + Ts*e_b3*e_tau2*(D[8] - D[39])
+ Tsq*e_b3*e_tau*(D[8] - D[39]) + D[20]*Ts*e_tau*(u3 - u3_in)
+ D[16]*e_b3*(u3*(Ts*e_tau2 + Tsq*e_tau) - bias3*(Ts*e_tau2 + Tsq*e_tau))
+ D[23]*Tsq*e_b3*e_tau*(u3 - u3_in) + D[41]*Tsq*e_b3*e_tau*(u3_in - u3)
+ D[26]*Tsq*e_b3*e_tau*(u3*(u3 - bias3) + u3_in*(bias3 - u3)))/Ts_e_tau2;
P[8] = (Q[5]*Tsq4 + e_tau4*(D[8] + Q[5]) + 2*Ts*e_tau3*(D[8] + 2*Q[5]) + 4*Q[5]*Tsq3*e_tau
+ Tsq*e_tau2*(D[8] + 6*Q[5] + u3*(D[27]*u3 + 2*D[23]) + u3_in*(D[27]*u3_in - 2*D[23]))
+ 2*D[23]*Ts*e_tau3*(u3 - u3_in) - 2*D[27]*Tsq*u3*u3_in*e_tau2)/Ts_e_tau4;
P[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[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[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[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[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[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[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[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]);
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++)
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]);
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)
if (X[9] > -1.5f) {
X[9] = -1.5f;
else if (X[9] < -5.5f) /* 4ms */
} else if (X[9] < -5.5f) { /* 4ms */
X[9] = -5.5f;
if (X[10] > 0.5f)
}
if (X[10] > 0.5f) {
X[10] = 0.5f;
else if (X[10] < -0.5f)
} else if (X[10] < -0.5f) {
X[10] = -0.5f;
if (X[11] > 0.5f)
}
if (X[11] > 0.5f) {
X[11] = 0.5f;
else if (X[11] < -0.5f)
} else if (X[11] < -0.5f) {
X[11] = -0.5f;
if (X[12] > 0.5f)
}
if (X[12] > 0.5f) {
X[12] = 0.5f;
else if (X[12] < -0.5f)
} else if (X[12] < -0.5f) {
X[12] = -0.5f;
}
}
@ -1269,8 +1283,8 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
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,
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
@ -1278,28 +1292,25 @@ static void AfInit(float X[AF_NUMX], float P[AF_NUMP])
// X[0] = X[1] = X[2] = 0.0f; // assume no rotation
// X[3] = X[4] = X[5] = 0.0f; // and no net torque
// X[6] = X[7] = 10.0f; // medium amount of strength
// X[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]));
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
//SystemIdentSetDefaults(SystemIdentHandle(), 0);
//SystemIdentBetaArrayGet(&X[6]);
memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta));
//SystemIdentTauGet(&X[9]);
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];
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];

10
flight/modules/ManualControl/manualcontrol.c
View File

@ -538,13 +538,12 @@ static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
*/
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
{
//uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE
|| position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
return (FLIGHTSTATUS_FLIGHTMODEASSIST_NONE);
return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
}
switch (FlightModeAssistMap[position]) {
@ -594,22 +593,19 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD:
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
// this is only for use with stabi mods with althold/vario.
//isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
//break;
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
default:
// this is the default for non stabi modes also
//isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
//break;
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
}
}
break;
}
//return isAssistedFlag;
// return isAssistedFlag;
return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
}
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */

4
flight/modules/ManualControl/stabilizedhandler.c
View File

@ -78,7 +78,8 @@ static float applyExpo(float value, float expo)
*/
void stabilizedHandler(__attribute__((unused)) bool newinit)
{
static bool inited=false;
static bool inited = false;
if (!inited) {
inited = true;
StabilizationDesiredInitialize();
@ -145,6 +146,7 @@ void stabilizedHandler(__attribute__((unused)) bool newinit)
// let autotune.c handle it
// because it must switch to Attitude after <user configurable> seconds
return;
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
default:
// Major error, this should not occur because only enter this block when one of these is true

2
flight/modules/Notify/inc/sequences.h
View File

@ -185,7 +185,7 @@ const LedSequence_t *flightModeMap[] = {
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_LAND],
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
[FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
[FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
};

305
flight/modules/Stabilization/innerloop.c
View File

@ -57,21 +57,21 @@
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define SYSTEM_IDENT_PERIOD ((uint32_t) 75)
#define SYSTEM_IDENT_PERIOD ((uint32_t)75)
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
#define powapprox fastpow
#define expapprox fastexp
#define powapprox fastpow
#define expapprox fastexp
#else
#define powapprox powf
#define expapprox expf
#define powapprox powf
#define expapprox expf
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// Private variables
@ -319,7 +319,7 @@ static void stabilizationInnerloopTask()
pid_scaler scaler = create_pid_scaler(t);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
}
break;
break;
case STABILIZATIONSTATUS_INNERLOOP_ACRO:
{
float stickinput[3];
@ -340,112 +340,25 @@ static void stabilizationInnerloopTask()
break;
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT:
case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT:
{
static int8_t identIteration = 0;
static float identOffsets[3] = {0};
float identOffsets[3];
if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) {
systemIdentTimeVal = PIOS_DELAY_GetRaw();
SystemIdentStateData systemIdentState;
SystemIdentStateGet(&systemIdentState);
// original code used 32 bit identIteration
#if 0
const float SCALE_BIAS = 7.1f;
float roll_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Roll);
float pitch_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch);
float yaw_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Yaw);
identIteration++;
SystemIdentStateData systemIdentState;
if (roll_scale > 0.25f)
roll_scale = 0.25f;
if (pitch_scale > 0.25f)
pitch_scale = 0.25f;
if (yaw_scale > 0.25f)
yaw_scale = 0.25f;
// yaw changes twice a cycle and roll/pitch changes once ?
switch(identIteration & 0x07) {
case 0:
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = yaw_scale;
break;
case 1:
identOffsets[0] = roll_scale;
identOffsets[1] = 0;
identOffsets[2] = 0;
break;
case 2:
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = -yaw_scale;
break;
case 3:
identOffsets[0] = -roll_scale;
identOffsets[1] = 0;
identOffsets[2] = 0;
break;
case 4:
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = yaw_scale;
break;
case 5:
identOffsets[0] = 0;
identOffsets[1] = pitch_scale;
identOffsets[2] = 0;
break;
case 6:
identOffsets[0] = 0;
identOffsets[1] = 0;
identOffsets[2] = -yaw_scale;
break;
case 7:
identOffsets[0] = 0;
identOffsets[1] = -pitch_scale;
identOffsets[2] = 0;
break;
}
#endif
//good stuff here
#if 0
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
float scale[3] = { expapprox(SCALE_BIAS - systemIdentState.Beta.Roll),
expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch),
expapprox(SCALE_BIAS - systemIdentState.Beta.Yaw) };
if (scale[0] > 0.25f)
scale[0] = 0.25f;
if (scale[1] > 0.25f)
scale[1] = 0.25f;
if (scale[2] > 0.25f)
scale[2] = 0.25f;
//why did he do this fsm?
//with yaw changing twice a cycle and roll/pitch changing once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 7;
uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
// if (identIteration & 2) scale[index] = -scale[index];
((uint8_t *)(&scale[index]))[3] ^= (identIteration & 2) << 6;
identOffsets[index] = scale[index];
#endif
// same as stock
#if 1
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
SystemIdentStateGet(&systemIdentState);
systemIdentTimeVal = PIOS_DELAY_GetRaw();
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration + 1) & 7;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 7;
uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
uint8_t index = ((uint8_t[]) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' }
)[identIteration];
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
@ -464,176 +377,6 @@ static void stabilizationInnerloopTask()
// when identIteration==7: identOffsets[1] = -pitch_scale;
// each change has one axis with an offset
// and another axis coming back to zero from having an offset
#endif
// since we are not calculating yaw, remove it and test roll/pitch more frequently
// perhaps this will converge faster
#if 0
const float SCALE_BIAS = 7.1f; /* I hope this isn't actually dependant on loop time */
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 3;
uint8_t index = ((uint8_t []) { '\0', '\0', '\1', '\1' } ) [identIteration];
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if (identIteration & 2) {
scale = -scale;
}
identOffsets[index] = scale;
// this results in:
// when identIteration==0: identOffsets[0] = roll_scale;
// when identIteration==1: identOffsets[1] = pitch_scale;
// when identIteration==2: identOffsets[0] = -roll_scale;
// when identIteration==3: identOffsets[1] = -pitch_scale;
// each change has one axis with an offset
// and another axis coming back to zero from having an offset
#endif
// since we are not calculating yaw, remove it
// for a cleaner roll / pitch signal
#if 0
const float SCALE_BIAS = 7.1f;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) & 7;
uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
//recode to this uint8_t index = identIteration >> 2;
if (identIteration & 1) {
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if (identIteration & 2) {
scale = -scale;
}
identOffsets[index] = scale;
}
// this results in:
// when identIteration==0: no offset
// when identIteration==1: identOffsets[0] = roll_scale;
// when identIteration==2: no offset
// when identIteration==3: identOffsets[0] = -roll_scale;
// when identIteration==4: no offset
// when identIteration==5: identOffsets[1] = pitch_scale;
// when identIteration==6: no offset
// when identIteration==7: identOffsets[1] = -pitch_scale;
// each change is either one axis with an offset
// or one axis coming back to zero from having an offset
#endif
// since we are not calculating yaw, remove it
// for a cleaner roll / pitch signal
#if 0
const float SCALE_BIAS = 7.1f;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) % 12;
// uint8_t index = ((uint8_t []) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' } ) [identIteration];
//recode to this uint8_t index = identIteration >> 2;
#if 0
if (identIteration < 5) {
index = 0;
} else {
index = 1;
}
#endif
uint8_t index = identIteration % 6 / 3;
uint8_t identIterationMod3 = identIteration % 3;
if (identIterationMod3 <= 1) {
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if ((identIterationMod3 == 1) ^ (identIteration >= 6)) {
scale = -scale;
}
identOffsets[index] = scale;
}
// this results in:
// when identIteration== 0: identOffsets[0] = roll_scale;
// when identIteration== 1: identOffsets[0] = -roll_scale;
// when identIteration== 2: no offset
// when identIteration== 3: identOffsets[1] = pitch_scale;
// when identIteration== 4: identOffsets[1] = -pitch_scale;
// when identIteration== 5: no offset
// when identIteration== 6: identOffsets[0] = -roll_scale;
// when identIteration== 7: identOffsets[0] = roll_scale;
// when identIteration== 8: no offset
// when identIteration== 9: identOffsets[1] = -pitch_scale;
// when identIteration==10: identOffsets[1] = pitch_scale;
// when identIteration==11: no offset
//
// each change is either an axis going from zero to +-scale
// or going from +-scale to -+scale
// there is a delay when changing axes
//
// it's not clear whether AfPredict() is designed to handle double scale perturbances on a particular axis
// resulting from -offset to +offset and needs -offset to zero to +offset
// as an EKF it should handle it
#endif
// one axis at a time
// full stroke with delay between axes
// for a cleaner signal
// a little more difficult to fly?
// makes slightly lower PIDs
// yaw pids seem way high and incorrect
#if 0
const float SCALE_BIAS = 7.1f;
// why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f;
identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f;
identIteration = (identIteration+1) % 18;
uint8_t index = identIteration % 9 / 3;
uint8_t identIterationMod3 = identIteration % 3;
// if (identIterationMod3 <= 1) {
{
float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) {
scale = 0.25f;
}
if ((identIterationMod3 == 1) ^ (identIteration >= 9)) {
scale = -scale;
}
identOffsets[index] = scale;
}
// this results in:
// when identIteration== 0: identOffsets[0] = roll_scale;
// when identIteration== 1: identOffsets[0] = -roll_scale;
// when identIteration== 2: no offset
// when identIteration== 3: identOffsets[1] = pitch_scale;
// when identIteration== 4: identOffsets[1] = -pitch_scale;
// when identIteration== 5: no offset
// when identIteration== 6: identOffsets[2] = yaw_scale;
// when identIteration== 7: identOffsets[2] = -yaw_scale;
// when identIteration== 8: no offset
// when identIteration== 9: identOffsets[0] = -roll_scale;
// when identIteration==10: identOffsets[0] = roll_scale;
// when identIteration==11: no offset
// when identIteration==12: identOffsets[1] = -pitch_scale;
// when identIteration==13: identOffsets[1] = pitch_scale;
// when identIteration==14: no offset
// when identIteration==15: identOffsets[2] = -yaw_scale;
// when identIteration==16: identOffsets[2] = yaw_scale;
// when identIteration==17: no offset
//
// each change is either an axis going from zero to +-scale
// or going from +-scale to -+scale
// there is a delay when changing axes
//
// it's not clear whether AfPredict() is designed to handle 2x scale perturbations on a particular axis
// resulting from -offset to +offset and instead needs -offset to zero to +offset
// ... as an EKF it should handle it
#endif
}
rate[t] = boundf(rate[t],
@ -643,8 +386,6 @@ static void stabilizationInnerloopTask()
pid_scaler scaler = create_pid_scaler(t);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
actuatorDesiredAxis[t] += identOffsets[t];
// we shouldn't do any clamping until after the motors are calculated and scaled?
//actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
break;
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */

8
flight/modules/Stabilization/stabilization.c
View File

@ -144,7 +144,7 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
// roll or pitch
if (t<=1) {
if (t <= 1) {
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
}
// else yaw (other modes don't worry about invalid thrust mode either)
@ -154,9 +154,9 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT;
break;
#else /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// no break, do not reorder this code
// for low power FCs just fall through to Attitude mode
// that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code
// no break, do not reorder this code
// for low power FCs just fall through to Attitude mode
// that means Yaw will be Attitude, but at least it is safe and creates no/minimal extra code
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
// do not reorder this code
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:

3
flight/pios/stm32f4xx/pios_delay.c
View File

@ -173,7 +173,8 @@ uint32_t PIOS_DELAY_DiffuS(uint32_t raw)
* @brief Subrtact two raw times and convert to us.
* @return Interval between raw times in microseconds
*/
uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later) {
uint32_t PIOS_DELAY_DiffuS2(uint32_t raw, uint32_t later)
{
return (later - raw) / us_ticks;
}
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */