1 files changed, 38 insertions, 33 deletions
diff --git a/main.cpp b/main.cpp
index f8c203f..9eb4594 100644
--- a/main.cpp
+++ b/main.cpp
@@ -138,38 +138,6 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 
 I2CThread i2c_thread;
 
-void foo(PWMDriver*) {
-	
-	int16_t thrust, pitch, roll, yaw;
-	
-	pitch = roll = 0;
-	
-	uint8_t shift = 8 - (usb_thread.data[8] >> 3);
-	
-	thrust = usb_thread.data[0] * 10;
-	
-	yaw = i2c_thread.z >> shift;
-	
-	
-	pwmEnableChannel(&PWMD2, 0, 1000 + thrust + pitch + roll - yaw);
-	pwmEnableChannel(&PWMD2, 1, 1000 + thrust + pitch - roll + yaw);
-	pwmEnableChannel(&PWMD2, 2, 1000 + thrust - pitch + roll + yaw);
-	pwmEnableChannel(&PWMD2, 3, 1000 + thrust - pitch - roll - yaw);
-}
-
-static PWMConfig pwmcfg = {
-	1000000,
-	1000000 / 50,
-	foo,
-	{
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
-		{PWM_OUTPUT_ACTIVE_HIGH, NULL}
-	},
-	0
-};
-
 icucnt_t last_width, last_period;
 
 int16_t ppm_input[4];
@@ -179,7 +147,7 @@ int ppm_n = 0;
 static void icuwidthcb(ICUDriver *icup) {
 
   last_width = icuGetWidthI(icup);
-	ppm_input[ppm_n] = last_width;
+	ppm_input[ppm_n] = last_width - 1000;
 }
 
 static void icuperiodcb(ICUDriver *icup) {
@@ -234,6 +202,43 @@ class ADCThread : public BaseThread<ADCThread, 128> {
 
 ADCThread adc_thread;
 
+void foo(PWMDriver*) {
+	
+	int16_t thrust, pitch, roll, yaw;
+	
+	thrust = ppm_input[2];
+	if(thrust < 0) {
+		thrust = 0;
+	}
+	if(thrust > 1000) {
+		thrust = 1000;
+	}
+	
+	pitch = roll = yaw = 0;
+	
+	//pitch = (-i2c_thread.x) >> 5;
+	//roll = i2c_thread.y >> 5;
+	//yaw = i2c_thread.z >> 5;
+	
+	pwmEnableChannel(&PWMD2, 0, 1000 + thrust + pitch + roll - yaw);
+	pwmEnableChannel(&PWMD2, 1, 1000 + thrust + pitch - roll + yaw);
+	pwmEnableChannel(&PWMD2, 2, 1000 + thrust - pitch + roll + yaw);
+	pwmEnableChannel(&PWMD2, 3, 1000 + thrust - pitch - roll - yaw);
+}
+
+static PWMConfig pwmcfg = {
+	1000000,
+	1000000 / 50,
+	foo,
+	{
+		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
+		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
+		{PWM_OUTPUT_ACTIVE_HIGH, NULL},
+		{PWM_OUTPUT_ACTIVE_HIGH, NULL}
+	},
+	0
+};
+
 int main(void) {
 	halInit();
 	chSysInit();