minghanchen
2249 lines
97 KiB
C++
2249 lines
97 KiB
C++
/*
|
|
* Dual-channel ZLG USBCANFD-200U inspection and semantic-control CLI.
|
|
*
|
|
* CAN0 is the passenger-seat native classic-CAN bus defined by
|
|
* passenger_seat_firmware. CAN1 is the remaining vehicle bus defined by the
|
|
* filled N7 matrix. The verified CAN1 implementation is included from the
|
|
* adjacent project-code/canfd library so this tool does not create a second,
|
|
* drifting copy of its byte-level compiler.
|
|
*
|
|
* No arbitrary raw-frame command is exposed. Monitoring is the default and
|
|
* --trace-all is always read-only. A control operation requires all of:
|
|
* can_read_trans send --channel can0|can1 --command "..." --execute
|
|
*/
|
|
|
|
#ifndef CANFD_LEGACY_SOURCE
|
|
#define CANFD_LEGACY_SOURCE "../../../project-code/canfd/can_read_trans.cpp"
|
|
#endif
|
|
|
|
#define main cognitive_agent_legacy_can_read_trans_main
|
|
#include CANFD_LEGACY_SOURCE
|
|
#undef main
|
|
|
|
#include "generated/can_semantic_protocol.hpp"
|
|
|
|
namespace cognitive_agent_can_cli {
|
|
|
|
using namespace comfort_can;
|
|
namespace shared_protocol = cognitive_agent_can_protocol;
|
|
|
|
enum class Action { Monitor, Status, Console, Send, Whitelist, SelfTest };
|
|
|
|
struct CliOptions {
|
|
Action action = Action::Monitor;
|
|
std::string channel = "all";
|
|
std::string command;
|
|
int duration_ms = 0;
|
|
bool execute = false;
|
|
bool trace_all = false;
|
|
bool sdk_debug = false;
|
|
bool can0_termination = false;
|
|
bool can1_termination = false;
|
|
bool show_help = false;
|
|
};
|
|
|
|
std::string join_words(const std::vector<std::string>& words) {
|
|
std::ostringstream output;
|
|
for (std::size_t index = 0; index < words.size(); ++index) {
|
|
if (index != 0)
|
|
output << ' ';
|
|
output << words[index];
|
|
}
|
|
return output.str();
|
|
}
|
|
|
|
std::string trim_ascii(std::string_view value) {
|
|
const auto first = value.find_first_not_of(" \t\r\n");
|
|
if (first == std::string_view::npos)
|
|
return {};
|
|
const auto last = value.find_last_not_of(" \t\r\n");
|
|
return std::string(value.substr(first, last - first + 1));
|
|
}
|
|
|
|
std::optional<CliOptions> parse_cli(int argc, char** argv) {
|
|
CliOptions options;
|
|
int index = 1;
|
|
if (index < argc) {
|
|
const std::string action = argv[index];
|
|
if (action == "monitor") {
|
|
options.action = Action::Monitor;
|
|
++index;
|
|
} else if (action == "status") {
|
|
options.action = Action::Status;
|
|
options.duration_ms = 3000;
|
|
++index;
|
|
} else if (action == "console") {
|
|
options.action = Action::Console;
|
|
++index;
|
|
} else if (action == "send") {
|
|
options.action = Action::Send;
|
|
++index;
|
|
} else if (action == "whitelist") {
|
|
options.action = Action::Whitelist;
|
|
++index;
|
|
} else if (action == "self-test") {
|
|
options.action = Action::SelfTest;
|
|
++index;
|
|
}
|
|
}
|
|
|
|
for (; index < argc; ++index) {
|
|
const std::string argument = argv[index];
|
|
if (argument == "--help" || argument == "-h") {
|
|
options.show_help = true;
|
|
} else if (argument == "--channel") {
|
|
if (++index >= argc) {
|
|
std::cerr << "--channel requires all, can0, or can1\n";
|
|
return std::nullopt;
|
|
}
|
|
options.channel = lower_ascii(argv[index]);
|
|
if (options.channel != "all" && options.channel != "can0" &&
|
|
options.channel != "can1") {
|
|
std::cerr << "--channel only accepts all, can0, or can1\n";
|
|
return std::nullopt;
|
|
}
|
|
} else if (argument == "--command") {
|
|
if (++index >= argc) {
|
|
std::cerr << "--command requires one quoted semantic command\n";
|
|
return std::nullopt;
|
|
}
|
|
options.command = argv[index];
|
|
} else if (argument == "--duration-ms") {
|
|
if (++index >= argc) {
|
|
std::cerr << "--duration-ms requires an integer\n";
|
|
return std::nullopt;
|
|
}
|
|
const auto value = parse_int(argv[index]);
|
|
if (!value || *value < 0 || *value > 3'600'000) {
|
|
std::cerr << "--duration-ms must be between 0 and 3600000\n";
|
|
return std::nullopt;
|
|
}
|
|
options.duration_ms = *value;
|
|
} else if (argument == "--execute") {
|
|
options.execute = true;
|
|
} else if (argument == "--trace-all") {
|
|
options.trace_all = true;
|
|
} else if (argument == "--sdk-debug") {
|
|
options.sdk_debug = true;
|
|
} else if (argument == "--can0-termination") {
|
|
options.can0_termination = true;
|
|
} else if (argument == "--can1-termination") {
|
|
options.can1_termination = true;
|
|
} else {
|
|
std::cerr << "unknown argument: " << argument << '\n';
|
|
return std::nullopt;
|
|
}
|
|
}
|
|
|
|
if (options.action == Action::Send) {
|
|
if (options.channel != "can0" && options.channel != "can1") {
|
|
std::cerr << "send requires --channel can0 or --channel can1\n";
|
|
return std::nullopt;
|
|
}
|
|
if (options.command.empty()) {
|
|
std::cerr << "send requires --command\n";
|
|
return std::nullopt;
|
|
}
|
|
if (options.trace_all) {
|
|
std::cerr << "--trace-all is read-only and cannot be combined with send\n";
|
|
return std::nullopt;
|
|
}
|
|
if (!options.execute) {
|
|
std::cerr << "send requires --execute; no CAN frame was submitted\n";
|
|
return std::nullopt;
|
|
}
|
|
} else if (options.action == Action::Console) {
|
|
if (!options.command.empty()) {
|
|
std::cerr << "console reads commands interactively; do not use --command\n";
|
|
return std::nullopt;
|
|
}
|
|
if (options.execute && options.trace_all) {
|
|
std::cerr << "--trace-all is read-only and cannot enable console transmission\n";
|
|
return std::nullopt;
|
|
}
|
|
} else if (options.execute) {
|
|
std::cerr << "--execute is only valid with send or console\n";
|
|
return std::nullopt;
|
|
}
|
|
return options;
|
|
}
|
|
|
|
void print_usage(const char* program) {
|
|
std::cout
|
|
<< "Usage:\n"
|
|
<< " " << program << " monitor [--channel all|can0|can1] [--trace-all] [--duration-ms N]\n"
|
|
<< " " << program << " status [--duration-ms N]\n"
|
|
<< " " << program << " console [--channel all|can0|can1] [--execute]\n"
|
|
<< " " << program << " send --channel can0|can1 --command \"semantic command\" --execute\n"
|
|
<< " " << program << " whitelist\n"
|
|
<< " " << program << " self-test\n\n"
|
|
<< "Safety options:\n"
|
|
<< " --can0-termination explicitly enable the adapter's CAN0 120-ohm resistor\n"
|
|
<< " --can1-termination explicitly enable the adapter's CAN1 120-ohm resistor\n"
|
|
<< " --trace-all print every received frame and force read-only operation\n"
|
|
<< " --sdk-debug enable temporary vendor SDK diagnostics\n\n"
|
|
<< "Both termination resistors are disabled by default. Enable one only when the\n"
|
|
<< "adapter is a physical endpoint and the bus topology requires it.\n";
|
|
}
|
|
|
|
struct Can0MotorState {
|
|
bool seen = false;
|
|
bool position_valid = false;
|
|
int task_state = 0;
|
|
int result = 0;
|
|
int direction = 0;
|
|
int value_x100 = 0;
|
|
int duty_percent = 0;
|
|
std::uint16_t sequence = 0;
|
|
std::uint64_t generation = 0;
|
|
std::chrono::steady_clock::time_point updated{};
|
|
};
|
|
|
|
struct Can0AckState {
|
|
bool seen = false;
|
|
int ack = 0;
|
|
int reject_reason = 0;
|
|
int system_state = 0;
|
|
int opcode = 0;
|
|
std::uint16_t sequence = 0;
|
|
int active_mask = 0;
|
|
bool calibration_busy = false;
|
|
bool safety_inhibit = false;
|
|
bool fault_summary = false;
|
|
std::uint64_t generation = 0;
|
|
};
|
|
|
|
struct Can0DynamicState {
|
|
bool seen = false;
|
|
int mode_id = 0;
|
|
int state = 0;
|
|
int phase = 0;
|
|
int fault_mask = 0;
|
|
int cycle = 0;
|
|
int elapsed_ms = 0;
|
|
std::uint64_t generation = 0;
|
|
};
|
|
|
|
struct Can0ThermalState {
|
|
bool seen = false;
|
|
int ventilation_raw = 0;
|
|
int heating_raw = 0;
|
|
std::uint64_t generation = 0;
|
|
std::chrono::steady_clock::time_point updated{};
|
|
};
|
|
|
|
class Can0StateStore {
|
|
public:
|
|
void update_ack(const Can0AckState& value) {
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
ack_ = value;
|
|
ack_.seen = true;
|
|
ack_.generation = ++generation_;
|
|
}
|
|
condition_.notify_all();
|
|
}
|
|
|
|
void update_motor(unsigned motor, const Can0MotorState& value) {
|
|
if (motor >= motors_.size())
|
|
return;
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
motors_[motor] = value;
|
|
motors_[motor].seen = true;
|
|
motors_[motor].generation = ++generation_;
|
|
motors_[motor].updated = std::chrono::steady_clock::now();
|
|
}
|
|
condition_.notify_all();
|
|
}
|
|
|
|
void update_dynamic(const Can0DynamicState& value) {
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
dynamic_ = value;
|
|
dynamic_.seen = true;
|
|
dynamic_.generation = ++generation_;
|
|
}
|
|
condition_.notify_all();
|
|
}
|
|
|
|
void update_thermal(const Can0ThermalState& value) {
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
thermal_ = value;
|
|
thermal_.seen = true;
|
|
thermal_.generation = ++generation_;
|
|
thermal_.updated = std::chrono::steady_clock::now();
|
|
}
|
|
condition_.notify_all();
|
|
}
|
|
|
|
std::optional<Can0MotorState> wait_fresh_motor(
|
|
unsigned motor, std::chrono::milliseconds timeout) {
|
|
if (motor >= motors_.size())
|
|
return std::nullopt;
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
const auto fresh = [&] {
|
|
return motors_[motor].seen &&
|
|
std::chrono::steady_clock::now() - motors_[motor].updated <=
|
|
std::chrono::milliseconds(2500);
|
|
};
|
|
if (!fresh() && !condition_.wait_for(lock, timeout, fresh))
|
|
return std::nullopt;
|
|
return motors_[motor];
|
|
}
|
|
|
|
std::optional<Can0ThermalState> wait_fresh_thermal(
|
|
std::chrono::milliseconds timeout) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
const auto fresh = [&] {
|
|
return thermal_.seen &&
|
|
std::chrono::steady_clock::now() - thermal_.updated <=
|
|
std::chrono::milliseconds(2500);
|
|
};
|
|
if (!fresh() && !condition_.wait_for(lock, timeout, fresh))
|
|
return std::nullopt;
|
|
return thermal_;
|
|
}
|
|
|
|
std::uint64_t ack_generation() {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return ack_.generation;
|
|
}
|
|
|
|
std::uint64_t motor_generation(unsigned motor) {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return motor < motors_.size() ? motors_[motor].generation : 0;
|
|
}
|
|
|
|
std::uint64_t dynamic_generation() {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return dynamic_.generation;
|
|
}
|
|
|
|
std::uint64_t thermal_generation() {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return thermal_.generation;
|
|
}
|
|
|
|
std::optional<Can0AckState> wait_ack(
|
|
std::uint16_t sequence, int opcode, std::uint64_t after_generation,
|
|
std::chrono::milliseconds timeout) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
if (!condition_.wait_for(lock, timeout, [&] {
|
|
return ack_.seen && ack_.generation > after_generation &&
|
|
ack_.sequence == sequence &&
|
|
ack_.opcode == opcode;
|
|
}))
|
|
return std::nullopt;
|
|
return ack_;
|
|
}
|
|
|
|
std::optional<Can0AckState> wait_all_idle(
|
|
std::uint64_t after_generation, std::chrono::milliseconds timeout) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
if (!condition_.wait_for(lock, timeout, [&] {
|
|
return ack_.seen && ack_.generation > after_generation &&
|
|
ack_.active_mask == 0 && !ack_.calibration_busy;
|
|
}))
|
|
return std::nullopt;
|
|
return ack_;
|
|
}
|
|
|
|
std::optional<Can0MotorState> wait_motor_terminal(
|
|
unsigned motor, std::uint16_t sequence, std::optional<int> target_x100,
|
|
std::uint64_t after_generation, std::chrono::milliseconds timeout) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
if (!condition_.wait_for(lock, timeout, [&] {
|
|
if (motor >= motors_.size())
|
|
return false;
|
|
const auto& state = motors_[motor];
|
|
if (!state.seen || state.generation <= after_generation ||
|
|
state.sequence != sequence ||
|
|
(state.task_state != 6 && state.task_state != 7))
|
|
return false;
|
|
return !target_x100 || state.task_state == 7 ||
|
|
std::abs(state.value_x100 - *target_x100) <= 100;
|
|
}))
|
|
return std::nullopt;
|
|
return motors_[motor];
|
|
}
|
|
|
|
std::optional<Can0DynamicState> wait_dynamic(
|
|
int mode_id, bool stopping, std::uint64_t after_generation,
|
|
std::chrono::milliseconds timeout) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
if (!condition_.wait_for(lock, timeout, [&] {
|
|
if (!dynamic_.seen || dynamic_.generation <= after_generation)
|
|
return false;
|
|
return stopping ? dynamic_.mode_id == 0 && dynamic_.state == 0
|
|
: dynamic_.mode_id == mode_id &&
|
|
(dynamic_.state == 1 || dynamic_.state == 2);
|
|
}))
|
|
return std::nullopt;
|
|
return dynamic_;
|
|
}
|
|
|
|
std::optional<Can0ThermalState> wait_thermal(
|
|
int ventilation_raw, int heating_raw, std::uint64_t after_generation,
|
|
std::chrono::milliseconds timeout) {
|
|
std::unique_lock<std::mutex> lock(mutex_);
|
|
if (!condition_.wait_for(lock, timeout, [&] {
|
|
return thermal_.seen && thermal_.generation > after_generation &&
|
|
thermal_.ventilation_raw == ventilation_raw &&
|
|
thermal_.heating_raw == heating_raw;
|
|
}))
|
|
return std::nullopt;
|
|
return thermal_;
|
|
}
|
|
|
|
private:
|
|
std::mutex mutex_;
|
|
std::condition_variable condition_;
|
|
std::uint64_t generation_ = 0;
|
|
std::array<Can0MotorState, 4> motors_{};
|
|
Can0AckState ack_{};
|
|
Can0DynamicState dynamic_{};
|
|
Can0ThermalState thermal_{};
|
|
};
|
|
|
|
class Can0Channel {
|
|
public:
|
|
explicit Can0Channel(bool termination_enabled) {
|
|
auto init = make_init_config();
|
|
if (VCI_InitCAN(kDevType, kDevIndex, 0, &init) == 0)
|
|
throw std::runtime_error("CAN0 VCI_InitCAN failed");
|
|
|
|
ZCAN_FILTER_TABLE filters{};
|
|
filters.size = sizeof(ZCAN_FILTER);
|
|
filters.table[0].type = 0;
|
|
filters.table[0].sid = 0x000;
|
|
filters.table[0].eid = 0x7FF;
|
|
if (VCI_SetReference(kDevType, kDevIndex, 0, CMD_CAN_FILTER,
|
|
&filters) == 0) {
|
|
print_line("[warning] CAN0 standard-ID filter was not applied; using SDK default");
|
|
}
|
|
|
|
U32 termination = termination_enabled ? 1U : 0U;
|
|
if (VCI_SetReference(kDevType, kDevIndex, 0, CMD_CAN_TRES,
|
|
&termination) == 0) {
|
|
throw std::runtime_error("CAN0 termination configuration failed");
|
|
}
|
|
if (VCI_StartCAN(kDevType, kDevIndex, 0) == 0)
|
|
throw std::runtime_error("CAN0 VCI_StartCAN failed");
|
|
started_ = true;
|
|
VCI_ClearBuffer(kDevType, kDevIndex, 0);
|
|
}
|
|
|
|
~Can0Channel() {
|
|
if (started_)
|
|
VCI_ResetCAN(kDevType, kDevIndex, 0);
|
|
}
|
|
|
|
Can0Channel(const Can0Channel&) = delete;
|
|
Can0Channel& operator=(const Can0Channel&) = delete;
|
|
|
|
U32 receive(ZCAN_20_MSG* messages, U32 count) {
|
|
std::lock_guard<std::mutex> lock(api_mutex_);
|
|
return VCI_Receive(kDevType, kDevIndex, 0, messages, count, 0);
|
|
}
|
|
|
|
U32 transmit(ZCAN_20_MSG& message) {
|
|
std::lock_guard<std::mutex> lock(api_mutex_);
|
|
return VCI_Transmit(kDevType, kDevIndex, 0, &message, 1);
|
|
}
|
|
|
|
bool read_status(ZCAN_STAT& status) {
|
|
std::lock_guard<std::mutex> lock(api_mutex_);
|
|
return VCI_ReadCANStatus(kDevType, kDevIndex, 0, &status) != 0;
|
|
}
|
|
|
|
private:
|
|
bool started_ = false;
|
|
std::mutex api_mutex_;
|
|
};
|
|
|
|
std::string can0_axis_name(unsigned motor) {
|
|
static constexpr std::array<std::string_view, 4> names{
|
|
"horizontal", "basin", "backrest", "legrest"
|
|
};
|
|
return motor < names.size() ? std::string(names[motor])
|
|
: "unknown(" + std::to_string(motor) + ')';
|
|
}
|
|
|
|
std::string can0_task_name(int state) {
|
|
static constexpr std::array<std::string_view, 8> names{
|
|
"idle", "accepted", "position", "timed", "calibration",
|
|
"stopping", "complete", "fault"
|
|
};
|
|
return state >= 0 && static_cast<std::size_t>(state) < names.size()
|
|
? std::string(names[static_cast<std::size_t>(state)])
|
|
: "unknown(" + std::to_string(state) + ')';
|
|
}
|
|
|
|
void decode_can0_frame(U32 id, const U8* data, std::size_t len,
|
|
StatusStore& public_store, Can0StateStore& can0_store) {
|
|
if (len < 8)
|
|
return;
|
|
if (id == 0x301) {
|
|
Can0AckState state;
|
|
state.ack = static_cast<int>(*get_bits(data, len, 0, 4));
|
|
state.system_state = static_cast<int>(*get_bits(data, len, 4, 4));
|
|
state.reject_reason = static_cast<int>(*get_bits(data, len, 8, 8));
|
|
state.sequence = static_cast<std::uint16_t>(*get_bits(data, len, 16, 16));
|
|
state.opcode = static_cast<int>(*get_bits(data, len, 32, 8));
|
|
state.active_mask = static_cast<int>(*get_bits(data, len, 40, 4));
|
|
state.safety_inhibit = *get_bits(data, len, 44, 1) != 0;
|
|
state.calibration_busy = *get_bits(data, len, 46, 1) != 0;
|
|
state.fault_summary = *get_bits(data, len, 47, 1) != 0;
|
|
can0_store.update_ack(state);
|
|
|
|
std::ostringstream summary;
|
|
summary << "ack=" << state.ack << " opcode=0x" << std::hex
|
|
<< std::uppercase << state.opcode << std::dec
|
|
<< " sequence=" << state.sequence
|
|
<< " system=" << state.system_state
|
|
<< " reject=" << state.reject_reason
|
|
<< " activeMask=0x" << std::hex << state.active_mask
|
|
<< " validMask=0x" << *get_bits(data, len, 52, 4)
|
|
<< std::dec << " inhibit=" << state.safety_inhibit
|
|
<< " calibration=" << state.calibration_busy
|
|
<< " fault=" << state.fault_summary
|
|
<< " fwRev=" << *get_bits(data, len, 60, 4);
|
|
public_store.update("CAN0/passenger/system", summary.str());
|
|
return;
|
|
}
|
|
if (id == 0x302) {
|
|
const unsigned motor = static_cast<unsigned>(*get_bits(data, len, 0, 8));
|
|
Can0MotorState state;
|
|
state.task_state = static_cast<int>(*get_bits(data, len, 8, 4));
|
|
state.result = static_cast<int>(*get_bits(data, len, 12, 4));
|
|
state.direction = static_cast<int>(*get_bits(data, len, 16, 2));
|
|
state.position_valid = *get_bits(data, len, 19, 1) != 0;
|
|
state.sequence = static_cast<std::uint16_t>(*get_bits(data, len, 24, 16));
|
|
state.value_x100 = static_cast<int>(*get_bits(data, len, 40, 16));
|
|
state.duty_percent = static_cast<int>(*get_bits(data, len, 56, 8));
|
|
can0_store.update_motor(motor, state);
|
|
|
|
std::ostringstream summary;
|
|
summary << "axis=" << can0_axis_name(motor)
|
|
<< " position=" << std::fixed << std::setprecision(2)
|
|
<< state.value_x100 / 100.0 << "% valid="
|
|
<< state.position_valid << " task="
|
|
<< can0_task_name(state.task_state) << " result=" << state.result
|
|
<< " direction=" << state.direction
|
|
<< " duty=" << state.duty_percent << "% sequence="
|
|
<< state.sequence << " near=" << *get_bits(data, len, 20, 1)
|
|
<< " far=" << *get_bits(data, len, 21, 1)
|
|
<< " stall=" << *get_bits(data, len, 22, 1)
|
|
<< " driverFault=" << *get_bits(data, len, 23, 1);
|
|
public_store.update("CAN0/passenger/" + can0_axis_name(motor),
|
|
summary.str());
|
|
return;
|
|
}
|
|
if (id == 0x303) {
|
|
const unsigned motor = static_cast<unsigned>(*get_bits(data, len, 0, 8));
|
|
std::ostringstream summary;
|
|
summary << "axis=" << can0_axis_name(motor)
|
|
<< " event=" << *get_bits(data, len, 8, 4)
|
|
<< " faultFlags=0x" << std::hex
|
|
<< *get_bits(data, len, 12, 4) << std::dec
|
|
<< " hall=" << *get_bits(data, len, 16, 16)
|
|
<< '/' << *get_bits(data, len, 32, 16)
|
|
<< " speedHalfPps=" << *get_bits(data, len, 48, 8)
|
|
<< " retry=" << *get_bits(data, len, 56, 4);
|
|
public_store.update("CAN0/passenger/diagnostic/" +
|
|
can0_axis_name(motor),
|
|
summary.str());
|
|
return;
|
|
}
|
|
if (id == 0x304) {
|
|
const unsigned motor = static_cast<unsigned>(*get_bits(data, len, 0, 8));
|
|
std::ostringstream summary;
|
|
summary << "axis=" << can0_axis_name(motor)
|
|
<< " sampleValid=" << *get_bits(data, len, 8, 1)
|
|
<< " baselineValid=" << *get_bits(data, len, 9, 1)
|
|
<< " highCurrent=" << *get_bits(data, len, 10, 1)
|
|
<< " hallLow=" << *get_bits(data, len, 11, 1)
|
|
<< " calibrating=" << *get_bits(data, len, 12, 1)
|
|
<< " current=" << *get_bits(data, len, 16, 16)
|
|
<< " baseline=" << *get_bits(data, len, 32, 16)
|
|
<< " stallDelta=" << *get_bits(data, len, 48, 16);
|
|
public_store.update("CAN0/passenger/current/" + can0_axis_name(motor),
|
|
summary.str());
|
|
return;
|
|
}
|
|
if (id == 0x305) {
|
|
std::ostringstream summary;
|
|
summary << "fanValid=" << *get_bits(data, len, 0, 1)
|
|
<< " heaterValid=" << *get_bits(data, len, 1, 1)
|
|
<< " fanRunning=" << *get_bits(data, len, 8, 1)
|
|
<< " heaterLevel=" << *get_bits(data, len, 9, 2)
|
|
<< " fanCurrent=" << *get_bits(data, len, 16, 16)
|
|
<< " heaterCurrent=" << *get_bits(data, len, 32, 16)
|
|
<< " fanAdcErrors=" << *get_bits(data, len, 48, 8)
|
|
<< " heaterAdcErrors=" << *get_bits(data, len, 56, 8);
|
|
public_store.update("CAN0/passenger/thermal-diagnostic", summary.str());
|
|
return;
|
|
}
|
|
if (id == 0x306) {
|
|
Can0DynamicState state;
|
|
state.mode_id = static_cast<int>(*get_bits(data, len, 0, 8));
|
|
state.state = static_cast<int>(*get_bits(data, len, 8, 8));
|
|
state.phase = static_cast<int>(*get_bits(data, len, 16, 8));
|
|
state.fault_mask = static_cast<int>(*get_bits(data, len, 28, 4));
|
|
state.cycle = static_cast<int>(*get_bits(data, len, 32, 16));
|
|
state.elapsed_ms = static_cast<int>(*get_bits(data, len, 48, 16));
|
|
can0_store.update_dynamic(state);
|
|
std::ostringstream summary;
|
|
summary << "mode=" << state.mode_id << " state=" << state.state
|
|
<< " phase=" << state.phase
|
|
<< " activeMask=0x" << std::hex
|
|
<< *get_bits(data, len, 24, 4)
|
|
<< " faultMask=0x" << state.fault_mask << std::dec
|
|
<< " cycle=" << state.cycle
|
|
<< " phaseElapsedMs=" << state.elapsed_ms;
|
|
public_store.update("CAN0/passenger/dynamic", summary.str());
|
|
return;
|
|
}
|
|
if (id == 0x4FA) {
|
|
Can0ThermalState state;
|
|
state.ventilation_raw = static_cast<int>(*get_bits(data, len, 0, 3));
|
|
state.heating_raw = static_cast<int>(*get_bits(data, len, 3, 3));
|
|
can0_store.update_thermal(state);
|
|
std::ostringstream summary;
|
|
summary << "ventRaw=" << state.ventilation_raw
|
|
<< " ventLevel=" << std::max(0, state.ventilation_raw - 1)
|
|
<< " heatRaw=" << state.heating_raw
|
|
<< " heatLevel=" << std::max(0, state.heating_raw - 1);
|
|
public_store.update("CAN0/passenger/thermal", summary.str());
|
|
}
|
|
}
|
|
|
|
struct Can1OutletSwitch {
|
|
std::string_view command;
|
|
std::string_view display_name;
|
|
U8 sub_id;
|
|
unsigned command_shift;
|
|
std::size_t vertical_feedback_byte;
|
|
std::size_t horizontal_feedback_byte;
|
|
unsigned gateway_feedback_start_bit;
|
|
};
|
|
|
|
inline constexpr std::array<Can1OutletSwitch, 4> kCan1OutletSwitches{{
|
|
{"driver-left", "主驾左", 0x02, 0, 41, 45, 64},
|
|
{"driver-right", "主驾右", 0x02, 2, 42, 46, 66},
|
|
{"passenger-left", "副驾左", 0x03, 0, 43, 47, 116},
|
|
{"passenger-right", "副驾右", 0x03, 2, 44, 48, 118},
|
|
}};
|
|
|
|
const Can1OutletSwitch* find_can1_outlet_switch(std::string_view command) {
|
|
const auto item = std::find_if(
|
|
kCan1OutletSwitches.begin(), kCan1OutletSwitches.end(),
|
|
[command](const auto& candidate) {
|
|
return candidate.command == command;
|
|
});
|
|
return item == kCan1OutletSwitches.end() ? nullptr : &*item;
|
|
}
|
|
|
|
std::string can1_outlet_switch_category(const Can1OutletSwitch& outlet) {
|
|
return "CAN1/outlet-switch/" + std::string(outlet.command);
|
|
}
|
|
|
|
std::optional<std::array<U8, 8>> parse_shared_payload_hex(
|
|
std::string_view text) {
|
|
std::array<U8, 8> payload{};
|
|
std::istringstream input{std::string(text)};
|
|
std::string token;
|
|
for (auto& byte : payload) {
|
|
if (!(input >> token) || token.size() != 2)
|
|
return std::nullopt;
|
|
unsigned value = 0;
|
|
const auto parsed = std::from_chars(
|
|
token.data(), token.data() + token.size(), value, 16);
|
|
if (parsed.ec != std::errc{} ||
|
|
parsed.ptr != token.data() + token.size() || value > 0xFF)
|
|
return std::nullopt;
|
|
byte = static_cast<U8>(value);
|
|
}
|
|
return input >> token ? std::nullopt
|
|
: std::optional<std::array<U8, 8>>{payload};
|
|
}
|
|
|
|
std::optional<std::array<U8, 8>> make_can1_outlet_switch_payload(
|
|
std::string_view target, std::string_view state) {
|
|
const auto* outlet = find_can1_outlet_switch(target);
|
|
if (!outlet || (state != "open" && state != "close"))
|
|
return std::nullopt;
|
|
const auto selector = std::string(target) + ':' + std::string(state);
|
|
const auto* generated = shared_protocol::find_static_payload(
|
|
"can1.outlet.switch", selector);
|
|
return generated ? parse_shared_payload_hex(generated->payload_hex)
|
|
: std::nullopt;
|
|
}
|
|
|
|
void decode_can1_outlet_switch_feedback(
|
|
U32 id, const U8* data, std::size_t len, StatusStore& store) {
|
|
if (id == 0x4BB && len >= 49) {
|
|
for (const auto& outlet : kCan1OutletSwitches) {
|
|
const U8 vertical = data[outlet.vertical_feedback_byte];
|
|
const U8 horizontal = data[outlet.horizontal_feedback_byte];
|
|
const std::string state = vertical == 0x80
|
|
? "open"
|
|
: vertical == 0x26
|
|
? "close"
|
|
: "unknown";
|
|
std::ostringstream summary;
|
|
summary << "state=" << state << " source=0x4BB"
|
|
<< " verticalRaw=0x" << std::hex << std::uppercase
|
|
<< std::setw(2) << std::setfill('0')
|
|
<< static_cast<unsigned>(vertical)
|
|
<< " horizontalRaw=0x" << std::setw(2)
|
|
<< static_cast<unsigned>(horizontal);
|
|
store.update(can1_outlet_switch_category(outlet), summary.str());
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (id == 0x37B && len >= 15 && data[0] == 0x50) {
|
|
for (const auto& outlet : kCan1OutletSwitches) {
|
|
const auto raw = get_bits(
|
|
data, len, outlet.gateway_feedback_start_bit, 2);
|
|
if (!raw)
|
|
continue;
|
|
const std::string state = *raw == 1
|
|
? "open"
|
|
: *raw == 2 ? "close" : "unknown";
|
|
store.update(
|
|
can1_outlet_switch_category(outlet),
|
|
"state=" + state + " source=0x37B/0x50 raw=" +
|
|
std::to_string(*raw));
|
|
}
|
|
}
|
|
}
|
|
|
|
bool channel_selected(const std::string& selection, unsigned channel) {
|
|
return selection == "all" || selection == "can" + std::to_string(channel);
|
|
}
|
|
|
|
void dual_receiver_loop(Can0Channel& can0, CanDevice& can1,
|
|
StatusStore& public_store, Can0StateStore& can0_store,
|
|
const std::atomic<bool>& running,
|
|
const std::string& selected_channel, bool trace_all) {
|
|
constexpr std::size_t batch_size = 64;
|
|
while (running.load(std::memory_order_relaxed)) {
|
|
bool received_any = false;
|
|
|
|
std::array<ZCAN_20_MSG, batch_size> can0_messages{};
|
|
const U32 can0_count = can0.receive(can0_messages.data(), batch_size);
|
|
if (can0_count <= batch_size)
|
|
received_any = received_any || can0_count != 0;
|
|
for (U32 index = 0; index < std::min<U32>(can0_count, batch_size); ++index) {
|
|
const auto& message = can0_messages[index];
|
|
if (message.hdr.inf.err) {
|
|
print_error_frame(message);
|
|
continue;
|
|
}
|
|
if (trace_all && channel_selected(selected_channel, 0))
|
|
print_trace_frame(message);
|
|
if (channel_selected(selected_channel, 0) &&
|
|
!message.hdr.inf.sef && !message.hdr.inf.sdf)
|
|
decode_can0_frame(message.hdr.id, message.dat,
|
|
std::min<std::size_t>(message.hdr.len, 8),
|
|
public_store, can0_store);
|
|
}
|
|
|
|
std::array<ZCAN_20_MSG, batch_size> can1_classic{};
|
|
const U32 classic_count = can1.receive(can1_classic.data(), batch_size, 0);
|
|
if (classic_count <= batch_size)
|
|
received_any = received_any || classic_count != 0;
|
|
for (U32 index = 0; index < std::min<U32>(classic_count, batch_size); ++index) {
|
|
const auto& message = can1_classic[index];
|
|
if (message.hdr.inf.err) {
|
|
print_error_frame(message);
|
|
continue;
|
|
}
|
|
if (trace_all && channel_selected(selected_channel, 1))
|
|
print_trace_frame(message);
|
|
if (channel_selected(selected_channel, 1) &&
|
|
!message.hdr.inf.sef && !message.hdr.inf.sdf) {
|
|
decode_can1_outlet_switch_feedback(
|
|
message.hdr.id, message.dat,
|
|
std::min<std::size_t>(message.hdr.len, 8), public_store);
|
|
decode_frame(message.hdr.id, message.dat,
|
|
std::min<std::size_t>(message.hdr.len, 8),
|
|
public_store);
|
|
}
|
|
}
|
|
|
|
std::array<ZCAN_FD_MSG, batch_size> can1_fd{};
|
|
const U32 fd_count = can1.receive_fd(can1_fd.data(), batch_size, 0);
|
|
if (fd_count <= batch_size)
|
|
received_any = received_any || fd_count != 0;
|
|
for (U32 index = 0; index < std::min<U32>(fd_count, batch_size); ++index) {
|
|
const auto& message = can1_fd[index];
|
|
if (message.hdr.inf.err) {
|
|
print_error_frame(message);
|
|
continue;
|
|
}
|
|
if (channel_selected(selected_channel, 1) &&
|
|
message.hdr.id == 0x4C1 && message.hdr.len == 8 &&
|
|
message.hdr.inf.fmt && message.hdr.inf.brs &&
|
|
!message.hdr.inf.sef && !message.hdr.inf.sdf) {
|
|
public_store.observe_driver_memory_frame(message.dat, 8);
|
|
}
|
|
if (trace_all && channel_selected(selected_channel, 1))
|
|
print_trace_frame(message);
|
|
if (channel_selected(selected_channel, 1) &&
|
|
!message.hdr.inf.sef && !message.hdr.inf.sdf) {
|
|
decode_can1_outlet_switch_feedback(
|
|
message.hdr.id, message.dat,
|
|
std::min<std::size_t>(message.hdr.len, 64), public_store);
|
|
decode_frame(message.hdr.id, message.dat,
|
|
std::min<std::size_t>(message.hdr.len, 64),
|
|
public_store);
|
|
}
|
|
}
|
|
|
|
if (!received_any)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
}
|
|
}
|
|
|
|
struct PassengerAxis {
|
|
unsigned motor;
|
|
std::string_view canonical;
|
|
std::string_view direction_a;
|
|
std::string_view direction_b;
|
|
int direction_a_sign;
|
|
int milliseconds_per_percent;
|
|
int duty_x10;
|
|
};
|
|
|
|
const PassengerAxis* passenger_axis(std::string_view name) {
|
|
static constexpr std::array<PassengerAxis, 4> axes{{
|
|
{0, "horizontal", "forward", "backward", -1, 120, 900},
|
|
{1, "basin", "up", "down", 1, 80, 900},
|
|
{2, "backrest", "forward", "backward", -1, 120, 900},
|
|
{3, "legrest", "up", "down", 1, 100, 500},
|
|
}};
|
|
if (name == "longitudinal")
|
|
name = "horizontal";
|
|
if (name == "height")
|
|
name = "basin";
|
|
const auto found = std::find_if(axes.begin(), axes.end(), [name](const auto& axis) {
|
|
return axis.canonical == name;
|
|
});
|
|
return found == axes.end() ? nullptr : &*found;
|
|
}
|
|
|
|
std::uint16_t next_can0_sequence() {
|
|
static std::atomic<std::uint32_t> sequence{1};
|
|
auto value = static_cast<std::uint16_t>(sequence.fetch_add(1));
|
|
if (value == 0)
|
|
value = static_cast<std::uint16_t>(sequence.fetch_add(1));
|
|
return value;
|
|
}
|
|
|
|
std::array<U8, 8> make_can0_move_payload(int opcode, unsigned motor,
|
|
int direction,
|
|
std::uint16_t sequence,
|
|
std::uint16_t parameter_a,
|
|
std::uint16_t parameter_b) {
|
|
std::array<U8, 8> payload{};
|
|
payload[0] = static_cast<U8>(opcode);
|
|
payload[1] = static_cast<U8>((motor & 0x03U) |
|
|
((static_cast<unsigned>(direction) & 0x03U) << 2U) |
|
|
(1U << 4U));
|
|
payload[2] = static_cast<U8>(sequence & 0xFFU);
|
|
payload[3] = static_cast<U8>((sequence >> 8U) & 0xFFU);
|
|
payload[4] = static_cast<U8>(parameter_a & 0xFFU);
|
|
payload[5] = static_cast<U8>((parameter_a >> 8U) & 0xFFU);
|
|
payload[6] = static_cast<U8>(parameter_b & 0xFFU);
|
|
payload[7] = static_cast<U8>((parameter_b >> 8U) & 0xFFU);
|
|
return payload;
|
|
}
|
|
|
|
bool submit_can0(Can0Channel& channel, U32 id,
|
|
const std::array<U8, 8>& payload, std::string_view command) {
|
|
auto frame = make_classic_message(0, id, payload);
|
|
print_line("[semantic/CAN0] " + std::string(command));
|
|
print_tx_frame(frame);
|
|
const U32 sent = channel.transmit(frame);
|
|
if (sent != 1) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
|
ZCAN_STAT status{};
|
|
if (channel.read_status(status)) {
|
|
print_line("[submit failed] VCI_Transmit=" + std::to_string(sent) +
|
|
" TXE=" + std::to_string(static_cast<unsigned>(status.TXE)) +
|
|
" RXE=" + std::to_string(static_cast<unsigned>(status.RXE)));
|
|
} else {
|
|
print_line("[submit failed] VCI_Transmit=" + std::to_string(sent));
|
|
}
|
|
return false;
|
|
}
|
|
print_line("[submitted] VCI_Transmit=1; this proves SDK submission only, not CAN ACK or seat motion");
|
|
return true;
|
|
}
|
|
|
|
enum class Can0AckOutcome {
|
|
Accepted,
|
|
SequenceOld,
|
|
Failed,
|
|
};
|
|
|
|
std::string_view can0_reject_name(int reject_reason) {
|
|
static constexpr std::array<std::string_view, 15> names{
|
|
"NONE", "BAD_DLC", "BAD_VERSION", "BAD_OPCODE", "BAD_MOTOR",
|
|
"BAD_DIRECTION", "PARAM_RANGE", "POSITION_INVALID", "SEQUENCE_OLD",
|
|
"BUSY_SAME_MOTOR", "INTERLOCK", "FAULT_ACTIVE",
|
|
"SERVICE_MODE_REQUIRED", "CAL_DATA_INVALID", "INTERNAL_ERROR",
|
|
};
|
|
return reject_reason >= 0 &&
|
|
static_cast<std::size_t>(reject_reason) < names.size()
|
|
? names[static_cast<std::size_t>(reject_reason)]
|
|
: "UNKNOWN";
|
|
}
|
|
|
|
Can0AckOutcome classify_can0_ack(const Can0AckState& ack,
|
|
int expected_ack) {
|
|
if (ack.ack == expected_ack && ack.reject_reason == 0 &&
|
|
(expected_ack == 4 || !ack.safety_inhibit))
|
|
return Can0AckOutcome::Accepted;
|
|
if (ack.ack == 2 && ack.reject_reason == 8 &&
|
|
!ack.safety_inhibit && !ack.fault_summary)
|
|
return Can0AckOutcome::SequenceOld;
|
|
return Can0AckOutcome::Failed;
|
|
}
|
|
|
|
Can0AckOutcome evaluate_can0_ack(const std::optional<Can0AckState>& ack,
|
|
int expected_ack = 1) {
|
|
if (!ack) {
|
|
print_line("[feedback timeout] no matching CAN0 0x301 ACK was observed");
|
|
return Can0AckOutcome::Failed;
|
|
}
|
|
const auto outcome = classify_can0_ack(*ack, expected_ack);
|
|
if (outcome == Can0AckOutcome::SequenceOld) {
|
|
print_line("[sequence stale] CAN0 0x301 rejected sequence=" +
|
|
std::to_string(ack->sequence) +
|
|
" reject=8(SEQUENCE_OLD)");
|
|
return outcome;
|
|
}
|
|
if (outcome != Can0AckOutcome::Accepted) {
|
|
print_line("[rejected] CAN0 0x301 ack=" + std::to_string(ack->ack) +
|
|
" reject=" + std::to_string(ack->reject_reason) +
|
|
"(" + std::string(can0_reject_name(ack->reject_reason)) + ")" +
|
|
" inhibit=" + std::to_string(ack->safety_inhibit) +
|
|
" fault=" + std::to_string(ack->fault_summary));
|
|
return Can0AckOutcome::Failed;
|
|
}
|
|
if (expected_ack == 4) {
|
|
print_line("[protocol ACK] CAN0 0x301 accepted STOP_ALL sequence=" +
|
|
std::to_string(ack->sequence) +
|
|
"; this establishes the firmware sequence barrier");
|
|
} else {
|
|
print_line("[protocol ACK] CAN0 0x301 accepted sequence=" +
|
|
std::to_string(ack->sequence) +
|
|
"; physical completion still requires 0x302/0x306 feedback");
|
|
if (ack->fault_summary) {
|
|
print_line("[warning] CAN0 aggregate fault summary is set; the command ACK remains accepted and commanded-axis feedback will decide completion");
|
|
}
|
|
}
|
|
return Can0AckOutcome::Accepted;
|
|
}
|
|
|
|
std::optional<std::uint16_t> submit_can0_command_with_ack(
|
|
Can0Channel& channel, Can0StateStore& states, int opcode, unsigned motor,
|
|
int direction, std::uint16_t parameter_a, std::uint16_t parameter_b,
|
|
std::string_view semantic_command, std::string_view protocol_rule_id) {
|
|
const auto* protocol_frame = shared_protocol::find_frame(
|
|
protocol_rule_id,
|
|
protocol_rule_id == "can0.seat.dynamic" ? "dynamic" : "move");
|
|
if (!protocol_frame || protocol_frame->format != "classic_can" ||
|
|
protocol_frame->brs || protocol_frame->dlc != 8 ||
|
|
protocol_frame->repeat_count != 1 ||
|
|
protocol_frame->feedback_ids_csv.find("0x301") == std::string_view::npos) {
|
|
print_line("[blocked] generated CAN0 motion protocol metadata is invalid");
|
|
return std::nullopt;
|
|
}
|
|
for (int attempt = 0; attempt < 2; ++attempt) {
|
|
const auto sequence = next_can0_sequence();
|
|
const auto payload = make_can0_move_payload(
|
|
opcode, motor, direction, sequence, parameter_a, parameter_b);
|
|
const auto ack_before = states.ack_generation();
|
|
if (!submit_can0(
|
|
channel, protocol_frame->frame_id, payload, semantic_command))
|
|
return std::nullopt;
|
|
const auto outcome = evaluate_can0_ack(states.wait_ack(
|
|
sequence, opcode, ack_before, std::chrono::milliseconds(1500)));
|
|
if (outcome == Can0AckOutcome::Accepted)
|
|
return sequence;
|
|
if (outcome != Can0AckOutcome::SequenceOld || attempt != 0)
|
|
return std::nullopt;
|
|
|
|
// The firmware explicitly defines STOP_ALL as the sequence
|
|
// resynchronization barrier. It is accepted regardless of the prior
|
|
// reference, stops every CAN0 motion safely, and makes its own sequence
|
|
// the new reference. Never guess or brute-force a newer sequence.
|
|
print_line("[sequence resync] submitting firmware-defined STOP_ALL before one retry");
|
|
const auto stop_sequence = next_can0_sequence();
|
|
const auto stop_payload = make_can0_move_payload(
|
|
0x09, 0, 0, stop_sequence, 0, 0);
|
|
const auto stop_ack_before = states.ack_generation();
|
|
if (!submit_can0(channel, protocol_frame->frame_id, stop_payload,
|
|
"protocol sequence resynchronization STOP_ALL"))
|
|
return std::nullopt;
|
|
const auto stop_outcome = evaluate_can0_ack(states.wait_ack(
|
|
stop_sequence, 0x09, stop_ack_before,
|
|
std::chrono::milliseconds(1500)), 4);
|
|
if (stop_outcome != Can0AckOutcome::Accepted) {
|
|
print_line("[sequence resync failed] original command was not retried");
|
|
return std::nullopt;
|
|
}
|
|
if (!states.wait_all_idle(stop_ack_before,
|
|
std::chrono::milliseconds(5000))) {
|
|
print_line("[sequence resync failed] STOP_ALL was ACKed but CAN0 did not report all axes idle; original command was not retried");
|
|
return std::nullopt;
|
|
}
|
|
print_line("[sequence resync] complete; retrying the original semantic command once");
|
|
}
|
|
return std::nullopt;
|
|
}
|
|
|
|
void best_effort_can0_stop(Can0Channel& channel) {
|
|
const auto sequence = next_can0_sequence();
|
|
const auto payload = make_can0_move_payload(0x09, 0, 0, sequence, 0, 0);
|
|
const auto* frame = shared_protocol::find_frame(
|
|
"can0.seat.position", "move");
|
|
if (frame)
|
|
submit_can0(channel, frame->frame_id, payload,
|
|
"safety stop-all after incomplete motion");
|
|
}
|
|
|
|
bool execute_can0_position(Can0Channel& channel, Can0StateStore& states,
|
|
const PassengerAxis& axis, int target_percent,
|
|
std::string_view semantic_command) {
|
|
if (target_percent < 0 || target_percent > 100)
|
|
return false;
|
|
const bool homing = target_percent == 0;
|
|
if (!homing) {
|
|
const auto preflight = states.wait_fresh_motor(
|
|
axis.motor, std::chrono::milliseconds(2500));
|
|
if (!preflight || !preflight->position_valid) {
|
|
print_line("[blocked] a fresh, position-valid CAN0 0x302 frame is required before non-zero absolute movement");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
const int opcode = homing ? 0x0B : 0x01;
|
|
const int target_x100 = target_percent * 100;
|
|
const auto motor_before = states.motor_generation(axis.motor);
|
|
const auto sequence = submit_can0_command_with_ack(
|
|
channel, states, opcode, axis.motor, 0,
|
|
static_cast<std::uint16_t>(homing ? 0x4B52 : target_x100),
|
|
static_cast<std::uint16_t>(homing ? 120 : 30'000), semantic_command,
|
|
"can0.seat.position");
|
|
if (!sequence)
|
|
return false;
|
|
|
|
const auto timeout = homing ? std::chrono::milliseconds(125'000)
|
|
: std::chrono::milliseconds(35'000);
|
|
const auto completion = states.wait_motor_terminal(
|
|
axis.motor, *sequence, target_x100, motor_before, timeout);
|
|
if (!completion) {
|
|
print_line("[completion timeout] no matching terminal CAN0 0x302 state; submitting bounded stop-all");
|
|
best_effort_can0_stop(channel);
|
|
return false;
|
|
}
|
|
if (completion->task_state != 6 || completion->result != 2) {
|
|
print_line("[motion failed] CAN0 0x302 task=" +
|
|
std::to_string(completion->task_state) + " result=" +
|
|
std::to_string(completion->result));
|
|
return false;
|
|
}
|
|
print_line("[physical feedback] CAN0 0x302 confirmed " +
|
|
std::string(axis.canonical) + "=" +
|
|
std::to_string(completion->value_x100 / 100.0) + "%");
|
|
return true;
|
|
}
|
|
|
|
bool execute_can0_relative(Can0Channel& channel, Can0StateStore& states,
|
|
const PassengerAxis& axis,
|
|
std::string_view direction_name, int delta_percent,
|
|
std::string_view semantic_command) {
|
|
if (delta_percent < 1 || delta_percent > 100)
|
|
return false;
|
|
int sign = 0;
|
|
if (direction_name == axis.direction_a)
|
|
sign = axis.direction_a_sign;
|
|
else if (direction_name == axis.direction_b)
|
|
sign = -axis.direction_a_sign;
|
|
else
|
|
return false;
|
|
|
|
const auto preflight = states.wait_fresh_motor(
|
|
axis.motor, std::chrono::milliseconds(2500));
|
|
if (preflight && preflight->position_valid) {
|
|
const int target_x100 = std::clamp(
|
|
preflight->value_x100 + sign * delta_percent * 100, 0, 10'000);
|
|
if (target_x100 == preflight->value_x100) {
|
|
print_line("[no-op] passenger seat is already at the requested-direction endpoint; no frame sent");
|
|
return true;
|
|
}
|
|
return execute_can0_position(channel, states, axis,
|
|
target_x100 / 100, semantic_command);
|
|
}
|
|
|
|
if (delta_percent > 15) {
|
|
print_line("[blocked] relative movement over 15% requires a fresh position-valid CAN0 0x302 frame");
|
|
return false;
|
|
}
|
|
const int duration_ms = std::max(300, delta_percent * axis.milliseconds_per_percent);
|
|
const int protocol_direction = sign > 0 ? 1 : 2;
|
|
const auto motor_before = states.motor_generation(axis.motor);
|
|
print_line("[fallback] position is unavailable; using firmware-bounded timed motion, not Opcode 0x02");
|
|
const auto sequence = submit_can0_command_with_ack(
|
|
channel, states, 0x03, axis.motor, protocol_direction,
|
|
static_cast<std::uint16_t>(duration_ms),
|
|
static_cast<std::uint16_t>(axis.duty_x10), semantic_command,
|
|
"can0.seat.relative");
|
|
if (!sequence)
|
|
return false;
|
|
const auto completion = states.wait_motor_terminal(
|
|
axis.motor, *sequence, std::nullopt, motor_before,
|
|
std::chrono::milliseconds(duration_ms + 5000));
|
|
if (!completion) {
|
|
print_line("[completion timeout] timed movement did not produce terminal 0x302; submitting stop-all");
|
|
best_effort_can0_stop(channel);
|
|
return false;
|
|
}
|
|
if (completion->task_state != 6 || completion->result != 2) {
|
|
print_line("[motion failed] timed CAN0 movement task=" +
|
|
std::to_string(completion->task_state) + " result=" +
|
|
std::to_string(completion->result));
|
|
return false;
|
|
}
|
|
print_line("[physical feedback] CAN0 0x302 confirmed bounded timed movement completion");
|
|
return true;
|
|
}
|
|
|
|
bool execute_can0_dynamic(Can0Channel& channel, Can0StateStore& states,
|
|
int mode_id, bool stopping,
|
|
std::string_view semantic_command) {
|
|
const int opcode = stopping ? 0x0F : 0x0E;
|
|
const auto dynamic_before = states.dynamic_generation();
|
|
const auto sequence = submit_can0_command_with_ack(
|
|
channel, states, opcode, 0, 0,
|
|
static_cast<std::uint16_t>(stopping ? 0 : mode_id), 0,
|
|
semantic_command, "can0.seat.dynamic");
|
|
if (!sequence)
|
|
return false;
|
|
const auto feedback = states.wait_dynamic(
|
|
mode_id, stopping, dynamic_before, std::chrono::milliseconds(5000));
|
|
if (!feedback) {
|
|
print_line("[feedback timeout] no matching CAN0 0x306 dynamic state was observed");
|
|
return false;
|
|
}
|
|
print_line("[physical feedback] CAN0 0x306 confirmed dynamic mode=" +
|
|
std::to_string(feedback->mode_id) + " state=" +
|
|
std::to_string(feedback->state));
|
|
return true;
|
|
}
|
|
|
|
int thermal_request_raw(std::string_view level) {
|
|
if (level == "off")
|
|
return 1;
|
|
if (level == "low")
|
|
return 2;
|
|
if (level == "medium")
|
|
return 3;
|
|
if (level == "high")
|
|
return 4;
|
|
return 0;
|
|
}
|
|
|
|
bool execute_can0_thermal(Can0Channel& channel, Can0StateStore& states,
|
|
std::string_view feature, std::string_view level,
|
|
std::string_view semantic_command) {
|
|
const auto* mode_frame = shared_protocol::find_frame(
|
|
"can0.seat.thermal", "thermal_mode");
|
|
const auto* control_frame = shared_protocol::find_frame(
|
|
"can0.seat.thermal", "thermal_control");
|
|
if (!mode_frame || !control_frame ||
|
|
mode_frame->format != "classic_can" || mode_frame->brs ||
|
|
control_frame->format != "classic_can" || control_frame->brs ||
|
|
mode_frame->dlc != 8 || control_frame->dlc != 8 ||
|
|
mode_frame->repeat_count != 1 || control_frame->repeat_count != 1 ||
|
|
control_frame->feedback_ids_csv != "0x4FA") {
|
|
print_line("[blocked] generated CAN0 thermal protocol metadata is invalid");
|
|
return false;
|
|
}
|
|
const int requested_raw = thermal_request_raw(level);
|
|
if (requested_raw == 0 || (feature != "vent" && feature != "heat"))
|
|
return false;
|
|
const auto current = states.wait_fresh_thermal(std::chrono::milliseconds(2500));
|
|
if (!current || current->ventilation_raw < 1 || current->ventilation_raw > 4 ||
|
|
current->heating_raw < 1 || current->heating_raw > 4) {
|
|
print_line("[blocked] a fresh valid CAN0 0x4FA is required to preserve the other thermal feature");
|
|
return false;
|
|
}
|
|
const int ventilation_raw = feature == "vent" ? requested_raw
|
|
: current->ventilation_raw;
|
|
const int heating_raw = feature == "heat" ? requested_raw
|
|
: current->heating_raw;
|
|
if (feature == "heat" && requested_raw > 1) {
|
|
std::array<U8, 8> mode{};
|
|
set_bits(mode.data(), mode.size(), 4, 2, 1);
|
|
if (!submit_can0(channel, mode_frame->frame_id, mode,
|
|
"seat passenger heat thermal-mode"))
|
|
return false;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
|
}
|
|
|
|
std::array<U8, 8> payload{};
|
|
set_bits(payload.data(), payload.size(), 0, 2,
|
|
ventilation_raw == 1 ? 2 : 1);
|
|
set_bits(payload.data(), payload.size(), 2, 2, 1);
|
|
set_bits(payload.data(), payload.size(), 4, 2,
|
|
heating_raw == 1 ? 2 : 1);
|
|
set_bits(payload.data(), payload.size(), 16, 4,
|
|
static_cast<unsigned>(std::max(0, ventilation_raw - 1)));
|
|
set_bits(payload.data(), payload.size(), 20, 4,
|
|
static_cast<unsigned>(std::max(0, heating_raw - 1)));
|
|
const auto thermal_before = states.thermal_generation();
|
|
if (!submit_can0(
|
|
channel, control_frame->frame_id, payload, semantic_command))
|
|
return false;
|
|
const auto feedback = states.wait_thermal(
|
|
ventilation_raw, heating_raw, thermal_before,
|
|
std::chrono::milliseconds(2500));
|
|
if (!feedback) {
|
|
print_line("[feedback timeout] CAN0 0x4FA did not confirm the requested thermal state");
|
|
return false;
|
|
}
|
|
print_line("[physical feedback] CAN0 0x4FA confirmed ventilationRaw=" +
|
|
std::to_string(feedback->ventilation_raw) + " heatingRaw=" +
|
|
std::to_string(feedback->heating_raw));
|
|
return true;
|
|
}
|
|
|
|
bool can0_command_is_whitelisted(const std::vector<std::string>& words) {
|
|
return shared_protocol::match_semantic_command(words, 0) != nullptr;
|
|
}
|
|
|
|
bool execute_can0_command(const std::string& command, Can0Channel& channel,
|
|
Can0StateStore& states) {
|
|
auto words = split(command);
|
|
for (auto& word : words)
|
|
word = lower_ascii(std::move(word));
|
|
if (!can0_command_is_whitelisted(words)) {
|
|
print_line("[blocked] command is not in the current CAN0 semantic whitelist");
|
|
return false;
|
|
}
|
|
|
|
if (words[2] == "dynamic") {
|
|
if (words.size() == 4 && words[3] == "rocking-horse")
|
|
return execute_can0_dynamic(channel, states, 1, false, command);
|
|
if (words.size() == 4 && words[3] == "cradle")
|
|
return execute_can0_dynamic(channel, states, 2, false, command);
|
|
if (words.size() == 4 && words[3] == "stop")
|
|
return execute_can0_dynamic(channel, states, 0, true, command);
|
|
if (words.size() == 5 && words[3] == "start" &&
|
|
words[4] == "rocking-horse")
|
|
return execute_can0_dynamic(channel, states, 1, false, command);
|
|
if (words.size() == 5 && words[3] == "start" &&
|
|
words[4] == "cradle")
|
|
return execute_can0_dynamic(channel, states, 2, false, command);
|
|
return false;
|
|
}
|
|
if ((words[2] == "vent" || words[2] == "heat") && words.size() == 4)
|
|
return execute_can0_thermal(channel, states, words[2], words[3], command);
|
|
|
|
const auto* axis = passenger_axis(words[2]);
|
|
if (!axis)
|
|
return false;
|
|
if (words.size() == 5 && words[3] == "position") {
|
|
const auto position = parse_int(words[4]);
|
|
return position && execute_can0_position(
|
|
channel, states, *axis, *position, command);
|
|
}
|
|
if (words.size() == 6 && words[3] == "relative") {
|
|
const auto delta = parse_int(words[5]);
|
|
return delta && execute_can0_relative(
|
|
channel, states, *axis, words[4], *delta, command);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool can1_command_is_whitelisted(const std::vector<std::string>& words) {
|
|
return shared_protocol::match_semantic_command(words, 1) != nullptr;
|
|
}
|
|
|
|
enum class SemanticChannel { None, Can0, Can1, Ambiguous };
|
|
|
|
SemanticChannel route_semantic_command(const std::vector<std::string>& words) {
|
|
const bool can0 = can0_command_is_whitelisted(words);
|
|
const bool can1 = can1_command_is_whitelisted(words);
|
|
if (can0 && can1)
|
|
return SemanticChannel::Ambiguous;
|
|
if (can0)
|
|
return SemanticChannel::Can0;
|
|
if (can1)
|
|
return SemanticChannel::Can1;
|
|
return SemanticChannel::None;
|
|
}
|
|
|
|
std::string_view semantic_channel_name(SemanticChannel channel) {
|
|
if (channel == SemanticChannel::Can0)
|
|
return "CAN0";
|
|
if (channel == SemanticChannel::Can1)
|
|
return "CAN1";
|
|
if (channel == SemanticChannel::Ambiguous)
|
|
return "ambiguous";
|
|
return "none";
|
|
}
|
|
|
|
bool selected_channel_allows(const std::string& selection,
|
|
SemanticChannel channel) {
|
|
if (selection == "all")
|
|
return channel == SemanticChannel::Can0 ||
|
|
channel == SemanticChannel::Can1;
|
|
return (selection == "can0" && channel == SemanticChannel::Can0) ||
|
|
(selection == "can1" && channel == SemanticChannel::Can1);
|
|
}
|
|
|
|
struct RoutedSemanticCommand {
|
|
std::string command;
|
|
std::vector<std::string> words;
|
|
SemanticChannel route = SemanticChannel::None;
|
|
};
|
|
|
|
enum class CommandExecutionOutcome {
|
|
FailedOrBlocked,
|
|
SubmittedUnverified,
|
|
VerifiedSuccess,
|
|
};
|
|
|
|
bool batch_may_continue(CommandExecutionOutcome outcome) {
|
|
return outcome != CommandExecutionOutcome::FailedOrBlocked;
|
|
}
|
|
|
|
constexpr std::size_t kMaxBatchCommands = 32;
|
|
constexpr std::size_t kMaxBatchCommandLength = 160;
|
|
|
|
bool is_batch_command_line(std::string_view line) {
|
|
const auto normalized = trim_ascii(line);
|
|
const auto separator = normalized.find_first_of(" \t");
|
|
if (separator == std::string::npos)
|
|
return lower_ascii(normalized) == "batch";
|
|
return lower_ascii(normalized.substr(0, separator)) == "batch";
|
|
}
|
|
|
|
bool is_can0_axis_motion(const RoutedSemanticCommand& command) {
|
|
return command.route == SemanticChannel::Can0 &&
|
|
command.words.size() >= 3 &&
|
|
passenger_axis(command.words[2]) != nullptr;
|
|
}
|
|
|
|
bool is_can0_dynamic_start(const RoutedSemanticCommand& command) {
|
|
if (command.route != SemanticChannel::Can0 ||
|
|
command.words.size() < 4 || command.words[2] != "dynamic")
|
|
return false;
|
|
return command.words[3] == "rocking-horse" ||
|
|
command.words[3] == "cradle" ||
|
|
(command.words.size() == 5 && command.words[3] == "start");
|
|
}
|
|
|
|
std::optional<std::vector<RoutedSemanticCommand>> validate_batch_command(
|
|
std::string_view line, const std::string& selected_channel,
|
|
std::string& error) {
|
|
error.clear();
|
|
const auto normalized = trim_ascii(line);
|
|
const auto prefix_end = normalized.find_first_of(" \t");
|
|
if (prefix_end == std::string::npos ||
|
|
lower_ascii(normalized.substr(0, prefix_end)) != "batch") {
|
|
error = "batch syntax is: batch <command> ; <command>";
|
|
return std::nullopt;
|
|
}
|
|
const auto payload = trim_ascii(normalized.substr(prefix_end + 1));
|
|
if (payload.empty()) {
|
|
error = "batch requires at least one semantic command";
|
|
return std::nullopt;
|
|
}
|
|
|
|
std::vector<RoutedSemanticCommand> commands;
|
|
std::size_t start = 0;
|
|
while (start <= payload.size()) {
|
|
const auto delimiter = payload.find(';', start);
|
|
const auto segment = trim_ascii(std::string_view(payload).substr(
|
|
start, delimiter == std::string::npos ? std::string::npos
|
|
: delimiter - start));
|
|
if (segment.empty()) {
|
|
error = "batch contains an empty command";
|
|
return std::nullopt;
|
|
}
|
|
if (segment.size() > kMaxBatchCommandLength) {
|
|
error = "batch command exceeds 160 characters";
|
|
return std::nullopt;
|
|
}
|
|
if (commands.size() >= kMaxBatchCommands) {
|
|
error = "batch accepts at most 32 commands";
|
|
return std::nullopt;
|
|
}
|
|
|
|
auto words = split(segment);
|
|
for (auto& word : words)
|
|
word = lower_ascii(std::move(word));
|
|
const auto route = route_semantic_command(words);
|
|
const std::size_t command_number = commands.size() + 1;
|
|
if (route == SemanticChannel::Ambiguous) {
|
|
error = "batch command " + std::to_string(command_number) +
|
|
" matches both channel whitelists";
|
|
return std::nullopt;
|
|
}
|
|
if (route == SemanticChannel::None) {
|
|
error = "batch command " + std::to_string(command_number) +
|
|
" is outside both semantic whitelists: " + segment;
|
|
return std::nullopt;
|
|
}
|
|
if (!selected_channel_allows(selected_channel, route)) {
|
|
error = "batch command " + std::to_string(command_number) +
|
|
" routes to " + std::string(semantic_channel_name(route)) +
|
|
" but this console selected --channel " + selected_channel;
|
|
return std::nullopt;
|
|
}
|
|
commands.push_back({join_words(words), std::move(words), route});
|
|
|
|
if (delimiter == std::string::npos)
|
|
break;
|
|
start = delimiter + 1;
|
|
}
|
|
|
|
const bool starts_dynamic = std::any_of(
|
|
commands.begin(), commands.end(), is_can0_dynamic_start);
|
|
const bool moves_axis = std::any_of(
|
|
commands.begin(), commands.end(), is_can0_axis_motion);
|
|
if (starts_dynamic && moves_axis) {
|
|
error = "a CAN0 dynamic-mode start cannot share a batch with CAN0 axis movement";
|
|
return std::nullopt;
|
|
}
|
|
return commands;
|
|
}
|
|
|
|
bool execute_can1_cycle(const std::vector<std::string>& words,
|
|
CanDevice& channel, StatusStore& states) {
|
|
if (words.size() != 3)
|
|
return false;
|
|
std::array<U8, 8> payload{};
|
|
const unsigned value = words[2] == "internal" ? 1U : 2U;
|
|
set_bits(payload.data(), payload.size(), 19, 2, value);
|
|
print_line("[semantic/CAN1] " + join_words(words));
|
|
const auto* frame = shared_protocol::find_frame("can1.climate.cycle");
|
|
if (!frame || frame->format != "classic_can" || frame->brs ||
|
|
frame->dlc != payload.size())
|
|
return false;
|
|
return send_control(
|
|
channel, states, "空调", frame->frame_id, payload, false,
|
|
frame->repeat_count, std::chrono::milliseconds(frame->period_ms));
|
|
}
|
|
|
|
CommandExecutionOutcome execute_can1_outlet_switch(
|
|
const std::vector<std::string>& words, CanDevice& channel,
|
|
StatusStore& states) {
|
|
if (words.size() != 3)
|
|
return CommandExecutionOutcome::FailedOrBlocked;
|
|
const auto* outlet = find_can1_outlet_switch(words[1]);
|
|
const auto payload = make_can1_outlet_switch_payload(words[1], words[2]);
|
|
if (!outlet || !payload)
|
|
return CommandExecutionOutcome::FailedOrBlocked;
|
|
|
|
const std::string category = can1_outlet_switch_category(*outlet);
|
|
const auto old_sequence = states.sequence(category);
|
|
print_line("[semantic/CAN1] " + join_words(words));
|
|
print_line("[风口开关请求] " + std::string(outlet->display_name) +
|
|
" state=" + words[2] +
|
|
" source=0x4EF/0x" +
|
|
std::string(outlet->sub_id == 0x02 ? "02" : "03"));
|
|
const auto* frame = shared_protocol::find_frame("can1.outlet.switch");
|
|
if (!frame || frame->format != "can_fd" || !frame->brs ||
|
|
frame->dlc != payload->size()) {
|
|
print_line("[blocked] generated CAN1 outlet protocol metadata is invalid");
|
|
return CommandExecutionOutcome::FailedOrBlocked;
|
|
}
|
|
if (!transmit_fd_sequence(channel, frame->frame_id, *payload,
|
|
frame->repeat_count,
|
|
std::chrono::milliseconds{frame->period_ms},
|
|
"单风口开关"))
|
|
return CommandExecutionOutcome::FailedOrBlocked;
|
|
|
|
const std::string expected = "state=" + words[2];
|
|
const auto feedback = states.wait_after_matching(
|
|
category, old_sequence, kStatusTimeout,
|
|
[&expected](const std::string& summary) {
|
|
return summary.find(expected) != std::string::npos;
|
|
});
|
|
if (feedback) {
|
|
print_line("[协议反馈] " + std::string(outlet->display_name) +
|
|
" 已匹配目标: " + *feedback);
|
|
print_line("[提醒] 0x4BB/0x37B 状态匹配与 CAN ACK 均不能替代实体风口核对。");
|
|
return CommandExecutionOutcome::VerifiedSuccess;
|
|
}
|
|
print_line("[警告] " + std::to_string(kStatusTimeout.count()) +
|
|
" ms 内未观察到“" + expected +
|
|
"”的 0x4BB 或 0x37B/0x50 后续反馈。");
|
|
print_line("[提交未确认] " + std::to_string(frame->repeat_count) +
|
|
" 帧均已由 SDK 提交，但协议反馈和实体动作尚未确认。");
|
|
return CommandExecutionOutcome::SubmittedUnverified;
|
|
}
|
|
|
|
CommandExecutionOutcome execute_can1_command(
|
|
const std::string& command, CanDevice& channel, StatusStore& states) {
|
|
auto words = split(command);
|
|
for (auto& word : words)
|
|
word = lower_ascii(std::move(word));
|
|
if (!can1_command_is_whitelisted(words)) {
|
|
print_line("[blocked] command is not in the current CAN1 semantic whitelist");
|
|
return CommandExecutionOutcome::FailedOrBlocked;
|
|
}
|
|
if (words.size() == 3 && words[0] == "ac" && words[1] == "cycle") {
|
|
return execute_can1_cycle(words, channel, states)
|
|
? CommandExecutionOutcome::VerifiedSuccess
|
|
: CommandExecutionOutcome::FailedOrBlocked;
|
|
}
|
|
if (words.size() == 3 && words[0] == "outlet")
|
|
return execute_can1_outlet_switch(words, channel, states);
|
|
bool quit = false;
|
|
print_line("[semantic/CAN1] " + join_words(words));
|
|
return handle_command(join_words(words), channel, states, false, quit)
|
|
? CommandExecutionOutcome::VerifiedSuccess
|
|
: CommandExecutionOutcome::FailedOrBlocked;
|
|
}
|
|
|
|
CommandExecutionOutcome execute_routed_command(
|
|
const RoutedSemanticCommand& command, Can0Channel& can0, CanDevice& can1,
|
|
Can0StateStore& can0_states, StatusStore& can1_states) {
|
|
if (command.route == SemanticChannel::Can0) {
|
|
return execute_can0_command(command.command, can0, can0_states)
|
|
? CommandExecutionOutcome::VerifiedSuccess
|
|
: CommandExecutionOutcome::FailedOrBlocked;
|
|
}
|
|
return execute_can1_command(command.command, can1, can1_states);
|
|
}
|
|
|
|
void print_whitelist() {
|
|
std::lock_guard<std::mutex> lock(g_output_mutex);
|
|
std::cout << "Shared protocol version " << shared_protocol::kProtocolVersion
|
|
<< " (source sha256=" << shared_protocol::kProtocolSourceSha256
|
|
<< ")\n"
|
|
<< "CAN0 passenger-seat semantic whitelist (Classic CAN, channel 0):\n";
|
|
for (const auto& rule : shared_protocol::kRules)
|
|
if (rule.channel == 0)
|
|
std::cout << " " << rule.help << '\n';
|
|
std::cout << "\nCAN1 vehicle semantic whitelist (channel 1):\n";
|
|
for (const auto& rule : shared_protocol::kRules)
|
|
if (rule.channel == 1)
|
|
std::cout << " " << rule.help << '\n';
|
|
std::cout << "\nThere is intentionally no raw ID/data transmit command.\n";
|
|
}
|
|
|
|
std::vector<std::string> can1_batch_regression_commands() {
|
|
std::vector<std::string> commands;
|
|
for (const auto state : {"lock", "unlock"})
|
|
commands.push_back("door rear " + std::string(state));
|
|
for (const auto target : {"all", "front", "rear", "front-left",
|
|
"front-right", "rear-left", "rear-right"}) {
|
|
for (const auto motion : {"open", "close", "stop"})
|
|
commands.push_back("window " + std::string(target) + ' ' + motion);
|
|
}
|
|
for (const auto target : {"front-left", "front-right", "rear-left",
|
|
"rear-right"}) {
|
|
for (const int position : {0, 100})
|
|
commands.push_back("window " + std::string(target) +
|
|
" position " + std::to_string(position));
|
|
}
|
|
commands.insert(commands.end(), {"ac on", "ac off"});
|
|
for (int level = 1; level <= 7; ++level)
|
|
commands.push_back("ac fan " + std::to_string(level));
|
|
for (const auto mode : {"face", "face-feet", "feet", "feet-defrost",
|
|
"defrost", "face-feet-defrost", "face-defrost"})
|
|
commands.push_back("ac mode " + std::string(mode));
|
|
for (const auto target : {"driver", "passenger", "rear"}) {
|
|
for (const int temperature : {18, 32})
|
|
commands.push_back("ac temp " + std::string(target) + ' ' +
|
|
std::to_string(temperature));
|
|
}
|
|
commands.insert(commands.end(), {
|
|
"ac control manual", "ac control auto",
|
|
"ac cycle internal", "ac cycle external",
|
|
});
|
|
for (const auto target : {"driver-left", "driver-right",
|
|
"passenger-left", "passenger-right"}) {
|
|
for (const auto state : {"open", "close"})
|
|
commands.push_back("outlet " + std::string(target) + ' ' + state);
|
|
commands.push_back("outlet " + std::string(target) +
|
|
" percent 0 0");
|
|
commands.push_back("outlet " + std::string(target) +
|
|
" percent 100 100");
|
|
}
|
|
commands.insert(commands.end(), {
|
|
"outlet driver-left position -25 -40",
|
|
"outlet driver-right position 30 30",
|
|
"outlet passenger-left position -10 -40",
|
|
"outlet passenger-right position 5 30",
|
|
});
|
|
for (const auto feature : {"vent", "heat"}) {
|
|
for (const auto level : {"off", "low", "high"})
|
|
commands.push_back("seat driver " + std::string(feature) + ' ' +
|
|
level);
|
|
}
|
|
commands.push_back("seat driver massage off");
|
|
for (const auto strength : {"weak", "medium", "strong"})
|
|
commands.push_back("seat driver massage strength " +
|
|
std::string(strength));
|
|
for (const auto mode : {"wave", "pulse", "stretch", "slack", "relax"})
|
|
commands.push_back("seat driver massage mode " + std::string(mode));
|
|
commands.insert(commands.end(), {
|
|
"seat driver horizontal forward 100",
|
|
"seat driver horizontal backward 2000",
|
|
"seat driver horizontal stop",
|
|
"seat driver height up 100",
|
|
"seat driver height down 2000",
|
|
"seat driver height stop",
|
|
"seat driver backrest forward 100",
|
|
"seat driver backrest backward 2000",
|
|
"seat driver backrest stop",
|
|
"seat driver basin up 100",
|
|
"seat driver basin down 2000",
|
|
"seat driver basin stop",
|
|
});
|
|
for (int slot = 1; slot <= 3; ++slot) {
|
|
commands.push_back("seat driver position " + std::to_string(slot));
|
|
commands.push_back("seat driver memory recall " +
|
|
std::to_string(slot));
|
|
}
|
|
for (const auto state : {"on", "off"}) {
|
|
commands.push_back("light indoor " + std::string(state));
|
|
commands.push_back("light reading all " + std::string(state));
|
|
commands.push_back("light ambient " + std::string(state));
|
|
}
|
|
commands.insert(commands.end(), {
|
|
"light ambient color 1", "light ambient color 127",
|
|
"light ambient brightness 1", "light ambient brightness 5",
|
|
});
|
|
for (const auto motion : {"open", "close", "half", "stop"})
|
|
commands.push_back("shade " + std::string(motion));
|
|
for (const int position : {0, 1, 50, 99, 100})
|
|
commands.push_back("shade position " + std::to_string(position));
|
|
return commands;
|
|
}
|
|
|
|
std::string make_batch_line(const std::vector<std::string>& commands,
|
|
std::size_t start, std::size_t count) {
|
|
std::ostringstream line;
|
|
line << "batch ";
|
|
const auto end = std::min(commands.size(), start + count);
|
|
for (std::size_t index = start; index < end; ++index) {
|
|
if (index != start)
|
|
line << " ; ";
|
|
line << commands[index];
|
|
}
|
|
return line.str();
|
|
}
|
|
|
|
bool self_test() {
|
|
bool passed = true;
|
|
const auto check = [&](bool condition, std::string_view label) {
|
|
std::cout << (condition ? "[PASS] " : "[FAIL] ") << label << '\n';
|
|
passed = passed && condition;
|
|
};
|
|
check(sizeof(ZCAN_INIT) == 20, "ZCAN_INIT ABI size is 20");
|
|
check(sizeof(ZCAN_20_MSG) == 24, "ZCAN_20_MSG ABI size is 24");
|
|
check(sizeof(ZCAN_FD_MSG) == 80, "ZCAN_FD_MSG ABI size is 80");
|
|
|
|
const auto absolute = make_can0_move_payload(0x01, 2, 0, 0x1234,
|
|
3500, 30'000);
|
|
check(absolute == std::array<U8, 8>{0x01, 0x12, 0x34, 0x12,
|
|
0xAC, 0x0D, 0x30, 0x75},
|
|
"CAN0 absolute-position payload uses firmware-native little endian fields");
|
|
const auto dynamic = make_can0_move_payload(0x0E, 0, 0, 1, 2, 0);
|
|
check(dynamic == std::array<U8, 8>{0x0E, 0x10, 0x01, 0x00,
|
|
0x02, 0x00, 0x00, 0x00},
|
|
"CAN0 cradle payload is Opcode 0x0E / ParamA 2");
|
|
const auto sequence_barrier = make_can0_move_payload(
|
|
0x09, 0, 0, 2, 0, 0);
|
|
check(sequence_barrier == std::array<U8, 8>{
|
|
0x09, 0x10, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00},
|
|
"CAN0 STOP_ALL sequence barrier matches the updated firmware protocol");
|
|
Can0AckState stale_ack;
|
|
stale_ack.ack = 2;
|
|
stale_ack.reject_reason = 8;
|
|
check(classify_can0_ack(stale_ack, 1) == Can0AckOutcome::SequenceOld,
|
|
"CAN0 RejectReason 8 selects bounded sequence resynchronization");
|
|
Can0AckState stop_ack;
|
|
stop_ack.ack = 4;
|
|
check(classify_can0_ack(stop_ack, 4) == Can0AckOutcome::Accepted,
|
|
"CAN0 STOP_ALL ACK is accepted as the sequence barrier");
|
|
Can0AckState accepted_with_aggregate_fault;
|
|
accepted_with_aggregate_fault.ack = 1;
|
|
accepted_with_aggregate_fault.fault_summary = true;
|
|
check(classify_can0_ack(accepted_with_aggregate_fault, 1) ==
|
|
Can0AckOutcome::Accepted,
|
|
"CAN0 aggregate fault summary does not turn an accepted command into a rejection");
|
|
check(passenger_axis("longitudinal") == passenger_axis("horizontal"),
|
|
"CAN0 longitudinal alias resolves to horizontal");
|
|
check(passenger_axis("height") == passenger_axis("basin"),
|
|
"CAN0 height alias resolves to basin");
|
|
check(passenger_axis("basin") &&
|
|
passenger_axis("basin")->direction_a_sign == 1,
|
|
"CAN0 basin up increases the current bench-normalized position");
|
|
check(can0_command_is_whitelisted(
|
|
split("seat passenger backrest relative backward 15")),
|
|
"CAN0 valid relative semantic command passes whitelist");
|
|
check(!can0_command_is_whitelisted(
|
|
split("seat passenger backrest relative sideways 15")),
|
|
"CAN0 invalid direction is rejected before hardware access");
|
|
check(!can0_command_is_whitelisted(
|
|
split("seat passenger backrest relative backward 101")),
|
|
"CAN0 out-of-range relative movement is rejected");
|
|
const auto* can0_position_frame = shared_protocol::find_frame(
|
|
"can0.seat.position", "move");
|
|
check(can0_position_frame && can0_position_frame->frame_id == 0x300 &&
|
|
can0_position_frame->format == "classic_can" &&
|
|
can0_position_frame->repeat_count == 1 &&
|
|
can0_position_frame->feedback_ids_csv == "0x301,0x302",
|
|
"CAN0 motion transport metadata comes from the generated protocol");
|
|
|
|
StatusStore decoded_public(false);
|
|
Can0StateStore decoded_private;
|
|
const std::array<U8, 8> sample_302{
|
|
0x02, 0x06, 0x08, 0x34, 0x12, 0xAC, 0x0D, 0x5A
|
|
};
|
|
decode_can0_frame(0x302, sample_302.data(), sample_302.size(),
|
|
decoded_public, decoded_private);
|
|
const auto decoded_entries = decoded_public.snapshot();
|
|
check(decoded_entries.count("CAN0/passenger/backrest") == 1,
|
|
"CAN0 0x302 decoder maps Motor 2 to passenger backrest state");
|
|
|
|
const auto windows = make_window_payload(kWindowAll, WindowMotion::Open);
|
|
check(windows[6] == 0x09 && windows[7] == 0x09,
|
|
"CAN1 all-window-open payload remains verified");
|
|
const auto* generated_shade_close = shared_protocol::find_static_payload(
|
|
"can1.shade.motion", "close");
|
|
const auto parsed_shade_close = generated_shade_close
|
|
? parse_shared_payload_hex(
|
|
generated_shade_close->payload_hex)
|
|
: std::nullopt;
|
|
check(parsed_shade_close &&
|
|
*parsed_shade_close ==
|
|
make_shade_motion_payload(ShadeMotion::Close),
|
|
"CAN1 generated shade template matches the verified C++ encoder");
|
|
const auto driver_left_close = make_can1_outlet_switch_payload(
|
|
"driver-left", "close");
|
|
const auto driver_right_open = make_can1_outlet_switch_payload(
|
|
"driver-right", "open");
|
|
const auto passenger_left_close = make_can1_outlet_switch_payload(
|
|
"passenger-left", "close");
|
|
const auto passenger_right_open = make_can1_outlet_switch_payload(
|
|
"passenger-right", "open");
|
|
check(driver_left_close && driver_right_open &&
|
|
passenger_left_close && passenger_right_open &&
|
|
*driver_left_close == std::array<U8, 8>{
|
|
0x02, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0xFF, 0xFF} &&
|
|
*driver_right_open == std::array<U8, 8>{
|
|
0x02, 0xFF, 0xFF, 0xFF, 0xFF, 0x04, 0xFF, 0xFF} &&
|
|
*passenger_left_close == std::array<U8, 8>{
|
|
0x03, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x00, 0x00} &&
|
|
*passenger_right_open == std::array<U8, 8>{
|
|
0x03, 0xFF, 0xFF, 0xFF, 0xFF, 0x04, 0x00, 0x00},
|
|
"CAN1 individual-outlet switch payloads preserve the observed head-unit bytes");
|
|
const auto* outlet_switch_frame = shared_protocol::find_frame(
|
|
"can1.outlet.switch");
|
|
check(outlet_switch_frame && outlet_switch_frame->frame_id == 0x4EF &&
|
|
outlet_switch_frame->format == "can_fd" &&
|
|
outlet_switch_frame->brs &&
|
|
outlet_switch_frame->repeat_count == 3 &&
|
|
outlet_switch_frame->period_ms == 100 &&
|
|
outlet_switch_frame->feedback_ids_csv == "0x4BB,0x37B",
|
|
"CAN1 outlet transport and feedback metadata come from the generated protocol");
|
|
std::array<U8, 64> outlet_4bb{};
|
|
outlet_4bb[41] = 0x26;
|
|
outlet_4bb[42] = 0x80;
|
|
outlet_4bb[43] = 0x80;
|
|
outlet_4bb[44] = 0x80;
|
|
outlet_4bb[45] = 0x2D;
|
|
decode_can1_outlet_switch_feedback(
|
|
0x4BB, outlet_4bb.data(), outlet_4bb.size(), decoded_public);
|
|
const auto outlet_feedback = decoded_public.entry_snapshot(
|
|
"CAN1/outlet-switch/driver-left");
|
|
check(outlet_feedback &&
|
|
outlet_feedback->summary.find("state=close") != std::string::npos,
|
|
"CAN1 0x4BB position signature maps the selected outlet to close");
|
|
check(can1_command_is_whitelisted(split("ac fan 2")),
|
|
"CAN1 valid semantic command passes whitelist");
|
|
check(can1_command_is_whitelisted(split("outlet driver-left close")) &&
|
|
!can1_command_is_whitelisted(split("outlet driver-left toggle")),
|
|
"CAN1 individual-outlet whitelist accepts only open or close");
|
|
check(!can1_command_is_whitelisted(split("door vehicle lock")),
|
|
"CAN1 unverified vehicle lock is rejected");
|
|
check(!can1_command_is_whitelisted(split("outlet mode driver person on")),
|
|
"legacy non-project outlet mode is rejected");
|
|
check(!can1_command_is_whitelisted(
|
|
split("seat driver height forward 500")) &&
|
|
!can1_command_is_whitelisted(
|
|
split("seat driver horizontal forward 99")) &&
|
|
!can1_command_is_whitelisted(
|
|
split("seat driver basin stop 500")),
|
|
"CAN1 driver-seat axis direction, duration, and stop syntax are prevalidated");
|
|
check(route_semantic_command(split("seat passenger dynamic cradle")) ==
|
|
SemanticChannel::Can0,
|
|
"console routes passenger-seat commands to CAN0");
|
|
check(route_semantic_command(split("seat driver heat low")) ==
|
|
SemanticChannel::Can1,
|
|
"console routes driver-seat commands to CAN1");
|
|
check(route_semantic_command(split("raw 0x300 01")) ==
|
|
SemanticChannel::None,
|
|
"console rejects arbitrary raw-frame syntax");
|
|
std::string batch_error;
|
|
const auto mixed_batch = validate_batch_command(
|
|
"batch seat passenger basin relative up 10; ac fan 2; light ambient on",
|
|
"all", batch_error);
|
|
check(mixed_batch && mixed_batch->size() == 3 &&
|
|
(*mixed_batch)[0].route == SemanticChannel::Can0 &&
|
|
(*mixed_batch)[1].route == SemanticChannel::Can1,
|
|
"console batch validates CAN0/CAN1 commands before execution");
|
|
check(batch_may_continue(CommandExecutionOutcome::VerifiedSuccess) &&
|
|
batch_may_continue(
|
|
CommandExecutionOutcome::SubmittedUnverified) &&
|
|
!batch_may_continue(
|
|
CommandExecutionOutcome::FailedOrBlocked),
|
|
"console batch continues after submitted-unverified outlet commands but stops on hard failure");
|
|
check(!validate_batch_command(
|
|
"batch ac fan 2; raw 0x300 01", "all", batch_error),
|
|
"console batch rejects the entire list when one command is outside the whitelist");
|
|
check(!validate_batch_command(
|
|
"batch seat passenger dynamic cradle; seat passenger basin relative up 10",
|
|
"all", batch_error),
|
|
"console batch rejects CAN0 dynamic-mode and axis-motion conflicts");
|
|
check(!validate_batch_command(
|
|
"batch ac fan 2; light ambient on", "can0", batch_error),
|
|
"console batch enforces the selected channel before execution");
|
|
const std::vector<std::string> mixed_can1_families{
|
|
"door rear lock",
|
|
"window all open",
|
|
"window front-right position 35",
|
|
"ac on",
|
|
"ac fan 2",
|
|
"ac mode face-feet",
|
|
"ac temp driver 24",
|
|
"ac control auto",
|
|
"ac cycle internal",
|
|
"outlet driver-right close",
|
|
"outlet driver-left percent 25 75",
|
|
"outlet passenger-right position 5 30",
|
|
"seat driver vent low",
|
|
"seat driver heat high",
|
|
"seat driver massage off",
|
|
"seat driver massage strength strong",
|
|
"seat driver massage mode relax",
|
|
"seat driver horizontal forward 500",
|
|
"seat driver height up 500",
|
|
"seat driver backrest backward 500",
|
|
"seat driver basin down 500",
|
|
"seat driver position 2",
|
|
"seat driver memory recall 3",
|
|
"light indoor on",
|
|
"light reading all off",
|
|
"light ambient on",
|
|
"light ambient color 70",
|
|
"light ambient brightness 3",
|
|
"shade open",
|
|
"shade position 30",
|
|
};
|
|
const auto mixed_can1_batch = validate_batch_command(
|
|
make_batch_line(mixed_can1_families, 0, mixed_can1_families.size()),
|
|
"can1", batch_error);
|
|
check(mixed_can1_batch &&
|
|
mixed_can1_batch->size() == mixed_can1_families.size() &&
|
|
std::all_of(mixed_can1_batch->begin(), mixed_can1_batch->end(),
|
|
[](const auto& command) {
|
|
return command.route == SemanticChannel::Can1;
|
|
}),
|
|
"console batch mixes every CAN1 semantic command family");
|
|
|
|
const auto all_can1_commands = can1_batch_regression_commands();
|
|
check(all_can1_commands.size() == 129 &&
|
|
std::all_of(all_can1_commands.begin(), all_can1_commands.end(),
|
|
[](const auto& command) {
|
|
const auto words = split(command);
|
|
return can1_command_is_whitelisted(words) &&
|
|
route_semantic_command(words) ==
|
|
SemanticChannel::Can1;
|
|
}),
|
|
"all 129 documented CAN1 canonical/boundary commands pass exact routing");
|
|
bool all_can1_batches_valid = true;
|
|
std::size_t can1_batch_count = 0;
|
|
for (std::size_t start = 0; start < all_can1_commands.size();
|
|
start += kMaxBatchCommands) {
|
|
const auto expected = std::min(kMaxBatchCommands,
|
|
all_can1_commands.size() - start);
|
|
const auto batch = validate_batch_command(
|
|
make_batch_line(all_can1_commands, start, expected),
|
|
"can1", batch_error);
|
|
all_can1_batches_valid = all_can1_batches_valid && batch &&
|
|
batch->size() == expected &&
|
|
std::all_of(batch->begin(), batch->end(),
|
|
[](const auto& command) {
|
|
return command.route ==
|
|
SemanticChannel::Can1;
|
|
});
|
|
++can1_batch_count;
|
|
}
|
|
check(all_can1_batches_valid && can1_batch_count == 5,
|
|
"all 129 CAN1 commands validate in five batches within the 32-command limit");
|
|
check(!validate_batch_command(
|
|
"batch ac fan 2; seat driver height forward 500; light ambient on",
|
|
"can1", batch_error) &&
|
|
!validate_batch_command(
|
|
"batch ac fan 2; seat driver horizontal forward 99; light ambient on",
|
|
"can1", batch_error) &&
|
|
!validate_batch_command(
|
|
"batch ac fan 2; seat driver basin stop 500; light ambient on",
|
|
"can1", batch_error),
|
|
"an invalid CAN1 seat command rejects its whole batch before transmission");
|
|
std::cout << (passed ? "SELF-TEST PASSED\n" : "SELF-TEST FAILED\n");
|
|
return passed;
|
|
}
|
|
|
|
void load_can1_resources(StatusStore& states) {
|
|
std::string error;
|
|
if (const auto memory = load_driver_memory_file(kDriverMemoryPath, error))
|
|
states.load_driver_memory_config(*memory);
|
|
else if (!error.empty())
|
|
print_line("[warning] driver-seat memory file was not loaded: " + error);
|
|
|
|
error.clear();
|
|
if (const auto calibration = load_air_outlet_calibration_file(
|
|
kAirOutletCalibrationPath, error)) {
|
|
if (!states.load_air_outlet_calibration(*calibration))
|
|
print_line("[warning] air-outlet calibration is incomplete");
|
|
} else if (!error.empty()) {
|
|
print_line("[warning] air-outlet calibration was not loaded: " + error);
|
|
}
|
|
}
|
|
|
|
void wait_for_monitor_end(int duration_ms) {
|
|
if (duration_ms > 0) {
|
|
const auto deadline = std::chrono::steady_clock::now() +
|
|
std::chrono::milliseconds(duration_ms);
|
|
while (!g_stop_requested && std::chrono::steady_clock::now() < deadline)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
return;
|
|
}
|
|
while (!g_stop_requested)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
|
|
void print_console_help(bool transmit_enabled,
|
|
const std::string& channel_selection) {
|
|
std::lock_guard<std::mutex> lock(g_output_mutex);
|
|
std::cout
|
|
<< "\nConsole commands:\n"
|
|
<< " status print the latest decoded CAN0/CAN1 state\n"
|
|
<< " whitelist print every accepted semantic control command\n"
|
|
<< " help print this console help\n"
|
|
<< " mark <text> add a timestamped diagnostic marker\n"
|
|
<< " quit | exit close both CAN channels and leave\n"
|
|
<< " <semantic command> auto-route passenger seat to CAN0 and other vehicle controls to CAN1\n\n"
|
|
<< " batch <command> ; <command> validate all commands, then execute sequentially\n\n"
|
|
<< "Examples:\n"
|
|
<< " seat passenger backrest position 35\n"
|
|
<< " seat passenger dynamic cradle\n"
|
|
<< " seat driver heat low\n"
|
|
<< " ac fan 2\n"
|
|
<< " shade open\n\n"
|
|
<< " batch seat passenger basin relative up 10 ; ac fan 2 ; light ambient on\n\n"
|
|
<< "Session: channel=" << channel_selection
|
|
<< " transmit=" << (transmit_enabled ? "ENABLED" : "READ-ONLY")
|
|
<< "\n\n";
|
|
}
|
|
|
|
void run_console(const CliOptions& options, Can0Channel& can0,
|
|
CanDevice& can1, StatusStore& public_states,
|
|
Can0StateStore& can0_states) {
|
|
const bool transmit_enabled = options.execute && !options.trace_all;
|
|
print_line(transmit_enabled
|
|
? "[console] transmit enabled for validated semantic commands; verify bench clearance before every command"
|
|
: "[console] read-only; semantic commands are validated but not transmitted (restart with --execute to enable)");
|
|
print_line("[console] periodic CAN state output is muted; enter status for one CAN0/CAN1 snapshot");
|
|
print_console_help(transmit_enabled, options.channel);
|
|
|
|
bool prompt_needed = true;
|
|
while (!g_stop_requested) {
|
|
if (prompt_needed) {
|
|
std::lock_guard<std::mutex> lock(g_output_mutex);
|
|
std::cout << "can[" << options.channel << "]> " << std::flush;
|
|
prompt_needed = false;
|
|
}
|
|
|
|
pollfd input{};
|
|
input.fd = STDIN_FILENO;
|
|
input.events = POLLIN;
|
|
const int poll_result = poll(&input, 1, 200);
|
|
if (poll_result < 0) {
|
|
if (errno == EINTR)
|
|
continue;
|
|
print_line("[console error] poll(stdin) failed");
|
|
break;
|
|
}
|
|
if (poll_result == 0)
|
|
continue;
|
|
if (input.revents & (POLLERR | POLLHUP | POLLNVAL))
|
|
break;
|
|
if (!(input.revents & POLLIN))
|
|
continue;
|
|
|
|
std::string line;
|
|
if (!std::getline(std::cin, line))
|
|
break;
|
|
prompt_needed = true;
|
|
auto words = split(line);
|
|
for (auto& word : words)
|
|
word = lower_ascii(std::move(word));
|
|
if (words.empty())
|
|
continue;
|
|
|
|
if (words.size() == 1 && (words[0] == "quit" || words[0] == "exit"))
|
|
break;
|
|
if (words.size() == 1 && words[0] == "status") {
|
|
print_status(public_states);
|
|
continue;
|
|
}
|
|
if (words.size() == 1 && words[0] == "help") {
|
|
print_console_help(transmit_enabled, options.channel);
|
|
continue;
|
|
}
|
|
if (words.size() == 1 && words[0] == "whitelist") {
|
|
print_whitelist();
|
|
continue;
|
|
}
|
|
if (words.size() >= 2 && words[0] == "mark") {
|
|
std::vector<std::string> label(words.begin() + 1, words.end());
|
|
print_line("[" + now_string() + "] [MARK] " + join_words(label));
|
|
continue;
|
|
}
|
|
|
|
if (is_batch_command_line(line)) {
|
|
std::string error;
|
|
const auto commands = validate_batch_command(
|
|
line, options.channel, error);
|
|
if (!commands) {
|
|
print_line("[batch blocked] " + error + "; no frame sent");
|
|
continue;
|
|
}
|
|
print_line("[batch validated] commands=" +
|
|
std::to_string(commands->size()) +
|
|
" execution=sequential stop-on-hard-failure");
|
|
for (std::size_t index = 0; index < commands->size(); ++index) {
|
|
const auto& command = (*commands)[index];
|
|
print_line("[batch plan " + std::to_string(index + 1) + "/" +
|
|
std::to_string(commands->size()) + "] " +
|
|
std::string(semantic_channel_name(command.route)) +
|
|
" <- " + command.command);
|
|
}
|
|
if (!transmit_enabled) {
|
|
print_line("[batch blocked] read-only console; validation passed but no frame was sent");
|
|
continue;
|
|
}
|
|
|
|
std::size_t submitted = 0;
|
|
std::size_t verified = 0;
|
|
std::size_t unverified = 0;
|
|
for (std::size_t index = 0; index < commands->size(); ++index) {
|
|
const auto& command = (*commands)[index];
|
|
print_line("[batch execute " + std::to_string(index + 1) + "/" +
|
|
std::to_string(commands->size()) + "] " +
|
|
std::string(semantic_channel_name(command.route)) +
|
|
" <- " + command.command);
|
|
const auto outcome = execute_routed_command(
|
|
command, can0, can1, can0_states, public_states);
|
|
if (!batch_may_continue(outcome)) {
|
|
print_line("[batch stopped] submitted=" +
|
|
std::to_string(submitted) + " failed=" +
|
|
std::to_string(index + 1) + " command=\"" +
|
|
command.command + "\"; remaining commands were not sent");
|
|
break;
|
|
}
|
|
++submitted;
|
|
if (outcome == CommandExecutionOutcome::VerifiedSuccess) {
|
|
++verified;
|
|
} else {
|
|
++unverified;
|
|
print_line(
|
|
"[batch continue] command=" +
|
|
std::to_string(index + 1) +
|
|
" was submitted but remains feedback-unverified; "
|
|
"continuing with the next independent command");
|
|
}
|
|
}
|
|
if (submitted == commands->size()) {
|
|
print_line("[batch complete] submitted=" +
|
|
std::to_string(submitted) + "/" +
|
|
std::to_string(commands->size()) +
|
|
" verified=" + std::to_string(verified) +
|
|
" unverified=" + std::to_string(unverified));
|
|
}
|
|
continue;
|
|
}
|
|
|
|
const auto route = route_semantic_command(words);
|
|
if (route == SemanticChannel::Ambiguous) {
|
|
print_line("[blocked] command matches more than one channel whitelist; no frame sent");
|
|
continue;
|
|
}
|
|
if (route == SemanticChannel::None) {
|
|
print_line("[blocked] command is outside both semantic whitelists; enter whitelist for supported syntax");
|
|
continue;
|
|
}
|
|
if (!selected_channel_allows(options.channel, route)) {
|
|
print_line("[blocked] command routes to " +
|
|
std::string(semantic_channel_name(route)) +
|
|
" but this console selected --channel " + options.channel);
|
|
continue;
|
|
}
|
|
print_line("[route] " + std::string(semantic_channel_name(route)) +
|
|
" <- " + join_words(words));
|
|
if (!transmit_enabled) {
|
|
print_line("[blocked] read-only console; no frame sent. Restart console with --execute after confirming bench safety");
|
|
continue;
|
|
}
|
|
|
|
const auto outcome =
|
|
route == SemanticChannel::Can0
|
|
? (execute_can0_command(join_words(words), can0, can0_states)
|
|
? CommandExecutionOutcome::VerifiedSuccess
|
|
: CommandExecutionOutcome::FailedOrBlocked)
|
|
: execute_can1_command(join_words(words), can1, public_states);
|
|
if (outcome == CommandExecutionOutcome::FailedOrBlocked) {
|
|
print_line("[command failed or blocked] no successful physical completion was established; console remains active");
|
|
} else if (outcome == CommandExecutionOutcome::SubmittedUnverified) {
|
|
print_line("[command submitted/unverified] transport submission completed; protocol feedback and physical effect remain unconfirmed");
|
|
}
|
|
}
|
|
print_line("[console] leaving interactive mode");
|
|
}
|
|
|
|
int run_with_hardware(const CliOptions& options) {
|
|
if (options.sdk_debug) {
|
|
const U32 result = VCI_Debug((1U << 29) | 1U);
|
|
print_line("[diagnostic] VCI_Debug=" + std::to_string(result));
|
|
}
|
|
print_line("====== opening one ZLG USBCANFD-200U for CAN0 + CAN1 ======");
|
|
CanDevice can1(1);
|
|
auto init = make_init_config();
|
|
if (!can1.init(init))
|
|
throw std::runtime_error("CAN1 VCI_InitCAN failed");
|
|
if (!can1.accept_all_standard_ids())
|
|
print_line("[warning] CAN1 standard-ID filter was not applied; using SDK default");
|
|
if (!can1.set_termination(options.can1_termination))
|
|
throw std::runtime_error("CAN1 termination configuration failed");
|
|
if (!can1.start())
|
|
throw std::runtime_error("CAN1 VCI_StartCAN failed");
|
|
VCI_ClearBuffer(kDevType, kDevIndex, 1);
|
|
|
|
Can0Channel can0(options.can0_termination);
|
|
print_line("[OK] CAN0 passenger seat: Classic CAN 500 kbit/s, standard 11-bit IDs");
|
|
print_line("[OK] CAN1 vehicle: 500 kbit/s arbitration / 2 Mbit/s data, Classic + CAN FD/BRS");
|
|
print_line("[termination] CAN0=" + std::string(options.can0_termination ? "on" : "off") +
|
|
" CAN1=" + std::string(options.can1_termination ? "on" : "off"));
|
|
|
|
// Console keeps receiving and decoding in the background, but state changes
|
|
// must not overwrite the interactive prompt. `status` prints one cached
|
|
// CAN0/CAN1 snapshot on demand. Monitor/status modes retain live change logs.
|
|
StatusStore public_states(options.action != Action::Console);
|
|
Can0StateStore can0_states;
|
|
load_can1_resources(public_states);
|
|
std::atomic<bool> running{true};
|
|
std::thread receiver(dual_receiver_loop, std::ref(can0), std::ref(can1),
|
|
std::ref(public_states), std::ref(can0_states),
|
|
std::cref(running), std::cref(options.channel),
|
|
options.trace_all);
|
|
bool succeeded = true;
|
|
try {
|
|
if (options.action == Action::Send) {
|
|
// Give the receive loop a short opportunity to capture cyclic preflight
|
|
// feedback. Individual commands still enforce their own fresh-state wait.
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
const auto outcome =
|
|
options.channel == "can0"
|
|
? (execute_can0_command(options.command, can0, can0_states)
|
|
? CommandExecutionOutcome::VerifiedSuccess
|
|
: CommandExecutionOutcome::FailedOrBlocked)
|
|
: execute_can1_command(options.command, can1, public_states);
|
|
succeeded = outcome == CommandExecutionOutcome::VerifiedSuccess;
|
|
if (outcome == CommandExecutionOutcome::SubmittedUnverified) {
|
|
print_line("[command submitted/unverified] standalone send keeps a non-zero exit status because protocol feedback was not confirmed");
|
|
} else if (outcome == CommandExecutionOutcome::FailedOrBlocked) {
|
|
print_line("[command failed or blocked] no successful physical completion was established");
|
|
}
|
|
} else if (options.action == Action::Console) {
|
|
run_console(options, can0, can1, public_states, can0_states);
|
|
} else {
|
|
if (options.trace_all)
|
|
print_line("[mode] trace-all is active; all transmit paths are disabled");
|
|
wait_for_monitor_end(options.duration_ms);
|
|
print_status(public_states);
|
|
}
|
|
} catch (...) {
|
|
running.store(false, std::memory_order_relaxed);
|
|
receiver.join();
|
|
throw;
|
|
}
|
|
running.store(false, std::memory_order_relaxed);
|
|
receiver.join();
|
|
print_line("====== CAN0 + CAN1 closed ======");
|
|
return succeeded ? 0 : 3;
|
|
}
|
|
|
|
} // namespace cognitive_agent_can_cli
|
|
|
|
int main(int argc, char** argv) {
|
|
using namespace cognitive_agent_can_cli;
|
|
const auto options = parse_cli(argc, argv);
|
|
if (!options)
|
|
return 2;
|
|
if (options->show_help) {
|
|
print_usage(argv[0]);
|
|
print_whitelist();
|
|
return 0;
|
|
}
|
|
if (options->action == Action::Whitelist) {
|
|
print_whitelist();
|
|
return 0;
|
|
}
|
|
if (options->action == Action::SelfTest)
|
|
return self_test() ? 0 : 1;
|
|
|
|
if (options->action == Action::Send) {
|
|
auto words = split(options->command);
|
|
for (auto& word : words)
|
|
word = lower_ascii(std::move(word));
|
|
const bool allowed = options->channel == "can0"
|
|
? can0_command_is_whitelisted(words)
|
|
: can1_command_is_whitelisted(words);
|
|
if (!allowed) {
|
|
print_line("[blocked] semantic command is outside the selected channel whitelist; USB was not opened");
|
|
return 3;
|
|
}
|
|
}
|
|
|
|
std::signal(SIGINT, signal_handler);
|
|
std::signal(SIGTERM, signal_handler);
|
|
try {
|
|
return run_with_hardware(*options);
|
|
} catch (const std::exception& error) {
|
|
print_line(std::string("[error] ") + error.what());
|
|
print_line("Check USB permissions, exclusive device ownership, wiring, timing, and termination topology.");
|
|
return 1;
|
|
}
|
|
}
|