| 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); |
| } |
| |