From c265553652444293f90189c7481fb7eb16f28115 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Sat, 19 Nov 2011 19:01:00 +0100 Subject: Separated AHRS and telemetry code. --- telemetry.cpp | 55 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 telemetry.cpp (limited to 'telemetry.cpp') diff --git a/telemetry.cpp b/telemetry.cpp new file mode 100644 index 0000000..d44412f --- /dev/null +++ b/telemetry.cpp @@ -0,0 +1,55 @@ +#include "telemetry.h" + +#include + +#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); \ No newline at end of file -- cgit v1.2.3