WebSVN - NaviCtrl - Diff - Rev 369 and 378

 volatile u32 SPI0_Timeout = 0;
 u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0;
 // tx packet buffer
 #define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
-volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN];
+volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN + 10];
 volatile u8 SPI_TxBufferIndex = 0;
 u8 *Ptr_TxChksum = NULL ;  // pointer to checksum in TxBuffer
 // rx packet buffer
 #define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
-volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN];
+volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN+10];
 volatile u8 SPI_RxBufferIndex = 0;
 volatile u8 SPI_RxBuffer_Request = 0;
         #define SPI_SYNC1       0
         #define SPI_SYNC2       1
         #define SPI_DATA        2
-        static u8 SPI_State = SPI_SYNC1;
+        static u8 SPI_State = SPI_SYNC1;
-        IENABLE;
+        //IENABLE;
         // clear pending bits
         SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
         SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
         // while RxFIFO not empty
-        while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET && (SD_WatchDog))
+        while(SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
         {
                 rxdata =  SSP0->DR; // catch the received byte
                 // Fill TxFIFO while its not full or end of packet is reached
-                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET && (SD_WatchDog))
+                while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
+                {
                         if (SPI_TxBufferIndex  < SPI_TXBUFFER_LEN)   // still data to send ?
                         {
                                 SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex];       // send a byte
                                 *Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
-                                if(MainWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication!
+                                if(SPIWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication!
                                 SPI_TxBufferIndex++; // pointer to next byte
+                        }
                                         SPI_State  = SPI_SYNC1; //jump back to sync1
+                                }
                                 break;
                         case SPI_DATA:
                                 SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
-                                if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
+                                if(SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)  // end of packet is reached
+                                {
                                         if (rxdata == rxchksum) // verify checksum byte
+                                        {
                                                 // copy SPI_RxBuffer -> FromFlightCtrl
                                                 if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
+                                                {
                                                         memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
                                                         SPI_RxBuffer_Request = 1;
+                                                }
+else
+{
+DebugOut.Analog[16]++;
+}
                                                 // reset timeout counter on good packet
                                                 SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
                                                 DebugOut.Analog[13]++;
+                                        }
                                         else // bad checksum byte
                                 SPI_State  = SPI_SYNC1;
                                 break;
+                }
+        }
+//      IDISABLE;
-        IDISABLE;
+        VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
+}
 //--------------------------------------------------------------
         SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
         SSP_InitStructure.SSP_ClockRate = 0;
         SSP_Init(SSP0, &SSP_InitStructure);
-        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
         SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
         SSP_Cmd(SSP0, ENABLE);
         // initialize the syncbytes in the tx buffer
         SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
         SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
         static u32 timeout = 0;
         static u8 counter = 50,hott_index = 0;
         static u8 CompassCalState = 0;
         s16 tmp;
         s32 i1,i2;
+    SPIWatchDog = 3500;    // stop communication to FC after this timeout
-        if (SPI_RxBuffer_Request)
+        if(SPI_RxBuffer_Request)
+        {
                 // avoid sending data via SPI during the update of the  ToFlightCtrl structure
                 VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
                 ToFlightCtrl.CompassHeading = Compass_Heading;
 //ToFlightCtrl.CompassHeading += 360 + ((s32) Poti8 - 128);
 //ToFlightCtrl.CompassHeading %= 360;
                 DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
                 if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
 //              ToFlightCtrl.MagVecX = MagVector.X;
 //              ToFlightCtrl.MagVecY = MagVector.Y;
                 ToFlightCtrl.MagVecZ = MagVector.Z;
                 if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
 #define FLAG_GPS_AID 0x01
                 switch (ToFlightCtrl.Command)
+                {
                         case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
-                                CalcHeadFree();
                                 ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
                                 ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
                                 ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
                                 ToFlightCtrl.Param.Byte[3]      = (u8) Kalman_Kompass;
                                 ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
 // 3 = 6,7
 // 4 = 8,9
 // 5 = 10,11
+                }
                 VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
                 switch(FromFlightCtrl.Command)
+                {
                         case SPI_FCCMD_USER:
                                 Parameter.User1 = FromFlightCtrl.Param.Byte[0];
                                 Parameter.User2 = FromFlightCtrl.Param.Byte[1];
                                 break;
                         case SPI_FCCMD_MISC:
                                 if(CompassCalState != FromFlightCtrl.Param.Byte[0])
+                                {       // put only new CompassCalState into queue to send via I2C
+                                        if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1)
-                                {       // put only new CompassCalState into queue to send via I2C
+                                         {
-                                        CompassCalState = FromFlightCtrl.Param.Byte[0];
+                                           CompassCalState = FromFlightCtrl.Param.Byte[0];
+                                           Compass_SetCalState(CompassCalState);
+                                         }
-                                        Compass_SetCalState(CompassCalState);
+                                         else CompassCalState = 0;
+                                }
                                 Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
                                 NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
                                 NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
+                }
 /*
 //+++++++++++++++++++++++++++++++++++++++++++++++++++
+*/
+/*
+if(Parameter.User8 < 100) FC.StatusFlags = 0;
+else
+if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START;
+else FC.StatusFlags = FC_STATUS_FLY | FC_STATUS_MOTOR_RUN;
-*/
+BL_MinOfMaxPWM = 255;
+*/
+                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
+                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
+                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
+                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
+                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
+                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
+                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
                 Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
                 // every time we got new data from the FC via SPI call the navigation routine
+                // and update GPSStick that are returned to FC
-                // and update GPSStick that are returned to FC
+                SPI_RxBuffer_Request = 0;
                 GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
-                ClearFCStatusFlags = 1;
                 ClearFCStatusFlags = 1;
                 if(counter)
+                {
                         counter--;                                       // count down to enable servo
                         if(!counter) TIMER2_Init();  // enable Servo Output
-                }
                 SPI_RxBuffer_Request = 0;
-                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
-                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
-                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
-                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
-                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
-                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
-                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
-                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
+                timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
                 Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
         }       // EOF if(SPI_RxBuffer_Request)
         else // no new SPI data
+        {

Subversion Repositories NaviCtrl

(root)/tags/V2.10c/spi_slave.c @ 811 - Rev 369 → 378