Subversion Repositories NaviCtrl

#include <stdio.h>
#include <stdarg.h>
#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[0] = GPS_FailsafePosition.Longitude;
                CanTxMessage[CAN_ID_FS_LAT].D.sLong[0] = 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(" 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] = 0;

        // clear receiving Buffer
        CanRxMessage[i].Id = 0;
        CanRxMessage[i].D.Long[0] = 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\n\r");
}

void ProcessCanBus(void)
{
 u32 errorcnt = 0;

 CanSend();
 CanReceive();

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

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;
  sprintf(PartnerErrorMSG,"     ---                ");
}

if(CanRxMessage[CAN_ID_VERSION].D.Byte[6] == 13) // dann ist der Testdummy angeschlossen -> nur zum Testen von EXT2
 {
  if((PartnerErrorMSG[17] + PartnerErrorMSG[18]) & 1) EXT2_ON;
  else EXT2_OFF;
 }

if(AllMsgsReceived)
 {
  u32 i;
  AllMsgsReceived = 0;
  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[0];
                GPS_FailsafePosition.Latitude = CanRxMessage[CAN_ID_FS_LAT].D.sLong[0];
  }
 }
}