Add motor speed to telemetry.

1 files changed, 14 insertions, 10 deletions

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];
 	}
 }