diff options
| -rw-r--r-- | .gdbinit | 2 | ||||
| -rw-r--r-- | ahrs.h | 25 | ||||
| -rw-r--r-- | main.cpp | 146 | ||||
| -rw-r--r-- | telemetry.cpp | 56 | ||||
| -rw-r--r-- | telemetry.h | 8 |
5 files changed, 2 insertions, 235 deletions
@@ -1,4 +1,4 @@ -target extended-remote :1234 +target extended-remote :4242 file suzumebachi.elf define flash @@ -1,25 +0,0 @@ -#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 @@ -1,86 +1,11 @@ #include "stm32.h" +#include "rcc.h" #include "interrupt.h" #include "thread.h" #include "time.h" -#include "ppmsum.h" -#include "i2c.h" - -#include "ahrs.h" -#include "telemetry.h" - -#include "usart.h" -#include "xbee.h" -#include "gps.h" - #include "pin.h" -template<class T> -inline void saturate(T& var, T absmax) { - if(var > absmax) { - var = absmax; - } else if(var < -absmax) { - var = -absmax; - } -} - -template<class T> -inline T limit(T var, T min, T max) { - if(var < min) { - return min; - } else if(var > max) { - return max; - } else { - return var; - } -} - -class PID { - private: - uint16_t Kp, Ki, Kd; - - int16_t last; - int32_t accum; - - public: - PID(uint16_t p, uint16_t i, uint16_t d) : Kp(p), Ki(i), Kd(d), last(0), accum(0) {} - - int16_t update(int16_t error) { - // P - int32_t corr_p = Kp * error; - - // I - accum += Ki * error; - int32_t corr_i = accum; - - // D - int32_t corr_d = Kd * (error - last); - last = error; - - return (corr_p + corr_i + corr_d) >> 16; - } -}; - -AHRS ahrs(I2C1); - -volatile uint16_t motors[4]; - -//GPS gps; - -void gps_thread_main() { - while(1) { - //P<GPSMsg> msg = gps.read(); - - //if(msg->n < 128) { - // xbee_send(2, msg->n, msg->buf); - //} - } -} - -uint32_t gps_stack[256]; - -Thread gps_thread(gps_stack, sizeof(gps_stack), gps_thread_main); - static Pin& led_green = PD12; static Pin& led_yellow = PD13; static Pin& led_red = PD14; @@ -100,44 +25,6 @@ int main() { led_red.set_mode(Pin::Output); led_blue.set_mode(Pin::Output); - //RCC.enable(RCC.DMA1); - //RCC.enable(RCC.ADC1); - - PB6.set_af(7); - PB7.set_af(7); - PB6.set_mode(Pin::Output); - PB7.set_mode(Pin::Output); - - // Give all hardware enough time to initialize. - Time::sleep(200); - - I2C1.enable(PB8, PB9); - ahrs.init(); - - //PPMSum ppmsum; - //ppmsum.enable(); - - //RCC.enable(RCC.TIM2); - //TIM2.PSC = 72; - //TIM2.ARR = 5000; - //TIM2.CCER = 0x1111; - //TIM2.CCMR1 = 0x6868; - //TIM2.CCMR2 = 0x6868; - - //TIM2.CR1 = 0x05; - - //PID pid_pitch(6000, 0, 0); - //PID pid_roll(6000, 0, 0); - //PID pid_yaw(6000, 0, 0); - - RCC.enable(RCC.USART1); - - usart_enable(); - //gps.enable(); - - telemetry_thread.start(); - //gps_thread.start(); - while(1) { led_green.on(); Time::sleep(100); @@ -154,36 +41,5 @@ int main() { led_blue.on(); Time::sleep(100); led_blue.off(); - - // Wait for a new update. - //while(!(TIM2.SR & 0x01)) { - // Thread::yield(); - //} - //TIM2.SR = 0; - - // Update AHRS. - ahrs.update(); - - // Update filter. - //int16_t throttle = ppmsum.channels[2] - 1000; - //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); - //motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000); - - //TIM2.CCR1 = 1000 + motors[0]; - //TIM2.CCR2 = 1000 + motors[1]; - //TIM2.CCR3 = 1000 + motors[2]; - //TIM2.CCR4 = 1000 + motors[3]; } } diff --git a/telemetry.cpp b/telemetry.cpp deleted file mode 100644 index 781fb60..0000000 --- a/telemetry.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#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); diff --git a/telemetry.h b/telemetry.h deleted file mode 100644 index 9c2efaf..0000000 --- a/telemetry.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef TELEMETRY_H -#define TELEMETRY_H - -#include <thread.h> - -extern Thread telemetry_thread; - -#endif |