ppmsum.cpp


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
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);
}