diff options
| -rw-r--r-- | motormixer.cpp | 29 |
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; |