Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Flight: Add CF Accel Cutoff (Experimental - Not for merge) #2252

Open
wants to merge 4 commits into
base: next
Choose a base branch
from
Open

Flight: Add CF Accel Cutoff (Experimental - Not for merge) #2252

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
373cbb7
Flight: Add CF Accel Cutoff
jihlein Apr 4, 2019
7624c61
attitudesettings: Change AccCutoff default, add limits.
glowtape Apr 9, 2019
b9330d4
attitude: Format stuff, move one sqrt() from attitude task.
glowtape Apr 9, 2019
05d65e7
Add accelConfidence to AttitudeActual UAVO
jihlein Apr 9, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 17 additions & 5 deletions flight/Modules/Attitude/attitude.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,10 @@ static struct cfvert cfvert; //!< State information for vertical filter

static float dT_expected = 0.001f; // assume 1KHz if we don't know.

#define ACCEL_ONE_G (9.8065f)
static float accel_confidence_decay = 2; /* 1 / sqrt(0.25) */
float accel_confidence;

// Private functions
static void AttitudeTask(void *parameters);

Expand Down Expand Up @@ -355,6 +359,8 @@ static void AttitudeTask(void *parameters)
complementary_filter_state.accel_filter_enabled = true;
}

accel_confidence_decay = 1.0f / sqrtf(bound_min_max(attitudeSettings.AccCutoff, 0.1f, 1.0f));

StateEstimationGet(&stateEstimation);
}

Expand Down Expand Up @@ -766,18 +772,23 @@ static int32_t updateAttitudeComplementary(float dT, bool first_run, bool second
mag_err[2] = 0;
}

/* Computes confidence in accelerometers when aircraft is being accelerated over
and above that due to gravity. (c) G.K. Egan */

accel_confidence = bound_min_max(1.0f - (accel_confidence_decay * sqrtf(fabs(accel_mag / ACCEL_ONE_G - 1.0f))), 0.0f, 1.0f);

// 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] * accKi * dT;
gyrosBias.y -= accel_err[1] * accKi * dT;
gyrosBias.x -= accel_err[0] * accKi * dT * accel_confidence;
gyrosBias.y -= accel_err[1] * accKi * dT * accel_confidence;
gyrosBias.z -= mag_err[2] * mgKi * dT;
GyrosBiasSet(&gyrosBias);

// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * accKp;
gyrosData.y += accel_err[1] * accKp;
gyrosData.z += accel_err[2] * accKp + mag_err[2] * mgKp;
gyrosData.x += accel_err[0] * accKp * accel_confidence;
gyrosData.y += accel_err[1] * accKp * accel_confidence;
gyrosData.z += accel_err[2] * accKp * accel_confidence + mag_err[2] * mgKp;

// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
Expand Down Expand Up @@ -991,6 +1002,7 @@ static int32_t setAttitudeComplementary()
AttitudeActualData attitude;
quat_copy(cf_q, &attitude.q1);
Quaternion2RPY(&attitude.q1,&attitude.Roll);
attitude.AccelConfidence = accel_confidence;
AttitudeActualSet(&attitude);

return 0;
Expand Down
3 changes: 3 additions & 0 deletions shared/uavobjectdefinition/attitudeactual.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,5 +26,8 @@
<field defaultvalue="0" elements="1" name="Yaw" type="float" units="degrees">
<description/>
</field>
<field defaultvalue="0" elements="1" name="AccelConfidence" type="float" units="">
<description/>
</field>
</object>
</xml>
3 changes: 3 additions & 0 deletions shared/uavobjectdefinition/attitudesettings.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,9 @@
<field defaultvalue="0.4" elements="1" name="AccKi" type="float" units="channel">
<description>Accelerometer integral gain for the attitude filter.</description>
</field>
<field defaultvalue="0.25" elements="1" name="AccCutoff" type="float" units="" limits="%BE:0.1:1">
<description>Accelerometer cutoff value.</description>
</field>
<field defaultvalue="0.1" elements="1" name="AccelTau" type="float" units="">
<description/>
</field>
Expand Down