From 12f9cd58f47eefe17a0398f24a603cdb65728a95 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Thu, 10 Nov 2011 19:58:58 +0100 Subject: Add motor speed to telemetry. --- main.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) (limited to 'main.cpp') diff --git a/main.cpp b/main.cpp index 35ccbb2..c45b87d 100644 --- a/main.cpp +++ b/main.cpp @@ -64,6 +64,8 @@ ITG3200 gyro(i2c); BMA150 accel(i2c); volatile uint16_t dmabuf[2]; +volatile uint16_t motors[4]; + void threadmain() { ADC1.CR2 = 0x9; while(ADC1.CR2 & 0x8); @@ -90,6 +92,10 @@ void threadmain() { accel.x, accel.y, accel.z, + motors[0], + motors[1], + motors[2], + motors[3], dmabuf[0], dmabuf[1], }; @@ -193,16 +199,14 @@ int main() { saturate(roll, max); saturate(yaw, max); - int cmds[] = { - 1000 + throttle + pitch - roll + yaw, - 1000 + throttle - pitch - roll - yaw, - 1000 + throttle - pitch + roll + yaw, - 1000 + throttle + pitch + roll - yaw, - }; + motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000); + motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000); + motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000); + motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000); - TIM2.CCR1 = limit(cmds[0], 1000, 2000); - TIM2.CCR2 = limit(cmds[1], 1000, 2000); - TIM2.CCR3 = limit(cmds[2], 1000, 2000); - TIM2.CCR4 = limit(cmds[3], 1000, 2000); + TIM2.CCR1 = 1000 + motors[0]; + TIM2.CCR2 = 1000 + motors[1]; + TIM2.CCR3 = 1000 + motors[2]; + TIM2.CCR4 = 1000 + motors[3]; } } -- cgit v1.2.3