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

OP-410 OP-333: Axis lock (heading hold) implemented

Browse Source
This commit is contained in:
James Cotton 2011-06-24 01:57:12 -05:00
parent 0dc541cbaa
commit 038f955cb1
2 changed files with 30 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,6 +79,8 @@ static xQueueHandle queue;
float dT = 1; 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};
uint8_t max_axis_lock = 0;
pid_type pids[PID_MAX]; pid_type pids[PID_MAX];
// Private functions // Private functions
@ -203,18 +205,32 @@ static void stabilizationTask(void* parameters)
float *attitudeDesiredAxis = &stabDesired.Roll; float *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll;
float *actualRateAxis = &attitudeRaw.gyros[0];
//Calculate desired rate //Calculate desired rate
for(int8_t ct=0; ct< MAX_AXES; ct++) for(uint8_t i=0; i< MAX_AXES; i++)
{ {
switch(stabDesired.StabilizationMode[ct]) switch(stabDesired.StabilizationMode[i])
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
rateDesiredAxis[ct] = attitudeDesiredAxis[ct]; rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], local_error[ct]); rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
// Track accumulated error between axis lock
axis_lock_accum[i] += (attitudeDesiredAxis[i] - actualRateAxis[i]) * dT;
if(axis_lock_accum[i] > max_axis_lock)
axis_lock_accum[i] = max_axis_lock;
else if(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]);
break; break;
} }
} }
@ -225,17 +241,11 @@ static void stabilizationTask(void* parameters)
//Calculate desired command //Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++) for(int8_t ct=0; ct< MAX_AXES; ct++)
{ {
if(fabs(rateDesiredAxis[ct]) > settings.MaximumRate[ct]) if(rateDesiredAxis[ct] > settings.MaximumRate[ct])
{
if(rateDesiredAxis[ct] > 0)
{
rateDesiredAxis[ct] = settings.MaximumRate[ct]; rateDesiredAxis[ct] = settings.MaximumRate[ct];
}else else if(rateDesiredAxis[ct] < settings.MaximumRate[ct])
{
rateDesiredAxis[ct] = -settings.MaximumRate[ct]; rateDesiredAxis[ct] = -settings.MaximumRate[ct];
}
}
switch(stabDesired.StabilizationMode[ct]) switch(stabDesired.StabilizationMode[ct])
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
@ -314,6 +324,8 @@ static void ZeroPids(void)
pids[ct].iAccumulator = 0; pids[ct].iAccumulator = 0;
pids[ct].lastErr = 0; pids[ct].lastErr = 0;
} }
for(uint8_t i = 0; i < 3; i++)
axis_lock_accum[i] = 0;
} }
@ -369,6 +381,9 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
pids[5].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI]; pids[5].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
pids[5].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]; pids[5].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
// The dT has some jitter iteration to iteration that we don't want to // 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 // 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 // based on a time (in ms) rather than a fixed multiplier. The error between

1
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -15,6 +15,7 @@
<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2,0,50"/> <field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2,0,50"/>
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/> <field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="5"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>