zmv
f87e5aad40
Send 0x1F6 risk updates through a bounded latest-result worker on CH0 and disable CH1 initialization, reception, transmission and live display. Preserve the four actual-position bytes from 0x391 and recorded-session compatibility. Include regression coverage, Orin camera/TensorRT/CAN validation, and the verified standalone binary with refreshed checksums.
192 lines
11 KiB
C++
192 lines
11 KiB
C++
#include "can_bus.hpp"
|
|
#include "zcan.h"
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <condition_variable>
|
|
#include <deque>
|
|
#include <iostream>
|
|
#include <mutex>
|
|
#include <stdexcept>
|
|
#include <thread>
|
|
#include <limits>
|
|
|
|
namespace {
|
|
using namespace std::chrono_literals;
|
|
std::mutex mutex;
|
|
std::condition_variable changed;
|
|
std::deque<ZCANDataObj> incoming;
|
|
std::atomic<int> opens{0},closes{0},resets{0},mergeCalls{0},txCalls{0},rxCalls{0};
|
|
std::atomic<bool> unavailable{false},initFailure{false},receiveFailure{false},txFailure{false};
|
|
ZCAN_20_MSG sentCan{};ZCAN_FD_MSG sentFd{};
|
|
std::vector<ZCAN_20_MSG> sentCanFrames;
|
|
int failedTxChannel=-1;
|
|
std::mutex txMutex;
|
|
std::condition_variable txWake;
|
|
std::atomic<bool> blockTx{false},txBlocked{false};
|
|
void check(bool ok,char const* message){if(!ok)throw std::runtime_error(message);}
|
|
template<class F> void waitFor(F f) {
|
|
auto deadline=std::chrono::steady_clock::now()+2s;
|
|
while(!f()){check(std::chrono::steady_clock::now()<deadline,"Asynchronous CAN condition timed out");std::this_thread::sleep_for(1ms);}
|
|
}
|
|
template<class F> void rejects(F f){bool failed=false;try{f();}catch(std::exception const&){failed=true;}check(failed,"Expected rejection");}
|
|
void reset() {
|
|
opens=closes=resets=mergeCalls=txCalls=rxCalls=0;
|
|
unavailable=initFailure=receiveFailure=txFailure=false;
|
|
sentCanFrames.clear();failedTxChannel=-1;
|
|
blockTx=false;txBlocked=false;
|
|
std::lock_guard<std::mutex> lock(mutex);incoming.clear();
|
|
}
|
|
void inject(unsigned channel,bool fd,std::uint32_t id,std::initializer_list<std::uint8_t> bytes,unsigned stamp=123) {
|
|
ZCANDataObj obj{};obj.chnl=channel;obj.dataType=fd?ZCAN_DT_ZCAN_CANFD_DATA:ZCAN_DT_ZCAN_CAN_DATA;
|
|
auto& hdr=fd?obj.data.zcanCANFDData.hdr:obj.data.zcanCANData.hdr;
|
|
hdr.id=id;hdr.chn=channel;hdr.len=bytes.size();hdr.ts=stamp;
|
|
auto* data=fd?obj.data.zcanCANFDData.dat:obj.data.zcanCANData.dat;
|
|
std::copy(bytes.begin(),bytes.end(),data);
|
|
{std::lock_guard<std::mutex> lock(mutex);incoming.push_back(obj);}changed.notify_all();
|
|
}
|
|
}
|
|
extern "C" {
|
|
U32 VCI_OpenDevice(U32 type,U32,U32){check(type==33,"Device type changed");++opens;return unavailable?0:1;}
|
|
U32 VCI_CloseDevice(U32,U32){++closes;return 1;}
|
|
U32 VCI_InitCAN(U32,U32,U32 port,ZCAN_INIT* init){
|
|
check(port==0,"CH1 initialized");
|
|
check(init->clk==60000000&&init->mode==0&&init->aset.brp==5&&init->dset.brp==1,"Reference timing not retained");
|
|
return initFailure?0:1;
|
|
}
|
|
U32 VCI_SetReference(U32,U32,U32 port,U32 ref,void* data){
|
|
check(port==0,"CH1 configured");
|
|
if(ref==ZCAN_CMD_SET_CHNL_RECV_MERGE){check(port==0&&*static_cast<U32*>(data)==1,"Invalid merged receive configuration");++mergeCalls;}return 1;
|
|
}
|
|
U32 VCI_StartCAN(U32,U32,U32 port){check(port==0,"CH1 started");return 1;}
|
|
U32 VCI_ResetCAN(U32,U32,U32 port){check(port==0,"CH1 reset");++resets;return 1;}
|
|
U32 VCI_ReceiveData(U32,U32,U32 port,ZCANDataObj* out,U32 capacity,U32){
|
|
check(port==0,"Multiple merged receive readers");++rxCalls;
|
|
if(receiveFailure)return std::numeric_limits<U32>::max();
|
|
std::unique_lock<std::mutex> lock(mutex);
|
|
changed.wait_for(lock,10ms,[]{return !incoming.empty();});
|
|
U32 count=0;while(!incoming.empty()&&count<capacity){out[count++]=incoming.front();incoming.pop_front();}return count;
|
|
}
|
|
U32 VCI_Transmit(U32,U32,U32 port,ZCAN_20_MSG* message,U32 count){
|
|
check(port==0&&count==1&&port==message->hdr.chn,"Invalid CAN send call");++txCalls;sentCan=*message;
|
|
{
|
|
std::unique_lock<std::mutex> lock(txMutex);sentCanFrames.push_back(*message);
|
|
if(blockTx) {
|
|
txBlocked=true;txWake.notify_all();
|
|
if(!txWake.wait_for(lock,2s,[]{return !blockTx.load();}))throw std::runtime_error("Test send gate timed out");
|
|
}
|
|
}
|
|
return txFailure||int(port)==failedTxChannel?0:1;
|
|
}
|
|
U32 VCI_TransmitFD(U32,U32,U32 port,ZCAN_FD_MSG* message,U32 count){
|
|
check(port==0&&count==1&&port==message->hdr.chn,"Invalid CANFD send call");++txCalls;sentFd=*message;return txFailure?0:1;
|
|
}
|
|
}
|
|
int main()try {
|
|
using namespace cms;
|
|
reset();{
|
|
CanConfig c;c.enabled=false;CanBus bus(c);bus.start();
|
|
check(bus.snapshot().status=="DISABLED"&&!bus.send({0,0x123,false,false,false,{1,2}}),"Disabled CAN used hardware");
|
|
}check(opens==0,"Disabled CAN opened device");
|
|
reset();unavailable=true;{
|
|
CanBus bus({});bus.start();check(bus.snapshot().status=="UNAVAILABLE","Missing adapter not reported");
|
|
}check(closes==0,"Closed unopened device");
|
|
reset();initFailure=true;{
|
|
CanBus bus({});bus.start();check(bus.snapshot().status=="UNAVAILABLE","Partial failure not reported");
|
|
}check(closes==1&&resets==0,"Partial initialization leaked device");
|
|
reset();{
|
|
CanConfig c;c.timeout_ms=150;CanBus bus(c);bus.start();
|
|
check(mergeCalls==1,"Merged receive configured more than once");
|
|
CanBus duplicate(c);rejects([&]{duplicate.start();});
|
|
inject(0,false,0x391,{1,37,128,255},123);
|
|
waitFor([&]{return bus.snapshot().received==1;});
|
|
check(bus.snapshot().channels[0].positions==std::array<int,4>{1,37,128,255},"Four byte positions overlap");
|
|
inject(1,true,0x391,{99,99,99,99},789);
|
|
inject(0,true,0x391,{31,25,76,100},456);
|
|
waitFor([&]{return bus.snapshot().received==2;});
|
|
auto s=bus.snapshot();
|
|
check(s.channels[0].positions==std::array<int,4>{31,25,76,100},"Actual positions not refreshed");
|
|
check(s.channels[1].received==0&&!s.channels[1].valid&&s.channels[1].positions==std::array<int,4>{-1,-1,-1,-1},"CH1 input accepted");
|
|
check(s.channels[0].valid&&s.channels[0].fd&&s.channels[0].device_timestamp_us==456,"Freshness/frame metadata wrong");
|
|
auto timestamp=s.channels[0].received_monotonic_ns;
|
|
inject(0,false,0x390,{9,9,9,9});inject(0,false,0x391,{99,99,99});
|
|
waitFor([&]{return bus.snapshot().received==4;});
|
|
s=bus.snapshot();check(s.channels[0].received_monotonic_ns==timestamp&&s.receive_errors==1,"Invalid input refreshed signal");
|
|
waitFor([&]{return !bus.snapshot().channels[0].valid;});
|
|
inject(0,false,0x391,{0,1,2,3});waitFor([&]{return bus.snapshot().channels[0].valid;});
|
|
check(bus.send({0,0x321,false,false,false,{1,2,3}}),"CAN send failed");
|
|
check(sentCan.hdr.len==3&&!sentCan.hdr.inf.sef&&sentCan.dat[2]==3,"CAN encoding incorrect");
|
|
CanFrame fd{0,0x12345,true,true,true,std::vector<std::uint8_t>(64,0xa5)};
|
|
check(bus.send(fd)&&sentFd.hdr.len==64&&sentFd.hdr.inf.brs&&sentFd.hdr.inf.sef&&sentFd.dat[63]==0xa5,"CANFD encoding incorrect");
|
|
fd.data.resize(9);rejects([&]{bus.send(fd);});
|
|
rejects([&]{bus.send({1,0x391,false,false,false,{}});});
|
|
rejects([&]{bus.send({0,0x800,false,false,false,{}});});
|
|
rejects([&]{bus.send({0,0x391,false,false,true,{}});});
|
|
txFailure=true;check(!bus.send({}),"SDK send failure ignored");
|
|
check(bus.snapshot().transmitted==2&&bus.snapshot().transmit_errors==1,"TX counters wrong");
|
|
receiveFailure=true;waitFor([&]{return bus.snapshot().status=="ERROR";});
|
|
check(!bus.snapshot().channels[0].valid&&!bus.send({}),"Receive error left bus live");
|
|
bus.requestStop();bus.join();check(closes==1&&resets==1,"Shutdown did not release CH0");
|
|
check(!bus.send({}),"Transmission after stop");
|
|
}check(closes==1,"Device closed twice");
|
|
reset();{
|
|
CanBus bus({});bus.start();
|
|
for(auto level:{"clear","watch","warning","danger","unavailable","unknown","","clear"}) {
|
|
auto sent=bus.sendRisk(level);
|
|
check(sent==std::array<bool,2>{true,false},"Risk must use CH0 only");
|
|
}
|
|
check(sentCanFrames.size()==8,"Risk must send exactly once per channel per result");
|
|
for(std::size_t i=0;i<sentCanFrames.size();++i) {
|
|
auto const& message=sentCanFrames[i];
|
|
check(message.hdr.id==0x1f6&&message.hdr.len==8&&message.hdr.chn==0,
|
|
"Risk ID, DLC or channel incorrect");
|
|
check(!message.hdr.inf.fmt&&!message.hdr.inf.sef&&!message.hdr.inf.brs&&!message.hdr.inf.sdf,
|
|
"Risk must use a standard classic CAN data frame");
|
|
check(message.dat[0]==(i==0||i==7?0x00:0x11),"Risk must fill both nibbles and reset both for clear");
|
|
for(int byte=1;byte<8;++byte)check(message.dat[byte]==0,"Risk reserved bits not zero");
|
|
}
|
|
failedTxChannel=0;
|
|
check(bus.sendRisk("danger")==std::array<bool,2>{false,false},"CH0 failure reported success");
|
|
failedTxChannel=-1;
|
|
check(bus.sendRisk("clear")==std::array<bool,2>{true,false},"CH0 did not recover after send failure");
|
|
check(bus.snapshot().transmitted==9&&bus.snapshot().transmit_errors==1,"Risk TX counters wrong");
|
|
auto calls=txCalls.load();bus.requestStop();
|
|
check(bus.sendRisk("danger")==std::array<bool,2>{false,false}&&txCalls==calls,"Risk sent after stop");
|
|
}
|
|
reset();{
|
|
CanConfig c;c.enabled=false;CanBus bus(c);bus.start();
|
|
check(bus.sendRisk("danger")==std::array<bool,2>{false,false}&&txCalls==0&&opens==0,
|
|
"Disabled risk send accessed hardware");
|
|
check(!bus.queueRisk("danger"),"Disabled bus accepted asynchronous risk");
|
|
}
|
|
reset();{
|
|
CanBus bus({});check(!bus.queueRisk("danger"),"Unstarted bus accepted risk");bus.start();
|
|
// A blocked hardware SDK call must not stall inference submission or snapshots.
|
|
blockTx=true;check(bus.queueRisk("danger"),"Risk queue rejected ready bus");
|
|
waitFor([]{return txBlocked.load();});
|
|
auto start=std::chrono::steady_clock::now();
|
|
for(int i=0;i<1000;++i)check(bus.queueRisk(i%2?"clear":"warning"),"Risk queue stalled/rejected update");
|
|
check(bus.snapshot().transmitted==0,"Blocked SDK falsely reported delivery");
|
|
check(std::chrono::steady_clock::now()-start<250ms,"Risk submission waited for the SDK");
|
|
{std::lock_guard<std::mutex> lock(txMutex);blockTx=false;}txWake.notify_all();
|
|
waitFor([&]{return bus.snapshot().transmitted==2;});
|
|
{
|
|
std::lock_guard<std::mutex> lock(txMutex);
|
|
check(sentCanFrames.size()==2,"Stale pending results accumulated");
|
|
for(int i=0;i<2;++i)check(sentCanFrames[i].hdr.chn==0&&
|
|
sentCanFrames[i].dat[0]==(i==0?0x11:0x00),"Async risk or latest result incorrect");
|
|
}
|
|
failedTxChannel=0;
|
|
check(bus.queueRisk("unavailable"),"Ready queue rejected unavailable risk");
|
|
waitFor([&]{auto s=bus.snapshot();return s.transmitted==2&&s.transmit_errors==1;});
|
|
failedTxChannel=-1;txBlocked=false;blockTx=true;
|
|
check(bus.queueRisk("clear"),"Ready queue rejected clear risk");waitFor([]{return txBlocked.load();});
|
|
check(bus.queueRisk("danger"),"Queue rejected pending stop fixture");
|
|
auto calls=txCalls.load();bus.requestStop();
|
|
check(!bus.queueRisk("danger"),"Stopped bus accepted risk");
|
|
{std::lock_guard<std::mutex> lock(txMutex);blockTx=false;}txWake.notify_all();
|
|
bus.join();check(txCalls==calls,"Stop sent a pending result or second channel");
|
|
check(closes==1&&resets==1,"Async shutdown leaked hardware");
|
|
}
|
|
std::cout<<"PASS CAN/CANFD RX decoding, CH0-only reception, expiry/recovery, TX validation, binary risk 0x1F6 on CH0 only, errors and lifecycle cleanup\n";
|
|
}catch(std::exception const& e){std::cerr<<e.what()<<'\n';return 1;}
|