Rev 222 | Rev 228 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 222 | Rev 223 | ||
---|---|---|---|
Line 281... | Line 281... | ||
281 | { |
281 | { |
282 | // 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 |
283 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
283 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
284 | ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
284 | ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
285 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
285 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
286 | ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
286 | ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
287 | // cycle spi commands |
287 | // cycle spi commands |
288 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
288 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
289 | // restart command cycle at the end |
289 | // restart command cycle at the end |
290 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
290 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
Line 291... | Line 291... | ||
291 | 291 | ||
292 | switch (ToFlightCtrl.Command) |
292 | switch (ToFlightCtrl.Command) |
293 | { |
293 | { |
294 | case SPI_NCCMD_KALMAN: |
294 | case SPI_NCCMD_KALMAN: |
295 | CalcHeadFree(); |
295 | CalcHeadFree(); |
296 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
296 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
297 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
297 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
298 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
298 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
299 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
299 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
300 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
300 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
301 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
301 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
302 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
302 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
Line 303... | Line 303... | ||
303 | break; |
303 | break; |
304 | 304 | ||
305 | case SPI_NCCMD_VERSION: |
305 | case SPI_NCCMD_VERSION: |
306 | ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR; |
306 | ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR; |
307 | ToFlightCtrl.Param.Byte[1] = VERSION_MINOR; |
307 | ToFlightCtrl.Param.Byte[1] = VERSION_MINOR; |
308 | ToFlightCtrl.Param.Byte[2] = VERSION_PATCH; |
308 | ToFlightCtrl.Param.Byte[2] = VERSION_PATCH; |
309 | ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE; |
309 | ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE; |
310 | ToFlightCtrl.Param.Byte[4] = BoardRelease; |
310 | ToFlightCtrl.Param.Byte[4] = BoardRelease; |
311 | break; |
311 | break; |
312 | 312 | ||
313 | case SPI_NCCMD_GPSINFO: |
313 | case SPI_NCCMD_GPSINFO: |
314 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
314 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
315 | ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats; |
315 | ToFlightCtrl.Param.Byte[1] = GPSData.NumOfSats; |
Line 338... | Line 338... | ||
338 | if(ClearFCFlags) |
338 | if(ClearFCFlags) |
339 | { |
339 | { |
340 | FC.Flags = 0; |
340 | FC.Flags = 0; |
341 | ClearFCFlags = 0; |
341 | ClearFCFlags = 0; |
342 | } |
342 | } |
343 | FC.Flags |= FromFlightCtrl.Param.Byte[8]; |
343 | FC.Flags |= FromFlightCtrl.Param.Byte[8]; |
344 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
344 | Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9]; |
345 | DebugOut.Analog[5] = FC.Flags; |
345 | DebugOut.Analog[5] = FC.Flags; |
346 | NaviData.FCFlags = FC.Flags; |
346 | NaviData.FCFlags = FC.Flags; |
347 | HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; |
347 | HeadFreeStartAngle = (s32) FromFlightCtrl.Param.Byte[10] * 20; // convert to 0.1° |
348 | break; |
348 | break; |
Line 349... | Line 349... | ||
349 | 349 | ||
350 | case SPI_FCCMD_ACCU: |
350 | case SPI_FCCMD_ACCU: |
351 | FC.BAT_Current = FromFlightCtrl.Param.Int[0]; |
351 | FC.BAT_Current = FromFlightCtrl.Param.Int[0]; |
352 | FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1]; |
352 | FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1]; |
353 | FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4]; |
353 | FC.BAT_Voltage = FromFlightCtrl.Param.Byte[4]; |
354 | Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5]; |
354 | Parameter.LowVoltageWarning = FromFlightCtrl.Param.Byte[5]; |
355 | NaviData.UBat = FC.BAT_Voltage; |
355 | NaviData.UBat = FC.BAT_Voltage; |