diff options
Diffstat (limited to 'main.cpp')
| -rw-r--r-- | main.cpp | 146 |
1 files changed, 1 insertions, 145 deletions
@@ -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]; } } |