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

Add piro compensation option to the vbar mode

Browse Source
This commit is contained in:
James Cotton 2012-02-21 18:02:37 -06:00
parent 3283b99d4e
commit 6719ee8639
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -86,6 +86,7 @@ float vbar_integral[3] = {0, 0, 0};
float vbar_decay = 0.991f;
pid_type pids[PID_MAX];
bool vbar_gyros_suppress;
bool vbar_piro_comp = false;
// Private functions
static void stabilizationTask(void* parameters);
@ -291,6 +292,20 @@ static void stabilizationTask(void* parameters)
}
}
// Piro compensation rotates the virtual flybar by yaw step to keep it
// rotated to external world
if (vbar_piro_comp) {
const float F_PI = 3.14159f;
float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT);
float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT);
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
vbar_integral[1] = vbar_pitch;
vbar_integral[0] = vbar_roll;
}
uint8_t shouldUpdate = 1;
#if defined(DIAGNOSTICS)
RateDesiredSet(&rateDesired);
@ -497,6 +512,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
vbar_gyros_suppress = settings.VbarGyroSuppress == STABILIZATIONSETTINGS_VBARGYROSUPPRESS_TRUE;
vbar_piro_comp = settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE;
}

1
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -20,6 +20,7 @@
<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="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="VbarPiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>