4,7 → 4,9 |
#include "timer1.h" |
#include "led.h" |
#include "canbus.h" |
#include "compass.h" |
#include "main.h" |
#include "uart0.h" |
#include "uart1.h" |
#include "GPS.h" |
|
11,6 → 13,7 |
canmsg CanMsg; |
canmsg RxCanMsg; |
volatile u32 AllMsgsReceived; |
u32 CanbusTimeOut = 0; |
|
GPIO_InitTypeDef GPIO_InitStructure; |
CAN_InitTypeDef CAN_InitStructure; |
32,16 → 35,12 |
CAN_RX_MSGOBJ = 1 |
}; |
|
#define CAN_MSG_ADR 0x100 |
#define CAN_MSG_ADR_MA 0x200 |
#define CAN_MSG_ADR_SL 0x300 |
|
#define CAN_ID_VERSION 0 |
#define CAN_ID_STATUS 1 |
#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 |
u32 CAN_IdRx; // I am Receiving in this ID-Range |
|
#define MAX_CAN_MSG 10 |
|
CanMessage_t CanTxMessage[MAX_CAN_MSG]; |
CanMessage_t CanRxMessage[MAX_CAN_MSG]; |
|
89,15 → 88,15 |
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; |
|
default: |
114,15 → 113,32 |
void CanSend(void) |
{ |
static u32 index = 1; |
CanTxMessage[4].D.sLong = DebugOut.Analog[24]; |
// 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; |
|
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]; |
} |
} |
} |
} |
151,18 → 167,39 |
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_MSG_ADR; |
CanTxMessage[i].Id = i + CAN_IdTx; |
CanTxMessage[i].Length = 8; |
CanTxMessage[i].D.Long = i; |
CanTxMessage[i].D.Long = 0; |
|
// clear receiving Buffer |
CanRxMessage[i].Id = 0; |
CanRxMessage[i].D.Long = 0; |
} |
|
CanTxMessage[CAN_ID_VERSION].D.Byte[0] = CAN_SLAVE_COMPATIBLE; |
CanTxMessage[CAN_ID_VERSION].D.Byte[1] = VERSION_PATCH; |
CanTxMessage[CAN_ID_VERSION].D.Byte[2] = VERSION_MINOR; |
CanTxMessage[CAN_ID_VERSION].D.Byte[3] = VERSION_MAJOR; |
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"); |
} |
169,26 → 206,46 |
|
void ProcessCanBus(void) |
{ |
|
u32 errorcnt = 0; |
CanSend(); |
CanReceive(); |
if(CAN_GetTransmitErrorCounter() > 200) CanbusInit(); |
|
errorcnt = CAN_GetTransmitErrorCounter(); |
DebugOut.Analog[18] = errorcnt; |
//if(errorcnt > 200) CanbusInit(); |
|
//DebugOut.Analog[16] = CAN->SR ; |
|
if(AllMsgsReceived) |
{ |
u32 i; |
AllMsgsReceived = 0; |
DebugOut.Analog[17] = CanRxMessage[4].D.sLong; |
DebugOut.Analog[19]++; |
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]; |
|
DebugOut.Analog[17] = CanRxMessage[CAN_ID_FS_LON].D.sLong / 100000; |
DebugOut.Analog[18] = CanRxMessage[CAN_ID_FS_LAT].D.sLong / 100000; |
CanTxMessage[CAN_ID_VERSION].D.Byte[1] = Parameter.ActiveSetting; |
|
if(DebugOut.Analog[31] == 0) |
DebugOut.Analog[19] = Partner.GyroCompassCorrected; |
|
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; |
} |
} |
} |