Subversion Repositories FlightCtrl

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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 / 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 / HELI_PITCH_SENSITIVITY + HELI_PITCH_BASE;
        if (pitch > HELI_MAX_PITCH) pitch = HELI_MAX_PITCH;

                servo[0] = 127 - pitch;
                servo[1] = 127 + pitch;
                servo[2] = 127 + pitch;


        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]] / HELI_NICK_SENSITIVITY;
        servo[0] -= nick;
        servo[1] += nick;
        servo[2] -= nick;

                // roll
        roll /= HELI_ROLL_GYRO_SENSITIVITY;
        roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / HELI_ROLL_SENSITIVITY;
        servo[0] += roll;
        servo[1] += roll;

        // yaw
        tmp = MesswertGier / HELI_YAW_GYRO_SENSITIVITY;
        servo[3] = 127 + HELI_YAW_BASE + tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] * 4 / HELI_YAW_SENSITIVITY);

        // 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