Rev 255 | Rev 261 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 255 | Rev 256 | ||
---|---|---|---|
Line 127... | Line 127... | ||
127 | sprintf(msg,"\n\r NaviCtrl V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH); |
127 | sprintf(msg,"\n\r NaviCtrl V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH); |
128 | UART1_PutString(msg); |
128 | UART1_PutString(msg); |
129 | } |
129 | } |
Line 130... | Line 130... | ||
130 | 130 | ||
131 | //---------------------------------------------------------------------------------------------------- |
- | |
132 | #define ERROR_FC_COMMUNICATION 0x01 |
- | |
133 | #define ERROR_MK3_COMMUNICATION 0x02 |
- | |
134 | #define ERROR_FC_INCOMPATIBLE 0x04 |
- | |
135 | #define ERROR_MK3_INCOMPATIBLE 0x08 |
- | |
136 | #define ERROR_GPS_COMMUNICATION 0x10 |
- | |
137 | #define ERROR_COMPASS_VALUE 0x20 |
- | |
Line 138... | Line 131... | ||
138 | //---------------------------------------------------------------------------------------------------- |
131 | //---------------------------------------------------------------------------------------------------- |
139 | 132 | ||
140 | void CheckErrors(void) |
133 | void CheckErrors(void) |
Line 153... | Line 146... | ||
153 | sprintf(ErrorMSG,"no FC communication "); |
146 | sprintf(ErrorMSG,"no FC communication "); |
154 | ErrorCode = 3; |
147 | ErrorCode = 3; |
155 | StopNavigation = 1; |
148 | StopNavigation = 1; |
156 | DebugOut.Status[0] &= ~0x01; // status of FC Present |
149 | DebugOut.Status[0] &= ~0x01; // status of FC Present |
157 | DebugOut.Status[0] &= ~0x02; // status of BL Present |
150 | DebugOut.Status[0] &= ~0x02; // status of BL Present |
158 | UART_VersionInfo.HardwareError[0] |= ERROR_FC_COMMUNICATION; |
151 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX; |
159 | } |
152 | } |
160 | else if(CheckDelay(I2C1_Timeout)) |
153 | else if(CheckDelay(I2C1_Timeout)) |
161 | { |
154 | { |
162 | LED_RED_ON; |
155 | LED_RED_ON; |
163 | sprintf(ErrorMSG,"no MK3Mag communication "); |
156 | sprintf(ErrorMSG,"no compass communication "); |
164 | //Reset I2CBus |
157 | //Reset I2CBus |
165 | I2C1_Deinit(); |
158 | I2C1_Deinit(); |
166 | I2C1_Init(); |
159 | I2C1_Init(); |
167 | ErrorCode = 4; |
160 | ErrorCode = 4; |
168 | StopNavigation = 1; |
161 | StopNavigation = 1; |
169 | UART_VersionInfo.HardwareError[0] |= ERROR_MK3_COMMUNICATION; |
162 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
170 | DebugOut.Status[1] |= 0x08; |
163 | DebugOut.Status[1] |= 0x08; |
171 | } |
164 | } |
172 | else if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
165 | else if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
173 | { |
166 | { |
174 | LED_RED_ON; |
167 | LED_RED_ON; |
175 | sprintf(ErrorMSG,"FC not compatible "); |
168 | sprintf(ErrorMSG,"FC not compatible "); |
176 | ErrorCode = 1; |
169 | ErrorCode = 1; |
177 | StopNavigation = 1; |
170 | StopNavigation = 1; |
178 | UART_VersionInfo.HardwareError[0] |= ERROR_FC_INCOMPATIBLE; |
171 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE; |
179 | } |
172 | } |
Line 180... | Line 173... | ||
180 | 173 | ||
181 | else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
174 | else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
182 | { |
175 | { |
Line 242... | Line 235... | ||
242 | { |
235 | { |
243 | sprintf(ErrorMSG,"MK3Mag not compatible "); |
236 | sprintf(ErrorMSG,"MK3Mag not compatible "); |
244 | LED_RED_ON; |
237 | LED_RED_ON; |
245 | ErrorCode = 2; |
238 | ErrorCode = 2; |
246 | StopNavigation = 1; |
239 | StopNavigation = 1; |
247 | UART_VersionInfo.HardwareError[0] |= ERROR_MK3_INCOMPATIBLE; |
240 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE; |
248 | } */ |
241 | } */ |
249 | else if(CheckDelay(UBX_Timeout)) |
242 | else if(CheckDelay(UBX_Timeout)) |
250 | { |
243 | { |
251 | LED_RED_ON; |
244 | LED_RED_ON; |
252 | sprintf(ErrorMSG,"no GPS communication "); |
245 | sprintf(ErrorMSG,"no GPS communication "); |
253 | ErrorCode = 5; |
246 | ErrorCode = 5; |
254 | StopNavigation = 1; |
247 | StopNavigation = 1; |
255 | UART_VersionInfo.HardwareError[0] |= ERROR_GPS_COMMUNICATION; |
248 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX; |
256 | } |
249 | } |
257 | else if(Compass_Heading < 0) |
250 | else if(Compass_Heading < 0) |
258 | { |
251 | { |
259 | LED_RED_ON; |
252 | LED_RED_ON; |
260 | sprintf(ErrorMSG,"bad compass value "); |
253 | sprintf(ErrorMSG,"bad compass value "); |
261 | ErrorCode = 6; |
254 | ErrorCode = 6; |
262 | StopNavigation = 1; |
255 | StopNavigation = 1; |
263 | UART_VersionInfo.HardwareError[0] |= ERROR_COMPASS_VALUE; |
256 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
264 | } |
257 | } |
265 | else if((FC.Error[1] & FC_ERROR1_SPI_RX)) |
258 | else if((FC.Error[1] & FC_ERROR1_SPI_RX)) |
266 | { |
259 | { |
267 | LED_RED_ON; |
260 | LED_RED_ON; |
268 | sprintf(ErrorMSG,"FC spi rx error "); |
261 | sprintf(ErrorMSG,"FC spi rx error "); |
Line 366... | Line 359... | ||
366 | LED_RED_ON; |
359 | LED_RED_ON; |
367 | } |
360 | } |
Line 368... | Line 361... | ||
368 | 361 | ||
Line 369... | Line -... | ||
369 | GPS_Init(); |
- | |
370 | - | ||
371 | /*{ |
- | |
372 | u8 i; |
- | |
373 | u8 text[20]; |
- | |
374 | u8 mesg[10]; |
- | |
375 | UART1_PutString("\r\nFill\r\n"); |
- | |
376 | for(i=0;i<20;i++) |
- | |
377 | { |
- | |
378 | text[i] = 0xAA; |
- | |
379 | sprintf(mesg,"%02X ", text[i]); |
- | |
380 | UART1_PutString(mesg); |
- | |
381 | } |
- | |
382 | - | ||
383 | sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(50, text, 18)); |
- | |
384 | UART1_PutString(mesg); |
- | |
385 | for(i=0;i<20;i++) |
- | |
386 | { |
- | |
387 | sprintf(mesg,"%02X ", text[i]); |
- | |
388 | UART1_PutString(mesg); |
- | |
389 | text[i] = i; |
- | |
390 | } |
- | |
391 | UART1_PutString("\r\nFill\r\n"); |
- | |
392 | for(i=0;i<20;i++) |
- | |
393 | { |
- | |
394 | sprintf(mesg,"%02X ", text[i]); |
- | |
395 | UART1_PutString(mesg); |
- | |
396 | } |
- | |
397 | sprintf(mesg, "\r\nEEWrite = %d\r\n", EEPROM_WriteBlock(0, text, 18)); |
- | |
398 | UART1_PutString(mesg); |
- | |
399 | for(i=0;i<20;i++) |
- | |
400 | { |
- | |
401 | sprintf(mesg,"%02X ", text[i]); |
- | |
402 | UART1_PutString(mesg); |
- | |
403 | text[i]=0xAA; |
- | |
404 | } |
- | |
405 | UART1_PutString("\r\nFill\r\n"); |
- | |
406 | for(i=0;i<20;i++) |
- | |
407 | { |
- | |
408 | sprintf(mesg,"%02X ", text[i]); |
- | |
409 | UART1_PutString(mesg); |
- | |
410 | } |
- | |
411 | sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(0, text, 18)); |
- | |
412 | UART1_PutString(mesg); |
- | |
413 | for(i=0;i<20;i++) |
- | |
414 | { |
- | |
415 | sprintf(mesg,"%02X ", text[i]); |
- | |
416 | UART1_PutString(mesg); |
- | |
417 | } |
- | |
418 | }*/ |
362 | GPS_Init(); |
419 | 363 | ||
420 | // ---------- Prepare the isr driven |
364 | // ---------- Prepare the isr driven |
421 | // set to absolute lowest priority |
365 | // set to absolute lowest priority |
422 | VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW); |
366 | VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW); |
Line 423... | Line 367... | ||
423 | // enable interrupts |
367 | // enable interrupts |
424 | VIC_ITCmd(EXTIT3_ITLine, ENABLE); |
- | |
425 | 368 | VIC_ITCmd(EXTIT3_ITLine, ENABLE); |
|
426 | Debug_OK("START"); |
369 | |
427 | 370 | Debug_OK("START"); |
|
428 | 371 | ||
429 | for (;;) // the endless main loop |
372 | for (;;) // the endless main loop |
430 | { |
373 | { |