rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

moving towards custom mixer table and proper fixedwing mixer. 

refactored all of mixer.c to use tables instead of hardcoded mix.
custom mixer is stored in flash, but currently has no UI to configure it.
do not choose mixer CUSTOM in console.

hover-tested QUADX, if upgrading anything else, please carefully check motor response first, preferably without props on heavy models.
tec

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@206 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2012-09-05 23:44:55 +00:00
parent fef43457c0
commit 98cba890e1
4 changed files with 2865 additions and 2822 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5355
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

6
src/config.c
View File

@ -13,7 +13,7 @@ config_t cfg;
const char rcChannelLetters[] = "AERT1234"; const char rcChannelLetters[] = "AERT1234";
static uint32_t enabledSensors = 0; static uint32_t enabledSensors = 0;
uint8_t checkNewConf = 27; uint8_t checkNewConf = 28;
void parseRcChannels(const char *input) void parseRcChannels(const char *input)
{ {
@ -196,6 +196,10 @@ void checkFirstTime(bool reset)
// serial (USART1) baudrate // serial (USART1) baudrate
cfg.serial_baudrate = 115200; cfg.serial_baudrate = 115200;
// custom mixer
for (i = 0; i < MAX_MOTORS; i++)
cfg.customMixer[i].throttle = 0.0f;
writeParams(0); writeParams(0);
} }

309
src/mixer.c
View File

