retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

J2534_WIN: Added ISO15765 multiframe messages compliant with new docs.

master
Jessy Diamond Exum 2017-10-10 01:16:03 -07:00
parent 4431147248
commit c132fadb44
8 changed files with 102 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
drivers/windows/pandaJ2534DLL/J2534Connection.cpp
View File

@ -116,6 +116,14 @@ void J2534Connection::schedultMsgTx(std::shared_ptr<MessageTx> msgout) {
}
}
void J2534Connection::rescheduleExistingTxMsgs() {
if (auto panda_ps = this->panda_dev.lock()) {
synchronized(staged_writes_lock) {
panda_ps->registerConnectionTx(shared_from_this());
}
}
}
void J2534Connection::processMessageReceipt(const J2534Frame& msg) {
//TX_MSG_TYPE should be set in RxStatus
if (!check_bmask(msg.RxStatus, TX_MSG_TYPE)) return;

2
drivers/windows/pandaJ2534DLL/J2534Connection.h
View File

@ -72,6 +72,8 @@ public:
void schedultMsgTx(std::shared_ptr<MessageTx> msgout);
void rescheduleExistingTxMsgs();
std::shared_ptr<PandaJ2534Device> getPandaDev() {
if (auto panda_dev_sp = this->panda_dev.lock())
return panda_dev_sp;

16
drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.cpp
View File

@ -63,30 +63,30 @@ void J2534Connection_ISO15765::processMessage(const J2534Frame& msg) {
switch (msg_get_type(msg, addrlen)) {
case FRAME_FLOWCTRL:
{
/*if (msg.Data.size() < addrlen + 3) return;
if (msg.Data.size() < addrlen + 3) return;
uint8_t flow_status = msg.Data[addrlen] & 0x0F;
uint8_t block_size = msg.Data[addrlen + 1];
uint8_t st_min = msg.Data[addrlen + 2];
auto txConvo = std::static_pointer_cast<MessageTx_ISO15765>(this->txbuff.back());
switch (flow_status) {
case FLOWCTRL_CONTINUE: {
if (st_min > 0xF9) break;
if (st_min >= 0xf1 && st_min <= 0xf9) {
this->rxConversations[fid]->tx_flowcontrol(block_size, std::chrono::microseconds((st_min & 0x0F) * 100));
txConvo->tx_flowcontrol(block_size, std::chrono::microseconds((st_min & 0x0F) * 100), block_size==0);
} else {
this->rxConversations[fid]->tx_flowcontrol(block_size, std::chrono::microseconds(st_min * 1000));
}
if (auto panda_dev_sp = this->panda_dev.lock()) {
panda_dev_sp->registerMultiPartTx(this->conversations[fid]);
txConvo->tx_flowcontrol(block_size, std::chrono::microseconds(st_min * 1000), block_size==0);
}
this->rescheduleExistingTxMsgs();
break;
}
case FLOWCTRL_WAIT:
this->rxConversations[fid]->tx_flowcontrol(0, std::chrono::microseconds(0));
txConvo->tx_flowcontrol(0, std::chrono::microseconds(0));
break;
case FLOWCTRL_ABORT:
this->rxConversations[fid] = nullptr;
break;
}*/
}
break;
}
case FRAME_SINGLE:

3
drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.h
View File

@ -12,7 +12,7 @@ typedef struct {
std::string remaining_payload;
} PRESTAGED_WRITE;
class J2534Connection_ISO15765 : public J2534Connection { //J2534Connection_CAN {
class J2534Connection_ISO15765 : public J2534Connection {
public:
J2534Connection_ISO15765(
std::shared_ptr<PandaJ2534Device> panda_dev,
@ -49,6 +49,5 @@ public:
}
private:
std::array<PRESTAGED_WRITE, 10> staged_writes;
std::array<std::shared_ptr<MessageRx>, 10> rxConversations;
};

6
drivers/windows/pandaJ2534DLL/MessageTx.h
View File

@ -11,7 +11,7 @@ public:
MessageTx(
std::shared_ptr<J2534Connection> connection,
PASSTHRU_MSG& to_send
) : connection(connection), fullmsg(to_send) { };//next(nullptr),
) : connection(connection), fullmsg(to_send) { };
virtual BOOL sendNextFrame() = 0;
@ -19,8 +19,10 @@ public:
virtual BOOL isFinished() = 0;
//std::shared_ptr<class MessageTx> next;
virtual BOOL txReady() = 0;
std::weak_ptr<J2534Connection> connection;
J2534Frame fullmsg;
std::chrono::microseconds separation_time;
};

52
drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.cpp
View File

@ -6,7 +6,7 @@ MessageTx_ISO15765::MessageTx_ISO15765(
std::shared_ptr<J2534Connection> connection_in,
PASSTHRU_MSG& to_send,
std::shared_ptr<J2534MessageFilter> filter
) : MessageTx(connection_in, to_send), filter(filter), frames_sent(0), consumed_count(0) {
) : MessageTx(connection_in, to_send), filter(filter), frames_sent(0), consumed_count(0), txInFlight(FALSE), sendAll(FALSE), block_size(0) {
CANid = ((uint8_t)fullmsg.Data[0]) << 24 | ((uint8_t)fullmsg.Data[1]) << 16 |
((uint8_t)fullmsg.Data[2]) << 8 | ((uint8_t)fullmsg.Data[3]);
@ -53,6 +53,10 @@ unsigned int MessageTx_ISO15765::addressLength() {
BOOL MessageTx_ISO15765::sendNextFrame() {
if (this->frames_sent >= this->framePayloads.size()) return FALSE;
if (block_size == 0 && !sendAll && this->frames_sent > 0)return FALSE;
if (block_size > 0 && !sendAll) block_size--;
txInFlight = TRUE;
if (auto conn_sp = this->connection.lock()) {
if (auto panda_dev_sp = conn_sp->getPandaDev()) {
@ -68,41 +72,49 @@ BOOL MessageTx_ISO15765::sendNextFrame() {
return TRUE;
}
//Returns TRUE if receipt is consumed by the msg, FALSE otherwise.
BOOL MessageTx_ISO15765::checkTxReceipt(J2534Frame frame) {
if (frames_sent < 1) return FALSE;
if (!txInFlight) return FALSE;
if (frame.Data.size() >= addressLength() + 1 && (frame.Data[addressLength()] & 0xF0) == FRAME_FLOWCTRL) return FALSE;
if (frame.Data == fullmsg.Data.substr(0, 4) + framePayloads[frames_sent-1]) {
if (frame.Data == fullmsg.Data.substr(0, 4) + framePayloads[frames_sent - 1]) { //Check receipt is expected
txInFlight = FALSE; //Received the expected receipt. Allow another msg to be sent.
if (auto conn_sp = std::static_pointer_cast<J2534Connection_ISO15765>(this->connection.lock())) {
BOOL sendTxDone = (frames_sent == framePayloads.size());
BOOL sendEcho = sendTxDone && conn_sp->loopback;
unsigned long flags = (filter == nullptr) ? fullmsg.TxFlags : this->filter->flags;
if (frames_sent == framePayloads.size()) { //Check message done
if (auto conn_sp = std::static_pointer_cast<J2534Connection_ISO15765>(this->connection.lock())) {
unsigned long flags = (filter == nullptr) ? fullmsg.TxFlags : this->filter->flags;
if (sendTxDone) {
J2534Frame outframe(ISO15765);
outframe.Timestamp = frame.Timestamp;
outframe.RxStatus = TX_INDICATION | (flags & (ISO15765_ADDR_TYPE | CAN_29BIT_ID));
outframe.Data = frame.Data.substr(0, addressLength());
conn_sp->addMsgToRxQueue(outframe);
}
if (sendEcho) {
J2534Frame outframe(ISO15765);
outframe.Timestamp = frame.Timestamp;
outframe.RxStatus = TX_MSG_TYPE | (flags & (ISO15765_ADDR_TYPE | CAN_29BIT_ID));
outframe.Data = this->fullmsg.Data;
conn_sp->addMsgToRxQueue(outframe);
}
} //TODO what if fails
if (conn_sp->loopback) {
J2534Frame outframe(ISO15765);
outframe.Timestamp = frame.Timestamp;
outframe.RxStatus = TX_MSG_TYPE | (flags & (ISO15765_ADDR_TYPE | CAN_29BIT_ID));
outframe.Data = this->fullmsg.Data;
conn_sp->addMsgToRxQueue(outframe);
}
} //TODO what if fails
}
return TRUE;
}
return FALSE;
}
BOOL MessageTx_ISO15765::isFinished() {
return 0;
return this->frames_sent == this->framePayloads.size() && !txInFlight;
}
BOOL MessageTx_ISO15765::txReady() {
return block_size > 0 || sendAll || this->frames_sent == 0;
}
void MessageTx_ISO15765::tx_flowcontrol(uint8_t block_size, std::chrono::microseconds separation_time, BOOL sendAll) {
this->block_size = block_size;
this->separation_time = separation_time;
this->sendAll = sendAll;
}

8
drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.h
View File

@ -22,13 +22,19 @@ public:
virtual BOOL isFinished();
virtual BOOL txReady();
void tx_flowcontrol(uint8_t block_size, std::chrono::microseconds separation_time, BOOL sendAll = FALSE);
std::shared_ptr<J2534MessageFilter> filter;
unsigned long frames_sent;
unsigned long consumed_count;
//uint8_t block_size;
uint8_t block_size;
unsigned long CANid;
std::string data_prefix;
std::string payload;
BOOL isMultipart;
std::vector<std::string> framePayloads;
BOOL txInFlight;
BOOL sendAll;
};

51
drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp
View File

@ -7,7 +7,7 @@ SCHEDULED_TX_MSG::SCHEDULED_TX_MSG(std::shared_ptr<MessageTx> msgtx) : msgtx(msg
}
void SCHEDULED_TX_MSG::refreshExpiration() {
//expire += separation_time;
expire += this->msgtx->separation_time;
}
@ -90,21 +90,52 @@ DWORD PandaJ2534Device::can_recv_thread() {
if (msg_in.is_receipt) {
synchronized(active_flow_control_txs_lock) {
if (txMsgsAwaitingEcho.size() > 0) {
auto& msgtx = txMsgsAwaitingEcho.front()->msgtx;
auto msgtx = txMsgsAwaitingEcho.front()->msgtx;
if (auto conn = msgtx->connection.lock()) {
if (conn->isProtoCan() && conn->getPort() == msg_in.bus) {
if (msgtx->checkTxReceipt(msg_out)) {
conn->processMessageReceipt(msg_out);
txMsgsAwaitingEcho.pop(); //nextWaitingMsg no longer valid
//Things to check:
// Frame not for this msg: Drop frame and alert. Error?
// Frame is for this msg, more tx frames required after a FC frame: Wait for FC frame to come and trigger next tx.
// Frame is for this msg, more tx frames required: Schedule next tx frame.
// Frame is for this msg, and is the final frame of the msg: Let conn process full msg, If another msg from this conn is available, register it.
auto tx_schedule = std::move(txMsgsAwaitingEcho.front());
txMsgsAwaitingEcho.pop(); //Remove the TX object and schedule record.
if (msgtx->isFinished()) {
conn->processMessageReceipt(msg_out); //Alert the connection a tx is done.
conn->txbuff.pop(); //Remove the finished TX message from the connection tx queue.
//Remove the connection from the active list if it has no more scheduled TX msgs.
if (conn->txbuff.size() == 0) {
//Update records showing the connection no longer has a tx record scheduled.
this->ConnTxSet.erase(conn);
} else {
//Add the next scheduled tx from this conn
auto fcwrite = std::make_unique<SCHEDULED_TX_MSG>(conn->txbuff.back());
this->insertMultiPartTxInQueue(std::move(fcwrite));
}
} else {
if (msgtx->txReady()) { //Not finished, ready to send next frame.
tx_schedule->refreshExpiration();
insertMultiPartTxInQueue(std::move(tx_schedule));
} else {
//Not finished, but next frame not ready (maybe waiting for flow control).
//Do not schedule more messages from this connection.
this->ConnTxSet.erase(conn);
}
}
}
}
} else {
txMsgsAwaitingEcho.pop(); //nextWaitingMsg no longer valid
//Connection has died. Clear out the tx entry from device records.
txMsgsAwaitingEcho.pop();
this->ConnTxSet.erase(conn); //connection is already dead, no need to schedule future tx msgs.
}
}
}
} else {
// TODO: Make this more efficient
for (auto& conn : this->connections)
if (conn->isProtoCan() && conn->getPort() == msg_in.bus)
conn->processMessage(msg_out);
@ -138,10 +169,10 @@ DWORD PandaJ2534Device::msg_tx_thread() {
goto break_flow_ctrl_loop;
}
if (std::chrono::steady_clock::now() >= this->active_flow_control_txs.front()->expire) {
auto fcontrol_write_real = std::move(this->active_flow_control_txs.front());
auto scheduled_msg = std::move(this->active_flow_control_txs.front()); //Get the scheduled tx record.
this->active_flow_control_txs.pop_front();
if (fcontrol_write_real->msgtx->sendNextFrame())
txMsgsAwaitingEcho.push(std::move(fcontrol_write_real));
if (scheduled_msg->msgtx->sendNextFrame())
txMsgsAwaitingEcho.push(std::move(scheduled_msg));
} else { //Ran out of things that need to be sent now. Sleep!
auto time_diff = std::chrono::duration_cast<std::chrono::milliseconds>
(this->active_flow_control_txs.front()->expire - std::chrono::steady_clock::now());
@ -170,8 +201,6 @@ void PandaJ2534Device::registerConnectionTx(std::shared_ptr<J2534Connection> con
synchronized(ConnTxMutex) {
auto ret = this->ConnTxSet.insert(conn);
if (ret.second == FALSE) return; //Conn already exists.
this->ConnTxQueue.push(conn);
this->txInProgress = TRUE;
auto fcwrite = std::make_unique<SCHEDULED_TX_MSG>(conn->txbuff.back());
this->insertMultiPartTxInQueue(std::move(fcwrite));