WebSVN - NaviCtrl - Diff - Rev 696 and 698

 #include "91x_lib.h"
 #include "91x_can.h"
 #include "timer1.h"
 #include "led.h"
 #include "canbus.h"
+#include "compass.h"
 #include "main.h"
+#include "uart0.h"
 #include "uart1.h"
 #include "GPS.h"
 canmsg CanMsg;
 canmsg RxCanMsg;
+volatile u32 AllMsgsReceived;
-volatile u32 AllMsgsReceived;
+u32 CanbusTimeOut = 0;
   enum {
         CAN_TX_MSGOBJ = 0,
         CAN_RX_MSGOBJ = 1
   };
+#define CAN_MSG_ADR_MA  0x200
-#define CAN_MSG_ADR     0x100
-#define CAN_ID_VERSION  0
-#define CAN_ID_STATUS   1
+#define CAN_MSG_ADR_SL  0x300
-#define CAN_ID_FS_LON   2
-#define CAN_ID_FS_LAT   3
-#define CAN_ID_FS_ALT   4
 u32 CAN_IdTx; // I am Sending in this ID-Range
-#define MAX_CAN_MSG     10
+u32 CAN_IdRx; // I am Receiving in this ID-Range
       case 1 /* CAN_RX_MSGOBJ */:
         CAN_ReceiveMessage(msgobj, FALSE, &RxCanMsg);
                 CAN_ReleaseRxMessage(msgobj);
-                if(RxCanMsg.Id >= CAN_MSG_ADR && RxCanMsg.Id < CAN_MSG_ADR + MAX_CAN_MSG)
+                if(RxCanMsg.Id >= CAN_IdRx && RxCanMsg.Id < CAN_IdRx + MAX_CAN_MSG)
+                 {
-                        memcpy(&CanRxMessage[RxCanMsg.Id - CAN_MSG_ADR], &RxCanMsg,sizeof(RxCanMsg));
+                        memcpy(&CanRxMessage[RxCanMsg.Id - CAN_IdRx], &RxCanMsg,sizeof(RxCanMsg));
+                 }
-                if(RxCanMsg.Id == CAN_MSG_ADR + MAX_CAN_MSG - 1)
+                if(RxCanMsg.Id == CAN_IdRx + MAX_CAN_MSG - 1)
+                 {
                   AllMsgsReceived = 1;
+                 }
-                DebugOut.Analog[16] = RxCanMsg.Id;
+//              DebugOut.Analog[] = RxCanMsg.Id;
   break;
+}
 void CanSend(void)
+{
-static u32 index = 1;
-CanTxMessage[4].D.sLong = DebugOut.Analog[24];
+static u32 index = 1;
 //  CAN_SendMessage(CAN_TX_MSGOBJ, &TxCanMsg[1]);
   if(CAN_SendMessage(CAN_TX_MSGOBJ, &CanTxMessage[index]) == SUCCESS)
+   {
     if(++index >= MAX_CAN_MSG)
+         {
                 u32 i;
+                index = 0;
+                CanTxMessage[CAN_ID_STATUS].D.Byte[0] = ErrorCode;
                 index = 0;
                 CanTxMessage[CAN_ID_FS_LON].D.sLong = GPS_FailsafePosition.Longitude;
+                CanTxMessage[CAN_ID_FS_LAT].D.sLong = GPS_FailsafePosition.Latitude;
+                CanTxMessage[CAN_ID_STATUS].D.Byte[0] = ErrorCode;
+                CanTxMessage[CAN_ID_STATUS].D.Byte[1] = FC.StatusFlags;
+                CanTxMessage[CAN_ID_STATUS].D.Byte[2] = FC.StatusFlags2;
+                CanTxMessage[CAN_ID_STATUS].D.Byte[3] = FC.StatusFlags3;
+                CanTxMessage[CAN_ID_STATUS].D.Byte[4] = NC_To_FC_Flags;
+                CanTxMessage[CAN_ID_STATUS].D.Byte[5] = EarthMagneticField/5; // in %
+                CanTxMessage[CAN_ID_STATUS].D.Int[3]  = GyroCompassCorrected;
+                for(i=0; i<8;i++)
+                 {
+                  CanTxMessage[CAN_ID_TEXT1].D.Byte[i] = ErrorMSG[i];
+                  CanTxMessage[CAN_ID_TEXT2].D.Byte[i] = ErrorMSG[i + 8];
+                  CanTxMessage[CAN_ID_TEXT3].D.Byte[i] = ErrorMSG[i + 16];
                 CanTxMessage[CAN_ID_FS_LAT].D.sLong = GPS_FailsafePosition.Latitude;
+         }
+   }
+}
  CAN_SetUnusedAllMsgObj();
  CAN_SetTxMsgObj(CAN_TX_MSGOBJ, CAN_STD_ID, DISABLE);
  CAN_SetRxMsgObj(CAN_RX_MSGOBJ, CAN_STD_ID, 0, CAN_LAST_STD_ID, TRUE);
+u32 i;
+if(IamMaster == SLAVE)
+{
+ CAN_IdRx = CAN_MSG_ADR_MA;
+ CAN_IdTx = CAN_MSG_ADR_SL;
+}
+else
+{
+ CAN_IdRx = CAN_MSG_ADR_SL;
+ CAN_IdTx = CAN_MSG_ADR_MA;
+}
 u32 i;
 for(i=0; i< MAX_CAN_MSG;i++)
+ {
         // clear TX Buffer
         CanTxMessage[i].IdType = CAN_STD_ID;
-        CanTxMessage[i].Id = i + CAN_MSG_ADR;
+        CanTxMessage[i].Id = i + CAN_IdTx;
         CanTxMessage[i].Length = 8;
+        CanTxMessage[i].D.Long = 0;
+        // clear receiving Buffer
+        CanRxMessage[i].Id = 0;
-        CanTxMessage[i].D.Long = i;
+        CanRxMessage[i].D.Long = 0;
+ }
+CanTxMessage[CAN_ID_VERSION].D.Byte[0]  = 0;
+CanTxMessage[CAN_ID_VERSION].D.Byte[1]  = Parameter.ActiveSetting;
- }
+CanTxMessage[CAN_ID_VERSION].D.Byte[2]  = GPS_Version/1000;
+CanTxMessage[CAN_ID_VERSION].D.Byte[3]  = UART_VersionInfo.HWMajor;
-CanTxMessage[CAN_ID_VERSION].D.Byte[0]  = CAN_SLAVE_COMPATIBLE;
+CanTxMessage[CAN_ID_VERSION].D.Byte[4]  = CAN_SLAVE_COMPATIBLE;
-CanTxMessage[CAN_ID_VERSION].D.Byte[1]  = VERSION_PATCH;
+CanTxMessage[CAN_ID_VERSION].D.Byte[5]  = VERSION_PATCH;
-CanTxMessage[CAN_ID_VERSION].D.Byte[2]  = VERSION_MINOR;
+CanTxMessage[CAN_ID_VERSION].D.Byte[6]  = VERSION_MINOR;
-CanTxMessage[CAN_ID_VERSION].D.Byte[3]  = VERSION_MAJOR;
+CanTxMessage[CAN_ID_VERSION].D.Byte[7]  = VERSION_MAJOR;
  UART1_PutString("ok");
