Rev 241 | Rev 248 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 241 | Rev 242 | ||
---|---|---|---|
Line 57... | Line 57... | ||
57 | #include <string.h> |
57 | #include <string.h> |
58 | #include "91x_lib.h" |
58 | #include "91x_lib.h" |
59 | #include "mk3mag.h" |
59 | #include "mk3mag.h" |
60 | #include "i2c.h" |
60 | #include "i2c.h" |
61 | #include "timer1.h" |
61 | #include "timer1.h" |
- | 62 | #include "led.h" |
|
62 | #include "spi_slave.h" |
63 | #include "spi_slave.h" |
63 | #include "main.h" |
64 | #include "main.h" |
64 | #include "uart1.h" |
65 | #include "uart1.h" |
- | 66 | #include "compass.h" |
|
Line 65... | Line 67... | ||
65 | 67 | ||
Line 66... | Line 68... | ||
66 | #define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address |
68 | #define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address |
- | 69 | ||
- | 70 | u8 MK3MAG_Present = 0; |
|
- | 71 | ||
- | 72 | typedef struct |
|
- | 73 | { |
|
- | 74 | s16 Nick; |
|
Line 67... | Line 75... | ||
67 | 75 | s16 Roll; |
|
68 | u8 MK3MAG_StopPolling = 1; |
- | |
69 | 76 | } __attribute__((packed)) MK3MAG_WriteAttitude_t; |
|
70 | // Transfer buffers |
- | |
71 | volatile MK3MAG_Heading_t MK3MAG_Heading; |
77 | |
72 | volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude; |
78 | // Transfer buffers |
73 | volatile MK3MAG_MagVector_t MK3MAG_MagVector; |
79 | volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude; |
Line 74... | Line -... | ||
74 | volatile MK3MAG_Version_t MK3MAG_Version; |
- | |
75 | volatile MK3MAG_Cal_t MK3MAG_WriteCal; |
- | |
76 | volatile MK3MAG_Cal_t MK3MAG_ReadCal; |
- | |
77 | - | ||
78 | - | ||
79 | - | ||
80 | void MK3MAG_Init(void) |
- | |
81 | { |
- | |
82 | MK3MAG_Version.Major = 0xFF; |
- | |
83 | MK3MAG_Version.Minor = 0xFF; |
- | |
84 | MK3MAG_Version.Patch = 0xFF; |
- | |
85 | MK3MAG_Version.Compatible = 0xFF; |
80 | volatile MK3MAG_Version_t MK3MAG_Version; |
86 | 81 | volatile MK3MAG_Cal_t MK3MAG_WriteCal; |
|
Line 87... | Line 82... | ||
87 | MK3MAG_Heading.Heading = -1; |
82 | volatile MK3MAG_Cal_t MK3MAG_ReadCal; |
88 | } |
83 | |
Line 142... | Line 137... | ||
142 | } |
137 | } |
Line 143... | Line 138... | ||
143 | 138 | ||
144 | // rx data handler for magnetic vector request |
139 | // rx data handler for magnetic vector request |
145 | void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
140 | void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
146 | { // if crc is ok and number of byte are matching |
141 | { // if crc is ok and number of byte are matching |
147 | if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_MagVector)+1)) ) |
142 | if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) ) |
148 | { |
143 | { |
149 | memcpy((u8 *)&MK3MAG_MagVector, pRxBuffer, sizeof(MK3MAG_MagVector)); |
144 | memcpy((u8 *)&MagVector, pRxBuffer, sizeof(MagVector)); |
150 | } |
145 | } |
Line 151... | Line 146... | ||
151 | } |
146 | } |
152 | 147 | ||
153 | // rx data handler for heading request |
148 | // rx data handler for heading request |
154 | void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize) |
149 | void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize) |
155 | { // if crc is ok and number of byte are matching |
150 | { // if crc is ok and number of byte are matching |
156 | if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Heading)+1)) ) |
151 | if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(CompassHeading)+1)) ) |
157 | { |
152 | { |
158 | memcpy((u8 *)&MK3MAG_Heading, pRxBuffer, sizeof(MK3MAG_Heading)); |
153 | memcpy((u8 *)&CompassHeading, pRxBuffer, sizeof(CompassHeading)); |
Line 159... | Line 154... | ||
159 | } |
154 | } |
160 | } |
155 | } |
161 | 156 | ||
162 | //---------------------------------------------------------------- |
- | |
163 | void MK3MAG_SendCommand(u8 command) |
- | |
164 | { |
157 | //---------------------------------------------------------------- |
165 | // disable I2C IRQ to check state |
158 | void MK3MAG_SendCommand(u8 command) |
166 | //VIC_ITCmd(I2C1_ITLine, DISABLE); |
159 | { |
167 | // If I2C no transmission is in progress |
160 | // If I2C no transmission is in progress |
168 | if(I2C_State == I2C_STATE_IDLE) |
161 | if(I2C_State == I2C_STATE_IDLE) |
Line 186... | Line 179... | ||
186 | pRxHandlerFunc = &MK3MAG_UpdateCalibration; |
179 | pRxHandlerFunc = &MK3MAG_UpdateCalibration; |
187 | memcpy((u8*)I2C_TxBuffer+1, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal)); |
180 | memcpy((u8*)I2C_TxBuffer+1, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal)); |
188 | I2C_TxBufferSize += sizeof(MK3MAG_WriteCal); |
181 | I2C_TxBufferSize += sizeof(MK3MAG_WriteCal); |
189 | break; |
182 | break; |
190 | case MK3MAG_CMD_READ_MAGVECT: |
183 | case MK3MAG_CMD_READ_MAGVECT: |
191 | RxBytes = sizeof(MK3MAG_MagVector)+1; |
184 | RxBytes = sizeof(MagVector)+1; |
192 | pRxHandlerFunc = &MK3MAG_UpdateMagVector; |
185 | pRxHandlerFunc = &MK3MAG_UpdateMagVector; |
193 | break; |
186 | break; |
194 | case MK3MAG_CMD_READ_HEADING: |
187 | case MK3MAG_CMD_READ_HEADING: |
195 | RxBytes = sizeof(MK3MAG_Heading)+1; |
188 | RxBytes = sizeof(CompassHeading)+1; |
196 | pRxHandlerFunc = &MK3MAG_UpdateHeading; |
189 | pRxHandlerFunc = &MK3MAG_UpdateHeading; |
197 | // update attitude from spi rx buffer |
190 | // update attitude from spi rx buffer |
198 | VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy |
191 | VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy |
199 | MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll; |
192 | MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll; |
200 | MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick; |
193 | MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick; |
Line 208... | Line 201... | ||
208 | break; |
201 | break; |
209 | } |
202 | } |
210 | // update packet checksum |
203 | // update packet checksum |
211 | I2C_TxBuffer[I2C_TxBufferSize] = MK3MAG_CalcCRC((u8*)I2C_TxBuffer, I2C_TxBufferSize); |
204 | I2C_TxBuffer[I2C_TxBufferSize] = MK3MAG_CalcCRC((u8*)I2C_TxBuffer, I2C_TxBufferSize); |
212 | I2C_TxBufferSize++; |
205 | I2C_TxBufferSize++; |
213 | // enable I2C IRQ again |
- | |
214 | VIC_ITCmd(I2C1_ITLine, ENABLE); |
- | |
215 | // initiate I2C transmission |
206 | // initiate I2C transmission |
216 | I2C_Transmission(MK3MAG_SLAVE_ADDRESS, pRxHandlerFunc, RxBytes); |
207 | I2C_Transmission(MK3MAG_SLAVE_ADDRESS, pRxHandlerFunc, RxBytes); |
217 | } // EOF I2C_State == I2C_IDLE |
208 | } // EOF I2C_State == I2C_IDLE |
218 | else // I2C_State != I2C_IDLE |
- | |
219 | { |
- | |
220 | // re-enable I2C IRQ again |
- | |
221 | //VIC_ITCmd(I2C1_ITLine, ENABLE); |
- | |
222 | } |
- | |
223 | } |
209 | } |
Line 224... | Line 210... | ||
224 | 210 | ||
225 | 211 | ||
226 | //---------------------------------------------------------------- |
212 | //---------------------------------------------------------------- |
227 | void MK3MAG_GetVersion(void) |
213 | u8 MK3MAG_Init(void) |
228 | { |
214 | { |
229 | u8 msg[64]; |
215 | u8 msg[64]; |
Line 230... | Line -... | ||
230 | u8 repeat; |
- | |
231 | u32 timeout; |
- | |
232 | 216 | u8 repeat; |
|
Line 233... | Line 217... | ||
233 | UART1_PutString("\r\n Getting Version from MK3MAG"); |
217 | u32 timeout; |
234 | // stop polling of other commands |
218 | |
235 | MK3MAG_StopPolling = 1; |
219 | MK3MAG_Present = 0; |
236 | 220 | ||
- | 221 | MK3MAG_Version.Major = 0xFF; |
|
- | 222 | MK3MAG_Version.Minor = 0xFF; |
|
- | 223 | MK3MAG_Version.Patch = 0xFF; |
|
237 | MK3MAG_Version.Major = 0xFF; |
224 | MK3MAG_Version.Compatible = 0xFF; |
238 | MK3MAG_Version.Minor = 0xFF; |
225 | |
239 | MK3MAG_Version.Patch = 0xFF; |
226 | CompassHeading = -1; |
240 | MK3MAG_Version.Compatible = 0xFF; |
227 | |
241 | // polling of version info |
228 | // polling of version info |
Line 252... | Line 239... | ||
252 | repeat++; |
239 | repeat++; |
253 | }while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s |
240 | }while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s |
254 | // if we got it |
241 | // if we got it |
255 | if (MK3MAG_Version.Major != 0xFF) |
242 | if (MK3MAG_Version.Major != 0xFF) |
256 | { |
243 | { |
257 | sprintf(msg, "\r\n MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch); |
244 | sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch); |
258 | UART1_PutString(msg); |
- | |
259 | sprintf(msg, " Compatible: %d", MK3MAG_Version.Compatible); |
- | |
260 | UART1_PutString(msg); |
245 | UART1_PutString(msg); |
- | 246 | if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) |
|
- | 247 | { |
|
- | 248 | UART1_PutString("\n\r MK3MAG not compatible!"); |
|
- | 249 | LED_RED_ON; |
|
- | 250 | } |
|
- | 251 | else |
|
- | 252 | { // version ok |
|
- | 253 | MK3MAG_Present = 1; |
|
- | 254 | } |
|
261 | } |
255 | } |
262 | else UART1_PutString("\n\r No version information from MK3Mag."); |
256 | return(MK3MAG_Present); |
263 | - | ||
264 | MK3MAG_StopPolling = 0; // enable polling of heading command |
- | |
265 | } |
257 | } |
Line 266... | Line 258... | ||
266 | 258 | ||
267 | //---------------------------------------------------------------- |
259 | //---------------------------------------------------------------- |
268 | void MK3MAG_UpdateCompass(void) |
260 | void MK3MAG_UpdateCompass(void) |
269 | { |
261 | { |
Line 270... | Line 262... | ||
270 | static u32 TimerCompassUpdate = 0; |
262 | static u32 TimerCompassUpdate = 0; |
Line 271... | Line 263... | ||
271 | 263 | ||
272 | if( (I2C_State == I2C_STATE_OFF) || MK3MAG_StopPolling ) return; |
264 | if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return; |
273 | 265 | ||
274 | if(CheckDelay(TimerCompassUpdate)) |
266 | if(CheckDelay(TimerCompassUpdate)) |