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@30:021e13b62575, 2013-02-10 (annotated)
- Committer:
- maetugr
- Date:
- Sun Feb 10 22:08:10 2013 +0000
- Revision:
- 30:021e13b62575
- Parent:
- 29:8b7362a2ee14
- Child:
- 33:fd98776b6cc7
newest changes because of the needed faster refresh rate for the ESCs 495Hz and the new build of the same hardware in X-configuration.; ; RC angle steering not yet finished
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 | 29:8b7362a2ee14 | 11 | void Mixer::compute(unsigned long dt, 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 | } |