summaryrefslogtreecommitdiff
path: root/main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'main.cpp')
-rw-r--r--main.cpp25
1 files changed, 18 insertions, 7 deletions
diff --git a/main.cpp b/main.cpp
index eb36134..6bd8f6e 100644
--- a/main.cpp
+++ b/main.cpp
@@ -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;
}
}