mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
introducing flight frames and mixer tables, work
git-svn-id: https://arducopter.googlecode.com/svn/trunk@977 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
4ddf743cae
commit
14f500a688
@ -462,6 +462,20 @@ unsigned long elapsedTime = 0; // for doing custom events
|
||||
#define MODE3 3
|
||||
#define MODE4 4
|
||||
|
||||
// Frame models
|
||||
#define QUAD 0 // Normal Quad
|
||||
#define QUADCOAX 1 // Quad with double motors as coax
|
||||
#define HEXA 2 // Hexa
|
||||
#define HEXARADIAL 3
|
||||
#define HEXACOAX 4
|
||||
#define OCTO 5
|
||||
|
||||
#define PWM 0
|
||||
#define I2C 1
|
||||
#define UART 2
|
||||
|
||||
|
||||
|
||||
// Following variables stored in EEPROM
|
||||
float KP_QUAD_ROLL;
|
||||
float KI_QUAD_ROLL;
|
||||
|
||||
@ -74,6 +74,31 @@
|
||||
//#define GPSDEVICE GPSDEV_NMEA // For general NMEA compatible GPSEs
|
||||
//#dedine GPSDEVICE GPSDEV_IMU // For IMU Simulations only
|
||||
|
||||
|
||||
////////////////////////////////////////
|
||||
// Frame / Motor / ESC definitions
|
||||
|
||||
// Introducing new frame / Motor / ESC definitions for future expansion. Currently these are not in
|
||||
// use but they need to be here so implementation work can continue.
|
||||
|
||||
// New frame model definitions. (not in use yet, 28-11-10 jp)
|
||||
#define FRAME_MODEL QUAD // Quad frame model
|
||||
//#define FRAME_MODEL HEXA // Quad frame model
|
||||
//#define FRAME_MODEL OCTO // Quad frame model
|
||||
|
||||
|
||||
// New motor definition for different frame type (not in use yet, 28-11-10 jp)
|
||||
#define MAX_MOTORS 4 // Are we using more motors than 4, possible choises are 4, 6, 8
|
||||
// This has to be on main .pde to get included on all other header etc files
|
||||
|
||||
// Not in use yet, 28-11-10 jp
|
||||
#define MOTORTYPE PWM // Traditional PWM ESC's controlling motors
|
||||
//#define MOTORTYPE I2C // I2C style ESC's controlling motors
|
||||
//#define MOTORTYPE UART // UART style ESC's controlling motors
|
||||
|
||||
|
||||
|
||||
|
||||
////////////////////
|
||||
// Serial ports & speeds
|
||||
|
||||
|
||||
@ -227,17 +227,26 @@ void CALIB_AccOffset() {
|
||||
xx = xx / (loopy - 1);
|
||||
xy = xy / (loopy - 1);
|
||||
xz = xz / (loopy - 1);
|
||||
xz += 500; // Z-Axis correction
|
||||
|
||||
SerPriln("Averages as follows");
|
||||
SerPri(" ");
|
||||
tab();
|
||||
SerPri(xx);
|
||||
|
||||
tab();
|
||||
SerPri(xy);
|
||||
tab();
|
||||
SerPri(xz);
|
||||
SerPriln();
|
||||
|
||||
acc_offset_y = xy;
|
||||
acc_offset_x = xx;
|
||||
acc_offset_z = xz;
|
||||
|
||||
AN_OFFSET[3] = acc_offset_x;
|
||||
AN_OFFSET[4] = acc_offset_y;
|
||||
AN_OFFSET[5] = acc_offset_z;
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -6,8 +6,8 @@
|
||||
File : Mixer.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
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
|
||||
@ -32,3 +32,297 @@ 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
|
||||
|
||||
@ -74,6 +74,7 @@ void motor_output()
|
||||
command_rx_yaw = ToDeg(yaw);
|
||||
}
|
||||
|
||||
//#if MOTORTYPE == PWM
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(0, rightMotor);
|
||||
APM_RC.OutputCh(1, leftMotor);
|
||||
@ -83,7 +84,11 @@ void motor_output()
|
||||
// InstantPWM => Force inmediate output on PWM signals
|
||||
APM_RC.Force_Out0_Out1();
|
||||
APM_RC.Force_Out2_Out3();
|
||||
//#elif MOTORTYPE == I2C
|
||||
|
||||
//#else
|
||||
//# error You need to define your motor type on ArduUder.pde file
|
||||
//#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user