// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Servo Mixer // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #if 0 { } #else { int16_t nick, roll, tmp, pitch, throttle; int16_t servo[6]; // throttle throttle = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + HELI_THROTTLE_DEADBAND; if (throttle < 5) throttle = 0; tmp = throttle * 10 / HELI_THROTTLE_SENSITIVITY; if (tmp > HELI_MAX_THROTTLE * 2) tmp = HELI_MAX_THROTTLE * 2; DebugOut.Analog[7] = tmp; if (MotorenEin == 0 || MikroKopterFlags & FLAG_NOTLANDUNG) { tmp = 0; } servo[4] = tmp; // pitch pitch = throttle * 5 / HELI_PITCH_SENSITIVITY + HELI_PITCH_BASE; if (pitch > HELI_MAX_PITCH) pitch = HELI_MAX_PITCH; servo[0] = 127 - (pitch * SERVO1_DIR); servo[1] = 127 + (pitch * SERVO2_DIR); servo[2] = 127 + (pitch * SERVO3_DIR); nick = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); roll = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); // nick nick /= HELI_NICK_GYRO_SENSITIVITY; nick -= PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * 10 / HELI_NICK_SENSITIVITY; servo[0] -= (nick * SERVO1_DIR); servo[1] += (nick * SERVO2_DIR); servo[2] -= (nick * SERVO3_DIR); // roll roll /= HELI_ROLL_GYRO_SENSITIVITY; roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * 10 / HELI_ROLL_SENSITIVITY; servo[0] += (roll * SERVO1_DIR);; servo[1] += (roll * SERVO2_DIR);; // yaw tmp = MesswertGier / HELI_YAW_GYRO_SENSITIVITY; servo[3] = 127 + (tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] * 4 / HELI_YAW_SENSITIVITY) * SERVO4_DIR); // not used servo[5] = 127; for(tmp = 0; tmp < 6; tmp++) { int t; t = servo[tmp] + Mixer.Motor[tmp][3]; if (t < 0) t = 0; else if (t > 255) t = 255; servoValues[tmp] = t; } DebugOut.Analog[12] = servo[0]; DebugOut.Analog[13] = servo[1]; DebugOut.Analog[14] = servo[2]; DebugOut.Analog[15] = servo[3]; DebugOut.Analog[6] = pitch; } #endif