@ -1,78 +1,162 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
static uint8_t numberMotor = 4; static uint8_t numberMotor = 0;
uint8_t useServo = 0; uint8_t useServo = 0;
int16_t motor[MAX_MOTORS]; int16_t motor[MAX_MOTORS];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
typedef struct { static motorMixer_t currentMixer[MAX_MOTORS];
uint8_t enabled; // is this mix channel enabled
float throttle; // proportion of throttle affect
float pitch;
float roll;
float yaw;
} mixerPower_t;
static mixerPower_t customMixer[12]; static const motorMixer_t mixerTri[] = {
{ 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
{ 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT
{ 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT
};
static const motorMixer_t mixerQuadP[] = {
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
{ 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT
{ 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
};
static const motorMixer_t mixerQuadX[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
};
static const motorMixer_t mixerBi[] = {
{ 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT
{ 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
};
static const motorMixer_t mixerY6[] = {
{ 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR
{ 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT
{ 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT
{ 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR
{ 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT
{ 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT
};
static const motorMixer_t mixerHex6P[] = {
{ 1.0f, -1.0f, 0.866025f, 1.0f }, // REAR_R
{ 1.0f, -1.0f, -0.866025f, -1.0f }, // FRONT_R
{ 1.0f, 1.0f, 0.866025f, 1.0f }, // REAR_L
{ 1.0f, 1.0f, -0.866025f, -1.0f }, // FRONT_L
{ 1.0f, 0.0f, 0.866025f, 1.0f }, // FRONT
{ 1.0f, 0.0f, -0.866025f, -1.0f }, // REAR
};
static const motorMixer_t mixerY4[] = {
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW
{ 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW
};
static const motorMixer_t mixerHex6X[] = {
{ 1.0f, -0.866025f, 1.0f, 1.0f }, // REAR_R
{ 1.0f, -0.866025f, -1.0f, 1.0f }, // FRONT_R
{ 1.0f, 0.866025f, 1.0f, -1.0f }, // REAR_L
{ 1.0f, 0.866025f, -1.0f, -1.0f }, // FRONT_L
{ 1.0f, -0.866025f, 0.0f, -1.0f }, // RIGHT
{ 1.0f, 0.866025f, 0.0f, 1.0f }, // LEFT
};
static const motorMixer_t mixerOctoX8[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
{ 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R
{ 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R
{ 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L
{ 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L
};
static const motorMixer_t mixerOctoFlatP[] = {
{ 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L
{ 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R
{ 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R
{ 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
{ 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
{ 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT
};
static const motorMixer_t mixerOctoFlatX[] = {
{ 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L
{ 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R
{ 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R
{ 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L
{ 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L
{ 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R
{ 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L
};
static const motorMixer_t mixerVtail4[] = {
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
};
// Keep this synced with MultiType struct in mw.h!
static const mixer_t mixers[] = {
// Mo Se Mixtable
{ 0, 0, NULL }, // entry 0
{ 3, 1, mixerTri }, // MULTITYPE_TRI
{ 4, 0, mixerQuadP }, // MULTITYPE_QUADP
{ 4, 0, mixerQuadX }, // MULTITYPE_QUADX
{ 2, 1, mixerBi }, // MULTITYPE_BI
{ 0, 1, NULL }, // * MULTITYPE_GIMBAL
{ 6, 0, mixerY6 }, // MULTITYPE_Y6
{ 6, 0, mixerHex6P }, // MULTITYPE_HEX6
{ 1, 1, NULL }, // * MULTITYPE_FLYING_WING
{ 4, 0, mixerY4 }, // MULTITYPE_Y4
{ 6, 0, mixerHex6X }, // MULTITYPE_HEX6X
{ 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8
{ 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP
{ 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX
{ 1, 1, NULL }, // * MULTITYPE_AIRPLANE
{ 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM
{ 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG
{ 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
};
void mixerInit(void) void mixerInit(void)
{ {
int i; int i;
// enable servos for mixes that require them. note, this shifts motor counts. // enable servos for mixes that require them. note, this shifts motor counts.
if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_FLYING_WING || useServo = mixers[cfg.mixerConfiguration].useServo;
cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_GIMBAL) // if we want camstab/trig, that also enables servos, even if mixer doesn't
useServo = 1;
// if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later.
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
useServo = 1; useServo = 1;
switch (cfg.mixerConfiguration) { if (cfg.mixerConfiguration == MULTITYPE_CUSTOM) {
case MULTITYPE_GIMBAL: // load custom mixer into currentMixer
numberMotor = 0; for (i = 0; i < MAX_MOTORS; i++) {
break; // check if done
if (cfg.customMixer[i].throttle == 0.0f)
case MULTITYPE_AIRPLANE: break;
case MULTITYPE_FLYING_WING: currentMixer[i] = cfg.customMixer[i];
numberMotor = 1; numberMotor++;
break; }
} else {
case MULTITYPE_BI: numberMotor = mixers[cfg.mixerConfiguration].numberMotor;
numberMotor = 2; // copy motor-based mixers
break; if (mixers[cfg.mixerConfiguration].motor) {
for (i = 0; i < numberMotor; i++)
case MULTITYPE_TRI: currentMixer[i] = mixers[cfg.mixerConfiguration].motor[i];
numberMotor = 3; }
break;
case MULTITYPE_QUADP:
case MULTITYPE_QUADX:
case MULTITYPE_Y4:
case MULTITYPE_VTAIL4:
numberMotor = 4;
break;
case MULTITYPE_Y6:
case MULTITYPE_HEX6:
case MULTITYPE_HEX6X:
numberMotor = 6;
break;
case MULTITYPE_OCTOX8:
case MULTITYPE_OCTOFLATP:
case MULTITYPE_OCTOFLATX:
numberMotor = 8;
break;
case MULTITYPE_CUSTOM:
numberMotor = 0;
for (i = 0; i < MAX_MOTORS; i++) {
if (customMixer[i].enabled)
numberMotor++;
}
break;
} }
} }
@ -99,6 +183,11 @@ void writeServos(void)
break; break;
case MULTITYPE_GIMBAL:
pwmWriteServo(0, servo[0]);
pwmWriteServo(1, servo[1]);
break;
default: default:
// Two servos for SERVO_TILT, if enabled // Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) { if (feature(FEATURE_SERVO_TILT)) {
@ -129,8 +218,6 @@ void writeAllMotors(int16_t mc)
writeMotors(); writeMotors();
} }
#define PIDMIX(R, P, Y) rcCommand[THROTTLE] + axisPID[ROLL] * R + axisPID[PITCH] * P + cfg.yaw_direction * axisPID[YAW] * Y
static void airplaneMixer(void) static void airplaneMixer(void)
{ {
#if 0 #if 0
@ -191,115 +278,27 @@ void mixTable(void)
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
} }
switch (cfg.mixerConfiguration) { // motors for non-servo mixes
if (numberMotor > 1)
for (i = 0; i < numberMotor; i++)
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
// airplane / servo mixes
switch (cfg.mixerConfiguration) {
case MULTITYPE_BI: case MULTITYPE_BI:
motor[0] = PIDMIX(+1, 0, 0); //LEFT
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
servo[4] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT servo[4] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT servo[5] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
break; break;
case MULTITYPE_TRI: case MULTITYPE_TRI:
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
servo[5] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR servo[5] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
break; break;
case MULTITYPE_QUADP:
motor[0] = PIDMIX(0, +1, -1); //REAR
motor[1] = PIDMIX(-1, 0, +1); //RIGHT
motor[2] = PIDMIX(+1, 0, +1); //LEFT
motor[3] = PIDMIX(0, -1, -1); //FRONT
break;
case MULTITYPE_QUADX:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
break;
case MULTITYPE_Y4:
motor[0] = PIDMIX(+0, +1, -1); //REAR_1 CW
motor[1] = PIDMIX(-1, -1, 0); //FRONT_R CCW
motor[2] = PIDMIX(+0, +1, +1); //REAR_2 CCW
motor[3] = PIDMIX(+1, -1, 0); //FRONT_L CW
break;
case MULTITYPE_Y6:
motor[0] = PIDMIX(+0, +4 / 3, +1); //REAR
motor[1] = PIDMIX(-1, -2 / 3, -1); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, -1); //LEFT
motor[3] = PIDMIX(+0, +4 / 3, -1); //UNDER_REAR
motor[4] = PIDMIX(-1, -2 / 3, +1); //UNDER_RIGHT
motor[5] = PIDMIX(+1, -2 / 3, +1); //UNDER_LEFT
break;
case MULTITYPE_HEX6:
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
motor[1] = PIDMIX(-1 / 2, -1 / 2, -1); //FRONT_R
motor[2] = PIDMIX(+1 / 2, +1 / 2, +1); //REAR_L
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
motor[4] = PIDMIX(+0, -1, +1); //FRONT
motor[5] = PIDMIX(+0, +1, -1); //REAR
break;
case MULTITYPE_HEX6X:
motor[0] = PIDMIX(-4/5,+9/10,+1); //REAR_R
motor[1] = PIDMIX(-4/5,-9/10,+1); //FRONT_R
motor[2] = PIDMIX(+4/5,+9/10,-1); //REAR_L
motor[3] = PIDMIX(+4/5,-9/10,-1); //FRONT_L
motor[4] = PIDMIX(-4/5 ,+0 ,-1); //RIGHT
motor[5] = PIDMIX(+4/5 ,+0 ,+1); //LEFT
break;
case MULTITYPE_OCTOX8:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
motor[4] = PIDMIX(-1, +1, +1); //UNDER_REAR_R
motor[5] = PIDMIX(-1, -1, -1); //UNDER_FRONT_R
motor[6] = PIDMIX(+1, +1, -1); //UNDER_REAR_L
motor[7] = PIDMIX(+1, -1, +1); //UNDER_FRONT_L
break;
case MULTITYPE_OCTOFLATP:
motor[0] = PIDMIX(+7 / 10, -7 / 10, +1); //FRONT_L
motor[1] = PIDMIX(-7 / 10, -7 / 10, +1); //FRONT_R
motor[2] = PIDMIX(-7 / 10, +7 / 10, +1); //REAR_R
motor[3] = PIDMIX(+7 / 10, +7 / 10, +1); //REAR_L
motor[4] = PIDMIX(+0, -1, -1); //FRONT
motor[5] = PIDMIX(-1, +0, -1); //RIGHT
motor[6] = PIDMIX(+0, +1, -1); //REAR
motor[7] = PIDMIX(+1, +0, -1); //LEFT
break;
case MULTITYPE_OCTOFLATX:
motor[0] = PIDMIX(+1, -1 / 2, +1); //MIDFRONT_L
motor[1] = PIDMIX(-1 / 2, -1, +1); //FRONT_R
motor[2] = PIDMIX(-1, +1 / 2, +1); //MIDREAR_R
motor[3] = PIDMIX(+1 / 2, +1, +1); //REAR_L
motor[4] = PIDMIX(+1 / 2, -1, -1); //FRONT_L
motor[5] = PIDMIX(-1, -1 / 2, -1); //MIDFRONT_R
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
break;
case MULTITYPE_VTAIL4:
motor[0] = PIDMIX(+0, +1, +1); //REAR_R
motor[1] = PIDMIX(-1, -1, +0); //FRONT_R
motor[2] = PIDMIX(+0, +1, -1); //REAR_L
motor[3] = PIDMIX(+1, -1, -0); //FRONT_L
break;
case MULTITYPE_GIMBAL: case MULTITYPE_GIMBAL:
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max); servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max); servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
break; break;
case MULTITYPE_AIRPLANE: case MULTITYPE_AIRPLANE:
airplaneMixer(); airplaneMixer();
break; break;

17
src/mw.h
View File

@ -89,6 +89,19 @@ typedef enum GimbalFlags {
#define abs(x) ((x) > 0 ? (x) : -(x)) #define abs(x) ((x) > 0 ? (x) : -(x))
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) #define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
typedef struct motorMixer_t {
float throttle;
float roll;
float pitch;
float yaw;
} motorMixer_t;
typedef struct mixer_t {
uint8_t numberMotor;
uint8_t useServo;
const motorMixer_t *motor;
} mixer_t;
typedef struct config_t { typedef struct config_t {
uint8_t version; uint8_t version;
uint8_t mixerConfiguration; uint8_t mixerConfiguration;
@ -179,8 +192,10 @@ typedef struct config_t {
uint16_t nav_speed_min; // cm/sec uint16_t nav_speed_min; // cm/sec
uint16_t nav_speed_max; // cm/sec uint16_t nav_speed_max; // cm/sec
// serial(uart1) baudrate // serial(uart1) baudrate
uint32_t serial_baudrate; uint32_t serial_baudrate;
motorMixer_t customMixer[MAX_MOTORS]; // custom mixtable
} config_t; } config_t;
typedef struct flags_t { typedef struct flags_t {