diff options
| -rw-r--r-- | itg3200.h | 32 | ||||
| -rw-r--r-- | main.cpp | 25 |
2 files changed, 50 insertions, 7 deletions
diff --git a/itg3200.h b/itg3200.h new file mode 100644 index 0000000..2e90f62 --- /dev/null +++ b/itg3200.h @@ -0,0 +1,32 @@ +#ifndef ITG3200_H +#define ITG3200_H + +#include "i2c.h" + +class ITG3200 { + private: + I2C& i2c; + + public: + int16_t x, y, z; + + ITG3200(I2C& i2c_bus) : i2c(i2c_bus) { + + } + + void init() { + i2c.write_reg(0x68, 0x3e, 0x03); // Select clock reference. + i2c.write_reg(0x68, 0x16, 0x18 | 0x02); // 2000 deg/sec range, 98 Hz bandwidth. + } + + void update() { + uint8_t buf[6]; + i2c.read_reg(0x68, 0x1d, 6, buf); + + x = (buf[0] << 8 | buf[1]) - 0; + y = (buf[2] << 8 | buf[3]) - 0; + z = (buf[4] << 8 | buf[5]) - 0; + } +}; + +#endif @@ -4,6 +4,7 @@ #include "ppmsum.h" #include "i2c.h" +#include "itg3200.h" int main() { RCC.enable(RCC.AFIO); @@ -17,13 +18,10 @@ int main() { GPIOB.CRH = 0x4444ff44; I2C i2c; - i2c.enable(); - i2c.write_reg(0x68, 0x3e, 0x03); - i2c.write_reg(0x68, 0x16, 0x18 | 0x02); - - uint8_t buf[6]; + ITG3200 gyro(i2c); + gyro.init(); PPMSum ppmsum; ppmsum.enable(); @@ -42,7 +40,20 @@ int main() { while(!(TIM2.SR & 0x01)); TIM2.SR = 0; - i2c.read_reg(0x68, 0x1d, 6, buf); - //xbee_send(6, buf); + // Read sensors. + gyro.update(); + + // Update filter. + + // Generate motor mix. + int16_t throttle = ppmsum.channels[2] - 1000; + int16_t pitch = 0; + int16_t yaw = gyro.z >> 7; + int16_t roll = 0; + + TIM2.CCR1 = 1000 + throttle + pitch + roll - yaw; + TIM2.CCR2 = 1000 + throttle + pitch - roll + yaw; + TIM2.CCR3 = 1000 + throttle - pitch + roll + yaw; + TIM2.CCR4 = 1000 + throttle - pitch - roll - yaw; } } |