apps/canutils/libobd2: Add libobd2 for NuttX

This commit is contained in:
Alan Carvalho de Assis 2017-10-28 13:33:30 -06:00 committed by Gregory Nutt
parent ef353ed632
commit a1f1f68b72
11 changed files with 1023 additions and 0 deletions

2
canutils/libobd2/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
/.built
/libcanard-*

22
canutils/libobd2/Kconfig Normal file
View File

@ -0,0 +1,22 @@
#
# For a description of the syntax of this configuration file,
# see the file kconfig-language.txt in the NuttX tools repository.
#
config CANUTILS_LIBOBD2
bool "OBD-II Library"
default n
depends on CAN
---help---
Enable the OBD-II Library
if CANUTILS_LIBOBD2
config LIBOBD2_MULTIFRAME
bool "Enable to Multi-Frame support (increases 4KB RAM)"
default n
---help---
Enable the support for multi-frames of the OBD-II protocol.
In the multi-frame mode the ECU can send frame up to 4096 bytes.
endif

View File

@ -0,0 +1,39 @@
############################################################################
# apps/canutils/libobd2/Make.defs
# Adds selected applications to apps/ build
#
# Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
# Author: Alan Carvalho de Assis <acassis@gmail.com>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name NuttX nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
ifeq ($(CONFIG_CANUTILS_LIBOBD2),y)
CONFIGURED_APPS += canutils/libobd2
endif

45
canutils/libobd2/Makefile Normal file
View File

@ -0,0 +1,45 @@
############################################################################
# apps/canutils/libobd2/Makefile
#
# Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
# Author: Alan Carvalho de Assis <acassis@gmail.com>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name NuttX nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
-include $(TOPDIR)/Make.defs
# CAN utility library
ASRCS =
CSRCS = obd2.c obd_sendrequest.c obd_waitresponse.c obd_decodepid.c
APPNAME = libobd2
include $(APPDIR)/Application.mk

126
canutils/libobd2/obd2.c Normal file
View File

@ -0,0 +1,126 @@
/****************************************************************************
* canutils/libobd2/obd2.c
*
* Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
* Author: Alan Carvalho de Assis <acassis@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <errno.h>
#include <nuttx/can/can.h>
#include "canutils/obd.h"
#include "canutils/obd_pid.h"
#include "canutils/obd_frame.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: obd_init
*
* Description:
* Initialize the OBD-II with initial baudrate
*
* Returns a obd_dev_s with initial values or NULL if error.
*
****************************************************************************/
struct obd_dev_s *obd_init(char *devfile, int baudate, int mode)
{
struct obd_dev_s *dev;
int ret;
/* Alloc memory for this device */
dev = malloc(sizeof(struct obd_dev_s));
if (!dev)
{
printf("ERROR: Failed to alloc memory for obd_dev!\n");
return NULL;
}
/* Open the CAN device for reading/writing */
dev->can_fd = open(devfile, O_RDWR);
if (dev->can_fd < 0)
{
printf("ERROR: open %s failed: %d\n", devfile, errno);
return NULL;
}
/* Show bit timing information if provided by the driver. Not all CAN
* drivers will support this IOCTL.
*/
ret = ioctl(dev->can_fd, CANIOC_GET_BITTIMING,
(unsigned long)((uintptr_t)&dev->can_bt));
if (ret < 0)
{
printf("Bit timing not available: %d\n", errno);
return NULL;
}
else
{
printf("Bit timing:\n");
printf(" Baud: %lu\n", (unsigned long)dev->can_bt.bt_baud);
printf(" TSEG1: %u\n", dev->can_bt.bt_tseg1);
printf(" TSEG2: %u\n", dev->can_bt.bt_tseg2);
printf(" SJW: %u\n", dev->can_bt.bt_sjw);
}
/* FIXME: Setup the baudrate */
/* Setup the initial mode */
if (mode != CAN_STD && mode != CAN_EXT)
{
printf("ERROR: Invalid mode, it needs to be CAN_STD or CAN_EXT!\n");
return NULL;
}
dev->can_mode = mode;
printf("OBD-II device initialized!\n");
return dev;
}

View File

