Rev 110 | Rev 119 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 110 | Rev 118 | ||
---|---|---|---|
Line 99... | Line 99... | ||
99 | s32 FC_Kalman_K = 32; |
99 | s32 FC_Kalman_K = 32; |
100 | s32 Kalman_MaxDrift = 5 * 16; |
100 | s32 Kalman_MaxDrift = 5 * 16; |
101 | s32 Kalman_MaxFusion = 64; |
101 | s32 Kalman_MaxFusion = 64; |
102 | u32 CheckSPIOkay = 0; |
102 | u32 CheckSPIOkay = 0; |
Line 103... | Line 103... | ||
103 | 103 | ||
104 | u8 SPI_CommandSequence[] = { SPI_KALMAN };//, SPI_CMD_OSD_DATA, SPI_CMD_GPS_POS, SPI_KALMAN, SPI_CMD_GPS_TARGET}; |
104 | u8 SPI_CommandSequence[] = { SPI_KALMAN, SPI_EXTCTRL }; |
Line 105... | Line 105... | ||
105 | u8 SPI_CommandCounter = 0; |
105 | u8 SPI_CommandCounter = 0; |
Line 106... | Line 106... | ||
106 | 106 | ||
Line 286... | Line 286... | ||
286 | ToFlightCtrl.GPS_Nick = GPS_Stick.Nick; |
286 | ToFlightCtrl.GPS_Nick = GPS_Stick.Nick; |
287 | ToFlightCtrl.GPS_Roll = GPS_Stick.Roll; |
287 | ToFlightCtrl.GPS_Roll = GPS_Stick.Roll; |
288 | ToFlightCtrl.GPS_Yaw = GPS_Stick.Yaw; |
288 | ToFlightCtrl.GPS_Yaw = GPS_Stick.Yaw; |
289 | // cycle spi commands |
289 | // cycle spi commands |
290 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
290 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
- | 291 | if(ToFlightCtrl.Command == SPI_EXTCTRL) |
|
- | 292 | { |
|
- | 293 | // if no new data are available skip this command in sequence |
|
- | 294 | if(ExternControl.Status != NEWDATA) ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
|
- | 295 | } |
|
- | 296 | // restart command cycle at the end |
|
291 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
297 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
Line 292... | Line 298... | ||
292 | 298 | ||
293 | switch (ToFlightCtrl.Command) |
299 | switch (ToFlightCtrl.Command) |
294 | { |
300 | { |
295 | case SPI_KALMAN: |
301 | case SPI_KALMAN: |
296 | ToFlightCtrl.Param.sByte[0] = (s8) FC_Kalman_K; |
302 | ToFlightCtrl.Param.sByte[0] = (s8) FC_Kalman_K; |
297 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
303 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
298 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
304 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
299 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
305 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
Line -... | Line 306... | ||
- | 306 | break; |
|
- | 307 | ||
- | 308 | case SPI_EXTCTRL: |
|
- | 309 | ToFlightCtrl.Param.Byte[0] = ExternControl.Digital[0]; |
|
- | 310 | ToFlightCtrl.Param.Byte[1] = ExternControl.Digital[1]; |
|
- | 311 | ToFlightCtrl.Param.Byte[2] = ExternControl.RemoteButtons; |
|
- | 312 | ToFlightCtrl.Param.sByte[3] = ExternControl.Nick; |
|
- | 313 | ToFlightCtrl.Param.sByte[4] = ExternControl.Roll; |
|
- | 314 | ToFlightCtrl.Param.sByte[5] = ExternControl.Yaw; |
|
- | 315 | ToFlightCtrl.Param.Byte[6] = ExternControl.Gas; |
|
- | 316 | ToFlightCtrl.Param.sByte[7] = ExternControl.Height; |
|
- | 317 | ToFlightCtrl.Param.Byte[8] = ExternControl.free; |
|
- | 318 | ToFlightCtrl.Param.Byte[9] = ExternControl.Frame; |
|
- | 319 | ToFlightCtrl.Param.Byte[10] = ExternControl.Config; |
|
- | 320 | ExternControl.Status = PROCESSED; |
|
300 | break; |
321 | break; |
301 | 322 | ||
302 | default: |
323 | default: |
303 | break; |
324 | break; |