1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

Merged in corvusvcorax/librepilot/LP-604_Stabilization_Feedforward (pull request #521)

LP-604 Add a feed forward term to Stabilization outer loop

Approved-by: Eric Price <corvuscorax@cybertrench.com>
Approved-by: Alessio Morale <alessiomorale@gmail.com>
Approved-by: Lalanne Laurent <f5soh@free.fr>
This commit is contained in:
Eric Price 2019-02-21 21:26:52 +00:00 committed by Lalanne Laurent
commit 9653e7d258
7 changed files with 41 additions and 0 deletions

View File

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

View File

@ -38,6 +38,7 @@
#include <ratedesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <gyrostate.h>
#include <stabilizationstatus.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
@ -63,6 +64,7 @@ static DelayedCallbackInfo *callbackHandle;
static AttitudeStateData attitude;
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static float gyro_filtered[3] = { 0, 0, 0 };
static PiOSDeltatimeConfig timeval;
static bool pitchMin = false;
static bool pitchMax = false;
@ -71,6 +73,7 @@ static bool rollMax = false;
// Private functions
static void stabilizationOuterloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
void stabilizationOuterloopInit()
@ -78,6 +81,7 @@ void stabilizationOuterloopInit()
RateDesiredInitialize();
StabilizationDesiredInitialize();
AttitudeStateInitialize();
GyroStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
@ -85,6 +89,7 @@ void stabilizationOuterloopInit()
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);
GyroStateConnectCallback(GyroStateUpdatedCb);
AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
}
@ -190,6 +195,10 @@ static void stabilizationOuterloopTask()
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
// Feed forward: Assume things always get worse before they get better
local_error[0] = local_error[0] - (gyro_filtered[0] * stabSettings.stabBank.AttitudeFeedForward.Roll);
local_error[1] = local_error[1] - (gyro_filtered[1] * stabSettings.stabBank.AttitudeFeedForward.Pitch);
local_error[2] = local_error[2] - (gyro_filtered[2] * stabSettings.stabBank.AttitudeFeedForward.Yaw);
for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
@ -380,6 +389,18 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
#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]);
}
/**
* @}
* @}

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[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 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] * 0.1f;
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);
}
}
}

View File

@ -8,6 +8,7 @@
<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>

View File

@ -8,6 +8,7 @@
<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>

View File

@ -8,6 +8,7 @@
<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>

View File

@ -8,6 +8,7 @@
<field name="ManualRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="uint16" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
<field name="AttitudeFeedForward" units="sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:0:10; %BE:0:10; %BE:0:10;"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0030,0.0065,0.000033,0.3" limits="%BE:0:0.01; %BE:0:0.025; ; "/>