@ -0,0 +1,135 @@
/****************************************************************************
* canutils/libobd2/obd_decodepid.c
*
* Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
* Author: Alan Carvalho de Assis <acassis@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <errno.h>
#include <nuttx/can/can.h>
#include "canutils/obd.h"
#include "canutils/obd_pid.h"
#include "canutils/obd_frame.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define MAXDATA 16
/****************************************************************************
* Private Data
****************************************************************************/
static char g_data[MAXDATA];
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: obd_decode_pid
*
* Description:
* Decode the value returned for a determined PID.
*
* It will return the data decode as text string or NULL if error.
*
****************************************************************************/
FAR char *obd_decode_pid(FAR struct obd_dev_s *dev, uint8_t pid)
{
uint32_t pids;
int rpm;
/* Verify if received data is valid */
if (dev->data[2] != pid)
{
printf("Expecting PID %02x but received %02x!\n", pid, dev->data[2]);
return NULL;
}
switch (dev->data[2])
{
case OBD_PID_SUPPORTED:
pids = (dev->data[3] << 24) | (dev->data[4] << 16) | \
(dev->data[5] << 8) | dev->data[6];
snprintf(g_data, MAXDATA, "%08X", pids);
#ifdef CONFIG_DEBUG_INFO
printf("Supported PIDs: %08X\n");
#endif
break;
case OBD_PID_ENGINE_TEMPERATURE:
snprintf(g_data, MAXDATA, "%d", dev->data[3] - 40);
#ifdef CONFIG_DEBUG_INFO
printf("Engine Temperature = %d\n", dev->data[3] - 40);
#endif
break;
case OBD_PID_RPM:
rpm = ((256 * dev->data[3]) + dev->data[4])/4;
snprintf(g_data, MAXDATA, "%d", rpm);
#ifdef CONFIG_DEBUG_INFO
printf("RPM = %d\n", rpm);
#endif
break;
case OBD_PID_SPEED:
snprintf(g_data, MAXDATA, "%d", dev->data[3]);
#ifdef CONFIG_DEBUG_INFO
printf("SPEED = %d Km/h\n", dev->data[3]);
#endif
break;
case OBD_PID_THROTTLE_POSITION:
snprintf(g_data, MAXDATA, "%d", (100 * dev->data[3])/255);
#ifdef CONFIG_DEBUG_INFO
printf("Throttle position = %d\% \n", (100 * dev->data[3])/255);
#endif
break;
}
return g_data;
}

View File

@ -0,0 +1,151 @@
/****************************************************************************
* canutils/libobd2/obd_sendrequest.c
*
* Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
* Author: Alan Carvalho de Assis <acassis@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <errno.h>
#include <nuttx/can/can.h>
#include "canutils/obd.h"
#include "canutils/obd_pid.h"
#include "canutils/obd_frame.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: obd_sent_request
*
* Description:
* Send a "Request Message" to ECUs with requested PID.
*
* It will return an error case the message fails to be sent.
*
****************************************************************************/
int obd_send_request(FAR struct obd_dev_s *dev, uint8_t opmode, uint8_t pid)
{
int nbytes;
int msgdlc;
int msgsize;
uint8_t extended;
#ifdef CONFIG_DEBUG_INFO
printf("Going SendRequest opmode=%d pid=%d\n", opmode, pid);
#endif
/* Verify what is the current mode */
if (dev->can_mode == CAN_EXT)
{
extended = 1;
}
else
{
extended = 0;
}
/* Define the CAN Data Length */
msgdlc = 8;
/* Construct the TX message header */
if (extended)
{
dev->can_txmsg.cm_hdr.ch_id = OBD_PID_STD_REQUEST; /* MSG ID for PID Request */
}
else
{
#ifdef CONFIG_CAN_EXTID
dev->can_txmsg.cm_hdr.ch_id = OBD_PID_EXT_REQUEST; /* MSG ID for PID Request */
#endif
}
dev->can_txmsg.cm_hdr.ch_rtr = false; /* Not a Remote Frame */
dev->can_txmsg.cm_hdr.ch_dlc = msgdlc; /* Data length is 8 bytes */
#ifdef CONFIG_CAN_EXTID
dev->can_txmsg.cm_hdr.ch_extid = extended; /* Standard/Extend mode */
#endif
dev->can_txmsg.cm_hdr.ch_unused = 0; /* Unused */
/* Single Frame with two bytes data */
dev->can_txmsg.cm_data[0] = OBD_SINGLE_FRAME | OBD_SF_DATA_LEN(2);
/* Setup the Operation Mode */
dev->can_txmsg.cm_data[1] = opmode;
/* Setup the PID we are requesting */
dev->can_txmsg.cm_data[2] = pid;
/* Padding */
dev->can_txmsg.cm_data[3] = 0;
dev->can_txmsg.cm_data[4] = 0;
dev->can_txmsg.cm_data[5] = 0;
dev->can_txmsg.cm_data[6] = 0;
dev->can_txmsg.cm_data[7] = 0;
/* Send the TX message */
msgsize = CAN_MSGLEN(msgdlc);
nbytes = write(dev->can_fd, &dev->can_txmsg, msgsize);
if (nbytes != msgsize)
{
printf("ERROR: write(%ld) returned %ld\n",
(long)msgsize, (long)nbytes);
return -EAGAIN;
}
#ifdef CONFIG_DEBUG_INFO
printf("PID Request sent correctly!\n");
fflush(stdout);
#endif
return OK;
}

