summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--motormixer.cpp29
1 files changed, 23 insertions, 6 deletions
diff --git a/motormixer.cpp b/motormixer.cpp
index 1951f29..5718526 100644
--- a/motormixer.cpp
+++ b/motormixer.cpp
@@ -3,6 +3,15 @@
#include <ch.h>
#include <hal.h>
+template<class T>
+inline T abs(T val) {
+ if(val > 0) {
+ return val;
+ } else {
+ return -val;
+ }
+}
+
namespace {
uint16_t motors[4];
@@ -49,13 +58,21 @@ void MotorMixer::update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw
thrust = 1000;
}
- //pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;
- //roll = ((ppmsum.data[0] - 500) * thrust) >> 10;
- //yaw = ((ppmsum.data[3] - 500) * thrust) >> 10;
+ // Check that total thrust will not be exceeded.
+ if(abs(pitch) + abs(roll) > thrust) {
+ // If saturated by pitch/roll, drop yaw component.
+ yaw = 0;
+
+ // Scale pitch and roll.
+ int32_t df = ((abs(pitch) + abs(roll)) << 16) / thrust;
+
+ pitch = (pitch << 16) / df;
+ roll = (roll << 16) / df;
- //pitch = (-i2c_thread.x) >> 5;
- //roll = i2c_thread.y >> 5;
- //yaw = i2c_thread.z >> 5;
+ } else if(abs(pitch) + abs(roll) + abs(yaw) > thrust) {
+ // Scale yaw value.
+ yaw = (yaw > 0 ? 1 : -1) * (thrust - abs(pitch) - abs(roll));
+ }
motors[0] = thrust + pitch + roll - yaw;
motors[1] = thrust + pitch - roll + yaw;