Rev 254 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 254 | Rev 255 | ||
---|---|---|---|
Line 90... | Line 90... | ||
90 | #define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl) |
90 | #define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl) |
91 | volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
91 | volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
92 | volatile u8 SPI_RxBufferIndex = 0; |
92 | volatile u8 SPI_RxBufferIndex = 0; |
93 | volatile u8 SPI_RxBuffer_Request = 0; |
93 | volatile u8 SPI_RxBuffer_Request = 0; |
94 | #define SPI_COMMAND_INDEX 0 |
94 | #define SPI_COMMAND_INDEX 0 |
95 | volatile u8 FC_ErrorCode[5] = {0,0,0,0,0}; |
- | |
Line 96... | Line 95... | ||
96 | 95 | ||
97 | s32 Kalman_K = 32; |
96 | s32 Kalman_K = 32; |
98 | s32 Kalman_MaxDrift = 5 * 16; |
97 | s32 Kalman_MaxDrift = 5 * 16; |
99 | s32 Kalman_MaxFusion = 64; |
98 | s32 Kalman_MaxFusion = 64; |
Line 104... | Line 103... | ||
104 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
103 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
105 | s32 HeadFreeStartAngle = 0; |
104 | s32 HeadFreeStartAngle = 0; |
Line 106... | Line 105... | ||
106 | 105 | ||
Line 107... | Line -... | ||
107 | SPI_Version_t FC_Version; |
- | |
108 | - | ||
109 | 106 | SPI_Version_t FC_Version; |
|
110 | 107 | ||
111 | //-------------------------------------------------------------- |
108 | //-------------------------------------------------------------- |
112 | void SSP0_IRQHandler(void) |
109 | void SSP0_IRQHandler(void) |
113 | { |
110 | { |
Line 333... | Line 330... | ||
333 | Parameter.User4 = FromFlightCtrl.Param.Byte[3]; |
330 | Parameter.User4 = FromFlightCtrl.Param.Byte[3]; |
334 | Parameter.User5 = FromFlightCtrl.Param.Byte[4]; |
331 | Parameter.User5 = FromFlightCtrl.Param.Byte[4]; |
335 | Parameter.User6 = FromFlightCtrl.Param.Byte[5]; |
332 | Parameter.User6 = FromFlightCtrl.Param.Byte[5]; |
336 | Parameter.User7 = FromFlightCtrl.Param.Byte[6]; |
333 | Parameter.User7 = FromFlightCtrl.Param.Byte[6]; |
337 | Parameter.User8 = FromFlightCtrl.Param.Byte[7]; |
334 | Parameter.User8 = FromFlightCtrl.Param.Byte[7]; |
338 | if(ClearFCFlags) |
335 | if(ClearFCStatusFlags) |
339 | { |
336 | { |
340 | FC.Flags = 0; |
337 | FC.StatusFlags = 0; |
341 | ClearFCFlags = 0; |
338 | ClearFCStatusFlags = 0; |
342 | } |
339 | } |
343 | FC.Flags |= FromFlightCtrl.Param.Byte[8]; |
340 | FC.StatusFlags |= FromFlightCtrl.Param.Byte[8]; |
344 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
341 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
345 | DebugOut.Analog[5] = FC.Flags; |
342 | DebugOut.Analog[5] = FC.StatusFlags; |
346 | NaviData.FCFlags = FC.Flags; |
343 | NaviData.FCStatusFlags = FC.StatusFlags; |
347 | HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; // convert to 0.1° |
344 | HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; // convert to 0.1° |
348 | break; |
345 | break; |
Line 349... | Line 346... | ||
349 | 346 | ||
350 | case SPI_FCCMD_ACCU: |
347 | case SPI_FCCMD_ACCU: |
Line 427... | Line 424... | ||
427 | FC_Version.Major = FromFlightCtrl.Param.Byte[0]; |
424 | FC_Version.Major = FromFlightCtrl.Param.Byte[0]; |
428 | FC_Version.Minor = FromFlightCtrl.Param.Byte[1]; |
425 | FC_Version.Minor = FromFlightCtrl.Param.Byte[1]; |
429 | FC_Version.Patch = FromFlightCtrl.Param.Byte[2]; |
426 | FC_Version.Patch = FromFlightCtrl.Param.Byte[2]; |
430 | FC_Version.Compatible = FromFlightCtrl.Param.Byte[3]; |
427 | FC_Version.Compatible = FromFlightCtrl.Param.Byte[3]; |
431 | FC_Version.Hardware = FromFlightCtrl.Param.Byte[4]; |
428 | FC_Version.Hardware = FromFlightCtrl.Param.Byte[4]; |
432 | FC_ErrorCode[0] = FromFlightCtrl.Param.Byte[5]; |
429 | FC.Error[0] = FromFlightCtrl.Param.Byte[5]; |
433 | FC_ErrorCode[1] = FromFlightCtrl.Param.Byte[6]; |
430 | FC.Error[1] = FromFlightCtrl.Param.Byte[6]; |
434 | FC_ErrorCode[2] = FromFlightCtrl.Param.Byte[7]; |
431 | FC.Error[2] = FromFlightCtrl.Param.Byte[7]; |
- | 432 | FC.Error[3] = FromFlightCtrl.Param.Byte[8]; |
|
- | 433 | FC.Error[4] = FromFlightCtrl.Param.Byte[9]; |
|
435 | DebugOut.Status[0] |= 0x01; // status of FC Present |
434 | DebugOut.Status[0] |= 0x01; // status of FC Present |
436 | DebugOut.Status[0] |= 0x02; // status of BL Present |
435 | DebugOut.Status[0] |= 0x02; // status of BL Present |
437 | if(FC_ErrorCode[0] || FC_ErrorCode[1] || FC_ErrorCode[2]) DebugOut.Status[1] |= 0x01; |
436 | if(FC.Error[0] || FC.Error[1] || FC.Error[2] || FC.Error[3] || FC.Error[4]) DebugOut.Status[1] |= 0x01; |
438 | else DebugOut.Status[1] &= ~0x01; |
437 | else DebugOut.Status[1] &= ~0x01; |
439 | break; |
438 | break; |
440 | default: |
439 | default: |
441 | break; |
440 | break; |
442 | } |
441 | } |
Line 443... | Line 442... | ||
443 | 442 | ||
444 | // every time we got new data from the FC via SPI call the navigation routine |
443 | // every time we got new data from the FC via SPI call the navigation routine |
445 | // and update GPSStick that are returned to FC |
444 | // and update GPSStick that are returned to FC |
446 | GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); |
445 | GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); |
Line 447... | Line 446... | ||
447 | ClearFCFlags = 1; |
446 | ClearFCStatusFlags = 1; |
448 | 447 | ||
449 | if(counter) |
448 | if(counter) |
450 | { |
449 | { |