User | Revision | Line number | New contents of line |
agiembed |
0:d463d5c04541
|
1
|
short alti_con, roll_con, yaw_con, pitch_con;
|
agiembed |
0:d463d5c04541
|
2
|
char yaw_state1 = 0, yaw_state2 = 0, yaw_cnt = 0;
|
agiembed |
0:d463d5c04541
|
3
|
#define PID_max 80
|
agiembed |
0:d463d5c04541
|
4
|
#define Int_max 100
|
agiembed |
0:d463d5c04541
|
5
|
#define Int_min -100
|
agiembed |
0:d463d5c04541
|
6
|
|
agiembed |
0:d463d5c04541
|
7
|
float PIDRoll, PIDPitch, PIDYaw, PIDAlti;
|
agiembed |
0:d463d5c04541
|
8
|
float Int_Roll, Int_Pitch;
|
agiembed |
0:d463d5c04541
|
9
|
float Roll_error, Roll_prev_error, Roll_ref = 0, Roll_int_error = 0;
|
agiembed |
0:d463d5c04541
|
10
|
float Pitch_error, Pitch_prev_error, Pitch_ref = 0, Pitch_int_error = 0;
|
agiembed |
0:d463d5c04541
|
11
|
float Yaw_error, Yaw_prev_error, Yaw_ref, Yaw_prev_ref;
|
agiembed |
0:d463d5c04541
|
12
|
float Alti_error, Alti_prev_error, Z_rate_error, Alti_ref, Alti_I_error;
|
agiembed |
0:d463d5c04541
|
13
|
|
agiembed |
0:d463d5c04541
|
14
|
|
agiembed |
0:d463d5c04541
|
15
|
void PID_Roll() {
|
agiembed |
0:d463d5c04541
|
16
|
|
agiembed |
0:d463d5c04541
|
17
|
gainRoll.p = 0.65;//28; //1.05;
|
agiembed |
0:d463d5c04541
|
18
|
gainRoll.d = 0.5;
|
agiembed |
0:d463d5c04541
|
19
|
gainRoll.i = 0.035;
|
agiembed |
0:d463d5c04541
|
20
|
gainRoll.r = 0.175;//9;
|
agiembed |
0:d463d5c04541
|
21
|
//if(fcc.US < 1000){
|
agiembed |
0:d463d5c04541
|
22
|
//gainRoll.p = 1.05; //1.05 Take-off Gain
|
agiembed |
0:d463d5c04541
|
23
|
//gainRoll.r = 0.275; //0.275 Take-off Gain
|
agiembed |
0:d463d5c04541
|
24
|
//}
|
agiembed |
0:d463d5c04541
|
25
|
|
agiembed |
0:d463d5c04541
|
26
|
Roll_ref = (RC.rolls / 7);
|
agiembed |
0:d463d5c04541
|
27
|
if (Roll_ref>30) Roll_ref = 30;
|
agiembed |
0:d463d5c04541
|
28
|
else if (Roll_ref<-30) Roll_ref = -30;
|
agiembed |
0:d463d5c04541
|
29
|
|
agiembed |
0:d463d5c04541
|
30
|
Roll_error = fcc.roll - Roll_ref;
|
agiembed |
0:d463d5c04541
|
31
|
Roll_int_error += Roll_error;
|
agiembed |
0:d463d5c04541
|
32
|
|
agiembed |
0:d463d5c04541
|
33
|
if (Roll_int_error > Int_max) Roll_int_error = Int_max;
|
agiembed |
0:d463d5c04541
|
34
|
else if (Roll_int_error < Int_min) Roll_int_error = Int_min;
|
agiembed |
0:d463d5c04541
|
35
|
Int_Roll = gainRoll.i * Roll_int_error;
|
agiembed |
0:d463d5c04541
|
36
|
|
agiembed |
0:d463d5c04541
|
37
|
PIDRoll = gainRoll.p * Roll_error;
|
agiembed |
0:d463d5c04541
|
38
|
PIDRoll += Int_Roll;
|
agiembed |
0:d463d5c04541
|
39
|
PIDRoll += (gainRoll.d * (Roll_error - Roll_prev_error));
|
agiembed |
0:d463d5c04541
|
40
|
PIDRoll += (gainRoll.r * fcc.g_roll);
|
agiembed |
0:d463d5c04541
|
41
|
|
agiembed |
0:d463d5c04541
|
42
|
if (PIDRoll > PID_max) PIDRoll = PID_max;
|
agiembed |
0:d463d5c04541
|
43
|
else if (PIDRoll < -PID_max) PIDRoll = -PID_max;
|
agiembed |
0:d463d5c04541
|
44
|
|
agiembed |
0:d463d5c04541
|
45
|
Roll_prev_error = Roll_error;
|
agiembed |
0:d463d5c04541
|
46
|
return;
|
agiembed |
0:d463d5c04541
|
47
|
}
|
agiembed |
0:d463d5c04541
|
48
|
|
agiembed |
0:d463d5c04541
|
49
|
void PID_Pitch() {
|
agiembed |
0:d463d5c04541
|
50
|
|
agiembed |
0:d463d5c04541
|
51
|
gainPitch.p = gainRoll.p;//1.075; //1.35 //1.05;
|
agiembed |
0:d463d5c04541
|
52
|
gainPitch.d = gainRoll.d;//0.9635;
|
agiembed |
0:d463d5c04541
|
53
|
gainPitch.i = gainRoll.i;//0.021;
|
agiembed |
0:d463d5c04541
|
54
|
gainPitch.r = gainRoll.r;// 0.175; //0.29
|
agiembed |
0:d463d5c04541
|
55
|
//if(fcc.US < 1000){
|
agiembed |
0:d463d5c04541
|
56
|
// gainPitch.p = 1.05; //1.05 Take-off Gain
|
agiembed |
0:d463d5c04541
|
57
|
// gainPitch.r = 0.275; //0.275 Take-off Gain
|
agiembed |
0:d463d5c04541
|
58
|
//}
|
agiembed |
0:d463d5c04541
|
59
|
|
agiembed |
0:d463d5c04541
|
60
|
Pitch_ref = (-RC.pitchs / 6);
|
agiembed |
0:d463d5c04541
|
61
|
if (Pitch_ref>30) Pitch_ref = 30;
|
agiembed |
0:d463d5c04541
|
62
|
else if (Pitch_ref<-30) Pitch_ref = -30;
|
agiembed |
0:d463d5c04541
|
63
|
|
agiembed |
0:d463d5c04541
|
64
|
Pitch_error = fcc.pitch - Pitch_ref;
|
agiembed |
0:d463d5c04541
|
65
|
Pitch_int_error += Pitch_error;
|
agiembed |
0:d463d5c04541
|
66
|
|
agiembed |
0:d463d5c04541
|
67
|
if (Pitch_int_error > Int_max) Pitch_int_error = Int_max;
|
agiembed |
0:d463d5c04541
|
68
|
else if (Pitch_int_error < Int_min) Pitch_int_error = Int_min;
|
agiembed |
0:d463d5c04541
|
69
|
Int_Pitch = gainPitch.i * Pitch_int_error;
|
agiembed |
0:d463d5c04541
|
70
|
|
agiembed |
0:d463d5c04541
|
71
|
PIDPitch = (gainPitch.p * Pitch_error);
|
agiembed |
0:d463d5c04541
|
72
|
PIDPitch += Int_Pitch;
|
agiembed |
0:d463d5c04541
|
73
|
PIDPitch += (gainPitch.d * (Pitch_error - Pitch_prev_error));
|
agiembed |
0:d463d5c04541
|
74
|
PIDPitch += (gainPitch.r * fcc.g_pitch);
|
agiembed |
0:d463d5c04541
|
75
|
|
agiembed |
0:d463d5c04541
|
76
|
if (PIDPitch > PID_max) PIDPitch = PID_max;
|
agiembed |
0:d463d5c04541
|
77
|
else if (PIDPitch < -PID_max) PIDPitch = -PID_max;
|
agiembed |
0:d463d5c04541
|
78
|
|
agiembed |
0:d463d5c04541
|
79
|
Pitch_prev_error = Pitch_error;
|
agiembed |
0:d463d5c04541
|
80
|
return;
|
agiembed |
0:d463d5c04541
|
81
|
}
|
agiembed |
0:d463d5c04541
|
82
|
|
agiembed |
0:d463d5c04541
|
83
|
|
agiembed |
0:d463d5c04541
|
84
|
void PID_Yaw() {
|
agiembed |
0:d463d5c04541
|
85
|
|
agiembed |
0:d463d5c04541
|
86
|
gainYaw.p = 1.35; //1.35 Take-off Gain //1.15;
|
agiembed |
0:d463d5c04541
|
87
|
//gainYaw.d = 0;
|
agiembed |
0:d463d5c04541
|
88
|
gainYaw.r = 0.25; //0.35 Take-off Gain
|
agiembed |
0:d463d5c04541
|
89
|
|
agiembed |
0:d463d5c04541
|
90
|
if (RC.yaws>=50 || RC.yaws<=-50) yaw_state1 = 1;
|
agiembed |
0:d463d5c04541
|
91
|
if (yaw_state1 == 1)if (RC.yaws>50 || RC.yaws<-50) yaw_state2 = 1;
|
agiembed |
0:d463d5c04541
|
92
|
if (yaw_state1 == 1 && yaw_state2 == 1) yaw_cnt++;
|
agiembed |
0:d463d5c04541
|
93
|
if (yaw_cnt == 10) {
|
agiembed |
0:d463d5c04541
|
94
|
Yaw_ref = fcc.yaw;
|
agiembed |
0:d463d5c04541
|
95
|
yaw_state1 = yaw_state2 = yaw_cnt = 0;
|
agiembed |
0:d463d5c04541
|
96
|
}
|
agiembed |
0:d463d5c04541
|
97
|
Yaw_error = Yaw_ref - fcc.yaw;
|
agiembed |
0:d463d5c04541
|
98
|
if (Yaw_error>180) Yaw_error -= 360;
|
agiembed |
0:d463d5c04541
|
99
|
if (Yaw_error<-180) Yaw_error += 360;
|
agiembed |
0:d463d5c04541
|
100
|
|
agiembed |
0:d463d5c04541
|
101
|
PIDYaw = (gainYaw.p * Yaw_error);
|
agiembed |
0:d463d5c04541
|
102
|
//PIDYaw += (gainYaw.i * Yaw_error);
|
agiembed |
0:d463d5c04541
|
103
|
//PIDYaw += (gainYaw.d * (Yaw_error - Yaw_prev_error));
|
agiembed |
0:d463d5c04541
|
104
|
PIDYaw -= (gainYaw.r * fcc.g_yaw);
|
agiembed |
0:d463d5c04541
|
105
|
|
agiembed |
0:d463d5c04541
|
106
|
if (PIDYaw > PID_max) PIDYaw = PID_max;
|
agiembed |
0:d463d5c04541
|
107
|
if (PIDYaw < -PID_max) PIDYaw = -PID_max;
|
agiembed |
0:d463d5c04541
|
108
|
|
agiembed |
0:d463d5c04541
|
109
|
Yaw_prev_error = Yaw_error;
|
agiembed |
0:d463d5c04541
|
110
|
return;
|
agiembed |
0:d463d5c04541
|
111
|
}
|
agiembed |
0:d463d5c04541
|
112
|
|
agiembed |
0:d463d5c04541
|
113
|
|
agiembed |
0:d463d5c04541
|
114
|
|
agiembed |
0:d463d5c04541
|
115
|
void pid() {
|
agiembed |
0:d463d5c04541
|
116
|
|
agiembed |
0:d463d5c04541
|
117
|
alti_con = RC.throttles;
|
agiembed |
0:d463d5c04541
|
118
|
|
agiembed |
0:d463d5c04541
|
119
|
|
agiembed |
0:d463d5c04541
|
120
|
PID_Roll();
|
agiembed |
0:d463d5c04541
|
121
|
roll_con = - PIDRoll; //RC.rolls * 0.14 ;//
|
agiembed |
0:d463d5c04541
|
122
|
|
agiembed |
0:d463d5c04541
|
123
|
PID_Pitch();
|
agiembed |
0:d463d5c04541
|
124
|
pitch_con = PIDPitch; //RC.pitchs * 0.14; //
|
agiembed |
0:d463d5c04541
|
125
|
|
agiembed |
0:d463d5c04541
|
126
|
PID_Yaw();
|
agiembed |
0:d463d5c04541
|
127
|
yaw_con = (RC.yaws * 0.3) + PIDYaw;
|
agiembed |
0:d463d5c04541
|
128
|
if (alti_con < 5) yaw_con = 0;
|
agiembed |
0:d463d5c04541
|
129
|
|
agiembed |
0:d463d5c04541
|
130
|
}
|