Rev 218 | Rev 223 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 218 | Rev 222 | ||
|---|---|---|---|
| Line 98... | Line 98... | ||
| 98 | s32 Kalman_MaxFusion = 64; |
98 | s32 Kalman_MaxFusion = 64; |
| 99 | s32 ToFcGpsZ = 0; |
99 | s32 ToFcGpsZ = 0; |
| Line 100... | Line 100... | ||
| 100 | 100 | ||
| 101 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN}; |
101 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN}; |
| - | 102 | u8 SPI_CommandCounter = 0; |
|
| - | 103 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
|
| Line 102... | Line 104... | ||
| 102 | u8 SPI_CommandCounter = 0; |
104 | s32 HeadFreeStartAngle = 0; |
| Line 103... | Line 105... | ||
| 103 | 105 | ||
| 104 | SPI_Version_t FC_Version; |
106 | SPI_Version_t FC_Version; |
| Line 266... | Line 268... | ||
| 266 | SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT); |
268 | SPI0_Timeout = SetDelay(4*SPI0_TIMEOUT); |
| Line 267... | Line 269... | ||
| 267 | 269 | ||
| 268 | UART1_PutString("ok"); |
270 | UART1_PutString("ok"); |
| Line -... | Line 271... | ||
| - | 271 | } |
|
| 269 | } |
272 | |
| 270 | 273 | ||
| 271 | //------------------------------------------------------ |
274 | //------------------------------------------------------ |
| 272 | void SPI0_UpdateBuffer(void) |
275 | void SPI0_UpdateBuffer(void) |
| 273 | { |
276 | { |
| Line 278... | Line 281... | ||
| 278 | { |
281 | { |
| 279 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
282 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
| 280 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
283 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
| 281 | ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
284 | ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
| 282 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
285 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
| - | 286 | ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
|
| 283 | // cycle spi commands |
287 | // cycle spi commands |
| 284 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
288 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| 285 | // restart command cycle at the end |
289 | // restart command cycle at the end |
| 286 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
290 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| Line 287... | Line 291... | ||
| 287 | 291 | ||
| 288 | switch (ToFlightCtrl.Command) |
292 | switch (ToFlightCtrl.Command) |
| 289 | { |
293 | { |
| - | 294 | case SPI_NCCMD_KALMAN: |
|
| 290 | case SPI_NCCMD_KALMAN: |
295 | CalcHeadFree(); |
| 291 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
296 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
| 292 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
297 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
| 293 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
298 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
| 294 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
299 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
| - | 300 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
|
| - | 301 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
|
| 295 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
302 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
| Line 296... | Line 303... | ||
| 296 | break; |
303 | break; |
| 297 | 304 | ||
| 298 | case SPI_NCCMD_VERSION: |
305 | case SPI_NCCMD_VERSION: |
| Line 335... | Line 342... | ||
| 335 | } |
342 | } |
| 336 | FC.Flags |= FromFlightCtrl.Param.Byte[8]; |
343 | FC.Flags |= FromFlightCtrl.Param.Byte[8]; |
| 337 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
344 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
| 338 | DebugOut.Analog[5] = FC.Flags; |
345 | DebugOut.Analog[5] = FC.Flags; |
| 339 | NaviData.FCFlags = FC.Flags; |
346 | NaviData.FCFlags = FC.Flags; |
| - | 347 | HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; |
|
| 340 | break; |
348 | break; |
| Line 341... | Line 349... | ||
| 341 | 349 | ||
| 342 | case SPI_FCCMD_ACCU: |
350 | case SPI_FCCMD_ACCU: |
| 343 | FC.BAT_Current = FromFlightCtrl.Param.Int[0]; |
351 | FC.BAT_Current = FromFlightCtrl.Param.Int[0]; |