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
- Committer:
- maetugr
- Date:
- 2013-02-10
- Revision:
- 30:021e13b62575
- Parent:
- 29:8b7362a2ee14
- Child:
- 33:fd98776b6cc7
File content as of revision 30:021e13b62575:
#include "Mixer.h" 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) { // 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} } }; // 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]; } for(int i = 0; i < 4; i++) // make sure no motor stands still Motor_speed[i] = Motor_speed[i] > 50 ? Motor_speed[i] : 50; }