1 files changed, 71 insertions, 1 deletions
diff --git a/main.cpp b/main.cpp
index 75468ed..6eb1a27 100644
--- a/main.cpp
+++ b/main.cpp
@@ -109,6 +109,73 @@ void xbee_send(int len, const uint8_t* buf) {
 	usart_send(chsum);
 }
 
+class PPMSum {
+	friend void interrupt<Interrupt::TIM4>();
+	
+	private:
+		static PPMSum* self;
+		
+		uint8_t index;
+		uint16_t channels[16];
+		
+		void irq() {
+			int16_t sr = TIM4.SR;
+			TIM4.SR = 0;
+			
+			if(sr & 0x06) {
+				GPIOA.ODR = 1 << 5;
+			} else {
+				GPIOA.ODR = 0;
+			}
+			
+			
+			if(sr & 0x01) {
+				// Timeout.
+				
+				// TODO: Indicate failsafe.
+				
+			} else if(sr & 0x02) {
+				// Period.
+				
+				if(TIM4.CCR1 > 5000) {
+					index = 0;
+				} else {
+					index++;
+				}
+				
+			} else if(sr & 0x04) {
+				// Pulsewidth.
+				
+				channels[index] = TIM4.CCR2;
+			}
+		}
+	
+	public:
+		PPMSum() {
+			self = this;
+		}
+		
+		void start() {
+			RCC.enable(RCC.TIM4);
+			TIM4.PSC = 36;
+			TIM4.CCMR1 = 0x0201;
+			TIM4.SMCR = 0x54;
+			TIM4.CCER = 0x31;
+			TIM4.DIER = 0x07;
+			
+			Interrupt::enable(Interrupt::TIM4);
+			
+			TIM4.CR1 = 0x05;
+		}
+};
+
+PPMSum* PPMSum::self = 0;
+
+template<>
+void interrupt<Interrupt::TIM4>() {
+	PPMSum::self->irq();
+}
+
 int main() {
 	RCC.enable(RCC.AFIO);
 	RCC.enable(RCC.IOPA);
@@ -146,6 +213,9 @@ int main() {
 	
 	Interrupt::enable(Interrupt::USART1);
 	
+	PPMSum ppmsum;
+	ppmsum.start();
+	
 	while(1) {
 		cnt++;
 		if(cnt & (1 << 20)) {
@@ -156,7 +226,7 @@ int main() {
 		
 		if(!(cnt & ((1 << 20) - 1))) {
 			i2c_read_reg(0x68, 0x1d, 6, buf);
-			xbee_send(6, buf);
+			//xbee_send(6, buf);
 		}
 	}
 }