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 |