| 5,41 → 5,41 |
|---|
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include "main.h" |
| |
| //Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int |
| //verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr) |
| const int c_sin[] = {0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299,316,333,350,367,384,400, |
| 416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672,685,698,711,724,737,749, |
| 761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928,935,943,949,956,962,968, |
| 974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024,1024,1024,1023,1023,1022,1020, |
| 1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943,935,928,920,912,904,896, |
| 887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698,685,672,658,644,630,616, |
| 602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333,316,299,282,265,248,230, |
| 213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89,-107,-125,-143,-160,-178,-195, |
| -213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496,-512,-527,-543,-558,-573,-587, |
| -602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818,-828,-839,-849,-859,-868,-878, |
| -887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998,-1002,-1005,-1008,-1011,-1014,-1016, |
| -1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005,-1002,-998,-994,-989,-984,-979, |
| -974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839,-828,-818,-807,-796,-784,-773, |
| -761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527,-512,-496,-481,-465,-449,-433, |
| -416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125,-107,-89,-71,-54,-36,-18}; |
| //Sinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
| //verwendet werden kann. d.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
| const int c_sin[] = {0,17,34,52,69,87,104,121,139,156,173,190, |
| 207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544,559,573,587,601,615,629, |
| 642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874,882,891,898,906,913,920, |
| 927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999,1000,999,999,998,997,996, |
| 994,992,990,987,984,981,978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838, |
| 829,819,809,798,788,777,766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484, |
| 469,453,438,422,406,390,374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17, |
| -34,-52,-69,-87,-104,-121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406, |
| -422,-438,-453,-469,-484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719, |
| -731,-743,-754,-766,-777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927, |
| -933,-939,-945,-951,-956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000, |
| -999,-999,-998,-997,-996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927, |
| -920,-913,-906,-898,-891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719, |
| -707,-694,-682,-669,-656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406, |
| -390,-374,-358,-342,-325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17}; |
| |
| //Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1024 multipliziert, damit als Definition platzsparend int |
| //verwendet werden kann. D.h., es muss also in der späteren Berechnung noch durch 1024 dividiert werden //(051207Kr) |
| const int c_cos[] = {1024,1024,1023,1023,1022,1020,1018,1016,1014,1011,1008,1005,1002,998,994,989,984,979,974,968,962,956,949,943, |
| 935,928,920,912,904,896,887,878,868,859,849,839,828,818,807,796,784,773,761,749,737,724,711,698, |
| 685,672,658,644,630,616,602,587,573,558,543,527,512,496,481,465,449,433,416,400,384,367,350,333, |
| 316,299,282,265,248,230,213,195,178,160,143,125,107,89,71,54,36,18,0,-18,-36,-54,-71,-89, |
| -107,-125,-143,-160,-178,-195,-213,-230,-248,-265,-282,-299,-316,-333,-350,-367,-384,-400,-416,-433,-449,-465,-481,-496, |
| -512,-527,-543,-558,-573,-587,-602,-616,-630,-644,-658,-672,-685,-698,-711,-724,-737,-749,-761,-773,-784,-796,-807,-818, |
| -828,-839,-849,-859,-868,-878,-887,-896,-904,-912,-920,-928,-935,-943,-949,-956,-962,-968,-974,-979,-984,-989,-994,-998, |
| -1002,-1005,-1008,-1011,-1014,-1016,-1018,-1020,-1022,-1023,-1023,-1024,-1024,-1024,-1023,-1023,-1022,-1020,-1018,-1016,-1014,-1011,-1008,-1005, |
| -1002,-998,-994,-989,-984,-979,-974,-968,-962,-956,-949,-943,-935,-928,-920,-912,-904,-896,-887,-878,-868,-859,-849,-839, |
| -828,-818,-807,-796,-784,-773,-761,-749,-737,-724,-711,-698,-685,-672,-658,-644,-630,-616,-602,-587,-573,-558,-543,-527, |
| -512,-496,-481,-465,-449,-433,-416,-400,-384,-367,-350,-333,-316,-299,-282,-265,-248,-230,-213,-195,-178,-160,-143,-125, |
| -107,-89,-71,-54,-36,-18,0,18,36,54,71,89,107,125,143,160,178,195,213,230,248,265,282,299, |
| 316,333,350,367,384,400,416,433,449,465,481,496,512,527,543,558,573,587,602,616,630,644,658,672, |
| 685,698,711,724,737,749,761,773,784,796,807,818,828,839,849,859,868,878,887,896,904,912,920,928, |
| 935,943,949,956,962,968,974,979,984,989,994,998,1002,1005,1008,1011,1014,1016,1018,1020,1022,1023,1023,1024}; |
| //Cosinusfunktion von 0° bis 359°. Ergebnis wurde mit 1000 multipliziert, damit als Definition platzsparend int |
| //verwendet werden kann. D.h., es muss also in der späteren Berechnung noch mit 0,001 multipliziert werden //(090907Kr) |
| const int c_cos[] = {1000,999,999,998,997,996,994,992,990,987,984,981, |
| 978,974,970,965,961,956,951,945,939,933,927,920,913,906,898,891,882,874,866,857,848,838,829,819,809,798,788,777, |
| 766,754,743,731,719,707,694,682,669,656,642,629,615,601,587,573,559,544,529,515,500,484,469,453,438,422,406,390, |
| 374,358,342,325,309,292,275,258,241,224,207,190,173,156,139,121,104,87,69,52,34,17,0,-17,-34,-52,-69,-87,-104, |
| -121,-139,-156,-173,-190,-207,-224,-241,-258,-275,-292,-309,-325,-342,-358,-374,-390,-406,-422,-438,-453,-469, |
| -484,-500,-515,-529,-544,-559,-573,-587,-601,-615,-629,-642,-656,-669,-682,-694,-707,-719,-731,-743,-754,-766, |
| -777,-788,-798,-809,-819,-829,-838,-848,-857,-866,-874,-882,-891,-898,-906,-913,-920,-927,-933,-939,-945,-951, |
| -956,-961,-965,-970,-974,-978,-981,-984,-987,-990,-992,-994,-996,-997,-998,-999,-999,-1000,-999,-999,-998,-997, |
| -996,-994,-992,-990,-987,-984,-981,-978,-974,-970,-965,-961,-956,-951,-945,-939,-933,-927,-920,-913,-906,-898, |
| -891,-882,-874,-866,-857,-848,-838,-829,-819,-809,-798,-788,-777,-766,-754,-743,-731,-719,-707,-694,-682,-669, |
| -656,-642,-629,-615,-601,-587,-573,-559,-544,-529,-515,-500,-484,-469,-453,-438,-422,-406,-390,-374,-358,-342, |
| -325,-309,-292,-275,-258,-241,-224,-207,-190,-173,-156,-139,-121,-104,-87,-69,-52,-34,-17,0,17,34,52,69,87, |
| 104,121,139,156,173,190,207,224,241,258,275,292,309,325,342,358,374,390,406,422,438,453,469,484,500,515,529,544, |
| 559,573,587,601,615,629,642,656,669,682,694,707,719,731,743,754,766,777,788,798,809,819,829,838,848,857,866,874, |
| 882,891,898,906,913,920,927,933,939,945,951,956,961,965,970,974,978,981,984,987,990,992,994,996,997,998,999,999}; |
| |
| |
| |
| 292,8 → 292,8 |
|---|
| GPS_East = (P_Einfluss_East - D_Einfluss_East); |
| |
| //Umrechnen vom globalen North- bzw. East- in das körperfeste X- bzw. Y- Koordinatensystem |
| GPS_Nick = ((c_cos[KompassValue]*GPS_North)/1024 - (c_sin[KompassValue]*GPS_East)/1024); |
| GPS_Roll = ((c_cos[KompassValue]*GPS_East)/1024 + (c_sin[KompassValue]*GPS_North)/1024); |
| GPS_Nick = (-c_sin[KompassValue]*0.001*GPS_East + c_cos[KompassValue]*0.001*GPS_North); |
| GPS_Roll = (c_cos[KompassValue]*0.001*GPS_East + c_sin[KompassValue]*0.001*GPS_North); |
| |
| // Begrenzung des maximalen GPS Einflusses für positive und negative Werte |
| if (GPS_Nick > GPS_Limit) GPS_Nick = GPS_Limit; |