diff options
| -rw-r--r-- | main.cpp | 58 |
1 files changed, 51 insertions, 7 deletions
@@ -6,6 +6,41 @@ #include "itg3200.h" +template<class T> +inline void saturate(T& var, T absmax) { + if(var > absmax) { + var = absmax; + } else if(var < -absmax) { + var = -absmax; + } +} + +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; + } +}; + int main() { RCC.enable(RCC.AFIO); RCC.enable(RCC.IOPA); @@ -35,6 +70,10 @@ int main() { TIM2.CR1 = 0x05; + PID pid_pitch(2000, 0, 0); + PID pid_roll(2000, 0, 0); + PID pid_yaw(2000, 0, 0); + while(1) { // Wait for a new update. while(!(TIM2.SR & 0x01)); @@ -47,13 +86,18 @@ int main() { // Generate motor mix. int16_t throttle = ppmsum.channels[2] - 1000; - int16_t pitch = 0; - int16_t yaw = gyro.z >> 7; - int16_t roll = 0; + int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) - gyro.x); + int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) - gyro.y); + int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) - gyro.z); + + int16_t max = throttle > 250 ? 250 : throttle; + saturate(pitch, max); + saturate(roll, max); + saturate(yaw, max); - TIM2.CCR1 = 1000 + throttle + pitch + roll - yaw; - TIM2.CCR2 = 1000 + throttle + pitch - roll + yaw; - TIM2.CCR3 = 1000 + throttle - pitch + roll + yaw; - TIM2.CCR4 = 1000 + throttle - pitch - roll - yaw; + TIM2.CCR1 = 1000 + throttle + pitch + roll + yaw; + TIM2.CCR2 = 1000 + throttle + pitch - roll - yaw; + TIM2.CCR3 = 1000 + throttle - pitch + roll - yaw; + TIM2.CCR4 = 1000 + throttle - pitch - roll + yaw; } } |