Rev 369 | Rev 379 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 369 | Rev 378 | ||
---|---|---|---|
Line 82... | Line 82... | ||
82 | volatile u32 SPI0_Timeout = 0; |
82 | volatile u32 SPI0_Timeout = 0; |
83 | u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0; |
83 | u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0; |
Line 84... | Line 84... | ||
84 | 84 | ||
85 | // tx packet buffer |
85 | // tx packet buffer |
86 | #define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization |
86 | #define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization |
87 | volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN]; |
87 | volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN + 10]; |
88 | volatile u8 SPI_TxBufferIndex = 0; |
88 | volatile u8 SPI_TxBufferIndex = 0; |
Line 89... | Line 89... | ||
89 | u8 *Ptr_TxChksum = NULL ; // pointer to checksum in TxBuffer |
89 | u8 *Ptr_TxChksum = NULL ; // pointer to checksum in TxBuffer |
90 | 90 | ||
91 | // rx packet buffer |
91 | // rx packet buffer |
92 | #define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl) |
92 | #define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl) |
93 | volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
93 | volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN+10]; |
94 | volatile u8 SPI_RxBufferIndex = 0; |
94 | volatile u8 SPI_RxBufferIndex = 0; |
Line 95... | Line 95... | ||
95 | volatile u8 SPI_RxBuffer_Request = 0; |
95 | volatile u8 SPI_RxBuffer_Request = 0; |
Line 128... | Line 128... | ||
128 | 128 | ||
129 | #define SPI_SYNC1 0 |
129 | #define SPI_SYNC1 0 |
130 | #define SPI_SYNC2 1 |
130 | #define SPI_SYNC2 1 |
131 | #define SPI_DATA 2 |
131 | #define SPI_DATA 2 |
132 | static u8 SPI_State = SPI_SYNC1; |
- | |
133 | 132 | static u8 SPI_State = SPI_SYNC1; |
|
Line 134... | Line 133... | ||
134 | IENABLE; |
133 | //IENABLE; |
135 | 134 | ||
136 | // clear pending bits |
135 | // clear pending bits |
Line 137... | Line 136... | ||
137 | SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut); |
136 | SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut); |
138 | SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo); |
137 | SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo); |
139 | 138 | ||
140 | // while RxFIFO not empty |
139 | // while RxFIFO not empty |
141 | while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET && (SD_WatchDog)) |
140 | while(SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET) |
142 | { |
141 | { |
143 | rxdata = SSP0->DR; // catch the received byte |
142 | rxdata = SSP0->DR; // catch the received byte |
144 | // Fill TxFIFO while its not full or end of packet is reached |
143 | // Fill TxFIFO while its not full or end of packet is reached |
145 | while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET && (SD_WatchDog)) |
144 | while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET) |
146 | { |
145 | { |
147 | if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) // still data to send ? |
146 | if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) // still data to send ? |
148 | { |
147 | { |
149 | SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex]; // send a byte |
148 | SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex]; // send a byte |
150 | *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum |
149 | *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum |
151 | if(MainWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication! |
150 | if(SPIWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication! |
152 | SPI_TxBufferIndex++; // pointer to next byte |
151 | SPI_TxBufferIndex++; // pointer to next byte |
153 | } |
152 | } |
Line 182... | Line 181... | ||
182 | SPI_State = SPI_SYNC1; //jump back to sync1 |
181 | SPI_State = SPI_SYNC1; //jump back to sync1 |
183 | } |
182 | } |
184 | break; |
183 | break; |
185 | case SPI_DATA: |
184 | case SPI_DATA: |
186 | SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer |
185 | SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer |
187 | if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached |
186 | if(SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached |
188 | { |
187 | { |
189 | if (rxdata == rxchksum) // verify checksum byte |
188 | if (rxdata == rxchksum) // verify checksum byte |
190 | { |
189 | { |
191 | // copy SPI_RxBuffer -> FromFlightCtrl |
190 | // copy SPI_RxBuffer -> FromFlightCtrl |
192 | if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access |
191 | if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access |
193 | { |
192 | { |
194 | memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl)); |
193 | memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl)); |
195 | SPI_RxBuffer_Request = 1; |
194 | SPI_RxBuffer_Request = 1; |
196 | } |
195 | } |
- | 196 | else |
|
- | 197 | { |
|
- | 198 | DebugOut.Analog[16]++; |
|
- | 199 | } |
|
197 | // reset timeout counter on good packet |
200 | // reset timeout counter on good packet |
198 | SPI0_Timeout = SetDelay(SPI0_TIMEOUT); |
201 | SPI0_Timeout = SetDelay(SPI0_TIMEOUT); |
199 | DebugOut.Analog[13]++; |
202 | DebugOut.Analog[13]++; |
200 | } |
203 | } |
201 | else // bad checksum byte |
204 | else // bad checksum byte |
Line 213... | Line 216... | ||
213 | SPI_State = SPI_SYNC1; |
216 | SPI_State = SPI_SYNC1; |
214 | break; |
217 | break; |
215 | } |
218 | } |
216 | } |
219 | } |
Line 217... | Line 220... | ||
217 | 220 | ||
- | 221 | // IDISABLE; |
|
218 | IDISABLE; |
222 | VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register |
Line 219... | Line 223... | ||
219 | } |
223 | } |
220 | 224 | ||
221 | //-------------------------------------------------------------- |
225 | //-------------------------------------------------------------- |
Line 257... | Line 261... | ||
257 | SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low; |
261 | SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low; |
258 | SSP_InitStructure.SSP_ClockRate = 0; |
262 | SSP_InitStructure.SSP_ClockRate = 0; |
Line 259... | Line 263... | ||
259 | 263 | ||
260 | SSP_Init(SSP0, &SSP_InitStructure); |
264 | SSP_Init(SSP0, &SSP_InitStructure); |
261 | SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE); |
- | |
262 | 265 | SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE); |
|
263 | SSP_Cmd(SSP0, ENABLE); |
266 | SSP_Cmd(SSP0, ENABLE); |
264 | // initialize the syncbytes in the tx buffer |
267 | // initialize the syncbytes in the tx buffer |
265 | SPI_TxBuffer[0] = SPI_TXSYNCBYTE1; |
268 | SPI_TxBuffer[0] = SPI_TXSYNCBYTE1; |
266 | SPI_TxBuffer[1] = SPI_TXSYNCBYTE2; |
269 | SPI_TxBuffer[1] = SPI_TXSYNCBYTE2; |
Line 286... | Line 289... | ||
286 | static u32 timeout = 0; |
289 | static u32 timeout = 0; |
287 | static u8 counter = 50,hott_index = 0; |
290 | static u8 counter = 50,hott_index = 0; |
288 | static u8 CompassCalState = 0; |
291 | static u8 CompassCalState = 0; |
289 | s16 tmp; |
292 | s16 tmp; |
290 | s32 i1,i2; |
293 | s32 i1,i2; |
- | 294 | ||
291 | 295 | ||
- | 296 | SPIWatchDog = 3500; // stop communication to FC after this timeout |
|
292 | if (SPI_RxBuffer_Request) |
297 | if(SPI_RxBuffer_Request) |
293 | { |
298 | { |
294 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
299 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
295 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
300 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
296 | ToFlightCtrl.CompassHeading = Compass_Heading; |
301 | ToFlightCtrl.CompassHeading = Compass_Heading; |
297 | - | ||
298 | //ToFlightCtrl.CompassHeading += 360 + ((s32) Poti8 - 128); |
302 | //ToFlightCtrl.CompassHeading += 360 + ((s32) Poti8 - 128); |
299 | //ToFlightCtrl.CompassHeading %= 360; |
303 | //ToFlightCtrl.CompassHeading %= 360; |
300 | - | ||
301 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
304 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
302 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
305 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
303 | // ToFlightCtrl.MagVecX = MagVector.X; |
306 | // ToFlightCtrl.MagVecX = MagVector.X; |
304 | // ToFlightCtrl.MagVecY = MagVector.Y; |
307 | // ToFlightCtrl.MagVecY = MagVector.Y; |
305 | ToFlightCtrl.MagVecZ = MagVector.Z; |
308 | ToFlightCtrl.MagVecZ = MagVector.Z; |
Line 310... | Line 313... | ||
310 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
313 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
311 | #define FLAG_GPS_AID 0x01 |
314 | #define FLAG_GPS_AID 0x01 |
312 | switch (ToFlightCtrl.Command) |
315 | switch (ToFlightCtrl.Command) |
313 | { |
316 | { |
314 | case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt |
317 | case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt |
315 | CalcHeadFree(); |
- | |
316 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
318 | ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
317 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
319 | ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
318 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
320 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
319 | ToFlightCtrl.Param.Byte[3] = (u8) Kalman_Kompass; |
321 | ToFlightCtrl.Param.Byte[3] = (u8) Kalman_Kompass; |
320 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
322 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
Line 535... | Line 537... | ||
535 | // 3 = 6,7 |
537 | // 3 = 6,7 |
536 | // 4 = 8,9 |
538 | // 4 = 8,9 |
537 | // 5 = 10,11 |
539 | // 5 = 10,11 |
538 | } |
540 | } |
539 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
541 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
540 | - | ||
541 | switch(FromFlightCtrl.Command) |
542 | switch(FromFlightCtrl.Command) |
542 | { |
543 | { |
543 | case SPI_FCCMD_USER: |
544 | case SPI_FCCMD_USER: |
544 | Parameter.User1 = FromFlightCtrl.Param.Byte[0]; |
545 | Parameter.User1 = FromFlightCtrl.Param.Byte[0]; |
545 | Parameter.User2 = FromFlightCtrl.Param.Byte[1]; |
546 | Parameter.User2 = FromFlightCtrl.Param.Byte[1]; |
Line 659... | Line 660... | ||
659 | break; |
660 | break; |
Line 660... | Line 661... | ||
660 | 661 | ||
661 | case SPI_FCCMD_MISC: |
662 | case SPI_FCCMD_MISC: |
662 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
663 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
- | 664 | { // put only new CompassCalState into queue to send via I2C |
|
- | 665 | if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1) |
|
663 | { // put only new CompassCalState into queue to send via I2C |
666 | { |
664 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
667 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
- | 668 | Compass_SetCalState(CompassCalState); |
|
- | 669 | } |
|
665 | Compass_SetCalState(CompassCalState); |
670 | else CompassCalState = 0; |
666 | } |
671 | } |
667 | Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1]; |
672 | Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1]; |
668 | NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch |
673 | NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch |
669 | NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm |
674 | NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm |
Line 715... | Line 720... | ||
715 | } |
720 | } |
Line 716... | Line 721... | ||
716 | 721 | ||
717 | /* |
722 | /* |
718 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
723 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 724 | */ |
|
- | 725 | /* |
|
- | 726 | if(Parameter.User8 < 100) FC.StatusFlags = 0; |
|
- | 727 | else |
|
- | 728 | if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START; |
|
- | 729 | else FC.StatusFlags = FC_STATUS_FLY | FC_STATUS_MOTOR_RUN; |
|
719 | */ |
730 | BL_MinOfMaxPWM = 255; |
- | 731 | */ |
|
- | 732 | DebugOut.Analog[0] = FromFlightCtrl.AngleNick; |
|
- | 733 | DebugOut.Analog[1] = FromFlightCtrl.AngleRoll; |
|
- | 734 | DebugOut.Analog[2] = FromFlightCtrl.AccNick; |
|
- | 735 | DebugOut.Analog[3] = FromFlightCtrl.AccRoll; |
|
- | 736 | DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg |
|
- | 737 | Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg |
|
- | 738 | Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg |
|
720 | 739 | Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg |
|
721 | // every time we got new data from the FC via SPI call the navigation routine |
740 | // every time we got new data from the FC via SPI call the navigation routine |
- | 741 | // and update GPSStick that are returned to FC |
|
722 | // and update GPSStick that are returned to FC |
742 | SPI_RxBuffer_Request = 0; |
723 | GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); |
743 | GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); |
724 | ClearFCStatusFlags = 1; |
- | |
725 | 744 | ClearFCStatusFlags = 1; |
|
726 | if(counter) |
745 | if(counter) |
727 | { |
746 | { |
728 | counter--; // count down to enable servo |
747 | counter--; // count down to enable servo |
729 | if(!counter) TIMER2_Init(); // enable Servo Output |
748 | if(!counter) TIMER2_Init(); // enable Servo Output |
730 | } |
- | |
731 | - | ||
732 | SPI_RxBuffer_Request = 0; |
749 | } |
Line 733... | Line -... | ||
733 | timeout = SetDelay(80); // 80 ms, new data are send every 20 ms |
- | |
734 | - | ||
735 | DebugOut.Analog[0] = FromFlightCtrl.AngleNick; |
- | |
736 | DebugOut.Analog[1] = FromFlightCtrl.AngleRoll; |
- | |
737 | DebugOut.Analog[2] = FromFlightCtrl.AccNick; |
- | |
738 | DebugOut.Analog[3] = FromFlightCtrl.AccRoll; |
- | |
739 | DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg |
- | |
740 | Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg |
- | |
741 | Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg |
750 | timeout = SetDelay(80); // 80 ms, new data are send every 20 ms |
742 | Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg |
751 | |
743 | } // EOF if(SPI_RxBuffer_Request) |
752 | } // EOF if(SPI_RxBuffer_Request) |
744 | else // no new SPI data |
753 | else // no new SPI data |
745 | { |
754 | { |