View File

@ -0,0 +1,160 @@
/****************************************************************************
* canutils/libobd2/obd_waitresponse.c
*
* Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
* Author: Alan Carvalho de Assis <acassis@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <nuttx/can/can.h>
#include "canutils/obd.h"
#include "canutils/obd_pid.h"
#include "canutils/obd_frame.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: obd_wait_response
*
* Description:
* Wait for a message from ECUs with requested PID that was sent using
* obd_send_request().
*
* It will return an error case it doesn't receive the msg after the elapsed
* "timeout" time.
*
****************************************************************************/
int obd_wait_response(FAR struct obd_dev_s *dev, uint8_t opmode, uint8_t pid,
int timeout)
{
int i;
int nbytes;
int msgdlc;
int msgsize;
uint8_t extended;
#ifdef CONFIG_DEBUG_INFO
printf("Waiting Response for pid=%d\n", pid);
#endif
/* Verify what is the current mode */
if (dev->can_mode == CAN_EXT)
{
extended = 1;
}
else
{
extended = 0;
}
for (; ; )
{
/* Read the RX message */
msgsize = sizeof(struct can_msg_s);
nbytes = read(dev->can_fd, &dev->can_rxmsg, msgsize);
if (nbytes < CAN_MSGLEN(0) || nbytes > msgsize)
{
printf("ERROR: read(%ld) returned %ld\n",
(long)msgsize, (long)nbytes);
return -EAGAIN;
}
#ifdef CONFIG_DEBUG_INFO
printf(" ID: %4u DLC: %u\n",
dev->can_rxmsg.cm_hdr.ch_id, dev->can_rxmsg.cm_hdr.ch_dlc);
#endif
msgdlc = dev->can_rxmsg.cm_hdr.ch_dlc;
#ifdef CONFIG_DEBUG_INFO
printf("Data received:\n");
for (i = 0; i < msgdlc; i++)
{
printf(" %d: 0x%02x\n", i, dev->can_rxmsg.cm_data[i]);
}
fflush(stdout);
#endif
/* Check if we received a Response Message */
if ((extended && dev->can_rxmsg.cm_hdr.ch_id == OBD_PID_EXT_RESPONSE) || \
(!extended && dev->can_rxmsg.cm_hdr.ch_id == OBD_PID_STD_RESPONSE))
{
/* Check if the Response if for the PID we are interested! */
if (dev->can_rxmsg.cm_data[1] == (opmode + OBD_RESP_BASE) && \
dev->can_rxmsg.cm_data[2] == pid)
{
/* Save received data (important for multi-frame mode) */
memcpy(dev->data, dev->can_rxmsg.cm_data, msgdlc);
return OK;
}
}
/* Verify if we timed out */
timeout--;
if (timeout < 0)
{
printf("Timeout trying to receive PID %d\n", pid);
return -ETIMEDOUT;
}
/* Wait 10ms */
usleep(10000);
}
/* Never should come here */
return OK;
}

132
include/canutils/obd.h Normal file
View File

