From 9b36dbb5bd91b9230e253d3ed8be6106a4c60bdb Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Thu, 28 Apr 2011 18:06:27 +0200 Subject: Added total thrust saturation in MotorMixer. --- motormixer.cpp | 29 +++++++++++++++++++++++------ 1 file 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 #include +template +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; -- cgit v1.2.3