Rev 518 | 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. |