summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--main.cpp58
1 files changed, 51 insertions, 7 deletions
diff --git a/main.cpp b/main.cpp
index 6bd8f6e..c0deaf9 100644
--- a/main.cpp
+++ b/main.cpp
@@ -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;
}
}