Control pitch/roll/yaw with ppm signal.

1 files changed, 18 insertions, 6 deletions

diff --git a/main.cpp b/main.cpp
index 9eb4594..4ce3f66 100644
--- a/main.cpp
+++ b/main.cpp
@@ -147,7 +147,7 @@ int ppm_n = 0;
 static void icuwidthcb(ICUDriver *icup) {

 

   last_width = icuGetWidthI(icup);

-	ppm_input[ppm_n] = last_width - 1000;

+	ppm_input[ppm_n] = last_width - 1050;

 }

 

 static void icuperiodcb(ICUDriver *icup) {

@@ -214,16 +214,28 @@ void foo(PWMDriver*) {
 		thrust = 1000;

 	}

 	

-	pitch = roll = yaw = 0;

+	pitch = ((ppm_input[1] - 500) * thrust) >> 10;

+	roll  = ((ppm_input[0] - 500) * thrust) >> 10;

+	yaw   = ((ppm_input[3] - 500) * thrust) >> 10;

 	

 	//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);

+	int16_t motor_1 = thrust + pitch + roll - yaw;

+	int16_t motor_2 = thrust + pitch - roll + yaw;

+	int16_t motor_3 = thrust - pitch + roll + yaw;

+	int16_t motor_4 = thrust - pitch - roll - yaw;

+	

+	sensordata[20] = motor_1;

+	sensordata[21] = motor_2;

+	sensordata[22] = motor_3;

+	sensordata[23] = motor_4;

+	

+	pwmEnableChannel(&PWMD2, 0, 1000 + motor_1);

+	pwmEnableChannel(&PWMD2, 1, 1000 + motor_2);

+	pwmEnableChannel(&PWMD2, 2, 1000 + motor_3);

+	pwmEnableChannel(&PWMD2, 3, 1000 + motor_4);

 }

 

 static PWMConfig pwmcfg = {