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:
commit
9653e7d258
@ -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;
|
||||
|
||||
|
||||
|
@ -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]);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
@ -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; ; "/>
|
||||
|
@ -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; ; "/>
|
||||
|
@ -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; ; "/>
|
||||
|
@ -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; ; "/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user