Seperated ppmsum-demuxer into seperate file.

3 files changed, 60 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();

 	

diff --git a/ppmsum.cpp b/ppmsum.cpp
new file mode 100644
index 0000000..ce601e5
--- /dev/null
+++ b/ppmsum.cpp
@@ -0,0 +1,39 @@
+#include "ppmsum.h"
+
+#include <ch.h>
+#include <hal.h>
+
+namespace {
+	icucnt_t last_width, last_period;
+	
+	PPMSum* ppmsum;
+	int ppm_n = 0;
+	
+	static void icuwidthcb(ICUDriver *icup) {
+		last_width = icuGetWidthI(icup);
+		ppmsum->data[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
+	};
+};
+
+void PPMSum::start() {
+	ppmsum = this;
+	
+	icuStart(&ICUD4, &icucfg);
+	icuEnable(&ICUD4);
+}
diff --git a/ppmsum.h b/ppmsum.h
new file mode 100644
index 0000000..82b62df
--- /dev/null
+++ b/ppmsum.h
@@ -0,0 +1,13 @@
+#ifndef PPMSUM_H
+#define PPMSUM_H
+
+#include <stdint.h>
+
+class PPMSum {
+	public:
+		int16_t data[4];
+		
+		void start();
+};
+
+#endif