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]) |