From 0c8adf15d10d13331ac2d01718aba300a7c48b17 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Sat, 21 May 2011 11:24:36 +0200 Subject: Better stabilization. --- main.cpp | 52 +++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 43 insertions(+), 9 deletions(-) diff --git a/main.cpp b/main.cpp index e3d9441..3bebf74 100644 --- a/main.cpp +++ b/main.cpp @@ -77,6 +77,15 @@ uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55}; uint8_t buf[64]; int16_t* sensordata = (int16_t*)buf; +template +inline void saturate(T& var, int absmax) { + if(var > absmax) { + var = absmax; + } else if(var < -absmax) { + var = -absmax; + } +} + class I2CThread : public BaseThread { public: ITG3200 gyro; @@ -108,9 +117,20 @@ class I2CThread : public BaseThread { IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z); //float pitch = asinf(2*(q0*q2 - q3*q1)); - int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; - int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; - int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767; + //int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; + //int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; + //int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767; + + float norm_x = 2*(q0*q2 - q1*q3); + float norm_y = 2*(q0*q1 + q2*q3); + float norm_z = (1 - 2*(q1*q1 + q2*q2)); + + float elev = acosf(norm_z); + float azim = atan2f(norm_y, norm_x); + + int16_t pitch = elev * sinf(azim) / M_PI * 32767; + int16_t roll = elev * cosf(azim) / M_PI * 32767; + int16_t yaw = 0; sensordata[0] = gyro.x; sensordata[1] = gyro.y; @@ -138,14 +158,28 @@ class I2CThread : public BaseThread { int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8; int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8; - //pitch_angle_accum += pitch_angle_target - pitch; - //roll_angle_accum += roll_angle_target - roll; + int16_t pitch_angle_error = pitch_angle_target - pitch; + int16_t roll_angle_error = roll_angle_target - roll; + + // 25 deg max error. + saturate(pitch_angle_error, 4551); + saturate(roll_angle_error, 4551); + + pitch_angle_accum += pitch_angle_error; + roll_angle_accum += roll_angle_error; + + // 20 deg s max error. + saturate(pitch_angle_error, 364088); + saturate(roll_angle_error, 364088); + + int32_t pitch_rate_target = (pitch_angle_error * 4 * 65536 + pitch_angle_accum * 98) >> 16; + int32_t roll_rate_target = (roll_angle_error * 4 * 65536 + roll_angle_accum * 98) >> 16; - int16_t pitch_rate_target = ((pitch_angle_target - pitch) * 4096 + pitch_angle_accum * 256) >> 16; - int16_t roll_rate_target = ((roll_angle_target - roll) * 4096 + roll_angle_accum * 256) >> 16; + int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16; + int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16; - int16_t pitch_rate_comp = ((pitch_rate_target - gyro.x) * 2048) >> 16; - int16_t roll_rate_comp = ((roll_rate_target - gyro.y) * 2048) >> 16; + saturate(pitch_rate_comp, 250); + saturate(roll_rate_comp, 250); motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0); -- cgit v1.2.3