Rev 421 | Rev 432 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 421 | Rev 426 | ||
|---|---|---|---|
| Line 67... | Line 67... | ||
| 67 | #include "config.h" |
67 | #include "config.h" |
| 68 | #include "main.h" |
68 | #include "main.h" |
| 69 | #include "compass.h" |
69 | #include "compass.h" |
| 70 | #include "params.h" |
70 | #include "params.h" |
| 71 | #include "stdlib.h" |
71 | #include "stdlib.h" |
| - | 72 | #include "settings.h" |
|
| Line 72... | Line 73... | ||
| 72 | 73 | ||
| 73 | #define SPI_RXSYNCBYTE1 0xAA |
74 | #define SPI_RXSYNCBYTE1 0xAA |
| 74 | #define SPI_RXSYNCBYTE2 0x83 |
75 | #define SPI_RXSYNCBYTE2 0x83 |
| 75 | #define SPI_TXSYNCBYTE1 0x81 |
76 | #define SPI_TXSYNCBYTE1 0x81 |
| Line 283... | Line 284... | ||
| 283 | 284 | ||
| 284 | //------------------------------------------------------ |
285 | //------------------------------------------------------ |
| 285 | void SPI0_UpdateBuffer(void) |
286 | void SPI0_UpdateBuffer(void) |
| 286 | { |
287 | { |
| 287 | static u32 timeout = 0; |
288 | static u32 timeout = 0; |
| 288 | static u8 counter = 50,hott_index = 0, last_error_code = 0; |
289 | static u8 counter = 50,hott_index = 0, last_error_code = 0, enable_injecting = 0; |
| - | 290 | static s16 last_wp_event = 0; |
|
| 289 | s16 tmp, last_wp_event = 0; |
291 | s16 tmp; |
| Line 290... | Line 292... | ||
| 290 | s32 i1,i2; |
292 | s32 i1,i2; |
| 291 | 293 | ||
| 292 | SPIWatchDog = 3500; // stop communication to FC after this timeout |
294 | SPIWatchDog = 3500; // stop communication to FC after this timeout |
| Line 300... | Line 302... | ||
| 300 | ToFlightCtrl.MagVecX = MagVector.X; |
302 | ToFlightCtrl.MagVecX = MagVector.X; |
| 301 | ToFlightCtrl.MagVecY = MagVector.Y; |
303 | ToFlightCtrl.MagVecY = MagVector.Y; |
| 302 | ToFlightCtrl.MagVecZ = MagVector.Z; |
304 | ToFlightCtrl.MagVecZ = MagVector.Z; |
| 303 | // ToFlightCtrl.NCStatus = 0; |
305 | // ToFlightCtrl.NCStatus = 0; |
| 304 | // cycle spi commands |
306 | // cycle spi commands |
| - | 307 | if(ErrorCode != last_error_code && enable_injecting) |
|
| - | 308 | { |
|
| 305 | if(ErrorCode != last_error_code) ToFlightCtrl.Command = SPI_NCCMD_VERSION; |
309 | ToFlightCtrl.Command = SPI_NCCMD_VERSION; |
| - | 310 | last_error_code = ErrorCode; |
|
| - | 311 | enable_injecting = 0; |
|
| - | 312 | } |
|
| 306 | else |
313 | else |
| 307 | if(FC_WP_EventChannel != last_wp_event) ToFlightCtrl.Command = SPI_NCCMD_GPSINFO; |
314 | if(FC_WP_EventChannel != last_wp_event && enable_injecting) |
| - | 315 | { |
|
| - | 316 | ToFlightCtrl.Command = SPI_NCCMD_GPSINFO; |
|
| - | 317 | last_wp_event = FC_WP_EventChannel; |
|
| - | 318 | enable_injecting = 0; |
|
| - | 319 | } |
|
| 308 | else |
320 | else |
| 309 | { |
321 | { |
| 310 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
322 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| 311 | // restart command cycle at the end |
323 | // restart command cycle at the end |
| 312 | if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
324 | if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| - | 325 | if(ToFlightCtrl.Command == SPI_NCCMD_KALMAN) enable_injecting = 1; |
|
| 313 | } |
326 | } |
| 314 | last_error_code = ErrorCode; |
- | |
| 315 | last_wp_event = FC_WP_EventChannel; |
- | |
| Line 316... | Line 327... | ||
| 316 | 327 | ||
| 317 | #define FLAG_GPS_AID 0x01 |
328 | #define FLAG_GPS_AID 0x01 |
| 318 | switch (ToFlightCtrl.Command) |
329 | switch (ToFlightCtrl.Command) |
| 319 | { |
330 | { |
| Line 377... | Line 388... | ||
| 377 | ToFlightCtrl.Param.Int[3] = NaviData.TargetPositionDeviation.Distance / 10; |
388 | ToFlightCtrl.Param.Int[3] = NaviData.TargetPositionDeviation.Distance / 10; |
| 378 | ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
389 | ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
| 379 | ToFlightCtrl.Param.Byte[9] = 0; |
390 | ToFlightCtrl.Param.Byte[9] = 0; |
| 380 | ToFlightCtrl.Param.Byte[10] = 0; |
391 | ToFlightCtrl.Param.Byte[10] = 0; |
| 381 | ToFlightCtrl.Param.Byte[11] = 0; |
392 | ToFlightCtrl.Param.Byte[11] = 0; |
| 382 | //DebugOut.Analog[16] = SpeakHoTT; |
- | |
| 383 | SpeakHoTT = 0; |
393 | SpeakHoTT = 0; |
| 384 | break; |
394 | break; |
| Line 385... | Line 395... | ||
| 385 | 395 | ||
| 386 | case SPI_NCCMD_GPSINFO: |
396 | case SPI_NCCMD_GPSINFO: |
| Line 571... | Line 581... | ||
| 571 | } |
581 | } |
| 572 | NaviData.UBat = FC.BAT_Voltage; |
582 | NaviData.UBat = FC.BAT_Voltage; |
| 573 | NaviData.Current = FC.BAT_Current; |
583 | NaviData.Current = FC.BAT_Current; |
| 574 | NaviData.UsedCapacity = FC.BAT_UsedCapacity; |
584 | NaviData.UsedCapacity = FC.BAT_UsedCapacity; |
| 575 | break; |
585 | break; |
| 576 | - | ||
| 577 | #define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; } |
- | |
| 578 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); } |
- | |
| 579 | - | ||
| 580 | case SPI_FCCMD_PARAMETER1: |
586 | case SPI_FCCMD_PARAMETER1: |
| 581 | CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255); |
587 | CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255); |
| 582 | CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255); |
588 | CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255); |
| 583 | CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255); |
589 | CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255); |
| 584 | CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255); |
590 | CHK_POTI_MM(Parameter.NaviGpsI,FromFlightCtrl.Param.Byte[3],0,255); |
| Line 605... | Line 611... | ||
| 605 | FC.Poti[3] = FromFlightCtrl.Param.Byte[7]; |
611 | FC.Poti[3] = FromFlightCtrl.Param.Byte[7]; |
| 606 | FC.Poti[4] = FromFlightCtrl.Param.Byte[8]; |
612 | FC.Poti[4] = FromFlightCtrl.Param.Byte[8]; |
| 607 | FC.Poti[5] = FromFlightCtrl.Param.Byte[9]; |
613 | FC.Poti[5] = FromFlightCtrl.Param.Byte[9]; |
| 608 | FC.Poti[6] = FromFlightCtrl.Param.Byte[10]; |
614 | FC.Poti[6] = FromFlightCtrl.Param.Byte[10]; |
| 609 | FC.Poti[7] = FromFlightCtrl.Param.Byte[11]; |
615 | FC.Poti[7] = FromFlightCtrl.Param.Byte[11]; |
| - | 616 | CHK_POTI_MM(WaypointAcceleration,WaypointAccelerationSetting,0,255); // that could be a Poti-Value |
|
| 610 | break; |
617 | break; |
| Line 611... | Line 618... | ||
| 611 | 618 | ||
| 612 | case SPI_FCCMD_MISC: |
619 | case SPI_FCCMD_MISC: |
| 613 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
620 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |