1 files changed, 32 insertions, 0 deletions
diff --git a/main.cpp b/main.cpp
index 27fab50..57d3d17 100644
--- a/main.cpp
+++ b/main.cpp
@@ -170,12 +170,44 @@ static PWMConfig pwmcfg = {
 	0
 };
 
+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;
+}
+
+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
+};
+
 int main(void) {
 	halInit();
 	chSysInit();
 	
 	led_thread.start();
 	
+	icuStart(&ICUD4, &icucfg);
+  icuEnable(&ICUD4);
+	
 	usbs.init();
 	
 	i2c_thread.start();