Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

default top_bottom ratio 1.0

Browse Source
This commit is contained in:
Jason Short 2011-11-06 22:45:07 -08:00
parent 3153257d37
commit d1431f1070
1 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopter/Attitude.pde
View File

@ -203,17 +203,18 @@ static float posError = 0;
static float prevSensedPos = 0;
// tuning for dead reckoning
static float synPosP = 25 * G_Dt;
static float synPosI = 100 * G_Dt;
static float synVeloP = 1.5 * G_Dt;
static float maxVeloCorrection = 5 * G_Dt;
static const float dt_50hz = 0.02;
static float synPosP = 10 * dt_50hz;
static float synPosI = 15 * dt_50hz;
static float synVeloP = 1.5 * dt_50hz;
static float maxVeloCorrection = 5 * dt_50hz;
static float maxSensedVelo = 1;
static float synPosFilter = 0.13; // lower to filter more. should approximate sensor filtering
static float synPosFilter = 0.5;
#define NUM_G_SAMPLES 200
// Z damping term.
static float fullDampP = 0.200;
static float fullDampP = 0.100;
float get_world_Z_accel()
{
@ -237,10 +238,10 @@ float dead_reckon_Z(float sensedPos, float sensedAccel)
// a noisy altitude and accelerometer data.
// synthesize uncorrected velocity by integrating acceleration
synVelo += (sensedAccel - estimatedAccelOffset) * G_Dt;
synVelo += (sensedAccel - estimatedAccelOffset) * dt_50hz;
// synthesize uncorrected position by integrating uncorrected velocity
synPos += synVelo * G_Dt;
synPos += synVelo * dt_50hz;
// filter synPos, the better this filter matches the filtering and dead time
// of the sensed position, the less the position estimate will lag.
@ -257,7 +258,7 @@ float dead_reckon_Z(float sensedPos, float sensedAccel)
// correct integrated velocity by the sensed position delta in a small proportion
// (i.e., the low frequency of the delta)
float sensedVelo = (sensedPos - prevSensedPos) / G_Dt;
float sensedVelo = (sensedPos - prevSensedPos) / dt_50hz;
synVelo += constrain(sensedVelo - synVelo, -maxSensedVelo, maxSensedVelo) * synVeloP;
prevSensedPos = sensedPos;