@ -0,0 +1,132 @@
/****************************************************************************
* include/canutils/obd.h
*
* Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
* Author: Alan Carvalho de Assis <acassis@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef __APPS_INCLUDE_CANUTILS_OBD_H
#define __APPS_INCLUDE_CANUTILS_OBD_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <errno.h>
#include <nuttx/can/can.h>
/****************************************************************************
* Public Types
****************************************************************************/
/* CAN Modes */
enum
{
CAN_STD = 0,
CAN_EXT,
};
/* OBD-II structure */
struct obd_dev_s
{
struct can_msg_s can_txmsg; /* TX Message */
struct can_msg_s can_rxmsg; /* RX Message */
struct canioc_bittiming_s can_bt; /* Current bitrate */
uint8_t can_mode; /* Current mode (Standard or Extended) */
int can_fd; /* File Descriptor of CAN Device */
#ifdef CONFIG_MULTIFRAME_SUPPORT
uint8_t data[4096]; /* Up to 4096 bytes */
#else
uint8_t data[8]; /* Single Frame = 8 bytes */
#endif
};
/****************************************************************************
* Name: obd_init
*
* Description:
* Initialize the OBD-II with initial baudrate
*
* Returns a obd_dev_s with initial values or NULL if error.
*
****************************************************************************/
FAR struct obd_dev_s *obd_init(FAR char *devfile, int baudate, int mode);
/****************************************************************************
* Name: obd_sent_request
*
* Description:
* Send a "Request Message" to ECUs with requested PID.
*
* It will return an error case the message fails to be sent.
*
****************************************************************************/
int obd_send_request(FAR struct obd_dev_s *dev, uint8_t opmode, uint8_t pid);
/****************************************************************************
* Name: obd_wait_response
*
* Description:
* Wait for a message from ECUs with requested PID that was sent using
* obd_send_request().
*
* It will return an error case it doesn't receive the msg after the elapsed
* "timeout" time.
*
****************************************************************************/
int obd_wait_response(FAR struct obd_dev_s *dev, uint8_t opmode, uint8_t pid,
int timeout);
/****************************************************************************
* Name: obd_decode_pid
*
* Description:
* Decode the value returned for a determined PID.
*
* It will return the decode PID as string or NULL if error.
*
****************************************************************************/
FAR char *obd_decode_pid(FAR struct obd_dev_s *dev, uint8_t pid);
#endif /*__APPS_INCLUDE_CANUTILS_OBD_H */

View File

@ -0,0 +1,73 @@
/****************************************************************************
* include/canutils/obd_frame.h
*
* Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
* Author: Alan Carvalho de Assis <acassis@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef __APPS_INCLUDE_CANUTILS_OBD_FRAME_H
#define __APPS_INCLUDE_CANUTILS_OBD_FRAME_H 1
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Frame Type
*
* Bits 7-4 of CAN Data 0
*
*/
#define OBD_FRAME_TYPE(x) (x & 0xf0) /* Mask bits 4-7 */
#define OBD_SINGLE_FRAME (0 << 4) /* Single frame */
#define OBD_FIRST_FRAME (1 << 4) /* First frame */
#define OBD_CONSEC_FRAME (2 << 4) /* Consecutive frame */
#define OBD_FLWCTRL_FRAME (3 << 4) /* Flow control frame */
/* Single Frame fields */
#define OBD_SF_DATA_LEN(x) (x & 0xf) /* Data Lenght of Single Frame */
/* First Frame fields */
#define OBD_FF_DATA_LEN_D0(x) ((x & 0xf) << 8) /* Data Lenght of First Frame D0 */
#define OBD_FF_DATA_LEN_D1(x) (x & 0xff) /* Data Lenght of First Frame D1 */
/* Consecutive Frame fields */
#define OBD_CF_SEQ_NUM(x) (x & 0xf) /* Consecutive Sequence Number */
/* Flow Control Frame fields */
#define OBD_FC_FLOW_STATUS(x) (x & 0xf) /* Flow Control Status */
#endif /* __APPS_INCLUDE_CANUTILS_OBD_FRAME_H */

138
include/canutils/obd_pid.h Normal file
View File

