rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +01:00
Code Issues Releases Activity

Implement a virtual flybar in a way inspired by behavior of Align 3G which

Browse Source 
seems apply a leaky integrator to the swash angle.  This is the similar to what
is done by Phubar (http://code.google.com/p/phubar/) as well although we refer
to the gyro term as the proportional and the flybar angle as the integral
This commit is contained in:
James Cotton 2012-02-20 07:29:43 -06:00
parent 1b55df733d
commit ff1b1a93cf
1 changed files with 32 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
flight/Modules/Stabilization/stabilization.c
View File

@ -74,7 +74,6 @@ typedef struct {
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
static StabilizationSettingsData settings; static StabilizationSettingsData settings;
static xQueueHandle queue; static xQueueHandle queue;
float dT = 1;
float gyro_alpha = 0; float gyro_alpha = 0;
float gyro_filtered[3] = {0,0,0}; float gyro_filtered[3] = {0,0,0};
float axis_lock_accum[3] = {0,0,0}; float axis_lock_accum[3] = {0,0,0};
@ -83,12 +82,13 @@ uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0; float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0; uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral; bool lowThrottleZeroIntegral;
float vbar_integral[3] = {0, 0, 0};
float vbar_decay = 0.1f;
pid_type pids[PID_MAX]; pid_type pids[PID_MAX];
// Private functions // Private functions
static void stabilizationTask(void* parameters); static void stabilizationTask(void* parameters);
static float ApplyPid(pid_type * pid, const float err); static float ApplyPid(pid_type * pid, const float err, float dT);
static float bound(float val); static float bound(float val);
static void ZeroPids(void); static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent * ev); static void SettingsUpdatedCb(UAVObjEvent * ev);
@ -154,6 +154,8 @@ static void stabilizationTask(void* parameters)
// Main task loop // Main task loop
ZeroPids(); ZeroPids();
while(1) { while(1) {
float dT;
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
@ -227,6 +229,7 @@ static void stabilizationTask(void* parameters)
switch(stabDesired.StabilizationMode[i]) switch(stabDesired.StabilizationMode[i])
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_VBAR:
rateDesiredAxis[i] = attitudeDesiredAxis[i]; rateDesiredAxis[i] = attitudeDesiredAxis[i];
// Zero attitude and axis lock accumulators // Zero attitude and axis lock accumulators
@ -251,7 +254,7 @@ static void stabilizationTask(void* parameters)
break; break;
} }
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]); rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
if(rateDesiredAxis[i] > settings.MaximumRate[i]) if(rateDesiredAxis[i] > settings.MaximumRate[i])
rateDesiredAxis[i] = settings.MaximumRate[i]; rateDesiredAxis[i] = settings.MaximumRate[i];
@ -275,7 +278,7 @@ static void stabilizationTask(void* parameters)
else if(axis_lock_accum[i] < -max_axis_lock) else if(axis_lock_accum[i] < -max_axis_lock)
axis_lock_accum[i] = -max_axis_lock; axis_lock_accum[i] = -max_axis_lock;
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]); rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
} }
if(rateDesiredAxis[i] > settings.MaximumRate[i]) if(rateDesiredAxis[i] > settings.MaximumRate[i])
@ -293,36 +296,51 @@ static void stabilizationTask(void* parameters)
#endif #endif
ActuatorDesiredGet(&actuatorDesired); ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command //Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++) for(uint32_t i = 0; i < MAX_AXES; i++)
{ {
switch(stabDesired.StabilizationMode[ct]) switch(stabDesired.StabilizationMode[i])
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{ {
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]); float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
actuatorDesiredAxis[ct] = bound(command); actuatorDesiredAxis[i] = bound(command);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_VBAR:
{
// Track the angle of the virtual flybar which includes a slow decay
vbar_decay = 0.991f; // expf(logf(0.01) / (1 / dT)) // Set the decay so that after 1 second at 1%
vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
float k1 = 0.2f;
float k2 = pids[PID_RATE_ROLL + i].i;
float k3 = pids[PID_RATE_ROLL + i].p;
// Command signal is composed of stick input added to the gyro and virtual flybar
float command = rateDesiredAxis[i] * k1 - vbar_integral[i] * k2 - gyro_filtered[i] * k3;
actuatorDesiredAxis[i] = bound(command);
break; break;
} }
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
switch (ct) switch (i)
{ {
case ROLL: case ROLL:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1; shouldUpdate = 1;
pids[PID_RATE_ROLL].iAccumulator = 0; pids[PID_RATE_ROLL].iAccumulator = 0;
pids[PID_ROLL].iAccumulator = 0; pids[PID_ROLL].iAccumulator = 0;
break; break;
case PITCH: case PITCH:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1; shouldUpdate = 1;
pids[PID_RATE_PITCH].iAccumulator = 0; pids[PID_RATE_PITCH].iAccumulator = 0;
pids[PID_PITCH].iAccumulator = 0; pids[PID_PITCH].iAccumulator = 0;
break; break;
case YAW: case YAW:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1; shouldUpdate = 1;
pids[PID_RATE_YAW].iAccumulator = 0; pids[PID_RATE_YAW].iAccumulator = 0;
pids[PID_YAW].iAccumulator = 0; pids[PID_YAW].iAccumulator = 0;
@ -360,7 +378,7 @@ static void stabilizationTask(void* parameters)
} }
} }
float ApplyPid(pid_type * pid, const float err) float ApplyPid(pid_type * pid, const float err, float dT)
{ {
float diff = (err - pid->lastErr); float diff = (err - pid->lastErr);
pid->lastErr = err; pid->lastErr = err;