NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
Mixer/Mixer.cpp@33:fd98776b6cc7, 2013-04-04 (annotated)
- Committer:
- maetugr
- Date:
- Thu Apr 04 14:25:21 2013 +0000
- Revision:
- 33:fd98776b6cc7
- Parent:
- 30:021e13b62575
- Child:
- 34:3aa1cbcde59d
New version developed in eastern holidays, ported Madgwick Filter, added support for chaning PID values while flying over bluetooth, still not flying stable or even controllable
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 26:96a072233d7a | 1 | #include "Mixer.h" |
maetugr | 26:96a072233d7a | 2 | |
maetugr | 30:021e13b62575 | 3 | Mixer::Mixer(int Configuration) |
maetugr | 26:96a072233d7a | 4 | { |
maetugr | 30:021e13b62575 | 5 | Mixer::Configuration = Configuration; |
maetugr | 30:021e13b62575 | 6 | |
maetugr | 26:96a072233d7a | 7 | for(int i=0; i<4; i++) |
maetugr | 26:96a072233d7a | 8 | Motor_speed[i]=0; |
maetugr | 26:96a072233d7a | 9 | } |
maetugr | 26:96a072233d7a | 10 | |
maetugr | 33:fd98776b6cc7 | 11 | void Mixer::compute(int Throttle, const float * controller_value) |
maetugr | 26:96a072233d7a | 12 | { |
maetugr | 30:021e13b62575 | 13 | // Mixing tables for each configuration |
maetugr | 30:021e13b62575 | 14 | float mix_table[2][4][3] = { |
maetugr | 30:021e13b62575 | 15 | { |
maetugr | 30:021e13b62575 | 16 | { 0, 1, 1}, // + configuration |
maetugr | 30:021e13b62575 | 17 | { 1, 0, -1}, |
maetugr | 30:021e13b62575 | 18 | { 0, -1, 1}, |
maetugr | 30:021e13b62575 | 19 | { -1, 0, -1} |
maetugr | 30:021e13b62575 | 20 | }, |
maetugr | 30:021e13b62575 | 21 | { |
maetugr | 30:021e13b62575 | 22 | { RT, RT, 1}, // X configuration |
maetugr | 30:021e13b62575 | 23 | { -RT, RT, -1}, |
maetugr | 30:021e13b62575 | 24 | { -RT, -RT, 1}, |
maetugr | 30:021e13b62575 | 25 | { RT, -RT, -1} |
maetugr | 30:021e13b62575 | 26 | } |
maetugr | 30:021e13b62575 | 27 | }; |
maetugr | 29:8b7362a2ee14 | 28 | |
maetugr | 30:021e13b62575 | 29 | // Calculate new motorspeeds |
maetugr | 30:021e13b62575 | 30 | for(int i=0; i<4; i++) { |
maetugr | 30:021e13b62575 | 31 | Motor_speed[i] = Throttle; |
maetugr | 30:021e13b62575 | 32 | for(int j = 0; j < 3; j++) |
maetugr | 30:021e13b62575 | 33 | Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j]; |
maetugr | 30:021e13b62575 | 34 | } |
maetugr | 26:96a072233d7a | 35 | |
maetugr | 30:021e13b62575 | 36 | for(int i = 0; i < 4; i++) // make sure no motor stands still |
maetugr | 26:96a072233d7a | 37 | Motor_speed[i] = Motor_speed[i] > 50 ? Motor_speed[i] : 50; |
maetugr | 26:96a072233d7a | 38 | } |