Rev 268 | Rev 293 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 268 | Rev 292 | ||
---|---|---|---|
Line 56... | Line 56... | ||
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
57 | #include "91x_lib.h" |
57 | #include "91x_lib.h" |
58 | #include "compass.h" |
58 | #include "compass.h" |
59 | #include "mk3mag.h" |
59 | #include "mk3mag.h" |
60 | #include "ncmag.h" |
60 | #include "ncmag.h" |
- | 61 | #include "spi_slave.h" |
|
- | 62 | #include "mymath.h" |
|
61 | #include "uart1.h" |
63 | #include "uart1.h" |
62 | #include "fifo.h" |
64 | #include "fifo.h" |
63 | #include "led.h" |
65 | #include "led.h" |
- | 66 | #include "main.h" |
|
Line 64... | Line 67... | ||
64 | 67 | ||
65 | 68 | ||
Line 106... | Line 109... | ||
106 | } |
109 | } |
107 | fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine); |
110 | fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine); |
108 | } |
111 | } |
Line 109... | Line 112... | ||
109 | 112 | ||
110 | 113 | ||
111 | void Compass_UpdateHeading(void) |
114 | void Compass_CalcHeading(void) |
112 | { |
115 | { |
113 | switch(Compass_Device) |
116 | if((UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) || Compass_CalState) |
- | 117 | { |
|
- | 118 | Compass_Heading = -1; |
|
- | 119 | } |
|
114 | { |
120 | else // fc attitude is avialable and no compass calibration active |
- | 121 | { |
|
115 | case COMPASS_MK3MAG: |
122 | // calculate attitude correction |
- | 123 | // a float implementation takes too long |
|
- | 124 | s16 tmp, Hx, Hy; |
|
- | 125 | s32 sinnick, cosnick, sinroll, cosroll; |
|
- | 126 | ||
116 | MK3MAG_UpdateCompass(); |
127 | tmp = FromFlightCtrl.AngleNick/10; // in deg |
- | 128 | sinnick = (s32)c_sin_8192(tmp); |
|
- | 129 | cosnick = (s32)c_cos_8192(tmp); |
|
117 | break; |
130 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
- | 131 | sinroll = (s32)c_sin_8192(tmp); |
|
- | 132 | cosroll = (s32)c_cos_8192(tmp); |
|
- | 133 | // tbd. compensation signs and oriantation has to be fixed |
|
118 | case COMPASS_NCMAG: |
134 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
- | 135 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
|
- | 136 | // calculate heading |
|
119 | NCMAG_UpdateCompass(); |
137 | tmp = (s16)(c_tan2_546(Hy, Hx)/546L); |
120 | break; |
138 | if (tmp > 0) tmp = 360 - tmp; |
121 | default: |
139 | else tmp = -tmp; |
122 | break; |
140 | Compass_Heading = tmp; |
Line 123... | Line 141... | ||
123 | } |
141 | } |
124 | } |
142 | } |
- | 143 | ||
- | 144 | void Compass_Update(void) |
|
- | 145 | { |
|
125 | 146 | // check for new cal state |
|
126 | void Compass_UpdateMagVector(void) |
147 | Compass_UpdateCalState(); |
127 | { |
148 | // initiate new compass communication |
128 | switch(Compass_Device) |
149 | switch(Compass_Device) |
129 | { |
150 | { |
130 | case COMPASS_MK3MAG: |
151 | case COMPASS_MK3MAG: |
131 | MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); |
152 | MK3MAG_Update(); |
132 | break; |
153 | break; |
133 | case COMPASS_NCMAG: |
154 | case COMPASS_NCMAG: |
134 | NCMAG_UpdateCompass(); |
155 | NCMAG_Update(); |
135 | default: |
156 | default: |