diff options
| -rw-r--r-- | ahrs.h | 25 | ||||
| -rw-r--r-- | main.cpp | 71 | ||||
| -rw-r--r-- | telemetry.cpp | 55 | ||||
| -rw-r--r-- | telemetry.h | 8 |
4 files changed, 99 insertions, 60 deletions
@@ -0,0 +1,25 @@ +#ifndef AHRS_H +#define AHRS_H + +#include "itg3200.h" +#include "bma150.h" + +class AHRS { + public: + ITG3200 gyro; + BMA150 accel; + + AHRS(I2C& i2c) : gyro(i2c), accel(i2c) {} + + void init() { + gyro.init(); + accel.init(); + } + + void update() { + gyro.update(); + accel.update(); + } +}; + +#endif @@ -6,8 +6,8 @@ #include "ppmsum.h" #include "i2c.h" -#include "itg3200.h" -#include "bma150.h" +#include "ahrs.h" +#include "telemetry.h" #include "usart.h" #include "xbee.h" @@ -60,55 +60,10 @@ class PID { }; I2C i2c; -ITG3200 gyro(i2c); -BMA150 accel(i2c); +AHRS ahrs(i2c); -volatile uint16_t dmabuf[2]; volatile uint16_t motors[4]; -void threadmain() { - 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[] = { - gyro.x, - gyro.y, - gyro.z, - accel.x, - accel.y, - 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 thstack[1024]; - -Thread thread(thstack, sizeof(thstack), threadmain); - GPS gps; void gps_thread_main() { @@ -148,9 +103,7 @@ int main() { Time::sleep(200); i2c.enable(); - - gyro.init(); - accel.init(); + ahrs.init(); PPMSum ppmsum; ppmsum.enable(); @@ -171,7 +124,7 @@ int main() { usart_enable(); gps.enable(); - thread.start(); + telemetry_thread.start(); //gps_thread.start(); while(1) { @@ -181,23 +134,21 @@ int main() { } TIM2.SR = 0; - // Read sensors. - gyro.update(); - accel.update(); + // Update AHRS. + ahrs.update(); // Update filter. - - // Generate motor mix. int16_t throttle = ppmsum.channels[2] - 1000; - int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - gyro.x); - int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - gyro.y); - int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - gyro.z); + int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - ahrs.gyro.x); + int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - ahrs.gyro.y); + int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - ahrs.gyro.z); int16_t max = throttle > 250 ? 250 : throttle; saturate(pitch, max); saturate(roll, max); saturate(yaw, max); + // Generate motor mix. motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000); motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000); motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000); 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 <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);
\ No newline at end of file diff --git a/telemetry.h b/telemetry.h new file mode 100644 index 0000000..9c2efaf --- /dev/null +++ b/telemetry.h @@ -0,0 +1,8 @@ +#ifndef TELEMETRY_H +#define TELEMETRY_H + +#include <thread.h> + +extern Thread telemetry_thread; + +#endif |