@ -0,0 +1,138 @@
/****************************************************************************
* include/canutils/obd_pid.h
*
* Copyright (C) 2017 Alan Carvalho de Assis. All rights reserved.
* Author: Alan Carvalho de Assis <acassis@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef __APPS_INCLUDE_CANUTILS_OBD_PID_H
#define __APPS_INCLUDE_CANUTILS_OBD_PID_H 1
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* PID REQUEST */
#define OBD_PID_STD_REQUEST 0x7df /* Standard PID REQUEST Message ID = 0x7df or 0x7e0 */
#define OBD_PID_EXT_REQUEST 0x18db33f1 /* Extended PID REQUEST Messaged ID = 0x18db33f1 */
/* PID RESPONSE */
#define OBD_PID_STD_RESPONSE 0x7e8 /* Standard PID RESPONSE Message ID = 0x7e8 */
#define OBD_PID_EXT_RESPONSE 0x18daf110 /* Extended PID RESPONSE Message ID = 0x18daf111 or 0x18daf11d */
#define OBD_RESP_BASE 0x40 /* Response mode = (0x40 + OpMode) */
/* OBD Operation Modes */
#define OBD_SHOW_DATA 0x01 /* Used to read current data from vehicle */
#define OBD_SHOW_FREEZED_DATA 0x02 /* Used to read freezed data from vehicle */
#define OBD_SHOW_DTC 0x03 /* Show Diagnostic Trouble Codes */
#define OBD_CLEAR_DTC 0x04 /* Clear Diagnotic Trouble Codes stored in the vehicle */
#define OBD_TEST_RESULT1 0x05 /* Test Results */
#define OBD_TEST_RESULT2 0x06 /* Test Results */
#define OBD_SHOW_PEND_DTC 0x07 /* Show Pending Diagnotic Trouble Codes */
#define OBD_CONTROL_OPERATION 0x08 /* Control Operation of on-board component/system */
#define OBD_RQST_VEHICLE_INFO 0x09 /* Request vehicle information */
#define OBD_PERMANENT_DTC 0x0a /* Permanent Diagnostic Trouble Codes */
/* Basic Standarized Sensor/Status */
#define OBD_PID_SUPPORTED 0x00 /* PIDs supported 00-20 */
#define OBD_PID_STATUS 0x01 /* Monitor status since DTCs cleared */
#define OBD_PID_STATUS_FREEZE_FRAME 0x02 /* DTC that caused required freeze frame data storage */
#define OBD_PID_FUEL_SYSTEM 0x03 /* Fuel system 1 and 2 status */
#define OBD_PID_ENGINE_LOAD 0x04 /* Calculated ENGINE LOAD Value */
#define OBD_PID_ENGINE_TEMPERATURE 0x05 /* Engine Coolant Temperature */
#define OBD_PID_SHORT_TERM_FUEL_TRIM13 0x06 /* Short Term Fuel Trim - Bank 1,3 */
#define OBD_PID_LONG_TERM_FUEL_TRIM13 0x07 /* Long Term Fuel Trim - Bank 1,3 */
#define OBD_PID_SHORT_TERM_FUEL_TRIM24 0x08 /* Short Term Fuel Trim - Bank 2,4 */
#define OBD_PID_LONG_TERM_FUEL_TRIM24 0x09 /* Long Term Fuel Trim - Bank 2,4 */
#define OBD_PID_FUEL_RAIL_PRESSURE 0x0a /* Fuel Rail Pressure (gauge) */
#define OBD_PID_MANIFOLD_ABS_PRESSURE 0x0b /* Intake Manifold Absolute Pressure (kPa) */
#define OBD_PID_RPM 0x0c /* Engine RPM */
#define OBD_PID_SPEED 0x0d /* Vehicle Speed Sensor */
#define OBD_PID_SPARK_ADVANCE 0x0e /* Ignition Timing Advance for #1 Cylinder */
#define OBD_PID_INTAKE_AIR_TEMPERATURE 0x0f /* Intake Air Temperature */
#define OBD_PID_MASS_AIR_FLOW 0x10 /* Air Flow Rate from Mass Air Flow Sensor */
#define OBD_PID_THROTTLE_POSITION 0x11 /* Absolute Throttle Position (0-100%) */
#define OBD_PID_AIR_STATUS 0x12 /* Commanded Secondary Air Status (Bit Encoded) */
#define OBD_PID_LOC_OXYGEN_SENSOR 0x13 /* Location of Oxygen Sensors (Bit Encoded) */
#define OBD_PID_OXYGEN_BANK1_SENSOR1 0x14 /* Bank 1 - Sensor 1 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_OXYGEN_BANK1_SENSOR2 0x15 /* Bank 1 - Sensor 2 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_OXYGEN_BANK1_SENSOR3 0x16 /* Bank 1 - Sensor 3 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_OXYGEN_BANK1_SENSOR4 0x17 /* Bank 1 - Sensor 4 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_OXYGEN_BANK2_SENSOR1 0x18 /* Bank 2 - Sensor 1 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_OXYGEN_BANK2_SENSOR2 0x19 /* Bank 2 - Sensor 2 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_OXYGEN_BANK2_SENSOR3 0x1a /* Bank 2 - Sensor 3 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_OXYGEN_BANK2_SENSOR4 0x1b /* Bank 2 - Sensor 4 Oxygen Sensor Output Voltage / Short Term Fuel Trim (V) */
#define OBD_PID_STANDARD_COMPLIANCE 0x1c /* OBD standards this vehicle conforms to */
#define OBD_PID_OXYGEN_SENSORS 0x1d /* Oxygen sensors present */
#define OBD_PID_AUXILIARY_INPUT_STATUS 0x1e /* Auxiliary input status */
#define OBD_PID_RUNTIME_ENGINE_START 0x1f /* Run time since engine start */
/* Extended Standarized Sensor_Status */
#define OBD_PID_SUPPORTED_EXT 0x20 /* PIDs supported 21-40 */
#define OBD_PID_DIST_TRAVELED_MIL 0x21 /* Distance traveled with malfunction indicator lamp (MIL) on */
#define OBD_PID_FUEL_RAIL_PRESS_VACUUM 0x22 /* Fuel Rail Pressure (relative to manifold vacuum) */
#define OBD_PID_FUEL_RAIL_PRESS_DIR_INJ 0x23 /* Fuel Rail Pressure (diesel, or gasoline direct inject) */
#define OBD_PID_O2S1_WR_LAMBDA_ERV 0x24 /* O2S1_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_O2S2_WR_LAMBDA_ERV 0x25 /* O2S2_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_O2S3_WR_LAMBDA_ERV 0x26 /* O2S3_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_O2S4_WR_LAMBDA_ERV 0x27 /* O2S4_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_O2S5_WR_LAMBDA_ERV 0x28 /* O2S5_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_O2S6_WR_LAMBDA_ERV 0x29 /* O2S6_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_O2S7_WR_LAMBDA_ERV 0x2a /* O2S7_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_O2S8_WR_LAMBDA_ERV 0x2b /* O2S8_WR_lambda(1): Equivalence Ratio Voltage */
#define OBD_PID_COMMANDED_EGR 0x2c /* Commanded EGR */
#define OBD_PID_EGR_ERROR 0x2d /* EGR Error */
#define OBD_PID_CMD_EVAPORAT_PURGE 0x2e /* Commanded evaporative purge */
#define OBD_PID_FUEL_LEVEL_INPUT 0x2f /* Fuel Level Input */
#define OBD_PID_WARMUP_CODES_CLEARED 0x30 /* Number of warm-ups since codes cleared */
#define OBD_PID_DIST_TRAV_CODES_CLEAR 0x31 /* Distance traveled since codes cleared */
#define OBD_PID_EVAP_SYS_VAPOR_PRESS 0x32 /* Evap. System Vapor Pressure */
#define OBD_PID_BAROMETRIC_PRESSURE 0x33 /* Barometric pressure */
#define OBD_PID_O2S1_WR_LAMBDA_ERC 0x34 /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_O2S2_WR_LAMBDA_ERC 0x35 /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_O2S3_WR_LAMBDA_ERC 0x36 /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_O2S4_WR_LAMBDA_ERC 0x37 /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_O2S5_WR_LAMBDA_ERC 0x38 /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_O2S6_WR_LAMBDA_ERC 0x39 /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_O2S7_WR_LAMBDA_ERC 0x3a /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_O2S8_WR_LAMBDA_ERC 0x3b /* O2S1_WR_lambda(1): Equivalence Ratio Current */
#define OBD_PID_CATAL_TEMP_BK1SS1 0x3c /* Catalyst Temperature Bank 1, Sensor 1 */
#define OBD_PID_CATAL_TEMP_BK2SS1 0x3d /* Catalyst Temperature Bank 2, Sensor 1 */
#define OBD_PID_CATAL_TEMP_BK1SS2 0x3e /* Catalyst Temperature Bank 1, Sensor 2 */
#define OBD_PID_CATAL_TEMP_BK2SS2 0x3f /* Catalyst Temperature Bank 2, Sensor 2 */
#endif /* __APPS_INCLUDE_CANUTILS_OBD_PID_H */