rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-984 Added data UAV object for current stabilization bank

Browse Source
This commit is contained in:
Les Newell 2013-12-10 19:58:05 +00:00
parent 8a742179a7
commit ec713be3ea
3 changed files with 179 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,6 +37,42 @@ enum { ROLL, PITCH, YAW, MAX_AXES };
int32_t StabilizationInitialize();
typedef struct stab_rate_pid{
float Kp;
float Ki;
float Kd;
float ILimit;
};
typedef struct stab_att_pid{
float Kp;
float Ki;
float ILimit;
};
typedef struct stab_rpy{
float Roll;
float Pitch;
float Yaw;
};
typedef struct stab_bank {
float RollMax;
float PitchMax;
float YawMax;
struct{
float Roll;
float Pitch;
float Yaw;
};
float ManualRate;
float iAccumulator;
float lastErr;
float lastDer;
};
#endif // STABILIZATION_H
/**

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

@ -47,6 +47,7 @@
#include "manualcontrol.h" // Just to get a macro
#include "taskinfo.h"
// Math libraries
#include "CoordinateConversions.h"
#include "pid.h"
@ -71,9 +72,6 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define FAILSAFE_TIMEOUT_MS 30
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
// Private variables
static xTaskHandle taskHandle;
static StabilizationSettingsData settings;
@ -88,12 +86,14 @@ bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
int flight_mode = 0;
// Private functions
static void stabilizationTask(void *parameters);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void UpdatePids(StabilizationSettings* settings);
/**
* Module initialization
@ -484,42 +484,75 @@ static float bound(float val, float range)
return val;
}
static void UpdatePids(StabilizationSettings* settings)
{
enum{RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET};
enum{ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET};
int offset = flight_mode * RATE_OFFSET;
// Set the roll rate PID constants
float* pid = &settings.RollRatePID.Kp1;
pid += offset;
pid_configure(&pids[PID_RATE_ROLL],
pid[RATE_P],
pid[RATE_I],
pid[RATE_D],
pid[RATE_LIMIT]);
// Set the pitch rate PID constants
pid = &settings.PitchRatePID.Kp1;
pid += offset;
pid_configure(&pids[PID_RATE_PITCH],
pid[RATE_P],
pid[RATE_I],
pid[RATE_D],
pid[RATE_LIMIT]);
// Set the yaw rate PID constants
pid = &settings.YawRatePID.Kp1;
pid += offset;
pid_configure(&pids[PID_RATE_YAW],
pid[RATE_P],
pid[RATE_I],
pid[RATE_D],
pid[RATE_LIMIT]);
offset = flight_mode * ATT_OFFSET;
// Set the roll attitude PI constants
pid = &settings.RollPI.Kp1;
pid += offset;
pid_configure(&pids[PID_ROLL],
pid[ATT_P],
pid[ATT_I],
0,
pid[ATT_LIMIT]);
// Set the pitch attitude PI constants
pid = &settings.PitchPI.Kp1;
pid += offset;
pid_configure(&pids[PID_PITCH],
pid[ATT_P],
pid[ATT_I],
0,
pid[ATT_LIMIT]);
// Set the yaw attitude PI constants
pid = &settings.YawPI.Kp1;
pid += offset;
pid_configure(&pids[PID_YAW],
pid[ATT_P],
pid[ATT_I],
0,
pid[ATT_LIMIT]);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationSettingsGet(&settings);
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.Kp,
settings.RollRatePID.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp,
pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki,
pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp,
pids[PID_RATE_YAW].i = settings.YawRatePID.Ki,
pids[PID_RATE_YAW].d = settings.YawRatePID.Kd,
pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], settings.RollPI.Kp,
settings.RollPI.Ki, 0,
pids[PID_ROLL].iLim = settings.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp,
pids[PID_PITCH].i = settings.PitchPI.Ki, 0,
settings.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], settings.YawPI.Kp,
settings.YawPI.Ki, 0,
settings.YawPI.ILimit);
UpdatePids(&settings);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);

74
shared/uavobjectdefinition/stabilizationbank.xml Normal file
View File

@ -0,0 +1,74 @@
<xml>
<object name="StabilizationBank" singleinstance="true" settings="false" category="Control">
<description>Currently selected PID bank</description>
<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
<field name="ManualRate" units="degrees/sec" type="float" 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="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="YawRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0035,0.0035,0,0.3" limits="%BE:0:0.01; %BE:0:0.01 ; ; "/>
<field name="RollPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
<object name="StabilizationSettings" singleinstance="true" settings="true" category="Control">
<description>PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired</description>
<!-- Note: The number of elements here must match the number of available flight modes -->
<field name="FlightModeMap" units="" type="uint8" elements="6" defaultvalue="0,0,0,0,0,0"/>
<field name="RollMax" units="degrees" type="uint8" elements="3" defaultvalue="42" limits="%BE:0:180"/>
<field name="PitchMax" units="degrees" type="uint8" elements="3" defaultvalue="42" limits="%BE:0:180"/>
<field name="YawMax" units="degrees" type="uint8" elements="3" defaultvalue="42" limits="%BE:0:180"/>
<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll1,Pitch1,Yaw1,Roll2,Pitch2,Yaw2,Roll3,Pitch3,Yaw3" defaultvalue="150,150,175,150,150,175,150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll1,Pitch1,Yaw1,Roll2,Pitch2,Yaw2,Roll3,Pitch3,Yaw3" defaultvalue="300,300,50,300,300,50,300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500; %BE:0:500"/>
<field name="RollRatePID" units="" type="float" elementnames="Kp1,Ki1,Kd1,ILimit1,Kp2,Ki2,Kd2,ILimit2,Kp3,Ki3,Kd3,ILimit3" defaultvalue="0.003,0.003,0.00002,0.3,0.003,0.003,0.00002,0.3,0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; ;%BE:0:0.01; %BE:0:0.01; ; ;%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="PitchRatePID" units="" type="float" elementnames="Kp1,Ki1,Kd1,ILimit1,Kp2,Ki2,Kd2,ILimit2,Kp3,Ki3,Kd3,ILimit3" defaultvalue="0.003,0.003,0.00002,0.3,0.003,0.003,0.00002,0.3,0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; ;%BE:0:0.01; %BE:0:0.01; ; ;%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="YawRatePID" units="" type="float" elementnames="Kp1,Ki1,Kd1,ILimit1,Kp2,Ki2,Kd2,ILimit2,Kp3,Ki3,Kd3,ILimit3" defaultvalue="0.0035,0.0035,0,0.3,0.0035,0.0035,0,0.3,0.0035,0.0035,0,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; ;%BE:0:0.01; %BE:0:0.01; ; ;%BE:0:0.01; %BE:0:0.01; ; "/>
<field name="RollPI" units="" type="float" elementnames="Kp1,Ki1,ILimit1,Kp2,Ki2,ILimit2,Kp3,Ki3,ILimit3" defaultvalue="2.5,0,50,2.5,0,50,2.5,0,50" limits="%BE:0:10; %BE:0:10; ;%BE:0:10; %BE:0:10; ;%BE:0:10; %BE:0:10; "/>
<field name="PitchPI" units="" type="float" elementnames="Kp1,Ki1,ILimit1,Kp2,Ki2,ILimit2,Kp3,Ki3,ILimit3" defaultvalue="2.5,0,50,2.5,0,50,2.5,0,50" limits="%BE:0:10; %BE:0:10; ;%BE:0:10; %BE:0:10; ;%BE:0:10; %BE:0:10; "/>
<field name="YawPI" units="" type="float" elementnames="Kp1,Ki1,ILimit1,Kp2,Ki2,ILimit2,Kp3,Ki3,ILimit3" defaultvalue="2,0,50,2,0,50,2,0,50" limits="%BE:0:10; %BE:0:10; ;%BE:0:10; %BE:0:10; ;%BE:0:10; %BE:0:10; "/>
<field name="VbarSensitivity" units="frac" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0.5,0.5,0.5"/>
<field name="VbarRollPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
<field name="VbarPitchPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
<field name="VbarYawPI" units="1/(deg/s)" type="float" elementnames="Kp,Ki" defaultvalue="0.005,0.002"/>
<field name="VbarTau" units="sec" type="float" elements="1" defaultvalue="0.5"/>
<field name="VbarGyroSuppress" units="%" type="int8" elements="1" defaultvalue="30"/>
<field name="VbarPiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="VbarMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="10"/>
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
<field name="DerivativeCutoff" units="Hz" type="uint8" elements="1" defaultvalue="20"/>
<field name="DerivativeGamma" units="" type="float" elements="1" defaultvalue="1"/>
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="30"/>
<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>
<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>
<field name="MaxWeakLevelingRate" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>