summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--main.cpp26
1 files changed, 22 insertions, 4 deletions
diff --git a/main.cpp b/main.cpp
index c0deaf9..ed03845 100644
--- a/main.cpp
+++ b/main.cpp
@@ -15,6 +15,17 @@ inline void saturate(T& var, T 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;
@@ -95,9 +106,16 @@ int main() {
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;
+ int cmds[] = {
+ 1000 + throttle + pitch + roll + yaw,
+ 1000 + throttle + pitch - roll - yaw,
+ 1000 + throttle - pitch + roll - yaw,
+ 1000 + throttle - pitch - roll + yaw,
+ };
+
+ TIM2.CCR1 = limit(cmds[0], 1000, 2000);
+ TIM2.CCR2 = limit(cmds[1], 1000, 2000);
+ TIM2.CCR3 = limit(cmds[2], 1000, 2000);
+ TIM2.CCR4 = limit(cmds[3], 1000, 2000);
}
}