Crash_Override/OVMS3
2
0
Fork 0
Code Issues 5 Pull requests Projects Releases 4 Wiki Activity
OVMS3/OVMS.V3/components/vehicle/vehicle_poller_vwtp.cpp

840 lines
27 KiB
C++
Raw Normal View History

Initial commit, fork from original Project
2022-04-05 22:04:46 +00:00
/*
; Project: Open Vehicle Monitor System
; Date: 14th March 2017
;
; Changes:
; 1.0 Initial release
;
; (C) 2011 Michael Stegen / Stegen Electronics
; (C) 2011-2017 Mark Webb-Johnson
; (C) 2011 Sonny Chen @ EPRO/DX
; (C) 2021 Michael Balzer <dexter@dexters-web.de>
;
; Permission is hereby granted, free of charge, to any person obtaining a copy
; of this software and associated documentation files (the "Software"), to deal
; in the Software without restriction, including without limitation the rights
; to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
; copies of the Software, and to permit persons to whom the Software is
; furnished to do so, subject to the following conditions:
;
; The above copyright notice and this permission notice shall be included in
; all copies or substantial portions of the Software.
;
; THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
; IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
; FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
; AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
; THE SOFTWARE.
*/
#include "ovms_log.h"
static const char *TAG = "vehicle-vwtp";
#include <stdio.h>
#include <algorithm>
#include "vehicle.h"
/**
* PollerVWTPStart: start next VW-TP request (internal method)
*/
void OvmsVehicle::PollerVWTPStart(bool fromTicker)
{
m_poll_vwtp.lastused = monotonictime;
// Check connection state:
if (m_poll_vwtp.bus != m_poll_bus ||
m_poll_vwtp.baseid != m_poll_entry.txmoduleid ||
m_poll_vwtp.moduleid != m_poll_entry.rxmoduleid)
{
// close or reconnect channel:
if (m_poll_vwtp.state != VWTP_Closed)
PollerVWTPEnter(VWTP_ChannelClose);
else if (m_poll_entry.rxmoduleid != 0)
PollerVWTPEnter(VWTP_ChannelSetup);
else if (m_poll_single_rxbuf)
{
m_poll_single_rxbuf->clear();
m_poll_single_rxbuf = NULL;
m_poll_single_rxerr = POLLSINGLE_OK;
m_poll_single_rxdone.Give();
}
return;
}
// Check communication state:
if (m_poll_vwtp.state < VWTP_Idle)
{
// previous channel setup/shutdown hasn't finished, start over:
PollerVWTPEnter(VWTP_ChannelSetup);
}
else
{
// abort transfer in progress if any:
if (m_poll_vwtp.state == VWTP_Transmit)
{
PollerVWTPEnter(VWTP_AbortXfer);
}
else if (m_poll_vwtp.state == VWTP_Receive)
{
PollerVWTPEnter(VWTP_AbortXfer);
usleep(m_poll_vwtp.septime * m_poll_vwtp.blocksize);
}
// start new poll:
PollerVWTPEnter(VWTP_StartPoll);
}
}
/**
* PollerVWTPEnter: channel state transition (internal method)
*/
void OvmsVehicle::PollerVWTPEnter(vwtp_channelstate_t state)
{
CAN_frame_t txframe = {};
txframe.callback = &m_poll_txcallback;
txframe.FIR.B.FF = CAN_frame_std;
if (m_poll_vwtp.state == state) return;
switch (state)
{
case VWTP_ChannelSetup:
{
// Open TP20 channel for current polling entry:
if (m_poll_protocol != VWTP_20)
{
ESP_LOGD(TAG, "PollerVWTPEnter/ChannelSetup: m_poll_protocol mismatch, abort");
}
else
{
m_poll_vwtp.bus = m_poll_bus;
m_poll_vwtp.baseid = m_poll_entry.txmoduleid;
m_poll_vwtp.moduleid = m_poll_entry.rxmoduleid;
m_poll_vwtp.txid = m_poll_vwtp.baseid;
m_poll_vwtp.rxid = m_poll_vwtp.baseid + m_poll_vwtp.moduleid;
// txid & rxid will be changed by the setup response frame, we offer a standard rxid
// matching observed client behaviour with baseid=0x200 → rxid=0x300:
uint16_t offer_rxid = m_poll_vwtp.baseid + 0x100;
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: channel setup request bus=%d txid=%03X rxid=%03X",
m_poll_vwtp.moduleid, m_poll_entry.pollbus, m_poll_vwtp.txid, m_poll_vwtp.rxid);
txframe.MsgID = m_poll_vwtp.txid;
txframe.FIR.B.DLC = 7;
txframe.data.u8[0] = m_poll_vwtp.moduleid;
txframe.data.u8[1] = 0xC0; // setup request
txframe.data.u8[2] = 0x00;
txframe.data.u8[3] = 0x10;
txframe.data.u8[4] = offer_rxid & 0xff;
txframe.data.u8[5] = offer_rxid >> 8;
txframe.data.u8[6] = 0x01;
m_poll_txmsgid = m_poll_vwtp.txid;
m_poll_moduleid_sent = m_poll_vwtp.txid;
m_poll_moduleid_low = m_poll_vwtp.rxid;
m_poll_moduleid_high = m_poll_vwtp.rxid;
m_poll_wait = 2;
m_poll_vwtp.state = VWTP_ChannelSetup;
m_poll_vwtp.lastused = monotonictime;
m_poll_vwtp.bus->Write(&txframe);
}
break;
}
case VWTP_ChannelParams:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: channel params request bus=%d txid=%03X rxid=%03X",
m_poll_vwtp.moduleid, m_poll_entry.pollbus, m_poll_vwtp.txid, m_poll_vwtp.rxid);
txframe.MsgID = m_poll_vwtp.txid;
txframe.FIR.B.DLC = 6;
txframe.data.u8[0] = 0xA0; // params request
txframe.data.u8[1] = 0x0F; // block size: 15 frames
txframe.data.u8[2] = 0x8A; // time to wait for ACK: 10ms x 10 = 100 ms
txframe.data.u8[3] = 0xFF; // always ff
txframe.data.u8[4] = 0x0A; // interval between two packets: 0.1ms x 10 = 1 ms
txframe.data.u8[5] = 0xFF; // always ff
m_poll_txmsgid = m_poll_vwtp.txid;
m_poll_moduleid_sent = m_poll_vwtp.txid;
m_poll_moduleid_low = m_poll_vwtp.rxid;
m_poll_moduleid_high = m_poll_vwtp.rxid;
m_poll_wait = 2;
m_poll_vwtp.state = VWTP_ChannelParams;
m_poll_vwtp.bus->Write(&txframe);
break;
}
case VWTP_ChannelClose:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: close channel bus=%d txid=%03X rxid=%03X",
m_poll_vwtp.moduleid, m_poll_entry.pollbus, m_poll_vwtp.txid, m_poll_vwtp.rxid);
txframe.MsgID = m_poll_vwtp.txid;
txframe.FIR.B.DLC = 1;
txframe.data.u8[0] = 0xA8; // close request
m_poll_txmsgid = m_poll_vwtp.txid;
m_poll_moduleid_sent = m_poll_vwtp.txid;
m_poll_moduleid_low = m_poll_vwtp.rxid;
m_poll_moduleid_high = m_poll_vwtp.rxid;
m_poll_wait = 2;
m_poll_vwtp.state = VWTP_ChannelClose;
m_poll_vwtp.bus->Write(&txframe);
break;
}
case VWTP_Closed:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: channel closed bus=%d txid=%03X rxid=%03X",
m_poll_vwtp.moduleid, m_poll_entry.pollbus, m_poll_vwtp.txid, m_poll_vwtp.rxid);
m_poll_vwtp = {};
m_poll_vwtp.state = VWTP_Closed;
m_poll_wait = 0;
break;
}
case VWTP_Idle:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: idle bus=%d txid=%03X rxid=%03X",
m_poll_vwtp.moduleid, m_poll_entry.pollbus, m_poll_vwtp.txid, m_poll_vwtp.rxid);
m_poll_vwtp.state = VWTP_Idle;
m_poll_vwtp.lastused = monotonictime;
m_poll_wait = 0;
break;
}
case VWTP_StartPoll:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: start poll type=%02X pid=%X",
m_poll_vwtp.moduleid, m_poll_entry.type, m_poll_entry.pid);
if (m_poll_entry.xargs.tag == POLL_TXDATA)
{
m_poll_tx_data = m_poll_entry.xargs.data;
m_poll_tx_remain = m_poll_entry.xargs.datalen;
}
else
{
m_poll_tx_data = m_poll_entry.args.data;
m_poll_tx_remain = m_poll_entry.args.datalen;
}
m_poll_tx_frame = 0;
m_poll_tx_offset = 0;
m_poll_ml_frame = 0;
m_poll_ml_offset = 0;
m_poll_ml_remain = 0;
// fall through to VWTP_Transmit
}
case VWTP_Transmit:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: transmit frame=%u remain=%u",
m_poll_vwtp.moduleid, m_poll_tx_frame, m_poll_tx_remain);
// Transmit next block of frames:
txframe.MsgID = m_poll_vwtp.txid;
int i;
uint8_t opcode;
for (int block = 1; block <= m_poll_vwtp.blocksize; block++)
{
i = 1;
if (m_poll_tx_frame == 0)
{
// First frame:
uint16_t txlen = m_poll_tx_remain + 1;
txframe.data.u8[3] = m_poll_entry.type;
if (POLL_TYPE_HAS_16BIT_PID(m_poll_entry.type))
{
txframe.data.u8[4] = m_poll_entry.pid >> 8;
txframe.data.u8[5] = m_poll_entry.pid & 0xff;
txlen += 2;
i = 6;
}
else if (POLL_TYPE_HAS_8BIT_PID(m_poll_entry.type))
{
txframe.data.u8[4] = m_poll_entry.pid & 0xff;
txlen += 1;
i = 5;
}
else
{
i = 4;
}
txframe.data.u8[1] = txlen >> 8;
txframe.data.u8[2] = txlen & 0xff;
}
while (i < 8 && m_poll_tx_remain > 0)
{
txframe.data.u8[i++] = m_poll_tx_data[m_poll_tx_offset++];
m_poll_tx_remain--;
}
txframe.FIR.B.DLC = i;
if (m_poll_tx_remain == 0)
opcode = 0x10; // last packet, waiting for ACK
else if (block < m_poll_vwtp.blocksize)
opcode = 0x20; // more packets following in this block
else
opcode = 0x00; // more packets following, waiting for ACK
txframe.data.u8[0] = opcode | (m_poll_vwtp.txseqnr & 0x0f);
if (m_poll_vwtp.txseqnr > 0)
usleep(m_poll_vwtp.septime);
m_poll_vwtp.txseqnr++;
m_poll_tx_frame++;
m_poll_vwtp.bus->Write(&txframe);
if (m_poll_tx_remain == 0)
break;
}
m_poll_txmsgid = m_poll_vwtp.txid;
m_poll_moduleid_sent = m_poll_vwtp.txid;
m_poll_moduleid_low = m_poll_vwtp.rxid;
m_poll_moduleid_high = m_poll_vwtp.rxid;
m_poll_wait = 2;
m_poll_vwtp.state = VWTP_Transmit;
m_poll_vwtp.lastused = monotonictime;
break;
}
case VWTP_Receive:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: receive frame=%u remain=%u",
m_poll_vwtp.moduleid, m_poll_ml_frame, m_poll_ml_remain);
m_poll_vwtp.state = VWTP_Receive;
m_poll_vwtp.lastused = monotonictime;
m_poll_wait = 2;
break;
}
case VWTP_AbortXfer:
{
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: abort xfer",
m_poll_vwtp.moduleid);
txframe.MsgID = m_poll_vwtp.txid;
txframe.FIR.B.DLC = 1;
if (m_poll_vwtp.state == VWTP_Receive)
{
txframe.data.u8[0] = 0x90 | (m_poll_vwtp.rxseqnr & 0x0f); // ACK, stop sending
}
else
{
txframe.data.u8[0] = 0x30 | (m_poll_vwtp.txseqnr & 0x0f); // last frame, abort
m_poll_vwtp.txseqnr++;
}
m_poll_txmsgid = m_poll_vwtp.txid;
m_poll_moduleid_sent = m_poll_vwtp.txid;
m_poll_moduleid_low = m_poll_vwtp.rxid;
m_poll_moduleid_high = m_poll_vwtp.rxid;
m_poll_tx_remain = 0;
m_poll_ml_remain = 0;
m_poll_vwtp.state = VWTP_Idle;
m_poll_vwtp.lastused = monotonictime;
m_poll_wait = 0;
m_poll_vwtp.bus->Write(&txframe);
break;
}
default:
ESP_LOGD(TAG, "PollerVWTPEnter[%02X]: idle bus=%d txid=%03X rxid=%03X",
m_poll_vwtp.moduleid, m_poll_entry.pollbus, m_poll_vwtp.txid, m_poll_vwtp.rxid);
m_poll_vwtp.state = VWTP_Idle;
m_poll_wait = 0;
break;
}
}
/**
* PollerVWTPTxCallback: CAN transmission result (internal)
*/
void OvmsVehicle::PollerVWTPTxCallback(const CAN_frame_t* frame, bool success)
{
if (!success)
{
// Todo: try to recover by repeating the transmission (unless CAN is offline)
// -- this needs a higher frequency ticker, e.g. 100 ms
// For now assume channel is terminated:
PollerVWTPEnter(VWTP_Closed);
}
}
/**
* PollerVWTPReceive: process VW-TP frame received (internal)
*/
bool OvmsVehicle::PollerVWTPReceive(CAN_frame_t* frame, uint32_t msgid)
{
OvmsRecMutexLock lock(&m_poll_mutex);
// Log utility:
auto logFrameDump = [=](const char* msg)
{
char *hexdump = NULL;
FormatHexDump(&hexdump, (const char*)frame->data.u8, frame->FIR.B.DLC, frame->FIR.B.DLC);
ESP_LOGW(TAG, "PollerVWTPReceive[%02X]: %s: %s", m_poll_vwtp.moduleid, msg, hexdump ? hexdump : "-");
if (hexdump) free(hexdump);
};
// Send channel keepalive response (0xA3 response):
auto sendPong = [=]()
{
CAN_frame_t txframe = {};
txframe.callback = &m_poll_txcallback;
txframe.FIR.B.FF = CAN_frame_std;
txframe.MsgID = m_poll_vwtp.txid;
txframe.FIR.B.DLC = 6;
txframe.data.u8[0] = 0xA1; // params response
txframe.data.u8[1] = 0x0F; // block size: 15 frames
txframe.data.u8[2] = 0x8A; // time to wait for ACK: 10ms x 10 = 100 ms
txframe.data.u8[3] = 0xFF; // always ff
txframe.data.u8[4] = 0x0A; // interval between two packets: 0.1ms x 10 = 1 ms
txframe.data.u8[5] = 0xFF; // always ff
m_poll_vwtp.bus->Write(&txframe);
};
// Send ACK:
auto sendAck = [=]()
{
CAN_frame_t txframe = {};
txframe.callback = &m_poll_txcallback;
txframe.FIR.B.FF = CAN_frame_std;
txframe.MsgID = m_poll_vwtp.txid;
txframe.FIR.B.DLC = 1;
txframe.data.u8[0] = 0xB0 | (m_poll_vwtp.rxseqnr & 0x0f); // ACK, continue
m_poll_vwtp.bus->Write(&txframe);
};
// After locking the mutex, check again for poll expectance match:
if (m_poll_vwtp.state == VWTP_Closed || !m_poll_vwtp.bus || msgid != m_poll_vwtp.rxid)
{
logFrameDump("dropping unexpected frame");
return false;
}
switch (m_poll_vwtp.state)
{
case VWTP_ChannelSetup:
{
// Expecting setup response, check if the frame fits:
uint8_t opcode = frame->data.u8[1];
if (frame->FIR.B.DLC != 7 || opcode < 0xD0 || opcode > 0xD8)
{
logFrameDump("channel setup: invalid/unexpected frame");
return false;
}
else if (opcode != 0xD0)
{
// Setup failed:
ESP_LOGD(TAG, "PollerVWTPReceive[%02X]: channel setup failed opcode=%02X", m_poll_vwtp.moduleid, opcode);
m_poll_vwtp.bus = NULL;
m_poll_vwtp.state = VWTP_Closed;
m_poll_wait = 0;
}
else
{
// Setup success, configure txid & rxid:
m_poll_vwtp.rxid = (uint16_t)frame->data.u8[3] << 8 | frame->data.u8[2];
m_poll_vwtp.txid = (uint16_t)frame->data.u8[5] << 8 | frame->data.u8[4];
ESP_LOGD(TAG, "PollerVWTPReceive[%02X]: channel setup OK, assigned txid=%03X rxid=%03X",
m_poll_vwtp.moduleid, m_poll_vwtp.txid, m_poll_vwtp.rxid);
// …and send channel parameters:
PollerVWTPEnter(VWTP_ChannelParams);
}
break;
}
case VWTP_ChannelParams:
{
// Expecting params response or channel abort:
uint8_t opcode = (frame->FIR.B.DLC > 0) ? frame->data.u8[0] : 0;
if (opcode == 0xA1)
{
// Params received, configure block size & timing:
const int timeunit[4] = { 100, 1000, 10000, 100000 };
m_poll_vwtp.blocksize = frame->data.u8[1];
m_poll_vwtp.acktime = (frame->data.u8[2] & 0b00111111) * timeunit[(frame->data.u8[2] & 0b11000000) >> 6];
m_poll_vwtp.septime = (frame->data.u8[4] & 0b00111111) * timeunit[(frame->data.u8[4] & 0b11000000) >> 6];
ESP_LOGD(TAG, "PollerVWTPReceive[%02X]: channel params OK: bs=%d acktime=%uus septime=%uus",
m_poll_vwtp.moduleid, m_poll_vwtp.blocksize, m_poll_vwtp.acktime, m_poll_vwtp.septime);
// …and proceed to data transmission:
PollerVWTPEnter(VWTP_StartPoll);
}
else if (opcode == 0xA8)
{
// Channel abort received, send ACK & set closed state:
PollerVWTPEnter(VWTP_ChannelClose);
PollerVWTPEnter(VWTP_Closed);
// TODO: application error callback
}
else
{
logFrameDump("channel params: invalid/unexpected frame");
return false;
}
break;
}
case VWTP_ChannelClose:
{
// Expecting channel close ACK:
uint8_t opcode = (frame->FIR.B.DLC == 1) ? frame->data.u8[0] : 0;
if (opcode == 0xA8)
{
// Close ACK received:
PollerVWTPEnter(VWTP_Closed);
// Check if we shall open another channel:
if (m_poll_protocol == VWTP_20 && m_poll_entry.rxmoduleid != 0)
PollerVWTPEnter(VWTP_ChannelSetup);
else if (m_poll_single_rxbuf)
{
m_poll_single_rxbuf->clear();
m_poll_single_rxbuf = NULL;
m_poll_single_rxerr = POLLSINGLE_OK;
m_poll_single_rxdone.Give();
}
}
else
{
logFrameDump("channel params: invalid/unexpected frame");
return false;
}
break;
}
case VWTP_Idle:
{
uint8_t opcode = frame->data.u8[0];
if (opcode == 0xA3)
{
// Channel ping received:
sendPong();
}
else if (opcode == 0xA8)
{
// Channel abort received, send ACK & set closed state:
PollerVWTPEnter(VWTP_ChannelClose);
PollerVWTPEnter(VWTP_Closed);
}
else if ((opcode & 0xf0) <= 0x30)
{
// Out of band data frame received in idle state, send ACK/abort:
m_poll_vwtp.rxseqnr++;
CAN_frame_t txframe = {};
txframe.callback = &m_poll_txcallback;
txframe.FIR.B.FF = CAN_frame_std;
txframe.MsgID = m_poll_vwtp.txid;
txframe.FIR.B.DLC = 1;
txframe.data.u8[0] = 0x90 | (m_poll_vwtp.rxseqnr & 0x0f); // ACK, abort
m_poll_vwtp.bus->Write(&txframe);
}
break;
}
case VWTP_Transmit:
{
uint8_t opcode = (frame->FIR.B.DLC == 1) ? frame->data.u8[0] : 0;
if ((opcode & 0xf0) == 0xB0)
{
// ACK, continue:
if (m_poll_tx_remain > 0)
PollerVWTPEnter(VWTP_Transmit);
else
PollerVWTPEnter(VWTP_Receive);
}
else if ((opcode & 0xf0) == 0x90)
{
// ACK, abort:
ESP_LOGD(TAG, "PollerVWTPReceive[%02X]: got abort request during transmission at frame=%u remain=%u",
m_poll_vwtp.moduleid, m_poll_tx_frame, m_poll_tx_remain);
m_poll_tx_remain = 0;
PollerVWTPEnter(VWTP_Idle);
// TODO: application error callback
}
else if (opcode == 0xA3)
{
// Channel ping received:
sendPong();
}
else if (opcode == 0xA8)
{
// Channel abort received, send ACK & set closed state:
PollerVWTPEnter(VWTP_ChannelClose);
PollerVWTPEnter(VWTP_Closed);
// TODO: application error callback
}
else
{
// Possibly remaining frames from previously aborted xfer:
logFrameDump("unhandled frame during transmission");
}
break;
}
case VWTP_Receive:
{
uint8_t opcode = frame->data.u8[0];
if (opcode == 0xA3)
{
// Channel ping received:
sendPong();
}
else if (opcode == 0xA8)
{
// Channel abort received, send ACK & set closed state:
PollerVWTPEnter(VWTP_ChannelClose);
PollerVWTPEnter(VWTP_Closed);
// TODO: application error callback
}
else if (opcode < 0x40)
{
//
// Data frame received: check sequence number
//
m_poll_vwtp.lastused = monotonictime;
uint32_t rxseqnr = m_poll_vwtp.rxseqnr++;
if ((opcode & 0x0f) != (rxseqnr & 0x0f))
{
logFrameDump("received out of sequence frame, abort");
PollerVWTPEnter(VWTP_AbortXfer);
return true;
}
//
// Get & validate OBD/UDS meta data
//
uint8_t* tp_data; // TP frame data section address
uint8_t tp_datalen; // TP frame data section length (0…7)
uint8_t response_type = 0; // OBD/UDS response type tag (expected: 0x40 + request type)
uint16_t response_pid = 0; // OBD/UDS response PID (expected: request PID)
uint8_t* response_data = NULL; // OBD/UDS frame payload address
uint16_t response_datalen = 0; // OBD/UDS frame payload length (0…7)
uint8_t error_type = 0; // OBD/UDS error response service type (expected: request type)
uint8_t error_code = 0; // OBD/UDS error response code (see ISO 14229 Annex A.1)
if (m_poll_ml_frame == 0)
{
// First frame: extract & validate meta data
// Note: upper nibble of byte 1 masked out, length assumed to by 12 bit
// and we've seen 0x80 on byte 1 for response type UDS_RESP_NRC_RCRRP
m_poll_ml_remain = (frame->data.u8[1] & 0x0f) << 8 | frame->data.u8[2];
tp_data = &frame->data.u8[3];
tp_datalen = frame->FIR.B.DLC - 3;
response_type = tp_data[0];
if (response_type == UDS_RESP_TYPE_NRC)
{
error_type = tp_data[1];
error_code = tp_data[2];
}
else if (POLL_TYPE_HAS_16BIT_PID(response_type-0x40))
{
response_pid = tp_data[1] << 8 | tp_data[2];
response_data = &tp_data[3];
response_datalen = tp_datalen - 3;
}
else if (POLL_TYPE_HAS_8BIT_PID(response_type-0x40))
{
response_pid = tp_data[1];
response_data = &tp_data[2];
response_datalen = tp_datalen - 2;
}
else
{
response_pid = m_poll_pid;
response_data = &tp_data[1];
response_datalen = tp_datalen - 1;
}
}
else
{
// Consecutive frame:
tp_data = &frame->data.u8[1];
tp_datalen = frame->FIR.B.DLC - 1;
response_type = 0x40+m_poll_type;
response_pid = m_poll_pid;
response_data = tp_data;
response_datalen = tp_datalen;
}
//
// Process OBD/UDS payload
//
if (response_type == UDS_RESP_TYPE_NRC && error_type == m_poll_type)
{
// Send ACK?
if ((opcode & 0xf0) <= 0x10)
sendAck();
// Negative Response Code:
if (error_code == UDS_RESP_NRC_RCRRP)
{
// Info: requestCorrectlyReceived-ResponsePending (server busy processing the request)
ESP_LOGD(TAG, "PollerVWTPReceive[%02X]: got OBD/UDS info %02X(%X) code=%02X (pending)",
m_poll_vwtp.moduleid, m_poll_type, m_poll_pid, error_code);
// reset wait time:
m_poll_wait = 2;
return true;
}
else
{
// Error: forward to application:
ESP_LOGD(TAG, "PollerVWTPReceive[%02X]: process OBD/UDS error %02X(%X) code=%02X",
m_poll_vwtp.moduleid, m_poll_type, m_poll_pid, error_code);
// Running single poll?
if (m_poll_single_rxbuf)
{
m_poll_single_rxerr = error_code;
m_poll_single_rxbuf = NULL;
m_poll_single_rxdone.Give();
}
else
{
IncomingPollError(frame->origin, m_poll_type, m_poll_pid, error_code);
}
// abort receive:
m_poll_ml_remain = 0;
PollerVWTPEnter(VWTP_Idle);
}
}
else if (response_type == 0x40+m_poll_type && response_pid == m_poll_pid)
{
// Send ACK?
if ((opcode & 0xf0) <= 0x10)
sendAck();
// Normal matching poll response, forward to application:
m_poll_ml_remain -= tp_datalen;
ESP_LOGD(TAG, "PollerVWTPReceive[%02X]: process OBD/UDS response %02X(%X) frm=%u len=%u off=%u rem=%u",
m_poll_vwtp.moduleid, m_poll_type, m_poll_pid,
m_poll_ml_frame, response_datalen, m_poll_ml_offset, m_poll_ml_remain);
// Running single poll?
if (m_poll_single_rxbuf)
{
if (m_poll_ml_frame == 0)
{
m_poll_single_rxbuf->clear();
m_poll_single_rxbuf->reserve(response_datalen + m_poll_ml_remain);
}
m_poll_single_rxbuf->append((char*)response_data, response_datalen);
if (m_poll_ml_remain == 0)
{
m_poll_single_rxerr = 0;
m_poll_single_rxbuf = NULL;
m_poll_single_rxdone.Give();
}
}
else
{
IncomingPollReply(frame->origin, m_poll_type, m_poll_pid, response_data, response_datalen, m_poll_ml_remain);
}
}
else
{
// Response type / PID mismatch: log & abort
char *hexdump = NULL;
FormatHexDump(&hexdump, (const char*)frame->data.u8, frame->FIR.B.DLC, frame->FIR.B.DLC);
ESP_LOGW(TAG, "PollerVWTPReceive[%02X]: OBD/UDS response type/PID mismatch, got %02X(%X) vs %02X(%X) => ignoring: %s",
m_poll_vwtp.moduleid, response_type, response_pid, 0x40+m_poll_type, m_poll_pid, hexdump ? hexdump : "-");
if (hexdump) free(hexdump);
PollerVWTPEnter(VWTP_AbortXfer);
return false;
}
// Do we expect more data?
if (m_poll_ml_remain)
{
m_poll_ml_frame++;
m_poll_ml_offset += response_datalen;
m_poll_wait = 2;
}
else
{
// Request response complete:
PollerVWTPEnter(VWTP_Idle);
}
}
else
{
logFrameDump("unhandled frame during reception");
}
break;
}
default:
// Ignore all frames received in other states
break;
}
// Immediately send the next poll for this tick if…
// - we are not waiting for another frame
// - poll throttling is unlimited or limit isn't reached yet
if (m_poll_wait == 0 &&
(!m_poll_sequence_max || m_poll_sequence_cnt < m_poll_sequence_max))
{
PollerSend(false);
}
return true;
}
/**
* PollerVWTPTicker: per second channel maintenance (internal)
*/
void OvmsVehicle::PollerVWTPTicker()
{
if (m_poll_wait > 0)
{
// State timeout?
if (m_poll_vwtp.state == VWTP_ChannelSetup || m_poll_vwtp.state == VWTP_ChannelParams)
{
ESP_LOGD(TAG, "PollerVWTPTicker[%02X]: setup/params timeout", m_poll_vwtp.moduleid);
PollerVWTPEnter(VWTP_Closed);
}
else if (m_poll_vwtp.state == VWTP_ChannelClose)
{
ESP_LOGD(TAG, "PollerVWTPTicker[%02X]: close timeout", m_poll_vwtp.moduleid);
PollerVWTPEnter(VWTP_Closed);
if (m_poll_protocol == VWTP_20)
PollerVWTPStart(true);
}
}
else
{
// Poll timeout:
if (m_poll_vwtp.state != VWTP_Closed && m_poll_vwtp.state != VWTP_Idle)
{
ESP_LOGD(TAG, "PollerVWTPTicker[%02X]: poll timeout", m_poll_vwtp.moduleid);
PollerVWTPEnter(VWTP_ChannelClose);
}
}
// Keepalive timeout?
if (m_poll_vwtp.state == VWTP_Idle && m_poll_ch_keepalive > 0 &&
m_poll_vwtp.lastused + m_poll_ch_keepalive < monotonictime)
{
ESP_LOGD(TAG, "PollerVWTPTicker[%02X]: channel inactivity timeout", m_poll_vwtp.moduleid);
PollerVWTPEnter(VWTP_ChannelClose);
}
}
Reference in a new issue Copy permalink