18,7 → 18,7 |
throttle = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + HELI_THROTTLE_DEADBAND; |
if (throttle < 5) throttle = 0; |
|
tmp = throttle / HELI_THROTTLE_SENSITIVITY; |
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) { |
27,12 → 27,12 |
servo[4] = tmp; |
|
// pitch |
pitch = throttle / HELI_PITCH_SENSITIVITY + HELI_PITCH_BASE; |
pitch = throttle * 5 / 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; |
servo[0] = 127 - (pitch * SERVO1_DIR); |
servo[1] = 127 + (pitch * SERVO2_DIR); |
servo[2] = 127 + (pitch * SERVO3_DIR); |
|
|
nick = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
40,20 → 40,20 |
|
// 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; |
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]] / HELI_ROLL_SENSITIVITY; |
servo[0] += roll; |
servo[1] += roll; |
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); |
servo[3] = 127 + (tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] * 4 / HELI_YAW_SENSITIVITY) * SERVO4_DIR); |
|
// not used |
servo[5] = 127; |