Code:
***************************/
#define REVISION "$Rev: 1 $:"
#define DATE "$Date:$:"
/**********************************************************************************/
#include <sched.h>
#include <pthread.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <signal.h>
#include <linux/serial.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <expat.h>
//CAN-Driver definitions, placed in include folder
#include <rhd.h>
#include <database.h>
#include <globalfunc.h>
//#include "canmsg.h"
#include "hakoadvcan.h"
#include "can4linux.h"
/******** Global variables *************/
int ret=0;
#define MAX 500
///Index pointers for the variable database
int iSteeringangleref,
iSteeringangle,iSpeedref,iEnginespeedref,iNavigationmoderef,icurvatureref,icurvature,
iNavigationmode,iOdopulses,iOdopulses1,iEnginespeed,iHakostate0,
iHakostate1,iHakostate2,iHakostate3,
iliftinggearpos,ipowertakeoffspeed,iliftinggearstateref,ipowertakeoffstateref,ihitchposref,icvtack,isteeringack,
ig01,ig02,iptor,iauxr,ifll,iflr,ihlght,irlght,ihorn,ihornack,iswitchack,size;
//PThread definitions
pthread_t canrx_thread;
pthread_attr_t attr;
// Creation of communication buffers
//canmsg_t tx[MAX];
///CAN port pointer
int can_dev,can_dev1;
///CAN port device identification string
char canDevString[64];
char canDevString1[64];
///Flag to indicate if HakoCan is running or not
static volatile int hakocanRunning = -1;
canmsg_t canrx[MAX];
canmsg_t cantx[MAX];
canmsg_t message;
//Function prototypes
int initHakocan(void);
void *canrx_can0(void *);
void *canrx_can1(void *);
//void *canrx_task1(void *);
/** \brief Initialization of the CAN-bus and all rx/tx buffers
*
* All buffers are initilized and database variables are created.
* Finally, the RX Thread is spawned
*
*/
int initHakocan(void)
{
// Open CAN port
// munlockall();
can_dev=open(canDevString,O_RDWR);
if (can_dev<0)
{
fprintf(stderr," HakoCan: Error CAN-BUS on %s\n",canDevString);
return -1;
}
if (canDevString1[0]!=0)
{
can_dev1=open(canDevString1,O_RDWR);
if (can_dev1<0)
{
fprintf(stderr," HakoCan: Error CAN-BUS on %s\n",canDevString1);
return -1;
}
else
{
printf(" HakoCan: Ports %s and %s open\n",canDevString,canDevString1) ;
}
}
//Create CAN RX Thread
pthread_attr_init(&attr);
pthread_attr_setinheritsched(&attr, PTHREAD_INHERIT_SCHED);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
if (pthread_create(&canrx_thread, &attr, canrx_can0, 0))
{
perror(" HakoADVCan: failed");
return -1;
}
return 1;
}
void *canrx_can0(void *not_used)
{
fprintf(stderr, " HakoADVCan: Canrx_task running (1030)\n");
//Lock memory - No more allocations
if (mlockall(MCL_CURRENT | MCL_FUTURE))
{
perror("mlockall");
exit(-1);
}
printf(" memory locked");
/* use real-time (fixed priority) scheduler
* set priority to one less than the maximum
*/
struct sched_param param;
param.sched_priority = sched_get_priority_max(SCHED_RR) - 1;
if (sched_setscheduler(0, SCHED_RR, ¶m))
{
perror("setscheduler");
pthread_exit(NULL);
};
if (signal(SIGPIPE, SIG_IGN) == SIG_ERR)
fprintf(stderr, "signal: can't ignore SIGPIPE.\n");
printf("is hakocanrunning?\n");
//Wait to make sure variables are created
while (hakocanRunning < 0)
{
printf("no so I sleep\n");
//usleep(10000);
}
while (hakocanRunning)
{
ret = 0;
printf("ready to read\n");
//receive one CAN message
ret=read(can_dev, &canrx , 1);
//usleep(10000);
if (ret<0)
{
fprintf(stderr,"Error receiving message on CAN-bus\n");
hakocanRunning = -1; //Shutdown!
}
else
{
//Parse input packages
//printf("Message Caught\n");
printf("ret: %d\t can Msg.id: %#X\n",ret,(int)canrx[0].id);
}
} //RX Loop ends
//Finish thread
fprintf(stderr,"HakoCan: Ending RX Thread!\n");
hakocanRunning = -1;
pthread_exit(NULL);
}
/** \brief Transmit messages to the CAN bus
*
* Periodic function to transmit variables and
* requests to the can-bus
*
*/
extern int periodic(int tick)
{
//~ typedef struct {
//~ /** flags, indicating or controlling special message properties */
//~ int flags;
//~ int cob; /**< CAN object number, used in Full CAN */
//~ unsigned long id; /**< CAN message ID, 4 bytes */
//~ struct timeval timestamp; /**< time stamp for received messages */
//~ short int length; /**< number of bytes in the CAN message */
//~ unsigned char data[CAN_MSG_LENGTH]; /**< data, 0...8 bytes */
//~ } canmsg_t;
//~
Send_par_t SendTeil;
/* our default 8 byte message */
message.id = 0x070;
message.cob = 0;
message.flags = 0;
message.length = 1;
message.data[0] = 0xA5;
SendTeil.Tx = &message;
ret = ioctl(can_dev, CAN_IOCTL_SEND, &SendTeil);
if (ret<0)
{
fprintf(stderr,"Error sending message on CAN-bus\n");
hakocanRunning = -1; //Shutdown!
}
printf("rettx: %d\n",ret);
return 1;
}
/************************** XML Initialization **************************/
///Struct for shared parse data
typedef struct
{
int depth;
char skip;
char enable;
char found;
}parseInfo;
//Parsing functions
void XMLCALL hakocanStartTag(void *, const char *, const char **);
void XMLCALL hakocanEndTag(void *, const char *);
/** \brief Initialize the Crossbow HAL
*
* Reads the XML file and sets up the Crossbow settings
*
* Finally the rx thread is started and the server
* is ready to accept connections
*
* \param[in] *char filename
* Filename of the XML file
*
* \returns int status
* Status of the initialization process. Negative on error.
*/
extern int initXML(char *filename)
{
parseInfo xmlParse;
char *xmlBuf = NULL;
int xmlFilelength;
int done = 0;
int len;
FILE *fp;
//Print initialization message
//Find revision number from SVN Revision
char *i,versionString[20] = REVISION, tempString[10];
i = strrchr(versionString,'$');
strncpy(tempString,versionString+6,(i-versionString-6));
tempString[(i-versionString-6)] = 0;
printf("HakoADVCan: Initializing HAKOADV CAN-Bus driver %s.%s\n",HAKOADVCANVERSION,tempString);
/* Initialize Expat parser*/
XML_Parser parser = XML_ParserCreate(NULL);
if (! parser) {
fprintf(stderr, "HakoADVCan: Couldn't allocate memory for XML parser\n");
return -1;
}
//Setup element handlers
XML_SetElementHandler(parser, hakocanStartTag, hakocanEndTag);
//Setup shared data
memset(&xmlParse,0,sizeof(parseInfo));
XML_SetUserData(parser,&xmlParse);
//Open and read the XML file
fp = fopen(filename,"r");
if(fp == NULL)
{
printf("HakoADVCan: Error reading: %s\n",filename);
return -1;
}
//Get the length of the file
fseek(fp,0,SEEK_END);
xmlFilelength = ftell(fp); //Get position
fseek(fp,0,SEEK_SET); //Return to start of file
//Allocate text buffer
xmlBuf = realloc(xmlBuf,xmlFilelength+10); //Allocate memory
if (xmlBuf == NULL) {
fprintf(stderr, " Couldn't allocate memory for XML File buffer\n");
return -1;
}
memset(xmlBuf,0,xmlFilelength);
len = fread(xmlBuf, 1, xmlFilelength, fp);
fclose(fp);
//Start parsing the XML file
if (XML_Parse(parser, xmlBuf, len, done) == XML_STATUS_ERROR)
{
fprintf(stderr, "HakoADVCan: XML Parse error at line %d: %s\n",
(int)XML_GetCurrentLineNumber(parser),
XML_ErrorString(XML_GetErrorCode(parser)));
return -1;
}
XML_ParserFree(parser);
free(xmlBuf);
//Print error, if no XML tag found
if (xmlParse.found <= 0) {
printf(" Error: No <hakoadvcan> XML tag found in plugins section\n");
return -1;
}
//Start crossbow thread after init
if (xmlParse.enable) hakocanRunning = initHakocan();
return hakocanRunning;
}
///Handle XML Start tags
void XMLCALL
hakocanStartTag(void *data, const char *el, const char **attr)
{
int i;
parseInfo *info = (parseInfo *) data;
info->depth++;
//Check for the right 1., 2. and 3. level tags
if (!info->skip) {
if (((info->depth == 1) && (strcmp("rhd",el) != 0)) ||
((info->depth == 2) && (strcmp("plugins",el) != 0)) ||
((info->depth == 3) && (strcmp("hakoadvcan",el) != 0))) {
info->skip = info->depth;
return;
} else if (info->depth == 3) info->found = 1;
} else return;
//Branch to parse the elements of the XML file.
if (!strcmp("hakoadvcan",el)) {
for(i = 0; attr[i]; i+=2) if ((strcmp("enable",attr[i]) == 0) && (strcmp("true",attr[i+1]) == 0)) {
info->enable = 1;
}
if (!info->enable) {
printf(" HakoADVCan: Use of HakoADVCan disabled in configuration\n");
info->skip = info->depth;
}
} else if (strcmp("controlcan",el) == 0) {
//Check for the correct depth for this tag
if(info->depth != 4) {
printf("Error: Wrong depth for the %s tag\n",el);
}
for(i = 0; attr[i]; i+=2) {
if (strcmp("port",attr[i]) == 0) strncpy(canDevString,attr[i+1],63);
if (strcmp("port1",attr[i]) == 0) strncpy(canDevString1,attr[i+1],63);
}
printf(" HakoADVCan: Using Control CAN-port %s %s\n",canDevString,canDevString1);
}
}
///Handle XML End tags
void XMLCALL
hakocanEndTag(void *data, const char *el)
{
parseInfo *info = (parseInfo *) data;
info->depth--;
if (info->depth < info->skip) info->skip = 0;
}