Diff: Mixer/Mixer.cpp

Revision:

30:021e13b62575

Parent:

29:8b7362a2ee14

Child:

33:fd98776b6cc7

--- a/Mixer/Mixer.cpp	Sat Dec 15 08:42:36 2012 +0000
+++ b/Mixer/Mixer.cpp	Sun Feb 10 22:08:10 2013 +0000
@@ -1,31 +1,38 @@
 #include "Mixer.h"
 
-Mixer::Mixer()
+Mixer::Mixer(int Configuration)
 {
+    Mixer::Configuration = Configuration;
+    
     for(int i=0; i<4; i++)
         Motor_speed[i]=0;
 }
 
 void Mixer::compute(unsigned long dt, int Throttle, const float * controller_value)
 {
-    // Calculate new motorspeeds
-    
-    for(int i=0; i<4; i++)
-        Motor_speed[i] = Throttle;
-    
-    Motor_speed[1] -= controller_value[0]; // Roll
-    Motor_speed[3] += controller_value[0];
+    // Mixing tables for each configuration
+    float mix_table[2][4][3] = {
+    {
+        {   0,  1,  1},       // + configuration
+        {   1,  0, -1},
+        {   0, -1,  1},
+        {  -1,  0, -1}
+    },
+    {
+        {  RT,  RT,  1},       // X configuration
+        { -RT,  RT, -1},
+        { -RT, -RT,  1},
+        {  RT, -RT, -1}
+    }
+    };
     
-    Motor_speed[0] -= controller_value[1]; // Pitch
-    Motor_speed[2] += controller_value[1];
+    // Calculate new motorspeeds
+    for(int i=0; i<4; i++) {
+        Motor_speed[i] = Throttle;
+        for(int j = 0; j < 3; j++)
+            Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j];
+    }
     
-    #if 0
-        Motor_speed[1] -= controller_value[2]; // Yaw
-        Motor_speed[3] -= controller_value[2];
-        Motor_speed[0] += controller_value[2];
-        Motor_speed[2] += controller_value[2];
-    #endif
-    
-    for(int i = 0; i < 4; i++) // make shure no motor stands still
+    for(int i = 0; i < 4; i++) // make sure no motor stands still
         Motor_speed[i] = Motor_speed[i] > 50 ? Motor_speed[i] : 50;
 }
\ No newline at end of file