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

Port OP-754 to Revolution attitude estimation module: subtract GyroBIAS to work on raw value

Browse Source 
Added MagKp and MagKi to AttitudeSetting UAVO
This commit is contained in:
Alessio Morale 2013-05-21 23:44:12 +02:00
parent 464d2be9f7
commit 0b5a28f19e
2 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/modules/Attitude/revolution/attitude.c
View File

@ -289,8 +289,6 @@ float accel_mag;
float qmag;
float attitudeDt;
float mag_err[3];
float magKi = 0.000001f;
float magKp = 0.01f;
static int32_t updateAttitudeComplementary(bool first_run)
{
@ -370,19 +368,18 @@ static int32_t updateAttitudeComplementary(bool first_run)
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
rollPitchBiasRate = 0.01f;
magKp = 1.0f;
attitudeSettings.MagKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
rollPitchBiasRate = 0.01f;
magKp = 1.0f;
attitudeSettings.MagKp = 1.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
rollPitchBiasRate = 0.0f;
magKp = 0.01f;
init = 1;
}
@ -453,22 +450,26 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + gyrosData.x * rollPitchBiasRate;;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi + gyrosData.y * rollPitchBiasRate;
gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias);
if(revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + (gyrosData.x + gyrosBias.x) * rollPitchBiasRate;;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi + (gyrosData.y + gyrosBias.y) * rollPitchBiasRate;
} else {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi + gyrosData.x * rollPitchBiasRate;;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi + gyrosData.y * rollPitchBiasRate;
if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
// if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
gyrosBias.z -= mag_err[2] * attitudeSettings.MagKi;
GyrosBiasSet(&gyrosBias);
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s

2
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -7,6 +7,8 @@
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/>
<field name="MagKp" units="" type="float" elements="1" defaultvalue="0"/>
<field name="AccelTau" units="" type="float" elements="1" defaultvalue="0"/>
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
<field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>