Rev 244 | Rev 255 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 244 | Rev 254 | ||
|---|---|---|---|
| Line 64... | Line 64... | ||
| 64 | #include "compass.h" |
64 | #include "compass.h" |
| 65 | #include "timer1.h" |
65 | #include "timer1.h" |
| 66 | #include "timer2.h" |
66 | #include "timer2.h" |
| 67 | #include "config.h" |
67 | #include "config.h" |
| 68 | #include "main.h" |
68 | #include "main.h" |
| 69 | #include "fifo.h" |
- | |
| 70 | #include "compass.h" |
69 | #include "compass.h" |
| Line 71... | Line 70... | ||
| 71 | 70 | ||
| 72 | 71 | ||
| Line 105... | Line 104... | ||
| 105 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
104 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
| 106 | s32 HeadFreeStartAngle = 0; |
105 | s32 HeadFreeStartAngle = 0; |
| Line 107... | Line 106... | ||
| 107 | 106 | ||
| Line 108... | Line -... | ||
| 108 | SPI_Version_t FC_Version; |
- | |
| 109 | - | ||
| Line 110... | Line -... | ||
| 110 | u8 CompassCalStateQueue[10]; |
- | |
| Line 111... | Line 107... | ||
| 111 | fifo_t CompassCalcStateFiFo; |
107 | SPI_Version_t FC_Version; |
| 112 | 108 | ||
| 113 | u8 CompassCalState = 0; |
109 | |
| 114 | 110 | ||
| Line 249... | Line 245... | ||
| 249 | SSP_InitStructure.SSP_ClockRate = 0; |
245 | SSP_InitStructure.SSP_ClockRate = 0; |
| Line 250... | Line 246... | ||
| 250 | 246 | ||
| 251 | SSP_Init(SSP0, &SSP_InitStructure); |
247 | SSP_Init(SSP0, &SSP_InitStructure); |
| Line 252... | Line -... | ||
| 252 | SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE); |
- | |
| 253 | - | ||
| 254 | fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine); |
248 | SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE); |
| 255 | 249 | ||
| 256 | SSP_Cmd(SSP0, ENABLE); |
250 | SSP_Cmd(SSP0, ENABLE); |
| 257 | // initialize the syncbytes in the tx buffer |
251 | // initialize the syncbytes in the tx buffer |
| 258 | SPI_TxBuffer[0] = SPI_TXSYNCBYTE1; |
252 | SPI_TxBuffer[0] = SPI_TXSYNCBYTE1; |
| Line 276... | Line 270... | ||
| 276 | //------------------------------------------------------ |
270 | //------------------------------------------------------ |
| 277 | void SPI0_UpdateBuffer(void) |
271 | void SPI0_UpdateBuffer(void) |
| 278 | { |
272 | { |
| 279 | static u32 timeout = 0; |
273 | static u32 timeout = 0; |
| 280 | static u8 counter = 50; |
274 | static u8 counter = 50; |
| - | 275 | static u8 CompassCalState = 0; |
|
| Line 281... | Line 276... | ||
| 281 | 276 | ||
| 282 | if (SPI_RxBuffer_Request) |
277 | if (SPI_RxBuffer_Request) |
| 283 | { |
278 | { |
| 284 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
279 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
| 285 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
280 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
| 286 | ToFlightCtrl.CompassHeading = CompassHeading; |
281 | ToFlightCtrl.CompassHeading = Compass_Heading; |
| 287 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
282 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
| 288 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
283 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
| 289 | // cycle spi commands |
284 | // cycle spi commands |
| 290 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
285 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| Line 397... | Line 392... | ||
| 397 | 392 | ||
| 398 | case SPI_FCCMD_MISC: |
393 | case SPI_FCCMD_MISC: |
| 399 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
394 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
| 400 | { // put only new CompassCalState into queue to send via I2C |
395 | { // put only new CompassCalState into queue to send via I2C |
| 401 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
396 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
| 402 | fifo_put(&CompassCalcStateFiFo, CompassCalState); |
397 | Compass_SetCalState(CompassCalState); |
| 403 | } |
398 | } |
| 404 | Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1]; |
399 | Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1]; |
| 405 | NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch |
400 | NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch |
| 406 | NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // is located at byte 2 and 3 |
401 | NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // is located at byte 2 and 3 |