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