- Code: Select all
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <termios.h>
#include <sys/select.h>
#include <time.h>
#include <getopt.h>
#define MAX_QUERY_MSG_LEN 64
#define MAX_RESPONSE_MSG_LEN 33
#define RPMH_OPT_IDX 0
#define RPML_OPT_IDX 1
#define MAPA_OPT_IDX 2
#define IGN_OPT_IDX 3
#define KCOR_OPT_IDX 4
#define MAFH_OPT_IDX 5
#define MAFL_OPT_IDX 6
#define INJ_OPT_IDX 7
#define MAX_OPTIONS 8
typedef struct ecu_t
{
int connected;
int fd;
};
typedef struct option_val_t
{
bool on;
u_int8_t value;
};
bool debug = false;
option_val_t options[] = {
{false, 0x0E}, /* RPM High */
{false, 0x0F}, /* RPM Low */
{false, 0x0D}, /* MAP Absolute */
{false, 0x11}, /* Ignition Timing */
{false, 0x22}, /* Knock correction */
{false, 0x13}, /* MAF High */
{false, 0x14}, /* MAF Low */
{false, 0x20} /* Fuel Inj#1 Pulse Width */
};
typedef struct log_values_t
{
unsigned int rpm;
float mapa;
unsigned int ign;
unsigned int kcor;
float maf;
float inj;
};
int connect(ecu_t *e)
{
struct termios options;
e->connected = 0; /* reset in case we fail to connect */
e->fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY);
if (e->fd == -1)
return 0;
else
fcntl(e->fd, F_SETFL, 0);
tcgetattr(e->fd, &options);
/* go to 4800 baud */
cfsetispeed(&options, B4800);
cfsetospeed(&options, B4800);
options.c_lflag = 0; /* No local flags */
options.c_oflag &= ~OPOST; /* No output processing */
options.c_iflag &= ~(IXON | IXOFF | INPCK | PARMRK | BRKINT | INLCR | ICRNL |
IUCLC | IXANY);
options.c_iflag |= IGNBRK; /* Ignore break conditions */
options.c_cflag |= (CLOCAL | CREAD); /* enable */
options.c_cflag &= ~CRTSCTS; /* Turn off RTS/CTS */
options.c_cflag &= ~PARENB; /* No parity */
options.c_cflag &= ~CSTOPB; /* 1 stop bit */
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; /* 8 bits */
/* set all of the options */
tcsetattr(e->fd, TCSANOW, &options);
tcflush(e->fd, TCIOFLUSH); /* Flush input and output buffers */
int status;
ioctl(e->fd, TIOCMGET, &status);
status |= TIOCM_DTR; /* Set DTR high */
status &= ~TIOCM_RTS; /* Set RTS low */
ioctl(e->fd, TIOCMSET, &status);
e->connected=1;
return 1;
}
void getState(ecu_t *e)
{
int status;
ioctl(e->fd, TIOCMGET, &status);
if (status & TIOCM_DTR)
printf("DTR ON\n");
else
printf("DTR OFF\n");
if (status & TIOCM_RTS)
printf("RTS ON\n");
else
printf("RTS OFF\n");
if (status & TIOCM_CTS)
printf("CTS ON\n");
else
printf("CTS OFF\n");
if (status & TIOCM_DSR)
printf("DSR ON\n");
else
printf("DSR OFF\n");
}
void disconnect(ecu_t *e)
{
tcflush(e->fd, TCIOFLUSH);
if (!e->connected)
return;
close (e->fd);
}
void printMsg(u_int8_t* msg, int len)
{
printf("Message: ");
for (int i = 0; i < len; i++)
printf("%X ", msg[i]);
printf("\n");
}
int sendMsg(ecu_t *e, u_int8_t* msg, int len)
{
int status;
int writeLen = 0;
tcflush(e->fd, TCIOFLUSH); /* Flush input and output buffers */
writeLen = write(e->fd, msg, len);
return writeLen;
}
int recvMsg(ecu_t *e, u_int8_t* msg, int len)
{
struct termios oldtio, options;
tcgetattr(e->fd, &oldtio);
tcgetattr(e->fd, &options);
options.c_lflag = 0;
options.c_cc[VTIME] = 0;
options.c_cc[VMIN] = len;
//tcflush(e->fd, TCIFLUSH);
tcsetattr(e->fd, TCSANOW, &options);
int readLen = 0;
readLen = read(e->fd, msg, len);
tcsetattr(e->fd, TCSANOW, &oldtio);
return readLen;
}
u_int8_t calcChecksum(u_int8_t* msg, int len)
{
u_int8_t checksum = 0;
for (int i = 0; i < len; i++)
checksum += msg[i];
return checksum;
}
int processResultString(u_int8_t rs[], int len, int qslen, log_values_t* values)
{
int idx = qslen + 5;
unsigned int rpmH = 0;
unsigned int rpmL = 0;
unsigned int mafH = 0;
unsigned int mafL = 0;
for (int i = 0; i < MAX_OPTIONS; i++)
{
if (idx >= len)
return -1;
if (options[i].on)
{
switch(i)
{
case RPMH_OPT_IDX:
rpmH = rs[idx++];
rpmH <<= 8;
break;
case RPML_OPT_IDX:
rpmL = rs[idx++];
break;
case MAPA_OPT_IDX:
values->mapa = (rs[idx++] * 37.0)/255;
break;
case IGN_OPT_IDX:
values->ign = (rs[idx++] - 128)/2;
break;
case KCOR_OPT_IDX:
values->kcor = (rs[idx++] - 128)/2;
break;
case MAFH_OPT_IDX:
mafH = rs[idx++];
mafH <<= 8;
break;
case MAFL_OPT_IDX:
mafL = rs[idx++];
break;
case INJ_OPT_IDX:
values->inj = rs[idx++] * 0.256;
break;
}
}
}
if (rpmH > 0 || rpmL > 0)
values->rpm = (rpmH + rpmL)/4;
if (mafH > 0 || mafL > 0)
values->maf = (mafH + mafL)/100.0;
return 0;
}
int makeQueryString(u_int8_t qs[], int len)
{
int idx = 0;
int option_cnt = 0;
/* String too short */
if (len < 10)
return -1;
qs[idx++] = 0x80;
qs[idx++] = 0x10;
qs[idx++] = 0xF0;
/* Set length to dummy value (0x00) initially */
qs[idx++] = 0x00;
qs[idx++] = 0xA8;
qs[idx++] = 0x00;
for (int i = 0; i < MAX_OPTIONS; i++)
{
if (options[i].on)
{
option_cnt++;
qs[idx++] = 0x00;
qs[idx++] = 0x00;
qs[idx++] = options[i].value;
}
}
/* Set length, each option is 3 bytes */
qs[3] = (option_cnt * 3) + 2;
return idx;
}
int getOptions(int argc, char** argv)
{
int c;
int option_index;
bool log_option_set = false;
static struct option long_options[] =
{
{"debug", 0, 0, 0},
{"rpm", 0, 0, 0},
{"mapa", 0, 0, 0},
{"ign", 0, 0, 0},
{"kcor", 0, 0, 0},
{"maf", 0, 0, 0},
{"inj", 0, 0, 0},
{"help", 0, 0, 0},
{0, 0, 0, 0}
};
while ((c = getopt_long(argc, argv, "", long_options, &option_index)) != EOF)
{
if (c == '?')
{
printf("Usage: %s [--debug] [--rpm] [--mapa] [--ign] [--kcor] [--maf] [--inj] [--help]\n", argv[0]);
continue;
}
switch(option_index)
{
case 0:
debug = true;
printf("Debugging ON\n");
break;
case 1:
options[RPMH_OPT_IDX].on = true;
options[RPML_OPT_IDX].on = true;
log_option_set = true;
printf("RPM is ON\n");
break;
case 2:
options[MAPA_OPT_IDX].on = true;
log_option_set = true;
printf("MAP (Manifold Absolute Pressure) in psig is ON\n");
break;
case 3:
options[IGN_OPT_IDX].on = true;
log_option_set = true;
printf("IGN (Ignition Timing) in degrees is ON\n");
break;
case 4:
options[KCOR_OPT_IDX].on = true;
log_option_set = true;
printf("KCOR (Knock Correction) in degrees is ON\n");
break;
case 5:
options[MAFH_OPT_IDX].on = true;
options[MAFL_OPT_IDX].on = true;
log_option_set = true;
printf("MAF (Mass Air Flow) in g/s is ON\n");
break;
case 6:
options[INJ_OPT_IDX].on = true;
log_option_set = true;
printf("INJ (Injector Pulse Width) in ms is ON\n");
break;
case 7:
default:
printf("Usage: %s [--debug] [--rpm] [--mapa] [--ign] [--kcor] [--maf] [--inj] [--help]\n", argv[0]);
break;
}
}
return log_option_set;
}
int main(int argc, char** argv)
{
ecu_t e;
struct timeval hrTime;
u_int8_t responseMsg[MAX_RESPONSE_MSG_LEN];
int bytesRead;
if (!getOptions(argc, argv))
{
printf("No options set for logging, exiting.\n");
exit(0);
}
if (debug)
printf("Opening serial port\n");
if (!connect(&e))
{
perror("Could not open port");
exit(-1);
}
else
{
if (debug)
printf("Port opened\n");
}
if (debug)
getState(&e);
u_int8_t queryMsg[MAX_QUERY_MSG_LEN];
int queryLen = makeQueryString(queryMsg, MAX_QUERY_MSG_LEN);
queryMsg[queryLen++] = calcChecksum(queryMsg, queryLen);
if (queryLen > 0)
{
printMsg(queryMsg, queryLen);
}
else
{
printf("Error generating query string\n");
exit(-1);
}
// sleep(1);
while(1)
{
if (debug)
printMsg(queryMsg, queryLen);
if (sendMsg(&e, queryMsg, queryLen) == -1)
{
perror("Could not write");
exit(-1);
}
memset(responseMsg, 0, MAX_RESPONSE_MSG_LEN);
/* Response message length = queryLen + 6 + (queryLen - 7)/3 */
bytesRead = recvMsg(&e, responseMsg, MAX_RESPONSE_MSG_LEN);
if (debug)
printMsg(responseMsg, MAX_RESPONSE_MSG_LEN);
/* 80 F0 10 05 E8 MAP RPMH RPML IGN KCOR CKSUM */
if (debug)
printf("RPMH: %X, RPML: %X, MAPA: %X IGN: %X\n", responseMsg[27],
responseMsg[28],
responseMsg[29],
responseMsg[30]);
unsigned int rpmH = responseMsg[27];
unsigned int rpmL = responseMsg[28];
rpmH <<= 8;
unsigned int rpm = (rpmH + rpmL)/4;
float map = (responseMsg[29] * 37.0)/255;
int ign = (responseMsg[30] - 128)/2;
int kcor = (responseMsg[31] - 128)/2;
/* 50ms for one parameter */
gettimeofday(&hrTime, NULL);
printf("%ld.%ld RPM: %i MAP: %.0f IGN: %i KCOR: %i\n", hrTime.tv_sec,
hrTime.tv_usec/1000,
rpm,
map,
ign,
kcor);
}
disconnect(&e);
return 0;
}