Subversion Repositories NaviCtrl

Rev

Rev 254 | Rev 258 | 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 
                {