Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -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

23
ArduCopter/Attitude.pde
View File

@ -72,7 +72,7 @@ get_stabilize_yaw(long target_angle)
error = rate - (long)(degrees(omega.z) * 100.0); error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt); rate = g.pi_rate_yaw.get_pi(error, G_Dt);
} }
// output control: // output control:
return (int)constrain(rate, -4500, 4500); return (int)constrain(rate, -4500, 4500);
#else #else
@ -80,7 +80,7 @@ get_stabilize_yaw(long target_angle)
error = rate - (long)(degrees(omega.z) * 100.0); error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt); rate = g.pi_rate_yaw.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
// output control: // output control:
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
#endif #endif
@ -203,17 +203,18 @@ static float posError = 0;
static float prevSensedPos = 0; static float prevSensedPos = 0;
// tuning for dead reckoning // tuning for dead reckoning
static float synPosP = 25 * G_Dt; static const float dt_50hz = 0.02;
static float synPosI = 100 * G_Dt; static float synPosP = 10 * dt_50hz;
static float synVeloP = 1.5 * G_Dt; static float synPosI = 15 * dt_50hz;
static float maxVeloCorrection = 5 * G_Dt; static float synVeloP = 1.5 * dt_50hz;
static float maxVeloCorrection = 5 * dt_50hz;
static float maxSensedVelo = 1; 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 #define NUM_G_SAMPLES 200
// Z damping term. // Z damping term.
static float fullDampP = 0.200; static float fullDampP = 0.100;
float get_world_Z_accel() float get_world_Z_accel()
{ {
@ -237,10 +238,10 @@ float dead_reckon_Z(float sensedPos, float sensedAccel)
// a noisy altitude and accelerometer data. // a noisy altitude and accelerometer data.
// synthesize uncorrected velocity by integrating acceleration // synthesize uncorrected velocity by integrating acceleration
synVelo += (sensedAccel - estimatedAccelOffset) * G_Dt; synVelo += (sensedAccel - estimatedAccelOffset) * dt_50hz;
// synthesize uncorrected position by integrating uncorrected velocity // 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 // filter synPos, the better this filter matches the filtering and dead time
// of the sensed position, the less the position estimate will lag. // 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 // correct integrated velocity by the sensed position delta in a small proportion
// (i.e., the low frequency of the delta) // (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; synVelo += constrain(sensedVelo - synVelo, -maxSensedVelo, maxSensedVelo) * synVeloP;
prevSensedPos = sensedPos; prevSensedPos = sensedPos;