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-604 Add suitable lowpass filter to feed forward term

Browse Source
This commit is contained in:
Eric Price 2018-10-19 08:29:25 +02:00
parent 73ba9ff589
commit b218909c7b
3 changed files with 34 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/modules/Stabilization/inc/stabilization.h
View File

@ -63,6 +63,7 @@ typedef struct {
struct pid innerPids[3], outerPids[3]; struct pid innerPids[3], outerPids[3];
// TPS [Roll,Pitch,Yaw][P,I,D] // TPS [Roll,Pitch,Yaw][P,I,D]
bool thrust_pid_scaling_enabled[3][3]; bool thrust_pid_scaling_enabled[3][3];
float feedForward_alpha[3];
} StabilizationData; } StabilizationData;

23
flight/modules/Stabilization/outerloop.c
View File

@ -64,6 +64,7 @@ static DelayedCallbackInfo *callbackHandle;
static AttitudeStateData attitude; static AttitudeStateData attitude;
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static float gyro_filtered[3] = { 0, 0, 0 };
static PiOSDeltatimeConfig timeval; static PiOSDeltatimeConfig timeval;
static bool pitchMin = false; static bool pitchMin = false;
static bool pitchMax = false; static bool pitchMax = false;
@ -72,6 +73,7 @@ static bool rollMax = false;
// Private functions // Private functions
static void stabilizationOuterloopTask(); static void stabilizationOuterloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
void stabilizationOuterloopInit() void stabilizationOuterloopInit()
@ -87,6 +89,7 @@ void stabilizationOuterloopInit()
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES); callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
GyroStateConnectCallback(GyroStateUpdatedCb);
AttitudeStateConnectCallback(AttitudeStateUpdatedCb); AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
} }
@ -99,13 +102,11 @@ void stabilizationOuterloopInit()
static void stabilizationOuterloopTask() static void stabilizationOuterloopTask()
{ {
AttitudeStateData attitudeState; AttitudeStateData attitudeState;
GyroStateData gyroState;
RateDesiredData rateDesired; RateDesiredData rateDesired;
StabilizationDesiredData stabilizationDesired; StabilizationDesiredData stabilizationDesired;
StabilizationStatusOuterLoopData enabled; StabilizationStatusOuterLoopData enabled;
AttitudeStateGet(&attitudeState); AttitudeStateGet(&attitudeState);
GyroStateGet(&gyroState);
StabilizationDesiredGet(&stabilizationDesired); StabilizationDesiredGet(&stabilizationDesired);
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
StabilizationStatusOuterLoopGet(&enabled); StabilizationStatusOuterLoopGet(&enabled);
@ -195,9 +196,9 @@ static void stabilizationOuterloopTask()
} }
// Feed forward: Assume things always get worse before they get better // Feed forward: Assume things always get worse before they get better
local_error[0] = local_error[0] - (gyroState.x * stabSettings.stabBank.AttitudeFeedForward.Roll); local_error[0] = local_error[0] - (gyro_filtered[0] * stabSettings.stabBank.AttitudeFeedForward.Roll);
local_error[1] = local_error[1] - (gyroState.y * stabSettings.stabBank.AttitudeFeedForward.Pitch); local_error[1] = local_error[1] - (gyro_filtered[1] * stabSettings.stabBank.AttitudeFeedForward.Pitch);
local_error[2] = local_error[2] - (gyroState.z * stabSettings.stabBank.AttitudeFeedForward.Yaw); local_error[2] = local_error[2] - (gyro_filtered[2] * stabSettings.stabBank.AttitudeFeedForward.Yaw);
for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) { for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]); reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
@ -388,6 +389,18 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
#endif #endif
} }
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
GyroStateData gyroState;
GyroStateGet(&gyroState);
gyro_filtered[0] = gyro_filtered[0] * stabSettings.feedForward_alpha[0] + gyroState.x * (1 - stabSettings.feedForward_alpha[0]);
gyro_filtered[1] = gyro_filtered[1] * stabSettings.feedForward_alpha[1] + gyroState.y * (1 - stabSettings.feedForward_alpha[1]);
gyro_filtered[2] = gyro_filtered[2] * stabSettings.feedForward_alpha[2] + gyroState.z * (1 - stabSettings.feedForward_alpha[2]);
}
/** /**
* @} * @}
* @} * @}

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

@ -364,6 +364,21 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f; stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f;
stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f; stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f;
stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f; stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
// based on a time (in ms) rather than a fixed multiplier. The error between
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation
const float fakeDt = 0.0025f;
for (int t = 0; t < STABILIZATIONBANK_ATTITUDEFEEDFORWARD_NUMELEM; t++) {
float tau = StabilizationBankAttitudeFeedForwardToArray(stabSettings.stabBank.AttitudeFeedForward)[t];
if (tau < 0.0001f) {
stabSettings.feedForward_alpha[t] = 0.0f; // not trusting this to resolve to 0
} else {
stabSettings.feedForward_alpha[t] = expf(-fakeDt / tau);
}
}
} }