Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
838 | MikeW | 1 | /* |
2 | Copyright 2008, by Michael Walter |
||
3 | |||
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
||
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
||
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
||
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
||
9 | |||
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
||
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
||
12 | unless it is stated otherwise. |
||
13 | */ |
||
14 | |||
15 | #include "kafi.h" |
||
16 | #include "KalmanFc.h" |
||
17 | #include "main.h" |
||
18 | #include "mm3.h" |
||
19 | |||
20 | #include "main.h" |
||
21 | #include "eeprom.c" |
||
22 | |||
23 | #define MAX_GAS 250 |
||
24 | #define MIN_GAS 15 |
||
25 | |||
26 | #define sin45 sin(45.F / 180.F * PI) |
||
27 | #define cos45 cos(45.F / 180.F * PI) |
||
28 | |||
29 | extern void GPSupdate(void); |
||
30 | |||
31 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0; |
||
32 | int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0; |
||
33 | int AverageRoll_X = 0, AverageRoll_Y = 0, AverageRoll_Z = 0; |
||
34 | int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0; |
||
35 | int Roll_X_Off = 0, Roll_Y_Off = 0, Roll_Z_Off = 0; |
||
36 | |||
37 | f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F; |
||
38 | f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F, ACC_Z_Offset = 0.F; |
||
39 | f32_t Phi, Theta, sinPhi, cosPhi, tanTheta, secTheta; |
||
40 | f32_t sinPhi, cosPhi, sinTheta, cosTheta; |
||
41 | f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P, sinPsi_P, cosPsi_P; |
||
42 | |||
43 | #ifdef USE_Extended_MM3_Measurement_Model |
||
44 | f32_t sinPsi_P, cosPsi_P; |
||
45 | extern f32_t MM3_Hx, MM3_Hy, MM3_Hz; |
||
46 | #endif |
||
47 | |||
48 | int DiffAltitude = 0, DeltaAltitude = 0, CurrentAltitude = 0, LastAltitude = 0, NominalAltitude = 0, InitialAltitude = 0; |
||
49 | int KompassValue = 0, SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0; |
||
50 | int GierMischanteil; |
||
51 | extern int GasMischanteil; |
||
52 | |||
53 | extern char Notlandung; |
||
54 | extern unsigned long maxDistance; |
||
55 | |||
56 | unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
||
57 | unsigned char MotorWert[5]; |
||
58 | |||
59 | extern signed int GPS_Nick, GPS_Roll; |
||
60 | |||
61 | int GasMischanteil; |
||
62 | char MotorenEin = 0; |
||
63 | char Notlandung = 0; |
||
64 | |||
65 | unsigned char SenderOkay = 0; |
||
66 | unsigned int I2CTimeout = 100; |
||
67 | |||
68 | struct mk_param_struct EE_Parameter; |
||
69 | |||
70 | /* **************************************************************************** |
||
71 | Functionname: SendMotorData */ /*! |
||
72 | Description: Senden der Motorwerte per I2C-Bus |
||
73 | |||
74 | @return void |
||
75 | @pre - |
||
76 | @post - |
||
77 | @author H. Buss / I. Busker |
||
78 | **************************************************************************** */ |
||
79 | void SendMotorData(void) |
||
80 | { |
||
81 | if(!MotorenEin) |
||
82 | { |
||
83 | Motor_Hinten = 0; |
||
84 | Motor_Vorne = 0; |
||
85 | Motor_Rechts = 0; |
||
86 | Motor_Links = 0; |
||
87 | if(MotorTest[0]) Motor_Vorne = MotorTest[0]; |
||
88 | if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
||
89 | if(MotorTest[2]) Motor_Links = MotorTest[2]; |
||
90 | if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
||
91 | } |
||
92 | |||
93 | //Start I2C Interrupt Mode |
||
94 | twi_state = 0; |
||
95 | motor = 0; |
||
96 | i2c_start(); |
||
97 | } |
||
98 | |||
99 | |||
100 | /* **************************************************************************** |
||
101 | Functionname: MotorControl */ /*! |
||
102 | Description: |
||
103 | |||
104 | @return void |
||
105 | @pre - |
||
106 | @post - |
||
107 | @author H. Buss / I. Busker |
||
108 | **************************************************************************** */ |
||
109 | void MotorControl(void) |
||
110 | { |
||
840 | MikeW | 111 | static unsigned int NotGasZeit; |
112 | static unsigned char delay_neutral = 0; |
||
113 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
||
114 | static unsigned int modell_fliegt = 0; |
||
115 | |||
838 | MikeW | 116 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
117 | // Gaswert ermitteln |
||
118 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
119 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
||
120 | if(GasMischanteil < 0) |
||
121 | { |
||
122 | GasMischanteil = 0; |
||
123 | } |
||
124 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
125 | // Emfang schlecht |
||
126 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
127 | |||
128 | if(SenderOkay < 100) |
||
129 | { |
||
130 | if(!PcZugriff) |
||
131 | { |
||
840 | MikeW | 132 | if(BeepMuster == 0xffff) |
838 | MikeW | 133 | { |
134 | beeptime = 15000; |
||
135 | BeepMuster = 0x0c00; |
||
136 | } |
||
137 | } |
||
138 | if(NotGasZeit) |
||
139 | { |
||
140 | NotGasZeit--; |
||
141 | } |
||
142 | else |
||
143 | { |
||
144 | MotorenEin = 0; |
||
145 | Notlandung = 0; |
||
146 | } |
||
147 | if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
||
148 | { |
||
149 | GasMischanteil = 100; //EE_Parameter.NotGas; |
||
150 | Notlandung = 1; |
||
151 | PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0; |
||
152 | PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0; |
||
153 | PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0; |
||
154 | } |
||
155 | else |
||
156 | { |
||
157 | MotorenEin = 0; |
||
158 | } |
||
159 | } |
||
160 | else |
||
840 | MikeW | 161 | { |
838 | MikeW | 162 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
840 | MikeW | 163 | // Emfang gut |
838 | MikeW | 164 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
840 | MikeW | 165 | if(SenderOkay > 140) |
166 | { |
||
167 | Notlandung = 0; |
||
168 | NotGasZeit = 200 * 50; //EE_Parameter.NotGasZeit * 50; |
||
169 | if(GasMischanteil > 40) |
||
838 | MikeW | 170 | { |
840 | MikeW | 171 | if(modell_fliegt < 0xffff) |
172 | { |
||
173 | modell_fliegt++; |
||
174 | } |
||
175 | } |
||
176 | |||
177 | if((GasMischanteil > 200) && MotorenEin == 0) |
||
178 | { |
||
179 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
180 | // auf Nullwerte kalibrieren |
||
181 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
182 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
||
183 | { |
||
184 | if(++delay_neutral > 50) // nicht sofort |
||
185 | { |
||
186 | MotorenEin = 0; |
||
187 | delay_neutral = 0; |
||
188 | modell_fliegt = 0; |
||
189 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
||
190 | { |
||
191 | if((AdWertAirPressure_Raw > 950) || (AdWertAirPressure_Raw < 750)) |
||
192 | { |
||
193 | SucheLuftruckOffset(); |
||
194 | } |
||
195 | } |
||
196 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
||
197 | SetNeutral(); |
||
198 | } |
||
199 | } |
||
200 | else delay_neutral = 0; |
||
201 | } |
||
838 | MikeW | 202 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
840 | MikeW | 203 | // Gas ist unten |
838 | MikeW | 204 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
840 | MikeW | 205 | if(GasMischanteil < 35) |
838 | MikeW | 206 | { |
840 | MikeW | 207 | // Starten |
208 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) |
||
209 | { |
||
210 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
211 | // Einschalten |
||
212 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
213 | if(++delay_einschalten > 100) |
||
214 | { |
||
215 | MotorenEin = 1; |
||
216 | modell_fliegt = 1; |
||
217 | delay_einschalten = 100; |
||
218 | } |
||
219 | } |
||
220 | else |
||
221 | { |
||
222 | delay_einschalten = 0; |
||
223 | } |
||
224 | //Auf Neutralwerte setzen |
||
225 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
226 | // Auschalten |
||
227 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
228 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) |
||
229 | { |
||
230 | if(++delay_ausschalten > 50) // nicht sofort |
||
231 | { |
||
232 | MotorenEin = 0; |
||
233 | modell_fliegt = 0; |
||
234 | delay_ausschalten = 50; |
||
235 | } |
||
236 | } |
||
237 | else |
||
238 | { |
||
239 | delay_ausschalten = 0; |
||
240 | } |
||
241 | } |
||
838 | MikeW | 242 | } |
840 | MikeW | 243 | } |
838 | MikeW | 244 | } |
245 | |||
246 | |||
247 | /* **************************************************************************** |
||
248 | Functionname: SetNeutral */ /*! |
||
249 | Description: |
||
250 | |||
251 | @param[in] |
||
252 | |||
253 | @return void |
||
254 | @pre - |
||
255 | @post - |
||
256 | @author Michael Walter |
||
257 | **************************************************************************** */ |
||
258 | void SetNeutral(void) |
||
259 | { |
||
260 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
||
261 | { |
||
262 | if((AdWertAirPressure_Raw > 950) || (AdWertAirPressure_Raw < 750)) |
||
263 | { |
||
264 | SucheLuftruckOffset(); |
||
265 | } |
||
266 | } |
||
267 | |||
268 | LastAltitude = CurrentAltitude; |
||
269 | Delay_ms_Mess(300); |
||
270 | |||
271 | ANALOG_OFF; |
||
272 | AdNeutralNick = AdWertNick_Raw; |
||
273 | AdNeutralRoll = AdWertRoll_Raw; |
||
274 | AdNeutralGier = AdWertGier_Raw; |
||
275 | NeutralAccY = AdWertAccRoll_Raw; |
||
276 | NeutralAccX = AdWertAccNick_Raw; |
||
277 | NeutralAccZ = AdWertAccHoch_Raw; |
||
278 | |||
279 | Roll_X_Offset = 0.F; |
||
280 | Roll_Y_Offset = 0.F; |
||
281 | Roll_Z_Offset = 0.F; |
||
282 | ACC_X_Offset = 0.F; |
||
283 | ACC_Y_Offset = 0.F; |
||
284 | ACC_Z_Offset = 0.F; |
||
285 | |||
286 | AccumulatedACC_X = 0; |
||
287 | AccumulatedACC_X_cnt = 0; |
||
288 | AccumulatedACC_Y = 0; |
||
289 | AccumulatedACC_Y_cnt = 0; |
||
290 | AccumulatedACC_Z = 0; |
||
291 | AccumulatedACC_Z_cnt = 0; |
||
292 | |||
293 | AccumulatedRoll_X = 0; |
||
294 | AccumulatedRoll_X_cnt = 0; |
||
295 | AccumulatedRoll_Y = 0; |
||
296 | AccumulatedRoll_Y_cnt = 0; |
||
297 | AccumulatedRoll_Z = 0; |
||
298 | AccumulatedRoll_Z_cnt = 0; |
||
299 | |||
300 | AveragerACC_X = 0; |
||
301 | AveragerACC_Y = 0; |
||
302 | AveragerACC_Z = 0; |
||
303 | |||
304 | AccumulatedACC_X = 0; |
||
305 | AccumulatedACC_X_cnt = 0; |
||
306 | AccumulatedACC_Y = 0; |
||
307 | AccumulatedACC_Y_cnt = 0; |
||
308 | AccumulatedACC_Z = 0; |
||
309 | AccumulatedACC_Z_cnt = 0; |
||
310 | ANALOG_ON; |
||
311 | |||
312 | SummeRoll = 0; |
||
313 | SummeNick = 0; |
||
314 | |||
315 | sollGier = status.iPsi10; |
||
316 | InitialAltitude = AdWertAirPressure_Raw; |
||
317 | beeptime = 50; |
||
318 | } |
||
319 | |||
320 | |||
321 | /* **************************************************************************** |
||
322 | Functionname: CalculateAverage */ /*! |
||
323 | Description: |
||
324 | |||
325 | @param[in] |
||
326 | |||
327 | @return void |
||
328 | @pre - |
||
329 | @post - |
||
330 | @author Michael Walter |
||
331 | **************************************************************************** */ |
||
332 | void CalculateAverage(void) |
||
333 | { |
||
334 | ANALOG_OFF; |
||
335 | AverageRoll_X = AccumulatedRoll_X / AccumulatedRoll_X_cnt; |
||
336 | AverageRoll_Y = AccumulatedRoll_Y / AccumulatedRoll_Y_cnt; |
||
337 | AverageRoll_Z = AccumulatedRoll_Z / AccumulatedRoll_Z_cnt; |
||
338 | |||
339 | /* Get Pressure Differenz */ |
||
340 | CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt; |
||
341 | AccumulatedAirPressure_cnt = 0; |
||
342 | AccumulatedAirPressure = 0; |
||
343 | |||
344 | static char AirPressureCnt = 0; |
||
345 | if (AirPressureCnt % 25 == 1) |
||
346 | { |
||
347 | DeltaAltitude = CurrentAltitude - LastAltitude; |
||
348 | LastAltitude = CurrentAltitude; |
||
349 | } |
||
350 | AirPressureCnt++; |
||
351 | |||
352 | if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
||
353 | { |
||
354 | Roll_X_Offset = 0.9995F * Roll_X_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_X))); |
||
355 | Roll_Y_Offset = 0.9995F * Roll_Y_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_Y))); |
||
356 | |||
357 | if (abs(StickGier) < 15 || MotorenEin == 0) |
||
358 | { |
||
359 | Roll_Z_Offset = 0.9998F * Roll_Z_Offset + 0.0002F * (float) ( MAX(-60, MIN(60,AverageRoll_Z))); |
||
360 | } |
||
361 | } |
||
362 | |||
363 | #if 0 |
||
364 | DebugOut.Analog[3] = AdWertGier_Raw; |
||
365 | DebugOut.Analog[4] = AdWertRoll_Raw; |
||
366 | DebugOut.Analog[5] = AdWertNick_Raw; |
||
367 | #endif |
||
368 | |||
369 | AverageRoll_X -= Roll_X_Offset; |
||
370 | AverageRoll_Y -= Roll_Y_Offset; |
||
371 | AverageRoll_Z -= Roll_Z_Offset; |
||
372 | |||
373 | AccumulatedRoll_X = 0; |
||
374 | AccumulatedRoll_X_cnt = 0; |
||
375 | AccumulatedRoll_Y = 0; |
||
376 | AccumulatedRoll_Y_cnt = 0; |
||
377 | AccumulatedRoll_Z = 0; |
||
378 | AccumulatedRoll_Z_cnt = 0; |
||
379 | |||
380 | #if 0 |
||
381 | ACC_X_Offset = 0.9999F * ACC_X_Offset + 0.0001F * ((float) AccumulatedACC_X / (float) AccumulatedACC_X_cnt); |
||
382 | ACC_Y_Offset = 0.9999F * ACC_Y_Offset + 0.0001F * ((float) AccumulatedACC_Y / (float) AccumulatedACC_Y_cnt); |
||
383 | ACC_Z_Offset = 0.9999F * ACC_Z_Offset + 0.0001F * ((float) AccumulatedACC_Z / (float) AccumulatedACC_Z_cnt); |
||
384 | #endif |
||
385 | |||
386 | AveragerACC_X = AccumulatedACC_X / AccumulatedACC_X_cnt; |
||
387 | AveragerACC_Y = AccumulatedACC_Y / AccumulatedACC_Y_cnt; |
||
388 | AveragerACC_Z = AccumulatedACC_Z / AccumulatedACC_Z_cnt; |
||
389 | |||
390 | AccumulatedACC_X = 0; |
||
391 | AccumulatedACC_X_cnt = 0; |
||
392 | AccumulatedACC_Y = 0; |
||
393 | AccumulatedACC_Y_cnt = 0; |
||
394 | AccumulatedACC_Z = 0; |
||
395 | AccumulatedACC_Z_cnt = 0; |
||
396 | |||
397 | ANALOG_ON; |
||
398 | |||
399 | if (Roll_X_Off > 60) |
||
400 | { |
||
401 | Roll_X_Off = 60; |
||
402 | } |
||
403 | if (Roll_X_Off < -60) |
||
404 | { |
||
405 | Roll_X_Off = -60; |
||
406 | } |
||
407 | |||
408 | if (Roll_Y_Off > 60) |
||
409 | { |
||
410 | Roll_Y_Off = 60; |
||
411 | } |
||
412 | if (Roll_Y_Off < -60) |
||
413 | { |
||
414 | Roll_Y_Off = -60; |
||
415 | } |
||
416 | |||
417 | if (Roll_Z_Off > 60) |
||
418 | { |
||
419 | Roll_Z_Off = 60; |
||
420 | } |
||
421 | if (Roll_Z_Off < -60) |
||
422 | { |
||
423 | Roll_Z_Off = -60; |
||
424 | } |
||
425 | } |
||
426 | |||
427 | |||
428 | /* **************************************************************************** |
||
429 | Functionname: PID_Regler */ /*! |
||
430 | Description: |
||
431 | |||
432 | @return void |
||
433 | @pre - |
||
434 | @post - |
||
435 | @author Michael Walter |
||
436 | **************************************************************************** */ |
||
437 | void PID_Regler(void) |
||
438 | { |
||
439 | int StickNick45,StickRoll45; |
||
440 | int DiffNick,DiffRoll, DiffGier; |
||
441 | int motorwert = 0; |
||
442 | int pd_ergebnis; |
||
443 | |||
444 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
445 | // neue Werte von der Funke |
||
446 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
447 | if(!NewPpmData--) |
||
448 | { |
||
449 | StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
||
450 | StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
||
451 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
||
452 | } // Ende neue Funken-Werte |
||
453 | |||
454 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
455 | // Bei Empfangsausfall im Flug |
||
456 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
457 | if(Notlandung) |
||
458 | { |
||
459 | StickGier = 0; |
||
460 | StickNick = 0; |
||
461 | StickRoll = 0; |
||
462 | } |
||
463 | |||
464 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
465 | // + Mischer und PID-Regler |
||
466 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
467 | |||
468 | /* G P S U p d a t e */ |
||
469 | static char CntGPS = 0; |
||
470 | if (CntGPS % 10 == 1) |
||
471 | { |
||
472 | //GPSupdate(); |
||
473 | } |
||
474 | CntGPS++; |
||
475 | |||
476 | StickRoll45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) - 0.707F * (float)(StickNick - GPS_Nick)); |
||
477 | StickNick45 = (int) (0.707F * (float)(StickRoll - GPS_Roll) + 0.707F * (float)(StickNick - GPS_Nick)); |
||
478 | |||
479 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
480 | // Gieren |
||
481 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
482 | |||
483 | if(GasMischanteil < 20) |
||
484 | { |
||
485 | sollGier = status.iPsi10; |
||
486 | SummeRoll = 0; |
||
487 | SummeNick = 0; |
||
488 | } |
||
489 | |||
490 | /* Gier-Anteil */ |
||
491 | if (abs(StickGier) > 4) |
||
492 | { |
||
493 | sollGier = status.iPsi10 + 4 * StickGier; |
||
494 | } |
||
495 | DiffGier = (sollGier - status.iPsi10); |
||
496 | GierMischanteil = (int) (-4 * DiffGier - 4* (AdWertGier - AdNeutralGier - Roll_Z_Off)) / 10; |
||
497 | |||
498 | #define MUL_G 0.8 |
||
499 | if(GierMischanteil > MUL_G * GasMischanteil) |
||
500 | { |
||
501 | GierMischanteil = MUL_G * GasMischanteil; |
||
502 | } |
||
503 | if(GierMischanteil < -MUL_G * GasMischanteil) |
||
504 | { |
||
505 | GierMischanteil = -MUL_G * GasMischanteil; |
||
506 | } |
||
507 | if(GierMischanteil > 50) |
||
508 | { |
||
509 | GierMischanteil = 50; |
||
510 | } |
||
511 | if(GierMischanteil < -50) |
||
512 | { |
||
513 | GierMischanteil = -50; |
||
514 | } |
||
515 | |||
516 | int scale_p = 7; |
||
517 | int scale_d = 10; |
||
518 | |||
519 | /* N i c k - A c h s e */ |
||
520 | DiffNick = -(status.iTheta10 + StickNick45); |
||
521 | /* R o l l - A c h s e */ |
||
522 | DiffRoll = -(status.iPhi10 + StickRoll45); |
||
523 | |||
524 | SummeNick += DiffNick; |
||
525 | if(SummeNick > 10000) |
||
526 | { |
||
527 | SummeNick = 10000; |
||
528 | } |
||
529 | if(SummeNick < -10000) |
||
530 | { |
||
531 | SummeNick = -10000; |
||
532 | } |
||
533 | |||
534 | SummeRoll += DiffRoll; |
||
535 | if(SummeRoll > 10000) |
||
536 | { |
||
537 | SummeRoll = 10000; |
||
538 | } |
||
539 | if(SummeRoll < -10000) |
||
540 | { |
||
541 | SummeRoll = -10000; |
||
542 | } |
||
543 | |||
544 | pd_ergebnis = ((scale_p *DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10) ; // + (int)(SummeNick / 2000); |
||
545 | if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
||
546 | { |
||
547 | pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
||
548 | } |
||
549 | if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil))) |
||
550 | { |
||
551 | pd_ergebnis = -(GasMischanteil + abs(GierMischanteil)); |
||
552 | } |
||
553 | |||
554 | /* M o t o r V o r n */ |
||
555 | motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; |
||
556 | if ((motorwert < 0)) |
||
557 | { |
||
558 | motorwert = 0; |
||
559 | } |
||
560 | else if(motorwert > MAX_GAS) |
||
561 | { |
||
562 | motorwert = MAX_GAS; |
||
563 | } |
||
564 | if (motorwert < MIN_GAS) |
||
565 | { |
||
566 | motorwert = MIN_GAS; |
||
567 | } |
||
568 | Motor_Vorne = motorwert; |
||
569 | |||
570 | /* M o t o r H e c k */ |
||
571 | motorwert = GasMischanteil - pd_ergebnis + GierMischanteil; |
||
572 | if ((motorwert < 0)) |
||
573 | { |
||
574 | motorwert = 0; |
||
575 | } |
||
576 | else if(motorwert > MAX_GAS) |
||
577 | { |
||
578 | motorwert = MAX_GAS; |
||
579 | } |
||
580 | if (motorwert < MIN_GAS) |
||
581 | { |
||
582 | motorwert = MIN_GAS; |
||
583 | } |
||
584 | Motor_Hinten = motorwert; |
||
585 | |||
586 | pd_ergebnis = ((scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10) ; //+ (int)(SummeRoll / 2000); |
||
587 | if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
||
588 | { |
||
589 | pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
||
590 | } |
||
591 | if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil))) |
||
592 | { |
||
593 | pd_ergebnis = -(GasMischanteil + abs(GierMischanteil)); |
||
594 | } |
||
595 | |||
596 | /* M o t o r L i n k s */ |
||
597 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
||
598 | if(motorwert > MAX_GAS) |
||
599 | { |
||
600 | motorwert = MAX_GAS; |
||
601 | } |
||
602 | if (motorwert < MIN_GAS) |
||
603 | { |
||
604 | motorwert = MIN_GAS; |
||
605 | } |
||
606 | Motor_Links = motorwert; |
||
607 | |||
608 | /* M o t o r R e c h t s */ |
||
609 | motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; |
||
610 | if(motorwert > MAX_GAS) |
||
611 | { |
||
612 | motorwert = MAX_GAS; |
||
613 | } |
||
614 | if (motorwert < MIN_GAS) |
||
615 | { |
||
616 | motorwert = MIN_GAS; |
||
617 | } |
||
618 | Motor_Rechts = motorwert; |
||
619 | |||
620 | #if 1 |
||
621 | DebugOut.Analog[0] = status.iTheta10; |
||
622 | DebugOut.Analog[1] = status.iPhi10; |
||
623 | DebugOut.Analog[2] = status.iPsi10 / 10; |
||
624 | //DebugOut.Analog[3] = KompassValue; |
||
625 | //DebugOut.Analog[4] = (sollGier - status.iPsi10 ) / 10; |
||
626 | //DebugOut.Analog[5] = AverageRoll_Z; |
||
627 | //DebugOut.Analog[6] = AverageRoll_X; |
||
628 | |||
629 | //DebugOut.Analog[9] = CurrentAltitude; |
||
630 | //DebugOut.Analog[10] = DiffAltitude; |
||
631 | //DebugOut.Analog[8] = AverageRoll_X; |
||
632 | //DebugOut.Analog[9] = AverageRoll_Y; |
||
633 | //DebugOut.Analog[10] = AverageRoll_Z; |
||
634 | |||
635 | //DebugOut.Analog[11] = GasMischanteil; |
||
636 | //DebugOut.Analog[13] = AverageRoll_X; |
||
637 | //DebugOut.Analog[14] = AverageRoll_Y; |
||
638 | //DebugOut.Analog[15] = AdWertAirPressure_Raw; |
||
639 | #endif |
||
640 | } |
||
641 | |||
642 | |||
643 | /***************************************************************************** |
||
644 | VARIABLES |
||
645 | *****************************************************************************/ |
||
646 | |||
647 | /* Kalman filter */ |
||
648 | kafi_t *p_kafi; |
||
649 | mat_Matrix_t *matP0; |
||
650 | mat_Matrix_t *matQ; |
||
651 | mat_Matrix_t *matR; |
||
652 | mat_Matrix_t *matXd; /* estimated state */ |
||
653 | mat_Matrix_t *matXs; /* predicted state */ |
||
654 | mat_Matrix_t *matPhi; /* transition matrix to predict new state from current state */ |
||
655 | mat_Matrix_t *matB; /* control matrix to predict new state from ext. control vector U */ |
||
656 | mat_Matrix_t *matU; /* control vector */ |
||
657 | mat_Matrix_t *matC; /* Jacobi matrix */ |
||
658 | mat_Matrix_t *matY; /* real measurements */ |
||
659 | mat_Matrix_t *matdY; /* innovation */ |
||
660 | mat_Matrix_t *matYs; /* predicted measurements */ |
||
661 | mat_Matrix_t *matP; /* error covariance matrix */ |
||
662 | mat_Matrix_t *matPDiag; /* diagonal covariance matrix */ |
||
663 | char *fYValid; /* measurement validity flag */ |
||
664 | |||
665 | |||
666 | void Kafi_Init() |
||
667 | { |
||
668 | /* create kalman filter and associated matrizes */ |
||
669 | p_kafi = KAFICreate(); |
||
670 | |||
671 | KAFIInit(p_kafi); |
||
672 | |||
673 | /* get pointers to all matrices */ |
||
674 | KAFIGetP0(p_kafi, &matP0); |
||
675 | KAFIGetQ(p_kafi, &matQ); |
||
676 | KAFIGetR(p_kafi, &matR); |
||
677 | KAFIGetXd(p_kafi, &matXd); |
||
678 | KAFIGetXs(p_kafi, &matXs); |
||
679 | KAFIGetPhi(p_kafi, &matPhi); |
||
680 | KAFIGetB(p_kafi, &matB); |
||
681 | KAFIGetU(p_kafi, &matU); |
||
682 | KAFIGetC(p_kafi, &matC); |
||
683 | KAFIGetY(p_kafi, &matY); |
||
684 | KAFIGetYs(p_kafi, &matYs); |
||
685 | KAFIGetPDiag(p_kafi, &matPDiag); |
||
686 | KAFIGetYValid(p_kafi, &fYValid); |
||
687 | KAFIGetdY(p_kafi,&matdY); |
||
688 | |||
689 | trResetKalmanFilter(); |
||
690 | } |
||
691 | |||
692 | /* **************************************************************************** |
||
693 | Functionname: trResetKalmanFilter */ /*! |
||
694 | Description: |
||
695 | |||
696 | @return void |
||
697 | @pre - |
||
698 | @post - |
||
699 | @author Michael Walter |
||
700 | **************************************************************************** */ |
||
701 | |||
702 | void trResetKalmanFilter() |
||
703 | { |
||
704 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
705 | |||
706 | /*--- VARIABLES ---*/ |
||
707 | ui32_t i, j; |
||
708 | |||
709 | /* set phi to identity matrix */ |
||
710 | for ( j = 0; j < p_kafi->uiDimX; j++ ) |
||
711 | { |
||
712 | for ( i = 0; i < p_kafi->uiDimX; i++ ) |
||
713 | { |
||
714 | matSetFull(matPhi, j, i, (j == i) ? 1.F : 0.F); |
||
715 | } |
||
716 | } |
||
717 | |||
718 | /* set diagonal of measurement noise covariance R */ |
||
719 | matSetDiagonal(matR, _ax, _ax, 10.0F); |
||
720 | matSetDiagonal(matR, _ay, _ay, 10.0F); |
||
721 | matSetDiagonal(matR, _az, _az, 10.0F); |
||
722 | #ifdef USE_Extended_MM3_Measurement_Model |
||
723 | matSetDiagonal(matR, _mx, _mx, 100.0F); |
||
724 | matSetDiagonal(matR, _my, _my, 100.0F); |
||
725 | matSetDiagonal(matR, _mz, _mz, 100.0F); |
||
726 | #else |
||
727 | matSetDiagonal(matR, _compass, _compass, 5.0F); |
||
728 | #endif |
||
729 | |||
730 | /* set diagonal of system noise covariance Q */ |
||
731 | matSetDiagonal(matQ, _Phi, _Phi, 0.00001F); |
||
732 | matSetDiagonal(matQ, _Theta, _Theta, 0.00001F); |
||
733 | matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
||
734 | |||
735 | /* set diagonal init. estimation error (covariance) P0 */ |
||
736 | matSetDiagonal(matP0, _Phi, _Phi_, 0.00001F); |
||
737 | matSetDiagonal(matP0, _Theta, _Theta, 0.00001F); |
||
738 | matSetDiagonal(matP0, _Psi, _Psi, 0.00001F); |
||
739 | |||
740 | matSetFull(matXd, _Phi, 0, 0.0F); |
||
741 | matSetFull(matXd, _Theta, 0, 0.0F); |
||
742 | matSetFull(matXd, _Psi, 0, 0.0F); |
||
743 | |||
744 | matSetFull(matXs, _Phi, 0, 0.0F); |
||
745 | matSetFull(matXs, _Theta, 0, 0.0F); |
||
746 | matSetFull(matXs, _Psi, 0, 0.0F); |
||
747 | memset( &status, 0, sizeof(status) ); |
||
748 | |||
749 | KAFISetP0(p_kafi); |
||
750 | } |
||
751 | |||
752 | |||
753 | |||
754 | |||
755 | /* **************************************************************************** |
||
756 | Functionname: trInnovate */ /*! |
||
757 | Description: Update the current System State based on the current Observation |
||
758 | |||
759 | |||
760 | @param[in] |
||
761 | |||
762 | @return void |
||
763 | @pre - |
||
764 | @post - |
||
765 | @author Michael Walter |
||
766 | **************************************************************************** */ |
||
767 | void trInnovate() |
||
768 | { |
||
769 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
770 | |||
771 | /*--- VARIABLES ---*/ |
||
772 | |||
773 | /*estimate the new system state (Xd), nominal case */ |
||
774 | KAFIInnovation(p_kafi); |
||
775 | KAFIUpdatePDiag(p_kafi); |
||
776 | KAFIUpdateP(p_kafi); |
||
777 | return; |
||
778 | } |
||
779 | |||
780 | |||
781 | |||
782 | /* **************************************************************************** |
||
783 | Functionname: trUpdatePhi */ /*! |
||
784 | Description: Setup matPhi, used to calculate the predicted state |
||
785 | from the current state |
||
786 | |||
787 | |||
788 | @param[in] |
||
789 | @param[in] fCycleTime |
||
790 | |||
791 | @return void |
||
792 | @pre - |
||
793 | @post - |
||
794 | @author Michael Walter |
||
795 | **************************************************************************** */ |
||
796 | void trUpdatePhi() |
||
797 | { |
||
798 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
799 | |||
800 | /*--- VARIABLES ---*/ |
||
801 | matSetFull(matPhi, _Phi, _Phi, 1.0F); |
||
802 | matSetFull(matPhi, _Theta, _Theta, 1.0F); |
||
803 | matSetFull(matPhi, _Psi, _Psi, 1.0F); |
||
804 | } |
||
805 | |||
806 | |||
807 | |||
808 | #ifdef USE_Extended_MM3_Measurement_Model |
||
809 | /* **************************************************************************** |
||
810 | Functionname: trUpdateJacobiMatrix */ /*! |
||
811 | Description: |
||
812 | |||
813 | |||
814 | @param[in] void |
||
815 | |||
816 | @return void |
||
817 | @pre - |
||
818 | @post - |
||
819 | @author Michael Walter |
||
820 | *****************************************************************************/ |
||
821 | void trUpdateJacobiMatrix(void) |
||
822 | { |
||
823 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
824 | /* |
||
825 | Simplified Version |
||
826 | Pre_ax = du - sinTheta * g; |
||
827 | Pre_ay = dv + cosTheta_sinPhi * g; |
||
828 | Pre_az = dw + cosTheta_cosPhi * g; |
||
829 | */ |
||
830 | |||
831 | /*--- VARIABLES ---*/ |
||
832 | f32_t Phi, Theta, Psi; |
||
833 | f32_t sinPsi, cosPsi; |
||
834 | |||
835 | f32_t g = 9.81F; |
||
836 | f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */ |
||
837 | f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */ |
||
838 | |||
839 | Phi = status.Phi; |
||
840 | Theta = status.Theta; |
||
841 | Psi = status.Psi; |
||
842 | |||
843 | sinPhi = sin(Phi); |
||
844 | cosPhi = cos(Phi); |
||
845 | sinTheta = sin(Theta); |
||
846 | cosTheta = cos(Theta); |
||
847 | sinPsi = sin(Psi); |
||
848 | cosPsi = cos(Psi); |
||
849 | |||
850 | //Pre_ax = du - sinTheta * g; |
||
851 | //matSetFull(matC, _ax, _Phi , 0); |
||
852 | matSetFull(matC, _ax, _Theta, -cosTheta * g); |
||
853 | //matSetFull(matC, _ax, _Psi , 0); |
||
854 | |||
855 | //Pre_ay = dv + cosTheta_sinPhi * g; |
||
856 | matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
||
857 | matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
||
858 | //matSetFull(matC, _ay, _Psi , 0); |
||
859 | |||
860 | //Pre_az = dw + cosTheta_cosPhi * g; |
||
861 | matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
||
862 | matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
||
863 | //matSetFull(matC, _az, _Psi , 0); |
||
864 | |||
865 | //Pre_mx = (cos45 * (cosTheta * cosPsi) + sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx + (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz; |
||
866 | //matSetFull(matC, _mx, _Phi , (sin45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi) )* Pre_Hx + ( sin45 * cosPhi * cosTheta) * Pre_Hz ); |
||
867 | //matSetFull(matC, _mx, _Theta, (cos45 * (-sinTheta * cosPsi) + sin45 * (sinPhi * cosTheta * cosPsi) )* Pre_Hx + (-cos45 * cosTheta - sin45 * sinPhi * sinTheta) * Pre_Hz); |
||
868 | //matSetFull(matC, _mx, _Psi , (-cos45 * (cosTheta * sinPsi) + sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi) )* Pre_Hx ); |
||
869 | |||
870 | //Pre_my = (-sin45 * (cosTheta * cosPsi) + cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx + (sin45 * sinTheta + cos45 * sinPhi * cosTheta) * Pre_Hz; |
||
871 | //matSetFull(matC, _my, _Phi , (cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi)) * Pre_Hx + ( cos45 * cosPhi * cosTheta) * Pre_Hz ); |
||
872 | //matSetFull(matC, _my, _Theta, (sin45 * (sinTheta * cosPsi) + cos45 * (sinPhi * cosTheta * cosPsi)) * Pre_Hx + (sin45 * cosTheta - cos45 * sinPhi * sinTheta) * Pre_Hz ); |
||
873 | //matSetFull(matC, _my, _Psi , (sin45 * (cosTheta * sinPsi) + cos45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi)) * Pre_Hx ); |
||
874 | |||
875 | //Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz; |
||
876 | //matSetFull(matC, _mz, _Phi , (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi) * Pre_Hx - sinPhi * cosTheta * Pre_Hz); |
||
877 | //matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi) * Pre_Hx - cosPhi * sinTheta * Pre_Hz ); |
||
878 | //matSetFull(matC, _mz, _Psi , (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi) * Pre_Hx ); |
||
879 | |||
880 | /* Runtime Optimised Version */ |
||
881 | f32_t cos45_sinTheta_cosPsi = cos45 * (sinTheta * cosPsi) ; |
||
882 | f32_t sin45_sinPhi_sinTheta = sin45 * sinPhi * sinTheta; |
||
883 | f32_t sin45_cosPhi_cosTheta = sin45 * cosPhi * cosTheta; |
||
884 | f32_t cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi = cos45 * (sinPhi * sinPsi + cosPhi * sinTheta * cosPsi); |
||
885 | f32_t cos45_cosTheta_sinPsi = cos45 * (cosTheta * sinPsi) ; |
||
886 | f32_t sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi = sin45 * (-cosPhi * cosPsi - sinPhi * sinTheta * sinPsi); |
||
887 | f32_t sin45_sinPhi_cosTheta_cosPsi = sin45 * (sinPhi * cosTheta * cosPsi); |
||
888 | f32_t tmp1= (cos45_sinPhi_sinPsi_cosPhi_sinTheta_cosPsi)* Pre_Hx + (sin45_cosPhi_cosTheta) * Pre_Hz; |
||
889 | |||
890 | //Pre_mx = (cos45 * (cosTheta * cosPsi) + sin45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi) )* Pre_Hx + (-cos45 * sinTheta + sin45 * sinPhi * cosTheta) * Pre_Hz; |
||
891 | matSetFull(matC, _mx, _Phi , tmp1); |
||
892 | matSetFull(matC, _mx, _Theta, (-cos45_sinTheta_cosPsi + sin45_sinPhi_cosTheta_cosPsi)* Pre_Hx + (-cos45 * cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz); |
||
893 | matSetFull(matC, _mx, _Psi , (-cos45_cosTheta_sinPsi+ sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi)* Pre_Hx ); |
||
894 | |||
895 | //Pre_my = (-sin45 * (cosTheta * cosPsi) + cos45 * (-cosPhi * sinPsi + sinPhi * sinTheta * cosPsi)) * Pre_Hx + (sin45 * sinTheta + cos45 * sinPhi * cosTheta) * Pre_Hz; |
||
896 | matSetFull(matC, _my, _Phi , tmp1); |
||
897 | matSetFull(matC, _my, _Theta, (cos45_sinTheta_cosPsi + sin45_sinPhi_cosTheta_cosPsi) * Pre_Hx + (sin45 * cosTheta - sin45_sinPhi_sinTheta) * Pre_Hz ); |
||
898 | matSetFull(matC, _my, _Psi , (cos45_cosTheta_sinPsi + sin45_cosPhi_cosPsi_sinPhi_sinTheta_sinPsi) * Pre_Hx ); |
||
899 | |||
900 | //Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz; |
||
901 | matSetFull(matC, _mz, _Phi , (cosPhi * sinPsi - sinPhi * sinTheta * cosPsi) * Pre_Hx - sinPhi * cosTheta * Pre_Hz); |
||
902 | matSetFull(matC, _mz, _Theta, (cosPhi * cosTheta * cosPsi) * Pre_Hx - cosPhi * sinTheta * Pre_Hz ); |
||
903 | matSetFull(matC, _mz, _Psi , (sinPhi * cosPsi - cosPhi * sinTheta * sinPsi) * Pre_Hx ); |
||
904 | } |
||
905 | #else |
||
906 | /* **************************************************************************** |
||
907 | Functionname: trUpdateJacobiMatrix */ /*! |
||
908 | Description: |
||
909 | |||
910 | |||
911 | @param[in] void |
||
912 | |||
913 | @return void |
||
914 | @pre - |
||
915 | @post - |
||
916 | @author Michael Walter |
||
917 | *****************************************************************************/ |
||
918 | void trUpdateJacobiMatrix(void) |
||
919 | { |
||
920 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
921 | /* |
||
922 | Simplified Version |
||
923 | Pre_ax = du - sinTheta * g; |
||
924 | Pre_ay = dv + cosTheta_sinPhi * g; |
||
925 | Pre_az = dw + cosTheta_cosPhi * g; |
||
926 | */ |
||
927 | |||
928 | /*--- VARIABLES ---*/ |
||
929 | f32_t Phi, Theta, Psi; |
||
930 | f32_t g = 9.81F; |
||
931 | |||
932 | Phi = status.Phi; |
||
933 | Theta = status.Theta; |
||
934 | Psi = status.Psi; |
||
935 | |||
936 | sinPhi = sin(Phi); |
||
937 | cosPhi = cos(Phi); |
||
938 | sinTheta = sin(Theta); |
||
939 | cosTheta = cos(Theta); |
||
940 | |||
941 | //Pre_ax = du - sinTheta * g; |
||
942 | //matSetFull(matC, _ax, _Phi , 0); |
||
943 | matSetFull(matC, _ax, _Theta, -cosTheta * g); |
||
944 | //matSetFull(matC, _ax, _Psi , 0); |
||
945 | |||
946 | //Pre_ay = dv + cosTheta_sinPhi * g; |
||
947 | matSetFull(matC, _ay, _Phi, cosTheta * cosPhi * g); |
||
948 | matSetFull(matC, _ay, _Theta , -sinTheta * sinPhi * g); |
||
949 | //matSetFull(matC, _ay, _Psi , 0); |
||
950 | |||
951 | //Pre_az = dw + cosTheta_cosPhi * g; |
||
952 | matSetFull(matC, _az, _Phi , -cosTheta * sinPhi * g); |
||
953 | matSetFull(matC, _az, _Theta , -sinTheta * cosPhi * g); |
||
954 | //matSetFull(matC, _az, _Psi , 0); |
||
955 | |||
956 | matSetFull(matC, _compass, _Psi , 1.0F); |
||
957 | } |
||
958 | #endif |
||
959 | |||
960 | |||
961 | #ifdef USE_Extended_MM3_Measurement_Model |
||
962 | /***************************************************************************** |
||
963 | Functionname: trPredictMeasurement */ /*! |
||
964 | Description: Predict Measurements: AX, AY, AZ, HX, HY, HZ |
||
965 | |||
966 | |||
967 | @param[in] void |
||
968 | |||
969 | @return void |
||
970 | @pre - |
||
971 | @post - |
||
972 | @author Michael Walter |
||
973 | *****************************************************************************/ |
||
974 | void trPredictMeasurement(void) |
||
975 | { |
||
976 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
977 | |||
978 | /* |
||
979 | Simplified Version |
||
980 | Pre_ax = du - sinTheta * g; |
||
981 | Pre_ay = dv + cosTheta_sinPhi * g; |
||
982 | Pre_az = dw + cosTheta_cosPhi * g; |
||
983 | */ |
||
984 | |||
985 | /*--- VARIABLES ---*/ |
||
986 | f32_t g = 9.81F; |
||
987 | f32_t Pre_ax, Pre_ay, Pre_az; |
||
988 | f32_t Phi, Theta, Psi; |
||
989 | |||
990 | f32_t Pre_mx; |
||
991 | f32_t Pre_my; |
||
992 | f32_t Pre_mz; |
||
993 | |||
994 | f32_t Pre_Hx = 4.78F; /* horizontal field component at the current location */ |
||
995 | f32_t Pre_Hz = 8.96F; /* vertical field component at the current location */ |
||
996 | |||
997 | f32_t cosTheta_cosPsi, sinPhi_cosTheta, cosPhi_sinPsi, sinPhi_sinTheta_cosPsi, cos45_cosTheta_cosPsi; |
||
998 | f32_t cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi, cos45_sinPhi_cosTheta, cos45_sinTheta; |
||
999 | |||
1000 | matGetFull(matXs, _Phi, 0, &Phi); |
||
1001 | matGetFull(matXs, _Theta, 0, &Theta); |
||
1002 | matGetFull(matXs, _Psi, 0, &Psi); |
||
1003 | |||
1004 | sinPhi_P = sin(Phi); |
||
1005 | cosPhi_P = cos(Phi); |
||
1006 | sinTheta_P = sin(Theta); |
||
1007 | cosTheta_P = cos(Theta); |
||
1008 | sinPsi_P = sin(Psi); |
||
1009 | cosPsi_P = cos(Psi); |
||
1010 | |||
1011 | cosTheta_cosPsi = cosTheta_P * cosPsi_P; |
||
1012 | sinPhi_cosTheta = sinPhi_P * cosTheta_P; |
||
1013 | cosPhi_sinPsi = cosPhi_P * sinPsi_P; |
||
1014 | sinPhi_sinTheta_cosPsi = sinPhi_P * sinTheta_P * cosPsi_P; |
||
1015 | |||
1016 | cos45_cosTheta_cosPsi = cos45 * (cosTheta_cosPsi); |
||
1017 | cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi = cos45 * (-cosPhi_sinPsi + sinPhi_sinTheta_cosPsi); |
||
1018 | cos45_sinPhi_cosTheta = cos45 * sinPhi_cosTheta; |
||
1019 | cos45_sinTheta = cos45 * sinTheta_P; |
||
1020 | |||
1021 | /* Simplified Version */ |
||
1022 | Pre_ax = -sinTheta_P * g; |
||
1023 | Pre_ay = cosTheta_P * sinPhi_P * g; |
||
1024 | Pre_az = cosTheta_P * cosPhi_P * g; |
||
1025 | |||
1026 | matSetFull(matYs, _ax, 0, Pre_ax); |
||
1027 | matSetFull(matYs, _ay, 0, Pre_ay); |
||
1028 | matSetFull(matYs, _az, 0, Pre_az); |
||
1029 | |||
1030 | //Pre_mx = (cos45 * (cosTheta_P * cosPsi_P) + sin45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P) )* Pre_Hx + (-cos45 * sinTheta_P + sin45 * sinPhi_P * cosTheta_P) * Pre_Hz; |
||
1031 | //Pre_my = (-sin45 * (cosTheta_P * cosPsi_P) + cos45 * (-cosPhi_P * sinPsi_P + sinPhi_P * sinTheta_P * cosPsi_P)) * Pre_Hx + (sin45 * sinTheta_P + cos45 * sinPhi_P * cosTheta_P) * Pre_Hz; |
||
1032 | //Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz; |
||
1033 | |||
1034 | Pre_mx = (cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi)* Pre_Hx + (-cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz; |
||
1035 | Pre_my = (-cos45_cosTheta_cosPsi + cos45_cosPhi_sinPsi_sinPhi_sinTheta_cosPsi) * Pre_Hx + (cos45_sinTheta + cos45_sinPhi_cosTheta) * Pre_Hz; |
||
1036 | Pre_mz = (sinPhi_P * sinPsi_P + cosPhi_P * sinTheta_P * cosPsi_P) * Pre_Hx + cosPhi_P * cosTheta_P * Pre_Hz; |
||
1037 | |||
1038 | //DebugOut.Analog[3] = Pre_mx * 100; |
||
1039 | //DebugOut.Analog[4] = Pre_my * 100; |
||
1040 | //DebugOut.Analog[5] = Pre_mz * 100; |
||
1041 | |||
1042 | matSetFull(matYs, _mx, 0, Pre_mx); |
||
1043 | matSetFull(matYs, _my, 0, Pre_my); |
||
1044 | matSetFull(matYs, _mz, 0, Pre_mz); |
||
1045 | } |
||
1046 | |||
1047 | #else |
||
1048 | /***************************************************************************** |
||
1049 | Functionname: trPredictMeasurement */ /*! |
||
1050 | Description: Predict Measurements: AX, AY, AZ, Compass |
||
1051 | |||
1052 | |||
1053 | @param[in] void |
||
1054 | |||
1055 | @return void |
||
1056 | @pre - |
||
1057 | @post - |
||
1058 | @author Michael Walter |
||
1059 | *****************************************************************************/ |
||
1060 | void trPredictMeasurement(void) |
||
1061 | { |
||
1062 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
1063 | |||
1064 | /* |
||
1065 | Simplified Version |
||
1066 | Pre_ax = du - sinTheta * g; |
||
1067 | Pre_ay = dv + cosTheta_sinPhi * g; |
||
1068 | Pre_az = dw + cosTheta_cosPhi * g; |
||
1069 | */ |
||
1070 | |||
1071 | /*--- VARIABLES ---*/ |
||
1072 | f32_t g = 9.81F; |
||
1073 | f32_t Pre_ax, Pre_ay, Pre_az; |
||
1074 | f32_t Phi, Theta, Psi; |
||
1075 | |||
1076 | matGetFull(matXs, _Phi, 0, &Phi); |
||
1077 | matGetFull(matXs, _Theta, 0, &Theta); |
||
1078 | matGetFull(matXs, _Psi, 0, &Psi); |
||
1079 | |||
1080 | sinPhi_P = sin(Phi); |
||
1081 | cosPhi_P = cos(Phi); |
||
1082 | sinTheta_P = sin(Theta); |
||
1083 | cosTheta_P = cos(Theta); |
||
1084 | |||
1085 | /* Simplified Version */ |
||
1086 | Pre_ax = -sinTheta_P * g; |
||
1087 | Pre_ay = cosTheta_P * sinPhi_P * g; |
||
1088 | Pre_az = cosTheta_P * cosPhi_P * g; |
||
1089 | |||
1090 | matSetFull(matYs, _ax, 0, Pre_ax); |
||
1091 | matSetFull(matYs, _ay, 0, Pre_ay); |
||
1092 | matSetFull(matYs, _az, 0, Pre_az); |
||
1093 | matSetFull(matYs, _compass, 0, Psi); |
||
1094 | } |
||
1095 | #endif |
||
1096 | |||
1097 | #ifdef USE_Extended_MM3_Measurement_Model |
||
1098 | /* **************************************************************************** |
||
1099 | Functionname: trMeasure */ /*! |
||
1100 | Description: |
||
1101 | |||
1102 | @param[in] |
||
1103 | |||
1104 | @return void |
||
1105 | @pre - |
||
1106 | @post - |
||
1107 | @author Michael Walter |
||
1108 | **************************************************************************** */ |
||
1109 | void trMeasure() |
||
1110 | { |
||
1111 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
1112 | |||
1113 | /*--- VARIABLES ---*/ |
||
1114 | |||
1115 | f32_t ADC_ax = 0.0F; |
||
1116 | f32_t ADC_ay = 0.0F; |
||
1117 | f32_t ADC_az = 0.0F; |
||
1118 | |||
1119 | fYValid[_ax] = 1; |
||
1120 | fYValid[_ay] = 1; |
||
1121 | fYValid[_az] = 1; |
||
1122 | |||
1123 | ADC_ax = AdWertAccNick * 0.047F; |
||
1124 | ADC_ay = -AdWertAccRoll * 0.047F; |
||
1125 | ADC_az = AdWertAccHoch * 0.047F; |
||
1126 | |||
1127 | matSetFull(matY, _ax, 0, ADC_ax); |
||
1128 | matSetFull(matY, _ay, 0, ADC_ay); |
||
1129 | matSetFull(matY, _az, 0, ADC_az); |
||
1130 | |||
1131 | //if (MM3_Ready == 1) |
||
1132 | if (1) |
||
1133 | { |
||
1134 | MM3_Heading(); |
||
1135 | matSetFull(matY, _mx, 0, MM3_Hx); |
||
1136 | matSetFull(matY, _my, 0, MM3_Hy); |
||
1137 | matSetFull(matY, _mz, 0, MM3_Hz); |
||
1138 | |||
1139 | fYValid[_mx] = 1; |
||
1140 | fYValid[_my] = 1; |
||
1141 | fYValid[_mz] = 1; |
||
1142 | //MM3_Ready = 0; |
||
1143 | |||
1144 | DebugOut.Analog[13] = MM3_Hx * 100; |
||
1145 | DebugOut.Analog[14] = MM3_Hy * 100; |
||
1146 | DebugOut.Analog[15] = MM3_Hz * 100; |
||
1147 | } |
||
1148 | else |
||
1149 | { |
||
1150 | fYValid[_mx] = 0; |
||
1151 | fYValid[_my] = 0; |
||
1152 | fYValid[_mz] = 0; |
||
1153 | } |
||
1154 | } |
||
1155 | #else |
||
1156 | /* **************************************************************************** |
||
1157 | Functionname: trMeasure */ /*! |
||
1158 | Description: |
||
1159 | |||
1160 | @param[in] |
||
1161 | |||
1162 | @return void |
||
1163 | @pre - |
||
1164 | @post - |
||
1165 | @author Michael Walter |
||
1166 | **************************************************************************** */ |
||
1167 | void trMeasure() |
||
1168 | { |
||
1169 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
1170 | |||
1171 | /*--- VARIABLES ---*/ |
||
1172 | |||
1173 | f32_t ADC_ax = 0.0F; |
||
1174 | f32_t ADC_ay = 0.0F; |
||
1175 | f32_t ADC_az = 0.0F; |
||
1176 | |||
1177 | fYValid[_ax] = 1; |
||
1178 | fYValid[_ay] = 1; |
||
1179 | fYValid[_az] = 1; |
||
1180 | |||
1181 | ADC_ax = AdWertAccNick * 0.047F; |
||
1182 | ADC_ay = -AdWertAccRoll * 0.047F; |
||
1183 | ADC_az = AdWertAccHoch * 0.047F; |
||
1184 | |||
1185 | matSetFull(matY, _ax, 0, ADC_ax); |
||
1186 | matSetFull(matY, _ay, 0, ADC_ay); |
||
1187 | matSetFull(matY, _az, 0, ADC_az); |
||
1188 | |||
1189 | static uint8_t updCompass = 0; |
||
1190 | f32_t Psi, Compass; |
||
1191 | |||
1192 | if (!updCompass--) |
||
1193 | { |
||
1194 | updCompass = 10; |
||
1195 | KompassValue = MM3_Heading(); |
||
1196 | } |
||
1197 | |||
1198 | Compass = KompassValue / 180.F * PI; |
||
1199 | |||
1200 | matGetFull(matXs, _Psi, 0, &Psi); |
||
1201 | |||
1202 | if (fabs(Compass + 2*PI - Psi) < fabs(Compass - Psi)) |
||
1203 | { |
||
1204 | Compass += 2 * PI; |
||
1205 | } |
||
1206 | else if (fabs(Compass - 2*PI - Psi) < fabs(Compass - Psi)) |
||
1207 | { |
||
1208 | Compass -= 2 * PI; |
||
1209 | } |
||
1210 | |||
1211 | matSetFull(matY, _compass, 0, Compass); |
||
1212 | |||
1213 | /* Roll and Yaw angle are smaller 8 Deg*/ |
||
1214 | if ((abs(status.iPhi10) < 80 ) && (abs(status.iTheta10) < 80)) |
||
1215 | { |
||
1216 | fYValid[_compass] = 1; |
||
1217 | } |
||
1218 | else |
||
1219 | { |
||
1220 | fYValid[_compass] = 0; |
||
1221 | } |
||
1222 | } |
||
1223 | #endif |
||
1224 | |||
1225 | /* **************************************************************************** |
||
1226 | Functionname: trUpdateBU */ /*! |
||
1227 | Description: Update control matrix B and vector U |
||
1228 | |||
1229 | |||
1230 | @param[in] |
||
1231 | |||
1232 | @return void |
||
1233 | @pre - |
||
1234 | @post - |
||
1235 | @author Michael Walter |
||
1236 | **************************************************************************** */ |
||
1237 | void trUpdateBU() |
||
1238 | { |
||
1239 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
1240 | |||
1241 | /*--- VARIABLES ---*/ |
||
1242 | /* |
||
1243 | dPhi | 1 sinPhi_tanTheta cosPhi_tanTheta | p |
||
1244 | dTheta = | 0 cosPhi -sinPhi | * q |
||
1245 | dPsi | 0 sinPhi_secTheta cosPhi_secTheta | w |
||
1246 | */ |
||
1247 | |||
1248 | f32_t ADC_p = 0.0F; |
||
1249 | f32_t ADC_q = 0.0F; |
||
1250 | f32_t ADC_r = 0.0F; |
||
1251 | |||
1252 | /* store predicted state vector in current state */ |
||
1253 | matGetFull(matXd, _Phi , 0, &Phi); |
||
1254 | matGetFull(matXd, _Theta , 0, &Theta); |
||
1255 | |||
1256 | sinPhi = sin(Phi); |
||
1257 | cosPhi = cos(Phi); |
||
1258 | |||
1259 | tanTheta = tan(Theta); |
||
1260 | secTheta = 1.0F / cos(Theta); |
||
1261 | |||
1262 | matSetFull(matB, _Phi, _p, 1.0F); |
||
1263 | matSetFull(matB, _Phi, _q, sinPhi * tanTheta ); |
||
1264 | matSetFull(matB, _Phi, _r, cosPhi * tanTheta ); |
||
1265 | |||
1266 | //matSetFull(matB, _Theta, _p, 0.0F); |
||
1267 | matSetFull(matB, _Theta, _q, cosPhi ); |
||
1268 | matSetFull(matB, _Theta, _r, -sinPhi ); |
||
1269 | |||
1270 | //matSetFull(matB, _Psi, _p, 0.0F); |
||
1271 | matSetFull(matB, _Psi, _q, sinPhi * secTheta ); |
||
1272 | matSetFull(matB, _Psi, _r, cosPhi * secTheta ); |
||
1273 | |||
1274 | ADC_p = -(float) AverageRoll_X * 0.00001F * fCycleTime; |
||
1275 | ADC_q = -(float) AverageRoll_Y * 0.00001F * fCycleTime; |
||
1276 | ADC_r = -(float) AverageRoll_Z * 0.00001F * fCycleTime; |
||
1277 | |||
1278 | matSetFull(matU, _p, 0, ADC_p ); |
||
1279 | matSetFull(matU, _q, 0, ADC_q); |
||
1280 | matSetFull(matU, _r, 0, ADC_r); |
||
1281 | } |
||
1282 | |||
1283 | |||
1284 | void AttitudeEstimation() |
||
1285 | { |
||
1286 | trPredictMeasurement(); |
||
1287 | |||
1288 | trUpdateJacobiMatrix(); |
||
1289 | |||
1290 | /* Extract measurements and store them in vector matY. |
||
1291 | Measurement validity is indicated by fYValid[] */ |
||
1292 | trMeasure(); |
||
1293 | |||
1294 | /* Innovation: calculate Xd, and Pd */ |
||
1295 | trInnovate(); |
||
1296 | |||
1297 | /* Update transition matrix Phi */ |
||
1298 | trUpdatePhi(); |
||
1299 | |||
1300 | /* Update control matrix B */ |
||
1301 | trUpdateBU(); |
||
1302 | |||
1303 | /* Predict new state Xs and Ps */ |
||
1304 | KAFIPrediction(p_kafi); |
||
1305 | |||
1306 | /* store innovated state vector in current state */ |
||
1307 | matGetFull(matXs, _Phi , 0, &status.Phi); |
||
1308 | matGetFull(matXs, _Theta , 0, &status.Theta); |
||
1309 | matGetFull(matXs, _Psi , 0, &status.Psi); |
||
1310 | |||
1311 | trLimitAngles(); |
||
1312 | } |
||
1313 | |||
1314 | |||
1315 | |||
1316 | /* **************************************************************************** |
||
1317 | Functionname: trEstimateVelocity */ /*! |
||
1318 | Description: |
||
1319 | |||
1320 | @param[in] |
||
1321 | |||
1322 | @return void |
||
1323 | @pre - |
||
1324 | @post - |
||
1325 | @author |
||
1326 | **************************************************************************** */ |
||
1327 | void trEstimateVelocity() |
||
1328 | { |
||
1329 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
1330 | |||
1331 | /*--- VARIABLES ---*/ |
||
1332 | |||
1333 | } |
||
1334 | |||
1335 | |||
1336 | /* **************************************************************************** |
||
1337 | Functionname: trLimitAngles */ /*! |
||
1338 | Description: |
||
1339 | |||
1340 | @param[in] |
||
1341 | |||
1342 | @return void |
||
1343 | @pre - |
||
1344 | @post - |
||
1345 | @author |
||
1346 | **************************************************************************** */ |
||
1347 | void trLimitAngles() |
||
1348 | { |
||
1349 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
1350 | |||
1351 | /*--- VARIABLES ---*/ |
||
1352 | f32_t Psi; |
||
1353 | |||
1354 | if (status.Phi > 2.0F * PI) |
||
1355 | { |
||
1356 | status.Phi -= 2.0F * PI; |
||
1357 | matSetFull(matXs, _Phi, 0, status.Phi); |
||
1358 | } |
||
1359 | if (status.Phi < -2.0F * PI) |
||
1360 | { |
||
1361 | status.Phi += 2.0F * PI; |
||
1362 | matSetFull(matXs, _Phi, 0, status.Phi); |
||
1363 | } |
||
1364 | if (status.Theta > 2.0F * PI) |
||
1365 | { |
||
1366 | status.Theta -= 2.0F * PI; |
||
1367 | matSetFull(matXs, _Theta, 0, status.Theta); |
||
1368 | } |
||
1369 | if (status.Theta < -2.0F * PI) |
||
1370 | { |
||
1371 | status.Theta += 2.0F * PI; |
||
1372 | matSetFull(matXs, _Theta, 0, status.Theta); |
||
1373 | } |
||
1374 | if (status.Psi > 2.0F * PI) |
||
1375 | { |
||
1376 | status.Psi -= 2.0F * PI; |
||
1377 | sollGier -= 3600; |
||
1378 | |||
1379 | matGetFull(matXs, _Psi, 0, &Psi); |
||
1380 | matSetFull(matXs, _Psi , 0, Psi - 2.0F * PI); |
||
1381 | matGetFull(matXd, _Psi, 0, &Psi); |
||
1382 | matSetFull(matXd, _Psi , 0, Psi - 2.0F * PI); |
||
1383 | } |
||
1384 | if (status.Psi < 0.0F * PI) |
||
1385 | { |
||
1386 | status.Psi += 2.0F * PI; |
||
1387 | sollGier += 3600; |
||
1388 | |||
1389 | matGetFull(matXs, _Psi, 0, &Psi); |
||
1390 | matSetFull(matXs, _Psi , 0, Psi + 2.0F * PI); |
||
1391 | matGetFull(matXd, _Psi, 0, &Psi); |
||
1392 | matSetFull(matXd, _Psi , 0, Psi + 2.0F * PI); |
||
1393 | } |
||
1394 | |||
1395 | status.iPhi10 = (int) (status.Phi * 573.0F); |
||
1396 | status.iTheta10 = (int) (status.Theta * 573.0F); |
||
1397 | status.iPsi10 = (int) (status.Psi * 573.0F); |
||
1398 | |||
1399 | if ((sollGier - status.iPsi10) > 3600) |
||
1400 | { |
||
1401 | sollGier -= 3600; |
||
1402 | } |
||
1403 | |||
1404 | if ((sollGier - status.iPsi10) < -3600) |
||
1405 | { |
||
1406 | sollGier += 3600; |
||
1407 | } |
||
1408 | } |
||
1409 | |||
1410 |