Subversion Repositories NaviCtrl

#include <string.h>

#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;

u32 CanbusTimeOut = 0;

GPIO_InitTypeDef    GPIO_InitStructure;

CAN_InitTypeDef     CAN_InitStructure;

typedef  enum {

    NA,

    Priority_1,

    Priority_2,

    Priority_3

  }VIC_Priority;

  /* buffer for receive messages */

  canmsg RxCanTxMsg;

  /* used message object numbers */

  enum {

        CAN_TX_MSGOBJ = 0,

        CAN_RX_MSGOBJ = 1

  };

#define CAN_MSG_ADR_MA  0x200

#define CAN_MSG_ADR_SL  0x300

u32 CAN_IdTx; // I am Sending in this ID-Range

u32 CAN_IdRx; // I am Receiving in this ID-Range

CanMessage_t CanTxMessage[MAX_CAN_MSG];

CanMessage_t CanRxMessage[MAX_CAN_MSG];

void CAN_IO_Init(void)

{

// P5.0 alternate input 1, CAN_RX pin 

        GPIO_StructInit(&GPIO_InitStructure);

        GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0;

        GPIO_InitStructure.GPIO_Direction=GPIO_PinInput;

        GPIO_InitStructure.GPIO_IPInputConnected=GPIO_IPInputConnected_Enable;

        GPIO_InitStructure.GPIO_Alternate=GPIO_InputAlt1;

        GPIO_Init(GPIO5,&GPIO_InitStructure);

// P5.1 CAN_TX

        GPIO_StructInit(&GPIO_InitStructure);

        GPIO_InitStructure.GPIO_Pin=GPIO_Pin_1;

        GPIO_InitStructure.GPIO_Direction=GPIO_PinOutput;

        GPIO_InitStructure.GPIO_Type=GPIO_Type_PushPull;

        GPIO_InitStructure.GPIO_Alternate=GPIO_OutputAlt2;

        GPIO_Init(GPIO5,&GPIO_InitStructure);

}

/*******************************************************************************

* Function Name  : CAN_IRQHandler

* Description    : This function handles the CAN interrupt request

*******************************************************************************/

void CAN_IRQHandler(void)

{

   u32 msgobj = 0;

  if(CAN->IDR == 0x8000)        /* status interrupt */

  {

    (void)CAN->SR;      /* read the status register to clear*/

  }

  else if(CAN->IDR >= 1 && CAN->IDR <= 32)

  {

    /* get the message object number that caused the interrupt to occur */

    switch(msgobj = CAN->IDR - 1)

    {

      case  0 /* CAN_TX_MSGOBJ */:

                CAN_ReleaseTxMessage(msgobj);

        break;

      case 1 /* CAN_RX_MSGOBJ */:

        CAN_ReceiveMessage(msgobj, FALSE, &RxCanMsg);

                CAN_ReleaseRxMessage(msgobj);

                if(RxCanMsg.Id >= CAN_IdRx && RxCanMsg.Id < CAN_IdRx + MAX_CAN_MSG)

                 {

                        memcpy(&CanRxMessage[RxCanMsg.Id - CAN_IdRx], &RxCanMsg,sizeof(RxCanMsg));

                 }

                if(RxCanMsg.Id == CAN_IdRx + MAX_CAN_MSG - 1)

                 {

                  AllMsgsReceived = 1;

                 }

//              DebugOut.Analog[] = RxCanMsg.Id;

  break;

      default:

        CAN_ReleaseMessage(msgobj);

        break;

    }

  }

   /*write any value to VIC0 VAR*/  

   VIC0->VAR = 0xFF;   

}

void CanSend(void)

{

static u32 index = 1;

  if(CAN_SendMessage(CAN_TX_MSGOBJ, (canmsg*) &CanTxMessage[index]) == SUCCESS)

   {

    if(++index >= MAX_CAN_MSG)

         {

                u32 i;

                index = 0;

                CanTxMessage[CAN_ID_STATUS].D.Byte[0] = ErrorCode;

                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];

                 }                                                                               

         }

   }

}

void CanReceive(void)

{

}

void CanbusInit(void)

{

 UART1_PutString("\r\n Canbus init...");

 CAN_IO_Init();

 SCU_APBPeriphClockConfig(__CAN, ENABLE);

 SCU_APBPeriphReset(__CAN, DISABLE);

 VIC_Config(CAN_ITLine, VIC_IRQ, Priority_1);

 CAN_InitStructure.CAN_ConfigParameters=CAN_CR_IE;

 CAN_InitStructure.CAN_Bitrate = CAN_BITRATE_1M;

 CAN_Init(&CAN_InitStructure);

 VIC_ITCmd(CAN_ITLine, ENABLE);

 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;

}

for(i=0; i< MAX_CAN_MSG;i++)

 {

        // clear TX Buffer

        CanTxMessage[i].IdType = CAN_STD_ID;

        CanTxMessage[i].Id = i + CAN_IdTx;

        CanTxMessage[i].Length = 8;

        CanTxMessage[i].D.Long = 0;

        // clear receiving Buffer

        CanRxMessage[i].Id = 0;

        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[4]  = CAN_SLAVE_COMPATIBLE;

CanTxMessage[CAN_ID_VERSION].D.Byte[5]  = VERSION_PATCH;

CanTxMessage[CAN_ID_VERSION].D.Byte[6]  = VERSION_MINOR;

CanTxMessage[CAN_ID_VERSION].D.Byte[7]  = VERSION_MAJOR;

 UART1_PutString("ok");

}

void ProcessCanBus(void)

{

 u32 errorcnt = 0;

 CanSend();

 CanReceive();

errorcnt = CAN_GetTransmitErrorCounter();

//DebugOut.Analog[18] = errorcnt;

//if(errorcnt > 200) CanbusInit();

//DebugOut.Analog[16] = CAN->SR ;

if(CanbusTimeOut < 3)

{

  if(CanbusTimeOut == 1) Partner.ErrorCode = 39; // ERR: Canbus

  Partner.StatusFlags = 0;

  Partner.StatusFlags2 = 0;

  Partner.StatusFlags3 = 0;

  Partner.NC_To_FC_Flags = 0;

  Partner.MagnetField = 0;

  Partner.GyroCompassCorrected = -1;

}

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];

  Partner.StatusFlags3                  = CanRxMessage[CAN_ID_STATUS].D.Byte[3];

  Partner.NC_To_FC_Flags                = CanRxMessage[CAN_ID_STATUS].D.Byte[4];

  Partner.MagnetField                   = CanRxMessage[CAN_ID_STATUS].D.Byte[5];

  Partner.GyroCompassCorrected  = CanRxMessage[CAN_ID_STATUS].D.Int[3];

  CanTxMessage[CAN_ID_VERSION].D.Byte[1] = Parameter.ActiveSetting;

if(IamMaster == SLAVE)

  {

                GPS_FailsafePosition.Longitude = CanRxMessage[CAN_ID_FS_LON].D.sLong;

                GPS_FailsafePosition.Latitude = CanRxMessage[CAN_ID_FS_LAT].D.sLong;

//DebugOut.Analog[17] = CanRxMessage[CAN_ID_FS_LON].D.sLong / 100000;

//DebugOut.Analog[18] = CanRxMessage[CAN_ID_FS_LAT].D.sLong / 100000;

  }

 }

}