Control thrust with ppm signal.

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