#if defined(BI) || defined(TRI) || defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING) || defined(CAMTRIG) || defined(FBALL)
#define SERVO
#endif
#if defined(GIMBAL) || defined(FLYING_WING)
#define NUMBER_MOTOR 0
#elif defined(FBALL)
#define NUMBER_MOTOR 1 // nur ein motor
#elif defined(BI)
#define NUMBER_MOTOR 2
#elif defined(TRI)
#define NUMBER_MOTOR 3
#elif defined(QUADP) || defined(QUADX) || defined(Y4)
#define NUMBER_MOTOR 4
#elif defined(Y6) || defined(HEX6) || defined(HEX6X)
#define NUMBER_MOTOR 6
#elif defined(OCTOX8) || defined(OCTOFLATP) || defined(OCTOFLATX)
#define NUMBER_MOTOR 8
#endif
...
...
#if defined(SERVO)
void initializeServo() {
#if defined(TRI)
DIGITAL_SERVO_TRI_PINMODE
#endif
#if defined(FBALL)
DIGITAL_SERVO_FBALL_ROLL_PINMODE
DIGITAL_SERVO_FBALL_PITCH_PINMODE
DIGITAL_SERVO_FBALL_YAW_PINMODE
#endif
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
DIGITAL_TILT_ROLL_PINMODE
DIGITAL_TILT_PITCH_PINMODE
#endif
#if defined(CAMTRIG)
DIGITAL_CAM_PINMODE
#endif
#if defined(BI)
DIGITAL_SERVO_TRI_PINMODE
DIGITAL_BI_LEFT_PINMODE
#endif
TCCR0A = 0; // normal counting mode
TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
}
...
...
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
#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 FBALL
motor[0] = PIDMIX(0, 0, 0);
servo[0] = constrain(FBALL_YAW_MIDDLE + YAW_DIRECTION * axisPID[YAW], FBALL_YAW_CONSTRAINT_MIN, FBALL_YAW_CONSTRAINT_MAX); //YAW
servo[1] = constrain(FBALL_ROLL_MIDDLE + ROLL_DIRECTION * axisPID[ROLL], FBALL_ROLL_CONSTRAINT_MIN, FBALL_ROLL_CONSTRAINT_MAX); //ROLL
servo[2] = constrain(FBALL_PITCH_MIDDLE + PITCH_DIRECTION * axisPID[PITCH], FBALL_PITCH_CONSTRAINT_MIN, FBALL_PITCH_CONSTRAINT_MAX); //PITCH
#endif