Beräkna kurs m.h.a. koordinater?

[Nerre]

Ja men grejen här är väl att man ska ha en lookuptabell "baklänges"? D.v.s. man räknar ut ett värde och slår sen upp vilken vinkel det motsvaras av.

Då ska "index" i tabellen alltså gå från 4 (för 1 grad) till 14666 (för 89 grader).

Visserligen borde man kunna dela med 4 och få index från 1 till 3666. Men det ryms ju inte under några omständigheter i en byte.

[bearing]

Här är enkel exempelkod i avr-gcc-C som visar hur jag menar.

Kod: Markera allt

/* ARCTAN för vinklar mellan 0 och 90 grader. */
uint8_t ARCTAN(uint16_t x, uint16_t y)
{
	uint32_t	kvot;
	uint8_t	vinkel;

	kvot=y*256;
	kvot/=x;
	
	for (vinkel=0; vinkel<90; vinkel++)
	{
		if ( (uint16_t)kvot < tableTAN256[vinkel]) break;
	}
	
	return vinkel;
}

/* Tabell med värdena tan(v*PI/180)*256 där v = 0.5, 1.5, 2.5 ... 89.5 */
static const uint16_t PROGMEM tableTAN256[90] = {
2,
6,
11,
15,
20,
24,
29,
33,
38,
42,
47,
52,
56,
61,
66,
70,
75,
80,
85,
90,
95,
100,
106,
111,
116,
122,
127,
133,
138,
144,
150,
156,
163,
169,
175,
182,
189,
196,
203,
211,
218,
226,
234,
242,
251,
260,
269,
279,
289,
299,
310,
321,
333,
345,
358,
372,
386,
401,
417,
434,
452,
471,
491,
513,
536,
561,
588,
618,
649,
684,
722,
765,
811,
864,
923,
989,
1066,
1154,
1258,
1381,
1529,
1712,
1944,
2246,
2658,
3252,
4185,
5863,
9776,
29334
};

[jadler]

Jag vet inte hur relevant det känns, det är skrivet i C, men på http://www.maartenlamers.com/nmea/ finns ett enkelt navigeringsbibliotek inklusive subrutin för att beräkna kurs och avstånd mellan två punkter.

När jag själv ville räkna på värden från en treaxlig accelerometer använde jag "vanliga" matrisberäkningar för att räkna fram rymdvinkelns projektion i olika plan, och kraftvektorns storlek längs de tre axlarna. Mina rutiner (i C för AVR) kör med flyttal rakt av, jag upplevde inte på något sätt att det var långsamt. Det finns bra C-bibliotek, om minnet räcker till och tillämpningen inte är väldigt tidskritisk unnar jag mig gärna bekvämligheten att använda dem.

Spridda klipp ur min kod:

Kod: Markera allt

struct coordFloat {
  float x, y, z;
};

struct coordFloat axisX = {1, 0, 0}, axisY = {0, 1, 0}, axisZ = {0, 0, 1};

float accAngleCalc(struct coordFloat a, struct coordFloat b, float c) {
  return( (c < 0) ? 180.0 - vectAngle(a, b) : vectAngle(a, b));
}

float vectAngle(struct coordFloat a, struct coordFloat b) {
  float c;
  c = vectDotProduct(a, b) / (vectMagnitude(a) * vectMagnitude(b));
  return(atan(sqrt( (1 - sq(c)) / sq(c) )) * 180 / PI);
}

void vectCrossProduct(struct coordFloat a, struct coordFloat b, struct coordFloat *c) {
  c->x = a.y * b.z - a.z * b.y;
  c->y = a.z * b.x - a.x * b.z;
  c->z = a.x * b.y - a.y * b.x;
}

float vectDotProduct(struct coordFloat a, struct coordFloat b) {
  return(a.x * b.x + a.y * b.y + a.z * b.z);
}

float vectMagnitude(struct coordFloat a) {
  return(sqrt(sq(a.x) + sq(a.y) + sq(a.z)));
}

Tillägg: Om din destination är en bit bort, avståndet är stort, kan du både för avstånd och vinkel behöva ta med i beräkningen att jordytan inte är plan, utan att du beräknar en färdväg längs ett storcirkelsegment. Mer finns att läsa på t.ex. http://williams.best.vwh.net/avform.htm

[bit96]

Om du MacLaurinutvecklar arctan så blir det nog ganska enkelt.
Du behöver nog bara ta med ett par tre termer.

v = a -(a^3)/3 + (a^5)/5 ...

Alltså endast multiplikation och division.

Edit: Alltså det är v=arctan(a) som är MacLaurinutvecklat ovan. OBS! Vinkeln är i rad, men att omvandla det till grader kan du säkert.

Edit2: Som jadler påpekar ovan måste man egentligen ta hänsyn till ytans krökning. Men m.h.a. sfärisk trigonometri är det ganska enkelt, som i sin tur kan MacLaurinutvecklas. I sfärisk trig. kan/skall du direkt räkna med lat och long.

[bearing]

CORDIC

float

v = a -(a^3)/3 + (a^5)/5 ...

Menar ni att han ska skriva sånt här i assembler till en microcontroller?

Om vi bortser från språket tror jag att rutinerna kommer ta mer plats, och exekveras långsammare, än tabell-baserade lösningar. När folk skrev "Demo-grafik" på PC använde dom ofta tabeller för sin/cos/tan eftersom att det gav bättre prestanda, på bekostnad av noggrannhet.

Hastigheten är visserligen nog inte så viktig här eftersom att en GPS ger data i bara 1-5 Hz.

[ahlsten]

bearing: Har du förstått CORDIC? Metoden använder enbart addition, subtraktion, skiftoperartioner och uppslagstabell... och det blir bra mycket exaktare.

Här finns en jämförelse mellan några av de metoder som diskuterats här (uppslagstabell, serieutveckling ...).

[bearing]

Nej, jag hade inte förstått CORDIC. Fick fel intryck när jag skummade igenom första gången. Verkar ju vara en elegant lösning. Ska läsa vidare om det.

[Ronny]

Flyttalsberäkningar och assembler...här är en applikationsnot med ett programpaket för 68HC11
Gordon Doughmans AN974 "MC68HC11 Floating-Point Package"

[bit96]

Jag vet inte vilka matematiska operationer Glattnos kan göra i sin mikrokontroller.

Men jag antog att om han skall göra vinkelberäkningar och översätta mellan olika koordinatsystem så bör han ha tillgång till åtminstone multiplikation och division. Annars finns det assemblerrutiner som gör detta utav add, sub, shift och lite viftande med flaggor.

Han har ju dessutom räknat om Lat och Long till till nån sorts metervärden så han har nog tillgång till mul och div.

Dessutom kan det inte vara tidskritiskt eftersom jag uppfattar det som om att Glattnos vill räkna ut EN vinkel från två kända lagrade punkter A och B. Det skall alltså inte göras en gång per sekund eller så, om jag nu uppfattat rätt.

Formeln som jag förslog (MacLaurin av v=tan(a)) innehåller endast några mul, div, sub och add.
Det behövs ingen trigonometri, inte ens för att bygga en tabell. Det behövs då heller inte nåt minne till en tabell.

[jadler]

Jag glömde nämna att matris-/vektorberäkningsrutinerna kommer från A Little C Primer på Wikibooks, avsnittet om variabler: http://en.wikibooks.org/wiki/A_Little_C ... _Constants

[Glattnos]

Tack för alla svar! Jag har fått det att fungera nu. Blev en tabell men ändå mycket kod för alla beräkningar.