| 0,0 → 1,78 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // 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 |