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]; |