Rev 1465 | Go to most recent revision | 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 | |||
| 21 | tmp = throttle / HELI_THROTTLE_SENSITIVITY; |
||
| 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 |
||
| 30 | pitch = throttle / HELI_PITCH_SENSITIVITY + HELI_PITCH_BASE; |
||
| 31 | if (pitch > HELI_MAX_PITCH) pitch = HELI_MAX_PITCH; |
||
| 32 | |||
| 33 | servo[0] = 127 - pitch; |
||
| 34 | servo[1] = 127 + pitch; |
||
| 35 | servo[2] = 127 + pitch; |
||
| 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; |
||
| 43 | nick -= PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / HELI_NICK_SENSITIVITY; |
||
| 44 | servo[0] -= nick; |
||
| 45 | servo[1] += nick; |
||
| 46 | servo[2] -= nick; |
||
| 47 | |||
| 48 | // roll |
||
| 49 | roll /= HELI_ROLL_GYRO_SENSITIVITY; |
||
| 50 | roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / HELI_ROLL_SENSITIVITY; |
||
| 51 | servo[0] += roll; |
||
| 52 | servo[1] += roll; |
||
| 53 | |||
| 54 | // yaw |
||
| 55 | tmp = MesswertGier / HELI_YAW_GYRO_SENSITIVITY; |
||
| 1467 | acid | 56 | servo[3] = 127 + tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] * 4 / HELI_YAW_SENSITIVITY); |
| 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 |