Subversion Repositories FlightCtrl

Rev

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