Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/archive/ArducopterNG/Mixer.pde

329 lines
8.0 KiB
Plaintext
Raw Blame History

/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Mixer.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Jose Julio, Jordi Muñoz,
Jani Hirvinen, Roberto Navoni,
Sandro Benigno, Chris Anderson
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
* ************************************************************** *
ChangeLog:
* ************************************************************** *
TODO:
* ************************************************************** */
#ifdef USEMIXER
#include <stdlib.h>
float MotorPitch[MAX_MOTORS];
float MotorRoll[MAX_MOTORS];
float MotorYaw[MAX_MOTORS];
float MotorGas[MAX_MOTORS];
float motorAxisCommandPitch[MAX_MOTORS];
float motorAxisCommandRoll[MAX_MOTORS];
float motorAxisCommandYaw[MAX_MOTORS];
//float motorAxisCommand[3]; // Command on Roll YAW PITCH Recived from IMU
#if FRAME_MODEL == QUAD
// This is an example of QuadX configuration for mixertable
void init_mixer_table()
{
// Example for Quad configuration
MotorGas[0] = 100;
MotorPitch[0] = -100;
MotorRoll[0] = 100;
MotorYaw[0] = 100;
MotorGas[1] = 100;
MotorPitch[1] = 100;
MotorRoll[1] = -100;
MotorYaw[1] = 100;
MotorGas[2] = 100;
MotorPitch[2] = -100;
MotorRoll[2] = -100;
MotorYaw[2] = -100;
MotorGas[3] = 100;
MotorPitch[3] = 100;
MotorRoll[3] = 100;
MotorYaw[3] = -100;
}
#elif FRAME_MODEL == HEXA
void init_mixer_table()
{
// Hexa configuration
MotorPitch[0] = -100;
MotorRoll[0] = 0;
MotorYaw[0] = 100;
MotorPitch[1] = -50;
MotorRoll[1] = -100;
MotorYaw[1] = -100;
MotorPitch[2] = +50;
MotorRoll[2] = -100;
MotorYaw[2] = 100;
MotorPitch[3] = +100;
MotorRoll[3] = 0;
MotorYaw[3] = -100;
MotorPitch[4] = +50;
MotorRoll[4] = 100;
MotorYaw[4] = 100;
MotorPitch[5] = -50;
MotorRoll[5] = 100;
MotorYaw[5] = -100;
}
#elif FRAME_MODEL == HEXARADIAL
void init_mixer_table()
{
// Example for Hexa configuration
MotorGas[0] = 100;
MotorPitch[0] = -100;
MotorRoll[0] = 0;
MotorYaw[0] = 100;
MotorGas[1] = 100;
MotorPitch[1] = -50;
MotorRoll[1] = -100;
MotorYaw[1] = -100;
MotorGas[2] = 100;
MotorPitch[2] = +50;
MotorRoll[2] = -100;
MotorYaw[2] = 100;
MotorGas[3] = 100;
MotorPitch[3] = +100;
MotorRoll[3] = 0;
MotorYaw[3] = -100;
MotorGas[4] = 100;
MotorPitch[4] = +50;
MotorRoll[4] = 100;
MotorYaw[4] = 100;
MotorGas[5] = 100;
MotorPitch[5] = -50;
MotorRoll[5] = 100;
MotorYaw[5] = -100;
}
#elif FRAME_MODEL == HEXACOAXIAL
void init_mixer_table()
{
// Example for Hexa configuration
MotorGas[0] = 95;
MotorPitch[0] = 100;
MotorRoll[0] = 0;
MotorYaw[0] = 100;
MotorGas[1] = 100;
MotorPitch[1] = 100;
MotorRoll[1] = 0;
MotorYaw[1] = -100;
MotorGas[2] = 95;
MotorPitch[2] = -50;
MotorRoll[2] = 100;
MotorYaw[2] = -100;
MotorGas[3] = 100;
MotorPitch[3] = -50;
MotorRoll[3] = 100;
MotorYaw[3] = 100;
MotorGas[4] = 95;
MotorPitch[4] = -50;
MotorRoll[4] = -100;
MotorYaw[4] = -100;
MotorGas[5] = 100;
MotorPitch[5] = -50;
MotorRoll[5] = -100;
MotorYaw[5] = 100;
}
#elif FRAME_MODEL == OCTO
void init_mixer_table()
{
// Octo configuration. Motors are numberd CW viewed from above starting at front = 1 (CW prop rotation)
// Motor rotation is CCW for odd numbered motors
MotorGas[0] = 100;
MotorPitch[0] = 100;
MotorRoll[0] = 0;
MotorYaw[0] = -100;
MotorGas[1] = 100;
MotorPitch[1] = 100;
MotorRoll[1] = -100;
MotorYaw[1] = 100;
MotorGas[2] = 100;
MotorPitch[2] = 0;
MotorRoll[2] = -100;
MotorYaw[2] = -100;
MotorGas[3] = 100;
MotorPitch[3] = -100;
MotorRoll[3] = -100;
MotorYaw[3] = 100;
MotorGas[4] = 100;
MotorPitch[4] = -100;
MotorRoll[4] = 0;
MotorYaw[4] = -100;
MotorGas[5] = 100;
MotorPitch[5] = -100;
MotorRoll[5] = 100;
MotorYaw[5] = 100;
MotorGas[6] = 100;
MotorPitch[6] = 0;
MotorRoll[6] = 100;
MotorYaw[6] = -100;
MotorGas[7] = 100;
MotorPitch[7] = 100;
MotorRoll[7] = 100;
MotorYaw[7] = 100;
}
#else
# error You need define your frame configuration on ArduUser.h
#endif
void motor_axis_correction()
{
int i;
for (i=0;i<MAX_MOTORS;i++)
{
motorAxisCommandPitch[i] = (control_pitch / 100.0) * MotorPitch[i];
motorAxisCommandRoll[i] = (control_roll / 100.0) * MotorRoll[i];
motorAxisCommandYaw[i] = (control_yaw / 100.0) * MotorYaw[i];
}
}
//After that we can mix them together:
void motor_matrix_command()
{
int i;
float valuemotor;
for (i=0;i<MAX_MOTORS;i++)
{
valuemotor = ((ch_throttle* MotorGas[i])/100) + motorAxisCommandPitch[i] + motorAxisCommandYaw[i] + motorAxisCommandRoll[i];
//valuemotor = Throttle + motorAxisCommandPitch[i] + motorAxisCommandYaw[i] + motorAxisCommandRoll[i]; // OLD VERSION WITHOUT GAS CONOL ON Mixertable
valuemotor = constrain(valuemotor, 1000, 2000);
motorCommand[i]=valuemotor;
}
}
void matrix_debug()
{
Serial.println();
Serial.println("--------------------------");
Serial.println(" Motori Mixertable " );
Serial.println("--------------------------");
Serial.println();
Serial.println("--------------------------");
Serial.println(" Quad Motor Debug " );
Serial.println("--------------------------");
Serial.print("AIL:");
Serial.print(ch_roll);
Serial.print(" ELE:");
Serial.print(ch_pitch);
Serial.print(" THR:");
Serial.print( ch_throttle);
Serial.print(" YAW:");
Serial.print( ch_yaw);
Serial.print(" AUX:");
Serial.print(ch_aux);
Serial.print(" AUX2:");
Serial.print(ch_aux2);
Serial.println();
Serial.print("CONTROL_ROLL:");
Serial.print(control_roll);
Serial.print(" CONTROL_PITCH:");
Serial.print(control_pitch);
Serial.print(" CONTROL_YAW:");
Serial.print(control_yaw);
Serial.print(" SONAR_VALUE:");
// Serial.print(sonar_value);
// Serial.print(" TARGET_SONAR_VALUE:");
// Serial.print(target_sonar_altitude);
// Serial.print(" ERR_SONAR_VALUE:");
// Serial.print(err_altitude);
// Serial.println();
// Serial.print("latitude:");
// Serial.print(GPS_np.Lattitude);
// Serial.print(" longitude:");
// Serial.print(GPS_np.Longitude);
Serial.print(" command gps roll:");
Serial.print(command_gps_roll);
Serial.print(" command gps pitch:");
Serial.print(command_gps_pitch);
// Serial.print(" Lon_diff:");
// Serial.print(Lon_diff);
// Serial.print(" Lon_diff");
// Serial.print(command_gps_pitch);
Serial.println();
Serial.print("AP MODE:");
Serial.print((int)AP_mode);
Serial.println();
#ifdef HEXARADIAL
Serial.println();
Serial.print((unsigned int)MotorI2C[5]);
comma();
Serial.print((unsigned int)MotorI2C[0]);
comma();
Serial.print((unsigned int)MotorI2C[1]);
comma();
Serial.println();
Serial.print((unsigned int)MotorI2C[4]);
comma();
Serial.print((unsigned int)MotorI2C[3]);
comma();
Serial.println((unsigned int)MotorI2C[2]);
Serial.println("---------------");
Serial.println();
#endif
}
#endif // usemixer
Reference in New Issue View Git Blame Copy Permalink