OVMS3/OVMS.V3/components/retools_pidscan/src/retools_pid.cpp

634 lines
19 KiB
C++

/*
; Project: Open Vehicle Monitor System
; Date: 14th September 2020
;
; Changes:
; 1.0 Initial release
;
; (C) 2011 Michael Stegen / Stegen Electronics
; (C) 2011-2017 Mark Webb-Johnson
; (C) 2011 Sonny Chen @ EPRO/DX
; (C) 2020 Chris Staite
;
; 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 = "re-pid";
#include "retools_pid.h"
#include "vehicle.h"
namespace {
bool ReadHexString(const char* value, unsigned long& output)
{
char* ptr = nullptr;
errno = 0;
output = strtoul(value, &ptr, 16);
if (ptr == value || errno != 0 || *ptr)
{
return false;
}
return true;
}
bool ReadHexRange(const char* value, unsigned long& output1, unsigned long& output2)
{
char* ptr = nullptr;
errno = 0;
output1 = strtoul(value, &ptr, 16);
if (ptr == value || errno != 0 || (*ptr && *ptr != '-'))
{
return false;
}
if (!*ptr)
{
output2 = output1;
}
else
{
value = ptr + 1;
output2 = strtoul(value, &ptr, 16);
if (ptr == value || errno != 0 || *ptr)
{
return false;
}
}
return true;
}
canbus* GetCan(int bus)
{
char name[] = "canx";
name[3] = '0' + bus;
auto can = reinterpret_cast<canbus*>(MyPcpApp.FindDeviceByName(name));
if (can != nullptr && (can->GetPowerMode() != On || can->m_mode != CAN_MODE_ACTIVE))
{
can = nullptr;
}
return can;
}
OvmsReToolsPidScanner* s_scanner = nullptr;
void scanStart(int, OvmsWriter* writer, OvmsCommand*, int argc, const char* const* argv)
{
if (s_scanner != nullptr)
{
if (s_scanner->Complete())
{
delete s_scanner;
s_scanner = nullptr;
}
else
{
writer->puts(
"Error: Scan already in progress, stop it or wait for it to complete"
);
return;
}
}
unsigned long bus = 0, ecu = 0, rxid_low = 0, rxid_high = 0, start = 0, end = 0;
int timeout = 3;
unsigned long polltype = VEHICLE_POLL_TYPE_OBDIIEXTENDED;
unsigned long step = 1;
bool valid = true, have_rxid = false;
int argpos = 0;
for (int i = 0; i < argc; i++)
{
if (argv[i][0] == '-')
{
switch (argv[i][1])
{
case 'r':
if (!ReadHexRange(argv[i]+2, rxid_low, rxid_high) ||
rxid_low > 0x7ff || rxid_high > 0x7ff || rxid_high < rxid_low)
{
writer->printf("Error: Invalid RX ID range %s\n", argv[i]+2);
valid = false;
}
else
{
have_rxid = true;
}
break;
case 's':
if (!ReadHexString(argv[i]+2, step) || step < 1 || step > 0xffff)
{
writer->printf("Error: Invalid step size %s\n", argv[i]+2);
valid = false;
}
break;
case 't':
if (!ReadHexString(argv[i]+2, polltype) || polltype < 1 || polltype > 0xff)
{
writer->printf("Error: Invalid poll type %s\n", argv[i]+2);
valid = false;
}
break;
case 'x':
timeout = atoi(argv[i]+2);
if (timeout < 1 || timeout > 10)
{
writer->printf("Error: Invalid timeout %s\n", argv[i]+2);
valid = false;
}
break;
default:
writer->printf("Error: Invalid argument %s\n", argv[i]);
valid = false;
break;
}
}
else
{
switch (++argpos)
{
case 1:
if (!ReadHexString(argv[i], bus) || bus < 1 || bus > 4)
{
writer->printf("Error: Invalid bus to scan %s\n", argv[i]);
valid = false;
}
break;
case 2:
if (!ReadHexString(argv[i], ecu) || ecu <= 0 || ecu >= 0xfff)
{
writer->printf("Error: Invalid ECU Id to scan %s\n", argv[i]);
valid = false;
}
break;
case 3:
if (!ReadHexString(argv[i], start) || start > 0xffff)
{
writer->printf("Error: Invalid Start PID to scan %s\n", argv[i]);
valid = false;
}
break;
case 4:
if (!ReadHexString(argv[i], end) || end > 0xffff)
{
writer->printf("Error: Invalid End PID to scan %s\n", argv[i]);
valid = false;
}
break;
default:
writer->printf("Error: Invalid argument %s\n", argv[i]);
valid = false;
break;
}
}
}
if (start > end)
{
writer->printf(
"Error: Invalid Start PID %04lx is after End PID %04lx\n", start, end
);
valid = false;
}
else if (step > 1)
{
end = start + ((end - start) / step) * step;
}
if (POLL_TYPE_HAS_8BIT_PID(polltype) && end > 0xff)
{
writer->printf("Error: Poll type %lx PID range is 00..ff\n", polltype);
valid = false;
}
if (!valid)
{
return;
}
if (!have_rxid)
{
rxid_low = rxid_high = ecu + 8;
}
canbus* can = GetCan(bus);
if (can == nullptr)
{
writer->puts("CAN not started in active mode, please start and try again");
valid = false;
}
if (valid)
{
s_scanner = new OvmsReToolsPidScanner(can, ecu, rxid_low, rxid_high, polltype, start, end, step, timeout);
writer->printf("Scan started: bus %ld, ecu %lx, rxid %lx-%lx, polltype %lx, PID %lx-%lx (step %lx), timeout %d seconds\n",
bus, ecu, rxid_low, rxid_high, polltype, start, end, step, timeout);
}
}
void scanStatus(int, OvmsWriter* writer, OvmsCommand*, int, const char* const*)
{
if (s_scanner == nullptr)
{
writer->puts("No scan running");
}
else if (s_scanner->Complete())
{
writer->printf(
"Scan complete (%03x %04x-%04x)\n",
s_scanner->Ecu(), s_scanner->Start(), s_scanner->End()
);
}
else
{
writer->printf(
"Scan running (%03x %04x-%04x): %04x\n",
s_scanner->Ecu(), s_scanner->Start(), s_scanner->End(), s_scanner->Current()
);
}
if (s_scanner != nullptr)
{
s_scanner->Output(writer);
}
}
void scanStop(int, OvmsWriter* writer, OvmsCommand*, int, const char* const*)
{
if (s_scanner == nullptr)
{
writer->puts("Error: No scan currently in progress");
return;
}
writer->printf(
"Scan results (%03x %04x-%04x):\n",
s_scanner->Ecu(), s_scanner->Start(),
s_scanner->Complete() ? s_scanner->End() : s_scanner->Current()
);
s_scanner->Output(writer);
delete s_scanner;
s_scanner = nullptr;
writer->puts("Scan stopped");
}
} // anon namespace
OvmsReToolsPidScanner::OvmsReToolsPidScanner(
canbus* bus, uint16_t ecu, uint16_t rxid_low, uint16_t rxid_high,
uint8_t polltype, int start, int end, int step, uint8_t timeout) :
m_frameCallback(std::bind(
&OvmsReToolsPidScanner::FrameCallback, this,
std::placeholders::_1, std::placeholders::_2
)),
m_bus(bus),
m_id(ecu),
m_rxid_low(rxid_low),
m_rxid_high(rxid_high),
m_pollType(polltype),
m_startPid(start),
m_endPid(end),
m_pidStep(step),
m_currentPid(end+1),
m_ticker(0u),
m_timeout(timeout),
m_lastFrame(0u),
m_startTime(0u),
m_lastResponseTime(0u),
m_mfRemain(0u),
m_task(nullptr),
m_rxqueue(nullptr),
m_found(),
m_foundMutex()
{
m_rxqueue = xQueueCreate(20,sizeof(CAN_frame_t));
xTaskCreatePinnedToCore(
&OvmsReToolsPidScanner::Task, "OVMS RE PID", 4096, this, 5, &m_task, CORE(1)
);
MyCan.RegisterListener(m_rxqueue, true);
m_currentPid = m_startPid - m_pidStep;
MyEvents.RegisterEvent(
TAG, "ticker.1",
std::bind(
&OvmsReToolsPidScanner::Ticker1, this,
std::placeholders::_1, std::placeholders::_2
)
);
MyEvents.SignalEvent("retools.pidscan.start", NULL);
time(&m_startTime);
SendNextFrame();
}
OvmsReToolsPidScanner::~OvmsReToolsPidScanner()
{
if (m_rxqueue)
{
MyEvents.DeregisterEvent(TAG);
MyCan.DeregisterListener(m_rxqueue);
vQueueDelete(m_rxqueue);
vTaskDelete(m_task);
MyEvents.SignalEvent("retools.pidscan.stop", NULL);
}
}
void OvmsReToolsPidScanner::Output(OvmsWriter* writer) const
{
OvmsMutexLock lock(&m_foundMutex);
struct tm tmu;
char tb[64];
localtime_r(&m_startTime, &tmu);
strftime(tb, sizeof(tb), "%Y-%m-%d %H:%M:%S %Z", &tmu);
writer->printf("Scan started : %s\n", tb);
if (m_found.size() == 0)
{
writer->puts("No valid responses received.");
}
else
{
localtime_r(&m_lastResponseTime, &tmu);
strftime(tb, sizeof(tb), "%Y-%m-%d %H:%M:%S %Z", &tmu);
writer->printf("Last response: %s\n", tb);
for (auto& found : m_found)
{
uint16_t rxid, pid;
std::vector<uint8_t> data;
std::tie(rxid, pid, data) = found;
writer->printf("%03x[%03x]:%04x", m_id, rxid, pid);
for (auto& byte : data)
{
writer->printf(" %02x", byte);
}
writer->printf("\n");
}
}
}
void OvmsReToolsPidScanner::Task(void *self)
{
reinterpret_cast<OvmsReToolsPidScanner*>(self)->Task();
}
void OvmsReToolsPidScanner::Task()
{
CAN_frame_t frame;
while (1)
{
if (xQueueReceive(
m_rxqueue, &frame, static_cast<portTickType>(portMAX_DELAY)
) == pdTRUE)
{
if (frame.origin == m_bus)
{
IncomingPollFrame(&frame);
}
}
}
}
void OvmsReToolsPidScanner::Ticker1(std::string, void*)
{
++m_ticker;
if (m_currentPid <= m_endPid && m_ticker - m_lastFrame > m_timeout)
{
ESP_LOGE(TAG, "Frame response timeout for %x:%x", m_id, m_currentPid);
SendNextFrame();
}
}
void OvmsReToolsPidScanner::FrameCallback(const CAN_frame_t* frame, bool success)
{
if (success == false)
{
ESP_LOGE(TAG, "Error sending the frame, terminating scan");
m_currentPid = m_endPid + 1;
MyEvents.SignalEvent("retools.pidscan.done", NULL);
}
}
void OvmsReToolsPidScanner::SendNextFrame()
{
if (m_currentPid == m_endPid)
{
m_currentPid = m_endPid + 1;
ESP_LOGI(TAG, "Scan of %x complete", m_id);
MyEvents.SignalEvent("retools.pidscan.done", NULL);
return;
}
m_currentPid += m_pidStep;
CAN_frame_t sendFrame = {
m_bus,
&m_frameCallback,
{ .B = { 8, 0, CAN_no_RTR, CAN_frame_std, 0 } },
m_id,
0
};
if (POLL_TYPE_HAS_16BIT_PID(m_pollType))
{
sendFrame.data = { .u8 = {
(ISOTP_FT_SINGLE << 4) + 3, m_pollType,
static_cast<uint8_t>(m_currentPid >> 8),
static_cast<uint8_t>(m_currentPid & 0xff)
} };
}
else
{
sendFrame.data = { .u8 = {
(ISOTP_FT_SINGLE << 4) + 2, m_pollType,
static_cast<uint8_t>(m_currentPid & 0xff)
} };
}
m_mfRemain = 0u;
if (m_bus->Write(&sendFrame) == ESP_FAIL)
{
ESP_LOGE(TAG, "Error sending test frame to PID %x:%x", m_id, m_currentPid);
m_currentPid = m_endPid + 1;
}
else
{
ESP_LOGV(TAG, "Sending test frame to PID %x:%x", m_id, m_currentPid);
m_lastFrame = m_ticker;
}
}
void OvmsReToolsPidScanner::IncomingPollFrame(const CAN_frame_t* frame)
{
if (m_currentPid > m_endPid)
{
return;
}
uint8_t frameType = frame->data.u8[0] >> 4;
uint16_t frameLength = frame->data.u8[0] & 0x0f;
const uint8_t* data = &frame->data.u8[1];
uint16_t dataLength = frameLength;
if (frame->MsgID < m_rxid_low || frame->MsgID > m_rxid_high)
{
// Frame not for us
return;
}
if (frameType == ISOTP_FT_SINGLE)
{
// All good
m_mfRemain = 0;
}
else if (frameType == ISOTP_FT_FIRST)
{
frameLength = (frameLength << 8) | data[0];
++data;
dataLength = (frameLength > 6 ? 6 : frameLength);
m_mfRemain = frameLength - dataLength;
}
else if (frameType == ISOTP_FT_CONSECUTIVE)
{
dataLength = (m_mfRemain > 7 ? 7 : m_mfRemain);
m_mfRemain -= dataLength;
}
else
{
// Don't support any other types
ESP_LOGI(TAG, "Received unknown frame type %x", frameType);
return;
}
if (frameType == ISOTP_FT_CONSECUTIVE)
{
{
OvmsMutexLock lock(&m_foundMutex);
std::copy(data, &data[dataLength], std::back_inserter(std::get<2>(m_found.back())));
}
if (m_mfRemain == 0u)
{
SendNextFrame();
}
else
{
m_lastFrame = m_ticker;
}
}
else if (dataLength == 3 && data[0] == UDS_RESP_TYPE_NRC && data[1] == m_pollType)
{
if (data[2] == UDS_RESP_NRC_RCRRP)
{
// ResponsePending: ignore, keep waiting
ESP_LOGD(TAG, "ResponsePending from %x[%x]:%x",
m_id, frame->MsgID, m_currentPid);
}
else
{
// …other negative response code:
ESP_LOGD(TAG, "Negative response from %x[%x]:%x code %02x",
m_id, frame->MsgID, m_currentPid, data[2]);
SendNextFrame();
}
}
else if (dataLength > 3 && data[0] == m_pollType + 0x40)
{
// Success
uint16_t responsePid;
const uint8_t* payload;
uint16_t payloadLength;
if (POLL_TYPE_HAS_16BIT_PID(m_pollType))
{
responsePid = data[1] << 8 | data[2];
payload = &data[3];
payloadLength = dataLength - 3;
}
else
{
responsePid = data[1];
payload = &data[2];
payloadLength = dataLength - 2;
}
if (responsePid == m_currentPid)
{
ESP_LOGD(
TAG,
"Success response from %x[%x]:%x length %d (0x%02x 0x%02x 0x%02x 0x%02x%s)",
m_id, frame->MsgID, m_currentPid, payloadLength, payload[0], payload[1], payload[2], payload[3],
(frameType == 0 ? "" : " ...")
);
time(&m_lastResponseTime);
if (frameType == ISOTP_FT_FIRST)
{
CAN_frame_t flowControl = {
m_bus,
&m_frameCallback,
{ .B = { 8, 0, CAN_no_RTR, CAN_frame_std, 0 } },
m_id,
{ .u8 = { 0x30, 0, 25, 0, 0, 0, 0, 0 } }
};
if (m_bus->Write(&flowControl) == ESP_FAIL)
{
ESP_LOGE(
TAG, "Error sending flow control frame to PID %x:%x",
m_id, m_currentPid
);
m_currentPid = m_endPid + 1;
}
else
{
m_lastFrame = m_ticker;
}
std::vector<uint8_t> response;
response.reserve(frameLength);
std::copy(payload, &payload[payloadLength], std::back_inserter(response));
OvmsMutexLock lock(&m_foundMutex);
m_found.push_back(std::make_tuple(frame->MsgID, responsePid, std::move(response)));
}
else
{
OvmsMutexLock lock(&m_foundMutex);
m_found.push_back(std::make_tuple(frame->MsgID,
responsePid, std::vector<uint8_t>(payload, &payload[payloadLength])
));
}
if (m_mfRemain == 0u)
{
SendNextFrame();
}
}
}
}
class OvmsReToolsPidScannerInit
{
public:
OvmsReToolsPidScannerInit();
} OvmsReToolsPidScannerInit __attribute__ ((init_priority (8801)));
OvmsReToolsPidScannerInit::OvmsReToolsPidScannerInit()
{
OvmsCommand* cmd_reobdii = MyCommandApp.FindCommandFullName("re obdii");
if (cmd_reobdii == nullptr)
{
ESP_LOGE(TAG, "PID scan command depends on re command");
return;
}
OvmsCommand* cmd_scan = cmd_reobdii->RegisterCommand("scan", "ECU PID scanning tool");
cmd_scan->RegisterCommand(
"start", "Scan PIDs on an ECU in a given range", &scanStart,
"<bus> <ecu> <start_pid> <end_pid> [-s<pid_step>] [-r<rxid>[-<rxid>]] [-t<poll_type>] [-x<timeout>]\n"
"Give all values except bus and timeout hexadecimal. Options can be positioned anywhere.\n"
"Default <rxid> is <ecu>+8, try 0-7ff if you don't know the responding ID.\n"
"Default <poll_type> is 22 (ReadDataByIdentifier, 16 bit PID).\n"
"Default <pid_step> is 1.\n"
"Default <timeout> is 3 seconds.",
4, 8
);
cmd_scan->RegisterCommand("status", "The status of the PID scan", &scanStatus);
cmd_scan->RegisterCommand("stop", "Stop the current scan", &scanStop);
}