#include "telemetry.h"

#include <time.h>

#include "stm32.h"
#include "ahrs.h"
#include "xbee.h"

extern volatile uint16_t motors[4];
extern AHRS ahrs;

volatile uint16_t dmabuf[2];

void telemetry_main() {
	ADC1.CR2 = 0x9;
	while(ADC1.CR2 & 0x8);
	ADC1.CR2 = 0x5;
	while(ADC1.CR2 & 0x4);
	
	DMA1.CH[0].CMAR = (uint32_t)&dmabuf;
	DMA1.CH[0].CPAR = (uint32_t)&ADC1.DR;
	DMA1.CH[0].CNDTR = 2;
	DMA1.CH[0].CCR = 0x05a1;
	
	ADC1.SMPR2 = 0x003f000;
	ADC1.SQR1 = 0x100000;
	ADC1.SQR3 = 5 | (4 << 5);
	ADC1.CR1 = 0x100;
	ADC1.CR2 = 0x103;
	ADC1.CR2 = 0x103;
	
	while(1) {
		uint16_t buf[] = {
			ahrs.gyro.x,
			ahrs.gyro.y,
			ahrs.gyro.z,
			ahrs.accel.x,
			ahrs.accel.y,
			ahrs.accel.z,
			motors[0],
			motors[1],
			motors[2],
			motors[3],
			dmabuf[0],
			dmabuf[1],
		};
		
		xbee_send(1, sizeof(buf), (uint8_t*)buf);
		Time::sleep(100);
	}
}

uint32_t telemetry_stack[1024];

Thread telemetry_thread(telemetry_stack, sizeof(telemetry_stack), telemetry_main);