Rev 119 | Rev 125 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 119 | Rev 121 | ||
|---|---|---|---|
| Line 100... | Line 100... | ||
| 100 | s32 FC_Kalman_K = 32; |
100 | s32 FC_Kalman_K = 32; |
| 101 | s32 Kalman_MaxDrift = 5 * 16; |
101 | s32 Kalman_MaxDrift = 5 * 16; |
| 102 | s32 Kalman_MaxFusion = 64; |
102 | s32 Kalman_MaxFusion = 64; |
| 103 | u32 CheckSPIOkay = 0; |
103 | u32 CheckSPIOkay = 0; |
| Line 104... | Line 104... | ||
| 104 | 104 | ||
| 105 | u8 SPI_CommandSequence[] = { SPI_KALMAN, SPI_EXTCTRL }; |
105 | u8 SPI_CommandSequence[] = { SPI_KALMAN }; |
| Line 106... | Line 106... | ||
| 106 | u8 SPI_CommandCounter = 0; |
106 | u8 SPI_CommandCounter = 0; |
| Line 107... | Line 107... | ||
| 107 | 107 | ||
| Line 287... | Line 287... | ||
| 287 | ToFlightCtrl.GPS_Nick = GPS_Stick.Nick; |
287 | ToFlightCtrl.GPS_Nick = GPS_Stick.Nick; |
| 288 | ToFlightCtrl.GPS_Roll = GPS_Stick.Roll; |
288 | ToFlightCtrl.GPS_Roll = GPS_Stick.Roll; |
| 289 | ToFlightCtrl.GPS_Yaw = GPS_Stick.Yaw; |
289 | ToFlightCtrl.GPS_Yaw = GPS_Stick.Yaw; |
| 290 | // cycle spi commands |
290 | // cycle spi commands |
| 291 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
291 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| 292 | if(ToFlightCtrl.Command == SPI_EXTCTRL) |
- | |
| 293 | { |
- | |
| 294 | // if no new data are available skip this command in sequence |
- | |
| 295 | if(ExternControl.Status != NEWDATA) ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
- | |
| 296 | } |
- | |
| 297 | // restart command cycle at the end |
292 | // restart command cycle at the end |
| 298 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
293 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| Line 299... | Line 294... | ||
| 299 | 294 | ||
| 300 | switch (ToFlightCtrl.Command) |
295 | switch (ToFlightCtrl.Command) |
| 301 | { |
296 | { |
| 302 | case SPI_KALMAN: |
297 | case SPI_KALMAN: |
| 303 | ToFlightCtrl.Param.sByte[0] = (s8) FC_Kalman_K; |
298 | ToFlightCtrl.Param.sByte[0] = (s8) FC_Kalman_K; |
| 304 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
299 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
| 305 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
300 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
| 306 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
301 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
| 307 | break; |
- | |
| 308 | - | ||
| 309 | case SPI_EXTCTRL: |
- | |
| 310 | ToFlightCtrl.Param.Byte[0] = ExternControl.Digital[0]; |
- | |
| 311 | ToFlightCtrl.Param.Byte[1] = ExternControl.Digital[1]; |
- | |
| 312 | ToFlightCtrl.Param.Byte[2] = ExternControl.RemoteButtons; |
- | |
| 313 | ToFlightCtrl.Param.sByte[3] = ExternControl.Nick; |
- | |
| 314 | ToFlightCtrl.Param.sByte[4] = ExternControl.Roll; |
- | |
| 315 | ToFlightCtrl.Param.sByte[5] = ExternControl.Yaw; |
- | |
| 316 | ToFlightCtrl.Param.Byte[6] = ExternControl.Gas; |
- | |
| 317 | ToFlightCtrl.Param.sByte[7] = ExternControl.Height; |
- | |
| 318 | ToFlightCtrl.Param.Byte[8] = ExternControl.free; |
- | |
| 319 | ToFlightCtrl.Param.Byte[9] = ExternControl.Frame; |
- | |
| 320 | ToFlightCtrl.Param.Byte[10] = ExternControl.Config; |
- | |
| 321 | ExternControl.Status = PROCESSED; |
- | |
| Line 322... | Line 302... | ||
| 322 | break; |
302 | break; |
| 323 | 303 | ||
| 324 | default: |
304 | default: |
| 325 | break; |
305 | break; |
| Line 362... | Line 342... | ||
| 362 | Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7]; |
342 | Parameter.NaviStickThreshold = FromFlightCtrl.Param.Byte[7]; |
| 363 | CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255); |
343 | CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255); |
| 364 | CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255); |
344 | CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255); |
| 365 | CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255); |
345 | CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255); |
| 366 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
346 | CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255); |
| 367 | // DebugOut.Analog[4] = Parameter.NaviRadiusAlert; |
- | |
| 368 | // DebugOut.Analog[5] = Parameter.NaviWindCorrection; |
- | |
| 369 | // DebugOut.Analog[6] = Parameter.NaviSpeedCompensation; |
- | |
| 370 | break; |
347 | break; |
| Line 371... | Line 348... | ||
| 371 | 348 | ||
| 372 | case SPI_CMD_STICK: |
349 | case SPI_CMD_STICK: |
| 373 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
350 | FC.StickGas = FromFlightCtrl.Param.sByte[0]; |
| 374 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
351 | FC.StickYaw = FromFlightCtrl.Param.sByte[1]; |
| Line 393... | Line 370... | ||
| 393 | CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[4],0,255); |
370 | CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[4],0,255); |
| 394 | CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[5],0,255); |
371 | CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[5],0,255); |
| 395 | CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[6],0,255); |
372 | CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[6],0,255); |
| 396 | break; |
373 | break; |
| Line 397... | Line 374... | ||
| 397 | 374 | ||
| 398 | case SPI_CMD_SERVO: |
375 | case SPI_CMD_SERVOS: |
| - | 376 | ServoParams.Refresh = FromFlightCtrl.Param.Byte[0]; |
|
| 399 | ServoParams.Refresh = FromFlightCtrl.Param.Byte[0]; |
377 | ServoParams.CompInvert = FromFlightCtrl.Param.Byte[1]; |
| 400 | ServoParams.NickOffset = FromFlightCtrl.Param.Byte[1]; |
378 | ServoParams.NickControl = FromFlightCtrl.Param.Byte[2]; |
| 401 | ServoParams.NickComp = FromFlightCtrl.Param.sByte[2]; |
379 | ServoParams.NickComp = FromFlightCtrl.Param.Byte[3]; |
| 402 | ServoParams.NickMin = FromFlightCtrl.Param.Byte[3]; |
380 | ServoParams.NickMin = FromFlightCtrl.Param.Byte[4]; |
| 403 | ServoParams.NickMax = FromFlightCtrl.Param.Byte[4]; |
381 | ServoParams.NickMax = FromFlightCtrl.Param.Byte[5]; |
| 404 | ServoParams.RollOffset = FromFlightCtrl.Param.Byte[5]; |
382 | ServoParams.RollControl = FromFlightCtrl.Param.Byte[6]; |
| 405 | ServoParams.RollComp = FromFlightCtrl.Param.sByte[6]; |
383 | ServoParams.RollComp = FromFlightCtrl.Param.Byte[7]; |
| 406 | ServoParams.RollMin = FromFlightCtrl.Param.Byte[7]; |
384 | ServoParams.RollMin = FromFlightCtrl.Param.Byte[8]; |
| 407 | ServoParams.RollMax = FromFlightCtrl.Param.Byte[8]; |
385 | ServoParams.RollMax = FromFlightCtrl.Param.Byte[9]; |
| Line 408... | Line 386... | ||
| 408 | break; |
386 | break; |
| 409 | 387 | ||
| 410 | case SPI_CMD_VERSION: |
388 | case SPI_CMD_VERSION: |