rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

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

Browse Source
This commit is contained in:
Eric Price 2018-10-18 19:15:28 +02:00
parent aba11f0e10
commit 73ba9ff589
5 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,6 +38,7 @@
#include <ratedesired.h> #include <ratedesired.h>
#include <stabilizationdesired.h> #include <stabilizationdesired.h>
#include <attitudestate.h> #include <attitudestate.h>
#include <gyrostate.h>
#include <stabilizationstatus.h> #include <stabilizationstatus.h>
#include <flightstatus.h> #include <flightstatus.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
@ -78,6 +79,7 @@ void stabilizationOuterloopInit()
RateDesiredInitialize(); RateDesiredInitialize();
StabilizationDesiredInitialize(); StabilizationDesiredInitialize();
AttitudeStateInitialize(); AttitudeStateInitialize();
GyroStateInitialize();
StabilizationStatusInitialize(); StabilizationStatusInitialize();
FlightStatusInitialize(); FlightStatusInitialize();
ManualControlCommandInitialize(); ManualControlCommandInitialize();
@ -97,11 +99,13 @@ 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);
@ -190,6 +194,10 @@ static void stabilizationOuterloopTask()
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
} }
// 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[1] = local_error[1] - (gyroState.y * stabSettings.stabBank.AttitudeFeedForward.Pitch);
local_error[2] = local_error[2] - (gyroState.z * 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]);

1
shared/uavobjectdefinition/stabilizationbank.xml
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="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="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="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="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; ; "/> <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; ; "/>

1
shared/uavobjectdefinition/stabilizationsettingsbank1.xml
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="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="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="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="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; ; "/> <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; ; "/>

1
shared/uavobjectdefinition/stabilizationsettingsbank2.xml
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="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="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="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="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; ; "/> <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; ; "/>

1
shared/uavobjectdefinition/stabilizationsettingsbank3.xml
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="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="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="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="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; ; "/> <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; ; "/>