Code: Select all
/* this parameter defines the maximum correction per axis multiwii can output before mixing the output to different motors
42 this parameter becomes inactive as soon as the stick are far from the center position in order to keep acro abilities
43 reducing this parameter can avoid big wobbles and allows higher PID settings
44 */
45 #define MAX_CORRECTION 100Code: Select all
void mixTable() {
int16_t maxMotor,a;
uint8_t i,axis;
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z
//limit big wobble issues
for(axis=0;axis<2;axis++) {
a = abs(rcCommand[axis]);
axisPID[axis] = constrain(axisPID[axis],-MAX_CORRECTION-a,+MAX_CORRECTION+a);
}
#if NUMBER_MOTOR > 3
//prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW],-100-abs(rcCommand[YAW]),+100+abs(rcCommand[YAW]));
#endif
#ifdef 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[0] = constrain(TRI_YAW_MIDDLE + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
#endif
