1 files changed, 39 insertions, 0 deletions
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);
+}