Seperated ppmsum-demuxer into seperate file.

1 files changed, 8 insertions, 35 deletions

diff --git a/main.cpp b/main.cpp
index 4ce3f66..3b37344 100644
--- a/main.cpp
+++ b/main.cpp
@@ -3,6 +3,7 @@
 #include "itg3200.h"

 #include "bma150.h"

 #include "ak8975.h"

+#include "ppmsum.h"

 

 #include <ch.h>

 #include <hal.h>

@@ -26,6 +27,8 @@ class LEDThread : public BaseThread<LEDThread, 128> {
 

 LEDThread led_thread;

 

+PPMSum ppmsum;

+

 class USBThread : public BaseThread<USBThread, 256> {

 	private:

 		typedef enum {W_S, W_N, W_V} w_s_t;

@@ -138,35 +141,6 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 

 I2CThread i2c_thread;

 

-icucnt_t last_width, last_period;

-

-int16_t ppm_input[4];

-

-int ppm_n = 0;

-

-static void icuwidthcb(ICUDriver *icup) {

-

-  last_width = icuGetWidthI(icup);

-	ppm_input[ppm_n] = last_width - 1050;

-}

-

-static void icuperiodcb(ICUDriver *icup) {

-

-  last_period = icuGetPeriodI(icup);

-	if(last_period > 5000) {

-		ppm_n = 0;

-	} else {

-		ppm_n = (ppm_n + 1) % 4;

-	}

-}

-

-static ICUConfig icucfg = {

-  ICU_INPUT_ACTIVE_HIGH,

-  1000000,

-  icuwidthcb,

-  icuperiodcb

-};

-

 static const ADCConversionGroup adcgrpcfg = {

 	FALSE,

 	2,

@@ -206,7 +180,7 @@ void foo(PWMDriver*) {
 	

 	int16_t thrust, pitch, roll, yaw;

 	

-	thrust = ppm_input[2];

+	thrust = ppmsum.data[2];

 	if(thrust < 0) {

 		thrust = 0;

 	}

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

 	}

 	

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

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

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

+	pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;

+	roll  = ((ppmsum.data[0] - 500) * thrust) >> 10;

+	yaw   = ((ppmsum.data[3] - 500) * thrust) >> 10;

 	

 	//pitch = (-i2c_thread.x) >> 5;

 	//roll = i2c_thread.y >> 5;

@@ -257,8 +231,7 @@ int main(void) {
 	

 	led_thread.start();

 	

-	icuStart(&ICUD4, &icucfg);

-  icuEnable(&ICUD4);

+	ppmsum.start();

 	

 	usbs.init();