64,7 → 64,6 |
#include "fifo.h" |
#include "led.h" |
#include "main.h" |
#include "ncmag.h" |
|
u8 CompassCalStateQueue[10]; |
fifo_t CompassCalcStateFiFo; |
95,10 → 94,10 |
else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
} |
break; |
|
|
case COMPASS_NCMAG: |
if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
else Compass_Device = COMPASS_NONE; |
else Compass_Device = COMPASS_NONE; |
break; |
|
case COMPASS_MK3MAG: |
123,16 → 122,16 |
// a float implementation takes too long |
s16 tmp, Hx, Hy; |
s32 sinnick, cosnick, sinroll, cosroll; |
|
tmp = FromFlightCtrl.AngleNick/10; // in deg |
|
tmp = FromFlightCtrl.AngleNick/10; // in deg |
sinnick = (s32)c_sin_8192(tmp); |
cosnick = (s32)c_cos_8192(tmp); |
tmp = FromFlightCtrl.AngleRoll/10; // in deg |
tmp = FromFlightCtrl.AngleRoll/10; // in deg |
sinroll = (s32)c_sin_8192(tmp); |
cosroll = (s32)c_cos_8192(tmp); |
// tbd. compensation signs and oriantation has to be fixed |
// tbd. compensation signs and oriantation has to be fixed |
Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
// calculate heading |
tmp = (s16)(c_atan2_546(Hy, Hx)/546L); |
if (tmp > 0) tmp = 360 - tmp; |
152,9 → 151,9 |
MK3MAG_Update(); |
break; |
case COMPASS_NCMAG: |
NCMAG_Update(); |
NCMAG_Update(); |
default: |
break; |
break; |
} |
DebugOut.Analog[24] = MagVector.X; |
DebugOut.Analog[25] = MagVector.Y; |
164,12 → 163,12 |
// put cal state into fifo |
void Compass_SetCalState(u8 CalState) |
{ |
fifo_put(&CompassCalcStateFiFo, CalState); |
fifo_put(&CompassCalcStateFiFo, CalState); |
} |
|
// get cal state from fifo |
void Compass_UpdateCalState() |
{ |
fifo_get(&CompassCalcStateFiFo, (u8*)&Compass_CalState); |
fifo_get(&CompassCalcStateFiFo, (u8*)&Compass_CalState); |
} |
|