Rev 119 | 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: |