Rev 1467 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1465 | acid | 1 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 2 | // Servo Mixer |
||
| 3 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 4 | #if 0 |
||
| 5 | { |
||
| 6 | |||
| 7 | |||
| 8 | |||
| 9 | |||
| 10 | } |
||
| 11 | #else |
||
| 12 | |||
| 13 | { |
||
| 14 | int16_t nick, roll, tmp, pitch, throttle; |
||
| 15 | int16_t servo[6]; |
||
| 16 | |||
| 17 | // throttle |
||
| 18 | throttle = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + HELI_THROTTLE_DEADBAND; |
||
| 19 | if (throttle < 5) throttle = 0; |
||
| 20 | |||
| 1478 | acid | 21 | tmp = throttle * 10 / HELI_THROTTLE_SENSITIVITY; |
| 1465 | acid | 22 | if (tmp > HELI_MAX_THROTTLE * 2) tmp = HELI_MAX_THROTTLE * 2; |
| 23 | DebugOut.Analog[7] = tmp; |
||
| 24 | if (MotorenEin == 0 || MikroKopterFlags & FLAG_NOTLANDUNG) { |
||
| 25 | tmp = 0; |
||
| 26 | } |
||
| 27 | servo[4] = tmp; |
||
| 28 | |||
| 29 | // pitch |
||
| 1478 | acid | 30 | pitch = throttle * 5 / HELI_PITCH_SENSITIVITY + HELI_PITCH_BASE; |
| 1465 | acid | 31 | if (pitch > HELI_MAX_PITCH) pitch = HELI_MAX_PITCH; |
| 32 | |||
| 1478 | acid | 33 | servo[0] = 127 - (pitch * SERVO1_DIR); |
| 34 | servo[1] = 127 + (pitch * SERVO2_DIR); |
||
| 35 | servo[2] = 127 + (pitch * SERVO3_DIR); |
||
| 1465 | acid | 36 | |
| 37 | |||
| 38 | nick = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
||
| 39 | roll = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
||
| 40 | |||
| 41 | // nick |
||
| 42 | nick /= HELI_NICK_GYRO_SENSITIVITY; |
||
| 1478 | acid | 43 | nick -= PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * 10 / HELI_NICK_SENSITIVITY; |
| 44 | servo[0] -= (nick * SERVO1_DIR); |
||
| 45 | servo[1] += (nick * SERVO2_DIR); |
||
| 46 | servo[2] -= (nick * SERVO3_DIR); |
||
| 1465 | acid | 47 | |
| 48 | // roll |
||
| 49 | roll /= HELI_ROLL_GYRO_SENSITIVITY; |
||
| 1478 | acid | 50 | roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * 10 / HELI_ROLL_SENSITIVITY; |
| 51 | servo[0] += (roll * SERVO1_DIR);; |
||
| 52 | servo[1] += (roll * SERVO2_DIR);; |
||
| 1465 | acid | 53 | |
| 54 | // yaw |
||
| 55 | tmp = MesswertGier / HELI_YAW_GYRO_SENSITIVITY; |
||
| 1478 | acid | 56 | servo[3] = 127 + (tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] * 4 / HELI_YAW_SENSITIVITY) * SERVO4_DIR); |
| 1465 | acid | 57 | |
| 58 | // not used |
||
| 59 | servo[5] = 127; |
||
| 60 | |||
| 61 | for(tmp = 0; tmp < 6; tmp++) { |
||
| 62 | int t; |
||
| 63 | t = servo[tmp] + Mixer.Motor[tmp][3]; |
||
| 64 | if (t < 0) t = 0; |
||
| 65 | else if (t > 255) t = 255; |
||
| 66 | servoValues[tmp] = t; |
||
| 67 | } |
||
| 68 | |||
| 69 | DebugOut.Analog[12] = servo[0]; |
||
| 70 | DebugOut.Analog[13] = servo[1]; |
||
| 71 | DebugOut.Analog[14] = servo[2]; |
||
| 72 | DebugOut.Analog[15] = servo[3]; |
||
| 73 | DebugOut.Analog[6] = pitch; |
||
| 74 | |||
| 75 | } |
||
| 76 | |||
| 77 | |||
| 78 | #endif |