rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16: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) #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
// we want to be able to use systemident with or without autotune // we want to be able to use systemident with or without autotune
// If this axis allows enabling an autotune behavior without the module // 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 // appropriate objects
// if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) && // if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) &&
// (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))) { // (!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 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#include "openpilot.h" #include <openpilot.h>
#include "pios.h" #include <pios.h>
#include "flightstatus.h" #include <flightstatus.h>
#include "manualcontrolcommand.h" #include <manualcontrolcommand.h>
#include "manualcontrolsettings.h" #include <manualcontrolsettings.h>
#include "gyrosensor.h" #include <gyrosensor.h>
#include "actuatordesired.h" #include <actuatordesired.h>
#include "stabilizationdesired.h" #include <stabilizationdesired.h>
#include "stabilizationsettings.h" #include <stabilizationsettings.h>
#include "systemidentsettings.h" #include <systemidentsettings.h>
#include "systemidentstate.h" #include <systemidentstate.h>
#include <pios_board_info.h> #include <pios_board_info.h>
#include "systemsettings.h" #include <systemsettings.h>
#include "taskinfo.h" #include <taskinfo.h>
#include "stabilization.h" #include <stabilization.h>
#include "hwsettings.h" #include <hwsettings.h>
#include "stabilizationsettingsbank1.h" #include <stabilizationsettingsbank1.h>
#include "stabilizationsettingsbank2.h" #include <stabilizationsettingsbank2.h>
#include "stabilizationsettingsbank3.h" #include <stabilizationsettingsbank3.h>
#include "accessorydesired.h" #include <accessorydesired.h>
#if defined(PIOS_EXCLUDE_ADVANCED_FEATURES) #if defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
#define powapprox fastpow #define powapprox fastpow
@ -96,7 +96,7 @@
#define SMOOTH_QUICK_TOGGLE_BASE 21 #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 }; enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING };
struct at_queued_data { struct at_queued_data {
@ -469,7 +469,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// gyro sensor callback // gyro sensor callback
// get gyro data and actuatordesired into a packet // get gyro data and actuatordesired into a packet
// and put it in the queue for later processing // 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 struct at_queued_data q_item;
static bool last_sample_unpushed = false; static bool last_sample_unpushed = false;
GyroSensorData gyro; 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 // that is a signal that the user wants to try the next PID settings
// on the scale from smooth to quick // on the scale from smooth to quick
// when it exceeds the quickest setting, it starts back at the smoothest setting // 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 uint32_t lastUpdateTime;
static uint8_t flightModePrev; static uint8_t flightModePrev;
static uint8_t counter; static uint8_t counter;
@ -548,7 +550,8 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) {
// it is used two ways: // it is used two ways:
// - on startup it reads settings so the user can reuse an old tune with smooth-quick // - on startup it reads settings so the user can reuse an old tune with smooth-quick
// - at tune time, it inits the state in preparation for tuning // - at tune time, it inits the state in preparation for tuning
static void InitSystemIdent(bool loadDefaults) { static void InitSystemIdent(bool loadDefaults)
{
SystemIdentSettingsGet(&systemIdentSettings); SystemIdentSettingsGet(&systemIdentSettings);
uint8_t smoothQuick = systemIdentSettings.SmoothQuick; 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 // these are stored in the settings for use with next battery
// and also in the state for logging purposes // and also in the state for logging purposes
static void UpdateSystemIdentState(const float *X, const float *noise, 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.Roll = X[6];
systemIdentState.Beta.Pitch = X[7]; systemIdentState.Beta.Pitch = X[7];
systemIdentState.Beta.Yaw = X[8]; 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 // the settings version is remembered after power off/on
systemIdentSettings.Tau = systemIdentState.Tau; systemIdentSettings.Tau = systemIdentState.Tau;
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
if (noise) { if (noise) {
systemIdentState.Noise.Roll = noise[0]; systemIdentState.Noise.Roll = noise[0];
systemIdentState.Noise.Pitch = noise[1]; systemIdentState.Noise.Pitch = noise[1];
systemIdentState.Noise.Yaw = noise[2]; systemIdentState.Noise.Yaw = noise[2];
} }
systemIdentState.Period = dT_s * 1000.0f; systemIdentState.Period = dT_s * 1000.0f;
systemIdentState.NumAfPredicts = predicts; systemIdentState.NumAfPredicts = predicts;
systemIdentState.NumSpilledPts = spills; systemIdentState.NumSpilledPts = spills;
systemIdentState.HoverThrottle = hover_throttle; systemIdentState.HoverThrottle = hover_throttle;
SystemIdentStateSet(&systemIdentState); 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 // when running AutoTune mode, this bypasses manualcontrol.c / stabilizedhandler.c
// to control exactly when the multicopter should be in Attitude mode vs. SystemIdent mode // to control exactly when the multicopter should be in Attitude mode vs. SystemIdent mode
static void UpdateStabilizationDesired(bool doingIdent) { static void UpdateStabilizationDesired(bool doingIdent)
{
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
ManualControlCommandData manualControlCommand; ManualControlCommandData manualControlCommand;
ManualControlCommandGet(&manualControlCommand); ManualControlCommandGet(&manualControlCommand);
stabDesired.Roll = manualControlCommand.Roll * rollMax; stabDesired.Roll = manualControlCommand.Roll * rollMax;
@ -695,7 +698,10 @@ static uint8_t CheckSettings()
if (systemIdentSettings.DisableSanityChecks) { if (systemIdentSettings.DisableSanityChecks) {
retVal = 0; 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; 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 // 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 // but we are given that [smoothDamp,smoothNoise] [defaultDamp,defaultNoise] [quickDamp,quickNoise] are all good parameterizations
// this code guarantees that we will get those exact parameterizations at (val =) min, (max+min)/2, and max
static void ProportionPidsSmoothToQuick(float min, float val, float max) static void ProportionPidsSmoothToQuick(float min, float val, float max)
{ {
float ratio, damp, noise; float ratio, damp, noise;
// translate from range [min, max] to range [0, max-min] // 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; val -= min;
max -= min; max -= min;
ratio = val / max; 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 }; 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]; 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]; D[i] = P[i];
}
const float e_tau2 = e_tau * e_tau; const float e_tau2 = e_tau * e_tau;
const float e_tau3 = e_tau * e_tau2; 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_tau2 = (Ts + e_tau) * (Ts + e_tau);
const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2; const float Ts_e_tau4 = Ts_e_tau2 * Ts_e_tau2;
// expanded with indentation for easier reading but uncrustify even damages comments
#if 0 #if 0
// covariance propagation - D is stored copy of covariance // covariance propagation - D is stored copy of covariance
P[0] = D[0] + Q[0] + 2 * Ts * e_b1 * ( 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 u3 - u3_in
) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2 ) - 2 * D[27] * Tsq * u3 * u3_in * e_tau2
) / Ts_e_tau4; ) / Ts_e_tau4;
#endif #endif /* if 0 */
// covariance propagation - D is stored copy of covariance // 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) 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 + 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]); X[12] = bias3 + P[38] * ((gyro_z - w3) / S[2]);
// update the duplicate cache // 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]; D[i] = P[i];
}
// This is an approximation that removes some cross axis uncertainty but // This is an approximation that removes some cross axis uncertainty but
// substantially reduces the number of calculations // 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]); P[42] = D[42] - D[38] * (D[38] / S[2]);
// apply limits to some of the state variables // apply limits to some of the state variables
if (X[9] > -1.5f) if (X[9] > -1.5f) {
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; X[9] = -5.5f;
if (X[10] > 0.5f) }
if (X[10] > 0.5f) {
X[10] = 0.5f; X[10] = 0.5f;
else if (X[10] < -0.5f) } else if (X[10] < -0.5f) {
X[10] = -0.5f; X[10] = -0.5f;
if (X[11] > 0.5f) }
if (X[11] > 0.5f) {
X[11] = 0.5f; X[11] = 0.5f;
else if (X[11] < -0.5f) } else if (X[11] < -0.5f) {
X[11] = -0.5f; X[11] = -0.5f;
if (X[12] > 0.5f) }
if (X[12] > 0.5f) {
X[12] = 0.5f; X[12] = 0.5f;
else if (X[12] < -0.5f) } else if (X[12] < -0.5f) {
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[0] = X[1] = X[2] = 0.0f; // assume no rotation
// X[3] = X[4] = X[5] = 0.0f; // and no net torque // 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[8] = 7.0f; // yaw strength
// X[9] = -4.0f; // and 50 (18?) ms time scale // X[9] = -4.0f; // and 50 (18?) ms time scale
// X[10] = X[11] = X[12] = 0.0f; // zero bias // 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])); 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) // 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 // 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)); memcpy(&X[6], &systemIdentState.Beta, sizeof(systemIdentState.Beta));
//SystemIdentTauGet(&X[9]);
X[9] = systemIdentState.Tau; X[9] = systemIdentState.Tau;
// P initialization // 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) static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
{ {
//uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM]; StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap); StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE if (flightMode == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE
|| position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) { || position >= STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
return (FLIGHTSTATUS_FLIGHTMODEASSIST_NONE); return FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
} }
switch (FlightModeAssistMap[position]) { 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_ALTITUDEHOLD:
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO: case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
// this is only for use with stabi mods with althold/vario. // this is only for use with stabi mods with althold/vario.
//isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
//break;
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST; return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL: case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL: case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
default: default:
// this is the default for non stabi modes also // this is the default for non stabi modes also
//isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
//break;
return FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST; 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) void stabilizedHandler(__attribute__((unused)) bool newinit)
{ {
static bool inited = false; static bool inited = false;
if (!inited) { if (!inited) {
inited = true; inited = true;
StabilizationDesiredInitialize(); StabilizationDesiredInitialize();
@ -145,6 +146,7 @@ void stabilizedHandler(__attribute__((unused)) bool newinit)
// let autotune.c handle it // let autotune.c handle it
// because it must switch to Attitude after <user configurable> seconds // because it must switch to Attitude after <user configurable> seconds
return; return;
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
default: default:
// Major error, this should not occur because only enter this block when one of these is true // 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: case STABILIZATIONSTATUS_INNERLOOP_SYSTEMIDENT:
{ {
static int8_t identIteration = 0; static int8_t identIteration = 0;
static float identOffsets[3] = {0}; float identOffsets[3];
if (PIOS_DELAY_DiffuS(systemIdentTimeVal) / 1000.0f > SYSTEM_IDENT_PERIOD) { 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; const float SCALE_BIAS = 7.1f;
float roll_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Roll); SystemIdentStateData systemIdentState;
float pitch_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Pitch);
float yaw_scale = expapprox(SCALE_BIAS - systemIdentState.Beta.Yaw);
identIteration++;
if (roll_scale > 0.25f) SystemIdentStateGet(&systemIdentState);
roll_scale = 0.25f; systemIdentTimeVal = PIOS_DELAY_GetRaw();
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[0] = 0.0f;
identOffsets[1] = 0.0f; identOffsets[1] = 0.0f;
identOffsets[2] = 0.0f; identOffsets[2] = 0.0f;
identIteration = (identIteration + 1) & 7; 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? // why does yaw change twice a cycle and roll/pitch change only once?
identOffsets[0] = 0.0f; uint8_t index = ((uint8_t[]) { '\2', '\0', '\2', '\0', '\2', '\1', '\2', '\1' }
identOffsets[1] = 0.0f; )[identIteration];
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]); float scale = expapprox(SCALE_BIAS - SystemIdentStateBetaToArray(systemIdentState.Beta)[index]);
if (scale > 0.25f) { if (scale > 0.25f) {
scale = 0.25f; scale = 0.25f;
@ -464,176 +377,6 @@ static void stabilizationInnerloopTask()
// when identIteration==7: identOffsets[1] = -pitch_scale; // when identIteration==7: identOffsets[1] = -pitch_scale;
// each change has one axis with an offset // each change has one axis with an offset
// and another axis coming back to zero from having 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], rate[t] = boundf(rate[t],
@ -643,8 +386,6 @@ static void stabilizationInnerloopTask()
pid_scaler scaler = create_pid_scaler(t); 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] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
actuatorDesiredAxis[t] += identOffsets[t]; actuatorDesiredAxis[t] += identOffsets[t];
// we shouldn't do any clamping until after the motors are calculated and scaled?
//actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
} }
break; break;
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ #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. * @brief Subrtact two raw times and convert to us.
* @return Interval between raw times in microseconds * @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; return (later - raw) / us_ticks;
} }
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */