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