-}
+}
 void ProcessCanBus(void)
+{
+ u32 errorcnt = 0;
+ CanSend();
-{
+ CanReceive();
- CanSend();
+errorcnt = CAN_GetTransmitErrorCounter();
- CanReceive();
+DebugOut.Analog[18] = errorcnt;
+//if(errorcnt > 200) CanbusInit();
 if(CAN_GetTransmitErrorCounter() > 200) CanbusInit();
+//DebugOut.Analog[16] = CAN->SR ;
-//DebugOut.Analog[16] = CAN->SR ;
+if(AllMsgsReceived)
+ {
+  u32 i;
+  AllMsgsReceived = 0;
+  DebugOut.Analog[16]++;
+  CanbusTimeOut = 1000;
+  for(i=0; i<8;i++)
+   {
+          PartnerErrorMSG[i]    = CanRxMessage[CAN_ID_TEXT1].D.Byte[i];
+          PartnerErrorMSG[i+8]  = CanRxMessage[CAN_ID_TEXT2].D.Byte[i];
+          PartnerErrorMSG[i+16] = CanRxMessage[CAN_ID_TEXT3].D.Byte[i];
+   }
+  Partner.ErrorCode                     = CanRxMessage[CAN_ID_STATUS].D.Byte[0];
+  Partner.StatusFlags                   = CanRxMessage[CAN_ID_STATUS].D.Byte[1];
+  Partner.StatusFlags2                  = CanRxMessage[CAN_ID_STATUS].D.Byte[2];
-if(AllMsgsReceived)
+  Partner.StatusFlags3                  = CanRxMessage[CAN_ID_STATUS].D.Byte[3];
- {
+  Partner.NC_To_FC_Flags                = CanRxMessage[CAN_ID_STATUS].D.Byte[4];
-  AllMsgsReceived = 0;
+  Partner.MagnetField                   = CanRxMessage[CAN_ID_STATUS].D.Byte[5];
-  DebugOut.Analog[17] = CanRxMessage[4].D.sLong;
+  Partner.GyroCompassCorrected  = CanRxMessage[CAN_ID_STATUS].D.Int[3];
   DebugOut.Analog[19]++;
+  CanTxMessage[CAN_ID_VERSION].D.Byte[1] = Parameter.ActiveSetting;
 DebugOut.Analog[19] = Partner.GyroCompassCorrected;
 DebugOut.Analog[17] = CanRxMessage[CAN_ID_FS_LON].D.sLong / 100000;
-DebugOut.Analog[18] = CanRxMessage[CAN_ID_FS_LAT].D.sLong / 100000;
+if(IamMaster == SLAVE)

Subversion Repositories NaviCtrl

(root)/tags/V2.14b/canbus.c @ 745 - Rev 696 → 698