Rev 518 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 518 | Rev 536 | ||
|---|---|---|---|
| Line 281... | Line 281... | ||
| 281 | // Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>5m) von der Home-Position weg befindet. |
281 | // Dieses Limit wirkt nur, wenn sich der MK im HOMING Modus befindet und sich noch weit (>5m) von der Home-Position weg befindet. |
| 282 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 500 || abs(GPS_Positionsabweichung_East) > 500)) |
282 | if (Poti3 >= 150 && GPS_Home_North != 0 && GPS_Home_East != 0 && (abs(GPS_Positionsabweichung_North) > 500 || abs(GPS_Positionsabweichung_East) > 500)) |
| 283 | { |
283 | { |
| 284 | if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil; |
284 | if (D_Einfluss_North > Limit_D_Anteil) D_Einfluss_North = Limit_D_Anteil; |
| 285 | if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil; |
285 | if (D_Einfluss_East > Limit_D_Anteil) D_Einfluss_East = Limit_D_Anteil; |
| 286 | if (D_Einfluss_North < -1 * Limit_D_Anteil) D_Einfluss_North = -1 * Limit_D_Anteil; |
286 | if (D_Einfluss_North < -Limit_D_Anteil) D_Einfluss_North = -Limit_D_Anteil; |
| 287 | if (D_Einfluss_East < -1 * Limit_D_Anteil) D_Einfluss_East = -1 * Limit_D_Anteil; |
287 | if (D_Einfluss_East < -Limit_D_Anteil) D_Einfluss_East = -Limit_D_Anteil; |
| 288 | } |
288 | } |
| Line 289... | Line 289... | ||
| 289 | 289 | ||
| 290 | // PD-Regler |
290 | // PD-Regler |
| 291 | GPS_North = (-P_Einfluss_North + D_Einfluss_North); |
291 | GPS_North = (-P_Einfluss_North + D_Einfluss_North); |
| Line 296... | Line 296... | ||
| 296 | GPS_Roll = ((c_cos[KompassValue]*GPS_East)/1024 + (c_sin[KompassValue]*GPS_North)/1024); |
296 | GPS_Roll = ((c_cos[KompassValue]*GPS_East)/1024 + (c_sin[KompassValue]*GPS_North)/1024); |
| Line 297... | Line 297... | ||
| 297 | 297 | ||
| 298 | // Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
298 | // Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
| 299 | if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit; |
299 | if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit; |
| 300 | if (GPS_Roll > GPS_Limit) GPS_Roll = GPS_Limit; |
300 | if (GPS_Roll > GPS_Limit) GPS_Roll = GPS_Limit; |
| 301 | if (GPS_Nick < -1 * GPS_Limit) GPS_Nick = -1 * GPS_Limit; |
301 | if (GPS_Nick < -GPS_Limit) GPS_Nick = -GPS_Limit; |
| Line 302... | Line 302... | ||
| 302 | if (GPS_Roll < -1 * GPS_Limit) GPS_Roll = -1 * GPS_Limit; |
302 | if (GPS_Roll < -GPS_Limit) GPS_Roll = -GPS_Limit; |
| 303 | 303 | ||
| 304 | //Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen. |
304 | //Funktion wird dadurch nur alle 250 ms aufgerufen, wenn neue Mittelwerte aus den GPS-Daten vorliegen. |
| 305 | //FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN. |
305 | //FUNKTIONIERT MOMENTAN NOCH NICHT, DA STÄNDIG OHNE UNTERBRECHUNG NEUE MITTELWERTE BERECHNET WERDEN. |