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

2
flight/libraries/sanitycheck.c
View File

@ -274,7 +274,7 @@ 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))) {

101
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
@ -96,7 +96,7 @@
#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 {
@ -469,7 +469,8 @@ 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;
@ -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;
@ -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;
@ -596,7 +599,8 @@ 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) {
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];
@ -609,17 +613,14 @@ static void UpdateSystemIdentState(const float *X, const float *noise,
// the settings version is remembered after power off/on
systemIdentSettings.Tau = systemIdentState.Tau;
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
if (noise) {
systemIdentState.Noise.Roll = noise[0];
systemIdentState.Noise.Pitch = noise[1];
systemIdentState.Noise.Yaw = noise[2];
}
systemIdentState.Period = dT_s * 1000.0f;
systemIdentState.NumAfPredicts = predicts;
systemIdentState.NumSpilledPts = spills;
systemIdentState.HoverThrottle = hover_throttle;
SystemIdentStateSet(&systemIdentState);
@ -628,9 +629,11 @@ 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;
@ -695,7 +698,10 @@ static uint8_t CheckSettings()
if (systemIdentSettings.DisableSanityChecks) {
retVal = 0;
} else if (systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_TRUE) {
}
// 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;
}
@ -860,12 +866,13 @@ 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
// that takes care of min < 0 case too
val -= min;
max -= min;
ratio = val / max;
@ -952,8 +959,9 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
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,6 +969,7 @@ __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 * (
@ -1107,7 +1116,7 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
u3 - u3_in
) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2
) / Ts_e_tau4;
#endif
#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
@ -1193,8 +1202,9 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
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
@ -1243,23 +1253,27 @@ __attribute__((always_inline)) static inline void AfPredict(float X[AF_NUMX], fl
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;
}
}
/**
@ -1278,7 +1292,7 @@ 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
@ -1286,10 +1300,7 @@ static void AfInit(float X[AF_NUMX], float P[AF_NUMP])
memset(X, 0, AF_NUMX * sizeof(X[0]));
// get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau)
// so that if they are changed there (mainly for future code changes), they will be changed here too
//SystemIdentSetDefaults(SystemIdentHandle(), 0);
//SystemIdentBetaArrayGet(&X[6]);
memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta));
//SystemIdentTauGet(&X[9]);
X[9] = systemIdentState.Tau;
// P initialization

8
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,15 +593,12 @@ 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;
}
}

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

@ -79,6 +79,7 @@ static float applyExpo(float value, float expo)
void stabilizedHandler(__attribute__((unused)) bool newinit)
{
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

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

@ -343,108 +343,21 @@ static void stabilizationInnerloopTask()
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?
SystemIdentStateGet(&systemIdentState);
systemIdentTimeVal = PIOS_DELAY_GetRaw();
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 */
// 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];
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) */

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) */