diff options
| -rw-r--r-- | main.cpp | 52 |
1 files changed, 43 insertions, 9 deletions
@@ -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);
|