Better stabilization.

1 files 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<class T>

+inline void saturate(T& var, int absmax) {

+	if(var > absmax) {

+		var = absmax;

+	} else if(var < -absmax) {

+		var = -absmax;

+	}

+}

+

 class I2CThread : public BaseThread<I2CThread, 256> {

 	public:

 		ITG3200 gyro;

@@ -108,9 +117,20 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 				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<I2CThread, 256> {
 				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);