100,6 → 100,8 |
|
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN}; |
u8 SPI_CommandCounter = 0; |
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
s32 HeadFreeStartAngle = 0; |
|
SPI_Version_t FC_Version; |
|
268,6 → 270,7 |
UART1_PutString("ok"); |
} |
|
|
//------------------------------------------------------ |
void SPI0_UpdateBuffer(void) |
{ |
280,6 → 283,7 |
VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
// cycle spi commands |
ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
// restart command cycle at the end |
288,11 → 292,14 |
switch (ToFlightCtrl.Command) |
{ |
case SPI_NCCMD_KALMAN: |
CalcHeadFree(); |
ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
break; |
|
case SPI_NCCMD_VERSION: |
337,6 → 344,7 |
Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
DebugOut.Analog[5] = FC.Flags; |
NaviData.FCFlags = FC.Flags; |
HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; |
break; |
|
case SPI_FCCMD_ACCU: |