diff --git a/.gitignore b/.gitignore index 4f208a0..d57c068 100644 --- a/.gitignore +++ b/.gitignore @@ -252,6 +252,7 @@ bin/pPowerMangerHost bin/pCCU bin/pccuTest bin/pCanBridge +bin/pMotor # CI test reports reports/ @@ -260,6 +261,7 @@ reports/ bin/PowerMangerTests bin/udpFeeder bin/fullFeeder +bin/pmotorTest # ============================================================ # 数据库与日志文件（全局忽略，一律不入库） diff --git a/docs/推进电机通信协议20260321.docx b/docs/推进电机通信协议20260321.docx new file mode 100644 index 0000000..565f1e0 Binary files /dev/null and b/docs/推进电机通信协议20260321.docx differ diff --git a/missions/h100.moos b/missions/h100.moos index cec1897..f7bad4b 100644 --- a/missions/h100.moos +++ b/missions/h100.moos @@ -1,6 +1,6 @@ // MOOS file // 板卡 (RK3588 / Ubuntu 20.04) 专用 mission 配置 -// 五个 systemd 服务（moosdb / pPowerManger / pPowerMangerHost / pCCU / pCanBridge） +// 六个 systemd 服务（moosdb / pPowerManger / pPowerMangerHost / pCCU / pCanBridge / pMotor） // 共用本文件。 // 由 scripts/deploy.sh 推送到板卡 /root/work/h100/missions/h100.moos @@ -87,11 +87,34 @@ ProcessConfig = pCanBridge // CAN<->MOOSDB 多通道透传：CANET（TCP Server 模式） // 每通道一条独立 TCP 连接；工作端口：CAN0=4001，CAN1=4002，...，CAN7=4008 channel = CAN0,192.168.0.222,4001 + channel = CAN1,192.168.0.222,4002 // channel = CAN1,192.168.0.222,4002 - // CAN_TX 下行默认通道（pCCU 未指定 m_sSrcAux 时使用；可省略） - default_channel = CAN0 + // CAN_TX 下行默认通道（pCCU 未指定 m_sSrcAux 时使用；可省略） + default_channel = CAN0 - // CAN 帧 SQLite 落库路径（can_frame 表，channel 列区分通道） - dbpath = /root/work/h100/data/pCanBridge_data.db + // CAN 帧 SQLite 落库路径（can_frame 表，channel 列区分通道） + dbpath = /root/work/h100/data/pCanBridge_data.db +} + +ProcessConfig = pMotor +{ + AppTick = 4 + CommsTick = 4 + + // 推进电机控制器：CAN1 通道（经 pCanBridge 透传，docs/推进电机通信协议20260321.docx） + // 下行 500ms 周期指令帧 0x18EF2010（网页 /api/motor_cmd 控制启停/复位/转速）， + // 上行订阅 CAN_0x* 解码故障/转速/母线电压/温度，网页 18081 展示 + can_channel = CAN1 + + // 冗余 CAN 通道（配置后指令帧同内容发 0x18EF2011；留空=不发送） + // red_channel = CAN2 + + cmd_period_ms = 500 + reset_hold_sec = 5 + + web_port = 18081 + web_enable = true + + logpath = /root/work/h100/data/pMotor.log } diff --git a/scripts/build-board.sh b/scripts/build-board.sh index 0268902..ea9483d 100755 --- a/scripts/build-board.sh +++ b/scripts/build-board.sh @@ -7,7 +7,7 @@ # # 用法: # ./scripts/build-board.sh # 同步+编译+部署（不重启） -# ./scripts/build-board.sh --start # 编译后重启全部服务（含 pCanBridge） +# ./scripts/build-board.sh --start # 编译后重启全部服务（含 pCanBridge/pMotor） # ./scripts/build-board.sh --clean # 板卡上重新 cmake 再编译 # ./scripts/build-board.sh --jobs 4 # 指定并行度（默认板卡 nproc） # @@ -68,7 +68,7 @@ DO_CLEAN=0 CCU_HOST="" CCU_PORT="" -SERVICES=(moosdb pPowerManger pPowerMangerHost pCCU pCanBridge) +SERVICES=(moosdb pPowerManger pPowerMangerHost pCCU pCanBridge pMotor) info() { printf '\033[1;36m[build-board]\033[0m %s\n' "$*"; } ok() { printf '\033[1;32m[build-board]\033[0m %s\n' "$*"; } @@ -211,12 +211,13 @@ ${SSH} "${SSH_TARGET}" "mkdir -p ${BOARD_BIN} && \ cp -f ${BOARD_SRC}/bin/pPowerMangerHost ${BOARD_BIN}/pPowerMangerHost && \ cp -f ${BOARD_SRC}/bin/pCCU ${BOARD_BIN}/pCCU && \ cp -f ${BOARD_SRC}/bin/pCanBridge ${BOARD_BIN}/pCanBridge && \ - chmod +x ${BOARD_BIN}/pPowerManger ${BOARD_BIN}/pPowerMangerHost ${BOARD_BIN}/pCCU ${BOARD_BIN}/pCanBridge" \ + cp -f ${BOARD_SRC}/bin/pMotor ${BOARD_BIN}/pMotor && \ + chmod +x ${BOARD_BIN}/pPowerManger ${BOARD_BIN}/pPowerMangerHost ${BOARD_BIN}/pCCU ${BOARD_BIN}/pCanBridge ${BOARD_BIN}/pMotor" \ || { err "部署产物失败"; exit 1; } ok "产物已部署" #------------------------------------------------------------------- -# 4.5 安装/刷新 pCanBridge systemd unit（幂等，其它 4 个 unit 已装） +# 4.5 安装/刷新 pCanBridge systemd unit（幂等，其它 5 个 unit 已装） #------------------------------------------------------------------- info "安装 pCanBridge.service ..." ${SSH} "${SSH_TARGET}" "cat > /etc/systemd/system/pCanBridge.service" </dev/null || true" ok "pCanBridge.service 已安装" +#------------------------------------------------------------------- +# 4.6 安装/刷新 pMotor systemd unit（幂等） +#------------------------------------------------------------------- +info "安装 pMotor.service ..." +${SSH} "${SSH_TARGET}" "cat > /etc/systemd/system/pMotor.service" </dev/null || true" +ok "pMotor.service 已安装" + echo "" ok "产物: ${BOARD_BIN}/pPowerManger (aarch64)" ${SSH} "${SSH_TARGET}" "file ${BOARD_BIN}/pPowerManger | cut -c1-60" diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ebb2c8e..726de24 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -22,6 +22,7 @@ add_subdirectory(pPMtest) add_subdirectory(pPowerMangerHost) add_subdirectory(pCCU) add_subdirectory(pCanBridge) +add_subdirectory(pMotor) ############################################################################## # END of CMakeLists.txt ############################################################################## diff --git a/src/pMotor/CMakeLists.txt b/src/pMotor/CMakeLists.txt new file mode 100644 index 0000000..b74ac62 --- /dev/null +++ b/src/pMotor/CMakeLists.txt @@ -0,0 +1,46 @@ +#-------------------------------------------------------- +# The CMakeLists.txt for: pMotor +# 推进电机操作程序：网页控制 <-> pCanBridge(CAN1) <-> 电机控制器 +#-------------------------------------------------------- + +if (${WIN32}) + SET(SYSTEM_LIBS wsock32) +else (${WIN32}) + SET(SYSTEM_LIBS m pthread) +endif (${WIN32}) + +# 复用仓库内 pPowerManger 的公共源码（相对路径引用，避免重复维护） +SET(PM_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../pPowerManger) + +SET(SHARED_SRC + ${PM_DIR}/logc/loguru.cpp + ${PM_DIR}/httpserver/mongoose.c +) + +SET(SRC + ${SHARED_SRC} + protocol/MotorCan.cpp + web/WebServer.cpp + Motor.cpp + Motor_Info.cpp + main.cpp +) + +ADD_EXECUTABLE(pMotor ${SRC}) + +TARGET_INCLUDE_DIRECTORIES(pMotor PRIVATE + ${PM_DIR} + ${PM_DIR}/httpserver + ${PM_DIR}/logc +) + +TARGET_LINK_LIBRARIES(pMotor + ${MOOS_LIBRARIES} + apputil + mbutil + m + pthread + jsoncpp + dl + ${SYSTEM_LIBS} +) diff --git a/src/pMotor/Motor.cpp b/src/pMotor/Motor.cpp new file mode 100644 index 0000000..5ce6f1f --- /dev/null +++ b/src/pMotor/Motor.cpp @@ -0,0 +1,467 @@ +#include "Motor.h" +#include "MBUtils.h" +#include "../pPowerManger/logc/loguru.hpp" +#include "json/json.h" +#include +#include + +using namespace motor; +using namespace std; + +//--------------------------------------------------------- +// Constructor / Destructor + +Motor::Motor() {} + +Motor::~Motor() { + if (m_web) { m_web->stop(); delete m_web; m_web = nullptr; } +} + +//--------------------------------------------------------- +// OnStartUp：读取配置并初始化各组件 + +bool Motor::OnStartUp() { + AppCastingMOOSApp::OnStartUp(); + + STRING_LIST sParams; + m_MissionReader.EnableVerbatimQuoting(false); + if (!m_MissionReader.GetConfiguration(GetAppName(), sParams)) + reportConfigWarning("No config block found for " + GetAppName()); + + STRING_LIST::iterator p; + for (p = sParams.begin(); p != sParams.end(); p++) { + string orig = *p; + string line = *p; + string param = stripBlankEnds(tolower(biteStringX(line, '='))); + string value = stripBlankEnds(line); + + bool handled = true; + if (param == "can_channel") m_canChannel = value; + else if (param == "red_channel") m_redChannel = value; + else if (param == "cmd_period_ms") m_cmdPeriodMs = atoi(value.c_str()); + else if (param == "reset_hold_sec") m_resetHoldSec = atof(value.c_str()); + else if (param == "web_port") m_webPort = atoi(value.c_str()); + else if (param == "web_enable") m_webEnable = (tolower(value) == "true" || value == "1"); + else if (param == "logpath") m_logPath = value; + else handled = false; + + if (!handled) + reportUnhandledConfigWarning(orig); + } + if (m_cmdPeriodMs < 50) m_cmdPeriodMs = 50; + + registerVariables(); + + // 日志文件 + if (m_logPath.empty()) m_logPath = "pMotor.log"; + loguru::add_file(m_logPath.c_str(), loguru::Append, loguru::Verbosity_MAX); + LOG_F(INFO, "pMotor log path: %s", m_logPath.c_str()); + + // 网页 + if (m_webEnable) { + m_web = new WebServer(); + m_web->setOnOpen([this]() { return buildSnapshot(); }); + m_web->setApiHandler([this](const std::string& uri, const std::string& query) { + return handleApi(uri, query); + }); + if (!m_web->start(m_webPort)) { + LOG_F(ERROR, "web server start failed on port %d", m_webPort); + delete m_web; + m_web = nullptr; + } + } + + LOG_F(INFO, "pMotor started: motor CAN via pCanBridge channel=%s (red_channel=%s), " + "cmd period=%dms, web=%s", + m_canChannel.c_str(), m_redChannel.c_str(), m_cmdPeriodMs, + m_web ? ("port " + to_string(m_webPort)).c_str() : "disabled"); + return true; +} + +//--------------------------------------------------------- +// OnConnectToServer + +bool Motor::OnConnectToServer() { + registerVariables(); + return true; +} + +//--------------------------------------------------------- +// registerVariables：订阅 pCanBridge 透传的全部 CAN 帧 + +void Motor::registerVariables() { + AppCastingMOOSApp::RegisterVariables(); + // 通配订阅必须用三参数重载（变量模式 + 来源模式） + Register("CAN_0x*", "*", 0); +} + +//--------------------------------------------------------- +// OnNewMail + +bool Motor::OnNewMail(MOOSMSG_LIST &NewMail) { + AppCastingMOOSApp::OnNewMail(NewMail); + MOOSMSG_LIST::iterator p; + for (p = NewMail.begin(); p != NewMail.end(); p++) { + CMOOSMsg &msg = *p; + if (msg.m_sKey.rfind("CAN_0x", 0) == 0) + handleCanMessage(msg); + } + return true; +} + +//--------------------------------------------------------- +// handleCanMessage：解码 pCanBridge 透传的 CAN 帧 +// +// 消息格式（pCanBridge 约定）： +// m_sKey = "CAN_0x%08X"（CAN ID，扩展帧） +// m_sVal = 二进制数据域（8 字节） +// m_dfVal2 = 原始帧信息字节（bit7 FF / bit6 RTR / bit3~0 DLC） + +void Motor::handleCanMessage(CMOOSMsg& msg) { + // 从变量名解析 CAN ID（跳过前缀 "CAN_0x" 共 6 字符） + uint32_t canId = 0; + if (std::sscanf(msg.m_sKey.c_str() + 6, "%x", &canId) != 1) return; + if (!isCanMotorId(canId)) return; + + if (!(msg.IsBinary() && msg.GetBinaryDataSize() >= 8)) return; + const unsigned char* d = msg.GetBinaryData(); + if (!d) return; + + // 日志值在锁内取副本，避免与 Web 线程快照读取竞争 + uint32_t rx = 0; + int spd = 0; + int bus = 0; + size_t f1 = 0, f2 = 0; + { + std::lock_guard lock(m_mutex); + if (!decodeMotorFrame(canId, d, static_cast(msg.GetBinaryDataSize()), m_state)) + return; + m_state.rxCount++; + double now = MOOSTime(false); + switch (canId) { + case MOTOR_CAN_FAULT1: case MOTOR_CAN_FAULT1_RED: + m_state.branch1.lastRxTime = now; break; + case MOTOR_CAN_FAULT2: case MOTOR_CAN_FAULT2_RED: + m_state.branch2.lastRxTime = now; break; + case MOTOR_CAN_STATE3: case MOTOR_CAN_STATE3_RED: + m_state.state3LastRx = now; break; + case MOTOR_CAN_STATE4: case MOTOR_CAN_STATE4_RED: + m_state.state4LastRx = now; break; + default: break; + } + rx = m_state.rxCount; + spd = m_state.feedbackSpeed; + bus = m_state.busVoltage; + f1 = m_state.branch1.faults.size(); + f2 = m_state.branch2.faults.size(); + } + + // 节流日志：1s 一条 + double now = MOOSTime(); + if (now - m_lastRxLog >= 1.0) { + m_lastRxLog = now; + LOG_F(INFO, "[Motor/CAN] rx=%u speed=%d rpm bus=%dV faults(b1=%zu,b2=%zu)", + rx, spd, bus, f1, f2); + } +} + +//--------------------------------------------------------- +// Iterate：周期任务（AppTick 次/秒） + +bool Motor::Iterate() { + AppCastingMOOSApp::Iterate(); + + double now = MOOSTime(); + + // 网页指令出队并应用 + processCmdQueue(); + + // 周期发送指令帧（500ms，协议要求） + sendCmdFrame(now); + + // 网页快照 1Hz 推送 + if (m_web) { + static double lastWebPush = 0; + if (now - lastWebPush >= 1.0) { + lastWebPush = now; + m_web->broadcast(buildSnapshot()); + } + } + + AppCastingMOOSApp::PostReport(); + return true; +} + +//--------------------------------------------------------- +// handleApi：/api/motor_cmd + +std::string Motor::handleApi(const std::string& uri, const std::string& query) { + if (uri == "/api/motor_cmd") + return handleMotorCmdApi(query); + return ""; +} + +//--------------------------------------------------------- +// handleMotorCmdApi：电机控制指令入队（按协议指令帧字段独立下发，无联动） +// +// GET /api/motor_cmd?action=enable 使能命令=1（Byte0 bit0） +// GET /api/motor_cmd?action=disable 使能命令=0，期望转速清0（Byte0 bit0，协议"停机"示例） +// GET /api/motor_cmd?action=reset 复位命令=1（Byte0 bit1，仅故障态有效） +// GET /api/motor_cmd?action=speed&speed=1000 设定期望转速（Byte2/3，s16） +// Web 线程不可直接 Notify，指令入队后由 Iterate（MOOS 线程）应用。 + +std::string Motor::handleMotorCmdApi(const std::string& query) { + // 简易 query 解析（k=v&k=v） + auto getParam = [&query](const char* key, std::string& out) -> bool { + std::string k = std::string(key) + "="; + size_t p = query.find(k); + if (p == std::string::npos) return false; + size_t e = query.find('&', p); + out = query.substr(p + k.size(), + (e == std::string::npos) ? std::string::npos + : e - p - k.size()); + return true; + }; + + std::string action, speedStr; + getParam("action", action); + getParam("speed", speedStr); + + MotorCmd c; + if (action == "enable") { + c.action = MOTOR_ACT_ENABLE; + } else if (action == "disable") { + c.action = MOTOR_ACT_DISABLE; + } else if (action == "reset") { + c.action = MOTOR_ACT_RESET; + } else if (action == "speed") { + c.action = MOTOR_ACT_SPEED; + c.speed = static_cast(atoi(speedStr.c_str())); + } else { + return "{\"ok\":false,\"error\":\"bad action (enable/disable/reset/speed)\"}"; + } + + { + std::lock_guard lock(m_mutex); + if (m_cmdQueue.size() >= 16) + return "{\"ok\":false,\"error\":\"queue full\"}"; + m_cmdQueue.push_back(c); + m_lastCmd = c; + m_lastCmdValid = true; + m_lastCmdTime = MOOSTime(false); + } + + LOG_F(INFO, "[Motor/CAN] 指令入队: action=%s speed=%d", action.c_str(), c.speed); + char buf[96]; + std::snprintf(buf, sizeof(buf), + "{\"ok\":true,\"action\":\"%s\",\"speed\":%d}", action.c_str(), c.speed); + return buf; +} + +//--------------------------------------------------------- +// processCmdQueue：出队网页指令并更新指令状态 + +void Motor::processCmdQueue() { + MotorCmd c; + double now = MOOSTime(); + { + std::lock_guard lock(m_mutex); + while (!m_cmdQueue.empty()) { + c = m_cmdQueue.front(); + m_cmdQueue.erase(m_cmdQueue.begin()); + + switch (c.action) { + case MOTOR_ACT_ENABLE: + m_enable = true; + break; + case MOTOR_ACT_DISABLE: + m_enable = false; + m_speed = 0; + m_resetUntil = 0; + break; + case MOTOR_ACT_SPEED: + m_speed = c.speed; + break; + case MOTOR_ACT_RESET: + m_resetUntil = now + m_resetHoldSec; + break; + default: + break; + } + LOG_F(INFO, "[Motor/CAN] 指令应用: action=%d speed=%d enable=%d", + c.action, m_speed, m_enable ? 1 : 0); + } + } +} + +//--------------------------------------------------------- +// sendCmdFrame：按周期编码并发送指令帧 +// 主通道 0x18EF2010；配置 red_channel 时同帧发 0x18EF2011 到冗余通道 + +void Motor::sendCmdFrame(double now) { + double lastTx = 0; + { + std::lock_guard lock(m_mutex); + if (now - m_lastTx < m_cmdPeriodMs / 1000.0) + return; + lastTx = m_lastTx; + } + if (now - lastTx < m_cmdPeriodMs / 1000.0) + return; + + uint8_t data[8]; + { + std::lock_guard lock(m_mutex); + bool reset = (now < m_resetUntil); + buildMotorCmdData(m_enable, reset, m_enable ? m_speed : 0, data); + m_lastTx = now; + } + + bool ok = postCanFrame(MOTOR_CAN_CMD, data, m_canChannel); + + int okCount = ok ? 1 : 0, errCount = ok ? 0 : 1; + if (!m_redChannel.empty()) { + bool okRed = postCanFrame(MOTOR_CAN_CMD_RED, data, m_redChannel); + okCount += okRed ? 1 : 0; + errCount += okRed ? 0 : 1; + } + { + std::lock_guard lock(m_mutex); + m_txCount += static_cast(okCount); + m_txErr += static_cast(errCount); + } +} + +//--------------------------------------------------------- +// postCanFrame：MOOS CAN_TX_0x%08X（二进制数据域，m_sSrcAux=目标通道） +// 链路：MOOS -> pCanBridge 订阅 -> 按通道路由 CanEndpoint(TCP) +// -> CANET -> CAN 总线 + +bool Motor::postCanFrame(uint32_t canId, const uint8_t data[8], const std::string& channel) { + char key[32]; + std::snprintf(key, sizeof(key), "CAN_TX_0x%08X", canId); + CMOOSMsg msg(MOOS_NOTIFY, key, 8u, data); + msg.SetSourceAux(channel); + bool ok = m_Comms.Post(msg); + if (!ok) + LOG_F(ERROR, "[Motor/CAN] 指令帧 0x%08X 投递 MOOS 失败 (通道=%s)", canId, channel.c_str()); + return ok; +} + +//--------------------------------------------------------- +// buildSnapshot：网页 JSON 快照（jsoncpp） + +namespace { + +void putI(Json::Value& j, const char* k, int v) { j[k] = Json::Value(v); } +void putU(Json::Value& j, const char* k, unsigned long v) { j[k] = Json::Value(static_cast(v)); } +void putD(Json::Value& j, const char* k, double v) { j[k] = Json::Value(v); } + +// age：距最后收到该帧的秒数；valid=false 时前端显示"从未收到" +void putAge(Json::Value& j, double lastRx, double now, bool valid) { + j["valid"] = valid ? 1 : 0; + j["age"] = valid ? (now - lastRx) : -1; +} + +Json::Value branchToJson(const MotorBranchState& br, double now) { + Json::Value j(Json::objectValue); + putAge(j, br.lastRxTime, now, br.valid); + Json::Value words(Json::arrayValue); + for (int i = 0; i < 4; ++i) words.append(static_cast(br.word[i])); + j["words"] = words; + Json::Value faults(Json::arrayValue); + for (size_t i = 0; i < br.faults.size(); ++i) faults.append(br.faults[i]); + j["faults"] = faults; + Json::Value alarms(Json::arrayValue); + for (size_t i = 0; i < br.alarms.size(); ++i) alarms.append(br.alarms[i]); + j["alarms"] = alarms; + return j; +} + +} // namespace + +std::string Motor::buildSnapshot() { + Json::Value root(Json::objectValue); + double now = MOOSTime(false); + + { + std::lock_guard lock(m_mutex); + + // 通信链路 + Json::Value link(Json::objectValue); + link["channel"] = m_canChannel; + link["redChannel"] = m_redChannel; + link["periodMs"] = m_cmdPeriodMs; + putU(link, "txCount", m_txCount); + putU(link, "txErr", m_txErr); + putU(link, "rxCount", m_state.rxCount); + link["lastTxValid"] = (m_lastTx > 0) ? 1 : 0; + putD(link, "lastTxAge", m_lastTx > 0 ? (now - m_lastTx) : -1); + root["link"] = link; + + // 指令状态 + Json::Value cmd(Json::objectValue); + putI(cmd, "enable", m_enable ? 1 : 0); + putI(cmd, "speed", m_speed); + putI(cmd, "resetActive", (now < m_resetUntil) ? 1 : 0); + cmd["lastCmdValid"] = m_lastCmdValid ? 1 : 0; + if (m_lastCmdValid) { + static const char* kActionName[] = {"", "enable", "disable", "speed", "reset"}; + int idx = (m_lastCmd.action >= 1 && m_lastCmd.action <= 4) ? m_lastCmd.action : 0; + cmd["lastCmdAction"] = kActionName[idx]; + putI(cmd, "lastCmdSpeed", m_lastCmd.speed); + putD(cmd, "lastCmdAge", m_lastCmdTime > 0 ? (now - m_lastCmdTime) : -1); + } + root["cmd"] = cmd; + + // 状态帧3：转速 / 母线电压 + Json::Value s3(Json::objectValue); + putAge(s3, m_state.state3LastRx, now, m_state.state3Valid); + putI(s3, "speed", m_state.feedbackSpeed); + putI(s3, "voltage", m_state.busVoltage); + root["state3"] = s3; + + // 状态帧4：温度 + Json::Value s4(Json::objectValue); + putAge(s4, m_state.state4LastRx, now, m_state.state4Valid); + putI(s4, "tA", m_state.tempA); + putI(s4, "tB", m_state.tempB); + putI(s4, "tC", m_state.tempC); + putI(s4, "tInv", m_state.tempInv); + putI(s4, "tFilter", m_state.tempFilter); + putI(s4, "tAir", m_state.tempAir); + root["state4"] = s4; + + // 故障/报警 + root["fault1"] = branchToJson(m_state.branch1, now); + root["fault2"] = branchToJson(m_state.branch2, now); + } + + Json::FastWriter writer; + return writer.write(root); +} + +//--------------------------------------------------------- +// buildReport + +bool Motor::buildReport() { + m_msgs << "============================================" << "\n"; + m_msgs << "pMotor 推进电机操作程序" << "\n"; + m_msgs << "============================================" << "\n"; + m_msgs << "CAN 通道: " << m_canChannel + << (m_redChannel.empty() ? "" : (" 冗余: " + m_redChannel)).c_str() + << " 指令周期: " << m_cmdPeriodMs << "ms" << "\n"; + m_msgs << "指令帧下发: " << m_txCount << " 成功 / " << m_txErr << " 失败" << "\n"; + { + std::lock_guard lock(m_mutex); + m_msgs << "电机报文接收: " << m_state.rxCount << "\n"; + m_msgs << "指令状态: enable=" << (m_enable ? 1 : 0) + << " speed=" << m_speed + << " reset=" << (MOOSTime(false) < m_resetUntil ? 1 : 0) << "\n"; + m_msgs << "反馈转速: " << m_state.feedbackSpeed + << " rpm 母线电压: " << m_state.busVoltage << "\n"; + m_msgs << "支路一故障: " << m_state.branch1.faults.size() + << " 条, 支路二故障: " << m_state.branch2.faults.size() << " 条" << "\n"; + } + return true; +} diff --git a/src/pMotor/Motor.h b/src/pMotor/Motor.h new file mode 100644 index 0000000..13219f4 --- /dev/null +++ b/src/pMotor/Motor.h @@ -0,0 +1,96 @@ +#ifndef PMOTOR_MOTOR_H +#define PMOTOR_MOTOR_H + +#define UNIX +#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +#include "protocol/MotorCan.h" +#include "web/WebServer.h" +#include + +namespace motor { + +//============================================================================ +// Motor：pMotor 程序主类（MOOS 应用外壳）——推进电机操作程序。 +// +// 数据流： +// 上行：电机控制器 -> CAN1 -> CANET -> pCanBridge -> MOOS CAN_0x* +// -> pMotor 订阅解码（故障/转速/母线电压/温度）-> SystemData 合成快照 +// 下行：网页 /api/motor_cmd（启停/复位/设定转速） +// -> 指令入队 -> Iterate（MOOS 线程）按 500ms 周期编码指令帧 +// -> MOOS CAN_TX_0x18EF2010（m_sSrcAux=CAN1）-> pCanBridge +// -> CANET(CAN1) -> 电机控制器 +// 网页：WebSocket 周期推送 JSON 快照（1Hz）。 +// +// 协议定义见 docs/推进电机通信协议20260321.docx。 +//============================================================================ + +class Motor : public AppCastingMOOSApp { +public: + Motor(); + ~Motor(); + +protected: + bool OnNewMail(MOOSMSG_LIST &NewMail); + bool Iterate(); + bool OnConnectToServer(); + bool OnStartUp(); + bool buildReport(); + void registerVariables(); + +private: + // MOOS 订阅消息处理（CAN_0x* 由 pCanBridge 发布） + void handleCanMessage(CMOOSMsg& msg); + + // 网页 + std::string buildSnapshot(); + std::string handleApi(const std::string& uri, const std::string& query); + + // 控制指令：/api/motor_cmd 解析入队（Web 线程） + std::string handleMotorCmdApi(const std::string& query); + // 出队并应用控制指令（Iterate 调用） + void processCmdQueue(); + // 周期（默认 500ms）发送指令帧 0x18EF2010（+可选冗余 0x18EF2011） + void sendCmdFrame(double now); + // 经 MOOS CAN_TX_0x%08X 发布一帧二进制数据（m_sSrcAux=通道名） + bool postCanFrame(uint32_t canId, const uint8_t data[8], const std::string& channel); + + // 配置 + std::string m_canChannel = "CAN1"; // 指令帧下行 / 帧上报来源通道 + std::string m_redChannel; // 冗余 CAN 通道（空=不发送冗余帧） + int m_cmdPeriodMs = 500; // 指令帧周期 + double m_resetHoldSec = 5.0; // 复位位保持时长（秒） + int m_webPort = 18081; + bool m_webEnable = true; + std::string m_logPath = "pMotor.log"; + + // 指令状态（processCmdQueue 修改，sendCmdFrame 读取；m_mutex 保护） + mutable std::mutex m_mutex; + bool m_enable = false; + int16_t m_speed = 0; + double m_resetUntil = 0; // 复位位有效的截止时间（MOOSTime） + + // 指令队列（Web 线程入队，Iterate 出队） + std::vector m_cmdQueue; + // 最近一次网页指令（供网页展示） + MotorCmd m_lastCmd; + bool m_lastCmdValid = false; + double m_lastCmdTime = 0; + + // 下行统计 + unsigned long m_txCount = 0; + unsigned long m_txErr = 0; + double m_lastTx = 0; + + // CAN 帧接收日志节流 + double m_lastRxLog = 0; + + // 电机状态（OnNewMail/Iterate 均在 MOOS 线程访问；buildSnapshot 经 m_mutex 读取） + MotorState m_state; + + // 组件 + WebServer* m_web = nullptr; +}; + +} // namespace motor + +#endif // PMOTOR_MOTOR_H diff --git a/src/pMotor/Motor_Info.cpp b/src/pMotor/Motor_Info.cpp new file mode 100644 index 0000000..702b35b --- /dev/null +++ b/src/pMotor/Motor_Info.cpp @@ -0,0 +1,103 @@ +/****************************************************************/ +/* NAME: Motor_Info */ +/* FILE: Motor_Info.cpp */ +/****************************************************************/ + +#include +#include +#include "Motor_Info.h" +#include "ColorParse.h" +#include "ReleaseInfo.h" + +using namespace std; + +void showSynopsis() { + blk("SYNOPSIS: "); + blk("------------------------------------ "); + blk(" The pMotor application is the propulsion motor operation "); + blk(" program. It talks to the motor controller over CAN (via "); + blk(" pCanBridge pass-through, default channel CAN1): "); + blk(" subscribes motor state/fault frames (CAN_0x*), sends the "); + blk(" 500ms periodic command frame 0x18EF2010 (enable/reset/speed) "); + blk(" via MOOS CAN_TX_0x*, and provides a web page for control "); + blk(" and monitoring. Protocol: "); + blk(" docs/推进电机通信协议20260321.docx. "); + blk(" "); +} + +void showHelpAndExit() { + blk(" "); + blu("=============================================================== "); + blu("Usage: pMotor file.moos [OPTIONS] "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("Options: "); + mag(" --alias","= "); + blk(" Launch pMotor with the given process name "); + blk(" rather than pMotor. "); + mag(" --example, -e "); + blk(" Display example MOOS configuration block. "); + mag(" --help, -h "); + blk(" Display this help message. "); + mag(" --interface, -i "); + blk(" Display MOOS publications and subscriptions. "); + mag(" --version,-v "); + blk(" Display the release version of pMotor. "); + blk(" "); + blk("Note: If argv[2] does not otherwise match a known option, "); + blk(" then it will be interpreted as a run alias. This is "); + blk(" to support pAntler launching conventions. "); + blk(" "); + exit(0); +} + +void showExampleConfigAndExit() { + blk(" "); + blu("=============================================================== "); + blu("pMotor Example MOOS Configuration "); + blu("=============================================================== "); + blk(" "); + blk("ProcessConfig = pMotor "); + blk("{ "); + blk(" AppTick = 4 "); + blk(" CommsTick = 4 "); + blk(" "); + blk(" can_channel = CAN1 // 电机 CAN 通道（pCanBridge 配置名） "); + blk(" red_channel = // 冗余 CAN 通道（空=不发冗余帧） "); + blk(" cmd_period_ms = 500 // 指令帧周期（协议 500ms） "); + blk(" reset_hold_sec= 5 // 复位位保持时长（秒） "); + blk(" web_port = 18081 // 网页端口(避开8080/8090/18080) "); + blk(" web_enable = true // 是否启用网页 "); + blk(" logpath = pMotor.log // 日志路径 "); + blk("} "); + blk(" "); + exit(0); +} + +void showInterfaceAndExit() { + blk(" "); + blu("=============================================================== "); + blu("pMotor INTERFACE "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("SUBSCRIPTIONS: "); + blk("------------------------------------ "); + blk(" CAN_0x* = pCanBridge 透传的 CAN 帧（电机状态/故障） "); + blk(" "); + blk("PUBLICATIONS: "); + blk("------------------------------------ "); + blk(" CAN_TX_0x18EF2010 = 主控指令帧（二进制 8 字节数据域， "); + blk(" m_sSrcAux=CAN1，经 pCanBridge 下行） "); + blk(" CAN_TX_0x18EF2011 = 指令帧冗余（配置 red_channel 时发送） "); + blk(" "); + exit(0); +} + +void showReleaseInfoAndExit() { + showReleaseInfo("pMotor", "gpl"); + exit(0); +} diff --git a/src/pMotor/Motor_Info.h b/src/pMotor/Motor_Info.h new file mode 100644 index 0000000..6060618 --- /dev/null +++ b/src/pMotor/Motor_Info.h @@ -0,0 +1,15 @@ +/****************************************************************/ +/* NAME: Motor_Info */ +/* FILE: Motor_Info.h */ +/****************************************************************/ + +#ifndef PMOTOR_INFO_HEADER +#define PMOTOR_INFO_HEADER + +void showSynopsis(); +void showHelpAndExit(); +void showExampleConfigAndExit(); +void showInterfaceAndExit(); +void showReleaseInfoAndExit(); + +#endif diff --git a/src/pMotor/main.cpp b/src/pMotor/main.cpp new file mode 100644 index 0000000..71065fb --- /dev/null +++ b/src/pMotor/main.cpp @@ -0,0 +1,54 @@ +/************************************************************/ +/* NAME: pMotor */ +/* FILE: main.cpp */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "ColorParse.h" +#include "Motor.h" +#include "Motor_Info.h" +#include "../pPowerManger/logc/loguru.hpp" + +using namespace std; + +int main(int argc, char *argv[]) +{ + loguru::init(argc, argv); + + string mission_file; + string run_command = argv[0]; + // 默认以程序文件名（去掉路径）作为进程名，保证与 .moos 中 ProcessConfig 匹配 + { + size_t slash = run_command.find_last_of('/'); + if (slash != string::npos) run_command = run_command.substr(slash + 1); + } + + for (int i = 1; i < argc; i++) { + string argi = argv[i]; + if ((argi == "-v") || (argi == "--version") || (argi == "-version")) + showReleaseInfoAndExit(); + else if ((argi == "-e") || (argi == "--example") || (argi == "-example")) + showExampleConfigAndExit(); + else if ((argi == "-h") || (argi == "--help") || (argi == "-help")) + showHelpAndExit(); + else if ((argi == "-i") || (argi == "--interface")) + showInterfaceAndExit(); + else if (strEnds(argi, ".moos") || strEnds(argi, ".moos++")) + mission_file = argv[i]; + else if (strBegins(argi, "--alias=")) + run_command = argi.substr(8); + else if (i == 2) + run_command = argi; + } + + if (mission_file == "") + showHelpAndExit(); + + LOG_F(INFO, "pMotor launching as %s", run_command.c_str()); + + motor::Motor Motor; + Motor.Run(run_command.c_str(), mission_file.c_str()); + + return 0; +} diff --git a/src/pMotor/pMotor.moos b/src/pMotor/pMotor.moos new file mode 100644 index 0000000..6865e35 --- /dev/null +++ b/src/pMotor/pMotor.moos @@ -0,0 +1,37 @@ +// MOOS file +// pMotor 推进电机操作程序 配置示例 +// +// 功能：经 pCanBridge（CAN1 通道）与推进电机控制器交互 +// 下行：网页 /api/motor_cmd（启停/复位/转速）-> 500ms 周期指令帧 0x18EF2010 +// 上行：订阅 pCanBridge 透传的 CAN_0x*（故障/转速/母线电压/温度）-> 网页展示 +// +// 协议定义：docs/推进电机通信协议20260321.docx + +ServerHost = localhost +ServerPort = 9000 +Community = h100 + +ProcessConfig = pMotor +{ + AppTick = 4 + CommsTick = 4 + + // 电机 CAN 通道（须与 pCanBridge 的 channel 配置名一致） + can_channel = CAN1 + + // 冗余 CAN 通道（配置后指令帧同内容发 0x18EF2011 到该通道；空=不发送） + // red_channel = CAN2 + + // 指令帧周期 ms（协议 500ms） + cmd_period_ms = 500 + + // 复位位保持时长（秒）：下发复位后 reset bit 保持高电平数秒， + // 仅故障状态下电机执行复位 + reset_hold_sec = 5 + + // 网页（避开 pCCU 8080 / pPowerManger 8090 / pPowerMangerHost 18080） + web_port = 18081 + web_enable = true + + logpath = pMotor.log +} diff --git a/src/pMotor/protocol/MotorCan.cpp b/src/pMotor/protocol/MotorCan.cpp new file mode 100644 index 0000000..213daca --- /dev/null +++ b/src/pMotor/protocol/MotorCan.cpp @@ -0,0 +1,219 @@ +#include "MotorCan.h" + +#include + +namespace motor { + +const char* kMotorWordName[4] = { + "故障字1 (Byte0|Byte1)", + "故障字2 (Byte2|Byte3)", + "故障字3 (Byte4|Byte5)", + "报警字 (Byte6|Byte7)", +}; + +namespace { + +// 故障/报警位描述（bit -> 中文描述），按字定义 +struct FaultDesc { + uint16_t bit; + const char* text; +}; + +//-------------------------------------------------------------------------- +// 故障字1（Byte0|Byte1）：支路一 / 支路二 仅 bit7 不同 +//-------------------------------------------------------------------------- +const FaultDesc kW0Branch1[] = { + {0, "CPLD自检故障标志"}, + {1, "温度继电器过温故障"}, + {2, "A相绕组过温故障"}, + {3, "B相绕组过温故障"}, + {4, "C相绕组过温故障"}, + {5, "逆变组件底板过温故障"}, + {6, "输出滤波器过温故障"}, + {7, "筒内空气过温故障"}, + {8, "超速故障"}, + {9, "紧急停机故障"}, + {11, "协调通信丢失故障"}, + {12, "协调故障停机"}, +}; +const FaultDesc kW0Branch2[] = { + {0, "CPLD自检故障标志"}, + {1, "温度继电器过温故障"}, + {2, "A相绕组过温故障"}, + {3, "B相绕组过温故障"}, + {4, "C相绕组过温故障"}, + {5, "逆变组件底板过温故障"}, + {6, "输出滤波器过温故障"}, + // bit7 支路二为预留 + {8, "超速故障"}, + {9, "紧急停机故障"}, + {11, "协调通信丢失故障"}, + {12, "协调故障停机"}, +}; + +//-------------------------------------------------------------------------- +// 故障字2（Byte2|Byte3） +//-------------------------------------------------------------------------- +const FaultDesc kW1[] = { + {0, "逆变A1相驱动1(上管)故障"}, + {1, "逆变A1相驱动2(下管)故障"}, + {4, "逆变B1相驱动3(上管)故障"}, + {5, "逆变B1相驱动4(下管)故障"}, + {8, "逆变C1相驱动5(上管)故障"}, + {9, "逆变C1相驱动6(下管)故障"}, + {12, "硬件总故障"}, + {13, "驱动总故障"}, + {14, "采样总故障"}, + {15, "24V欠压故障"}, +}; + +//-------------------------------------------------------------------------- +// 故障字3（Byte4|Byte5） +//-------------------------------------------------------------------------- +const FaultDesc kW2[] = { + {0, "A相软件过流故障"}, + {1, "B相软件过流故障"}, + {2, "C相软件过流故障"}, + {3, "母线电压过压故障"}, + {4, "母线电压欠压故障"}, + {5, "输出缺相故障"}, + {6, "预充电故障"}, + {8, "A1相硬件过流故障"}, + {9, "C1相硬件过流故障"}, + {15, "母线过压硬件故障"}, +}; + +//-------------------------------------------------------------------------- +// 报警字（Byte6|Byte7） +//-------------------------------------------------------------------------- +const FaultDesc kW3[] = { + {0, "直流母线1电压偏高报警"}, + {1, "直流母线1电压偏低报警"}, + {2, "A相绕组温度偏高报警"}, + {3, "B相绕组温度偏高报警"}, + {4, "C相绕组温度偏高报警"}, + {5, "逆变组件底板温度偏高报警"}, + {6, "输出滤波器温度偏高报警"}, + {7, "筒内空气温度偏高报警"}, + {8, "遥控CAN通信中断报警"}, + {9, "485通信中断报警"}, + {10, "协调CAN通信中断报警"}, + {11, "参数设置失败报警"}, + {12, "flash读取失败"}, + {13, "直流母线2电压偏高报警"}, + {14, "直流母线2电压偏低报警"}, + {15, "协调光纤报警"}, +}; + +const FaultDesc* tableOf(int branch, int wordIdx, size_t& count) { + switch (wordIdx) { + case 0: + if (branch == 2) { count = sizeof(kW0Branch2) / sizeof(kW0Branch2[0]); return kW0Branch2; } + count = sizeof(kW0Branch1) / sizeof(kW0Branch1[0]); return kW0Branch1; + case 1: + count = sizeof(kW1) / sizeof(kW1[0]); return kW1; + case 2: + count = sizeof(kW2) / sizeof(kW2[0]); return kW2; + case 3: + count = sizeof(kW3) / sizeof(kW3[0]); return kW3; + default: + count = 0; return nullptr; + } +} + +// 16 位字（Intel：低字节在前） +uint16_t wordOf(const uint8_t* d, int byteIdx) { + return static_cast(d[byteIdx] | (static_cast(d[byteIdx + 1]) << 8)); +} + +// 重建某支路的故障/报警描述列表（word 更新后调用） +void rebuildBranchTexts(MotorBranchState& br) { + br.faults.clear(); + br.alarms.clear(); + for (int w = 0; w < 3; ++w) { + std::vector tmp; + motorFaultTexts(br.branch, w, br.word[w], tmp); + br.faults.insert(br.faults.end(), tmp.begin(), tmp.end()); + } + motorFaultTexts(br.branch, 3, br.word[3], br.alarms); +} + +} // namespace + +//-------------------------------------------------------------------------- +// motorFaultTexts + +int motorFaultTexts(int branch, int wordIdx, uint16_t bits, + std::vector& out) { + out.clear(); + if (branch < 1 || branch > 2) return 0; + size_t count = 0; + const FaultDesc* table = tableOf(branch, wordIdx, count); + if (!table) return 0; + for (size_t i = 0; i < count; ++i) { + if (bits & (1u << table[i].bit)) + out.push_back(table[i].text); + } + return static_cast(out.size()); +} + +//-------------------------------------------------------------------------- +// buildMotorCmdData：指令帧数据域编码 +// byte0 = enable(bit0) | reset(bit1) +// byte2/3 = speed s16（Intel 低字节在前） + +void buildMotorCmdData(bool enable, bool reset, int16_t speed, uint8_t out[8]) { + std::memset(out, 0, 8); + out[0] = static_cast((enable ? 0x01u : 0u) | (reset ? 0x02u : 0u)); + uint16_t sp = static_cast(speed); + out[2] = static_cast(sp & 0xFFu); + out[3] = static_cast((sp >> 8) & 0xFFu); +} + +//-------------------------------------------------------------------------- +// decodeMotorFrame：电机上行报文解析（部分更新合并进 st） + +bool decodeMotorFrame(uint32_t canId, const uint8_t* data, int dlc, MotorState& st) { + if (!data || dlc < 8) return false; + + switch (canId) { + case MOTOR_CAN_FAULT1: + case MOTOR_CAN_FAULT1_RED: + case MOTOR_CAN_FAULT2: + case MOTOR_CAN_FAULT2_RED: { + bool isBranch2 = (canId == MOTOR_CAN_FAULT2 || canId == MOTOR_CAN_FAULT2_RED); + MotorBranchState& br = isBranch2 ? st.branch2 : st.branch1; + br.branch = isBranch2 ? 2 : 1; + br.word[0] = wordOf(data, 0); + br.word[1] = wordOf(data, 2); + br.word[2] = wordOf(data, 4); + br.word[3] = wordOf(data, 6); + rebuildBranchTexts(br); + br.valid = true; + return true; + } + case MOTOR_CAN_STATE3: + case MOTOR_CAN_STATE3_RED: { + uint16_t sp = wordOf(data, 0); + st.feedbackSpeed = static_cast(sp); + st.busVoltage = wordOf(data, 2); + st.state3Valid = true; + return true; + } + case MOTOR_CAN_STATE4: + case MOTOR_CAN_STATE4_RED: { + st.tempA = static_cast(data[0]); + st.tempB = static_cast(data[1]); + st.tempC = static_cast(data[2]); + st.tempInv = static_cast(data[3]); + st.tempFilter = static_cast(data[4]); + st.tempAir = static_cast(data[5]); + st.state4Valid = true; + return true; + } + default: + return false; + } +} + +} // namespace motor diff --git a/src/pMotor/protocol/MotorCan.h b/src/pMotor/protocol/MotorCan.h new file mode 100644 index 0000000..a56cd34 --- /dev/null +++ b/src/pMotor/protocol/MotorCan.h @@ -0,0 +1,154 @@ +#ifndef PMOTOR_CAN_H +#define PMOTOR_CAN_H + +#include +#include +#include + +namespace motor { + +//============================================================================ +// 推进电机控制器 CAN 通信协议（docs/推进电机通信协议20260321.docx） +// +// 数据来源：pCanBridge 透传的 MOOS 消息 +// 变量名 CAN_0x%08X = CAN ID（29 位扩展帧），m_sVal = 二进制数据域（8 字节）， +// m_dfVal2 = 原始帧信息字节。本模块只负责按报文 ID 解析/编码数据域字段。 +// +// 物理/链路层：CAN2.0B 扩展帧，250kbps（默认），数据域 8 字节， +// 多字节数值采用 Intel 编码（低字节在前，高字节在后）。 +// +// 【报文清单】 +// 下行（pMotor -> 电机控制器，周期 500ms）： +// 0x18EF2010 主控指令帧（冗余 CAN 0x18EF2011） +// 0x18EF2110 主控限制帧（冗余 CAN 0x18EF2111，全预留，暂不发送） +// 上行（电机控制器 -> pMotor）： +// 0x18EF1020 支路一故障报警状态帧1（200ms，冗余 CAN 0x18EF1021） +// 0x18EF1120 支路二故障报警状态帧2（200ms，冗余 CAN 0x18EF1121） +// 0x18EF1220 状态帧3：反馈转速/母线电压（100ms，冗余 0x18EF1221） +// 0x18EF1320 状态帧4：温度（100ms，冗余 0x18EF1321） +// +// 【指令帧 0x18EF2010】数据域 8 字节： +// byte0 bit0 = 使能命令（0 无效 1 有效，IGBT 开关；需先预充后使能） +// byte0 bit1 = 复位命令（0 无效 1 有效，仅故障状态下有效，故障支路复位） +// byte0 bit2 = 支路一通信状态（0 正常 1 中断） +// byte0 bit3 = 支路二通信状态（0 正常 1 中断） +// byte0 bit4~7 / byte1 / byte4~7 = 预留（发 0） +// byte2/3 = （转速模式）期望转速 s16（偏移 0，分辨率 1，Intel） +// 文档示例：启动并给定 1000 转速 -> 01 00 E8 03 00 00 00 00 +// 停机 -> 00 00 00 00 00 00 00 00 +// 复位 -> 02 00 00 00 00 00 00 00 +// +// 【状态帧3 0x18EF1220】byte0/1 反馈转速 s16（Intel），byte2/3 母线电压 u16（Intel） +// +// 【状态帧4 0x18EF1320】byte0~5 依次为 A/B/C 相绕组、逆变组件底板、 +// 输出滤波器、筒内空气温度，均为 s8（偏移 0，分辨率 1） +// +// 【故障帧 0x18EF1020/0x18EF1120】4 个 16 位字（byte0|1、byte2|3、byte4|5、byte6|7）， +// 字内 bit 定义见 MotorCan.cpp 中描述表；前两帧仅支路一/支路二 +// byte0|1 的 bit7 不同（支路一=筒内空气过温故障，支路二=预留）。 +//============================================================================ + +// CAN ID 定义（29 位扩展帧） +enum MotorCanId : uint32_t { + MOTOR_CAN_CMD = 0x18EF2010u, // 主控指令帧（pMotor -> 电机） + MOTOR_CAN_CMD_RED = 0x18EF2011u, // 主控指令帧 冗余 CAN + MOTOR_CAN_LIMIT = 0x18EF2110u, // 主控限制帧（全预留，暂不使用） + MOTOR_CAN_LIMIT_RED = 0x18EF2111u, // 主控限制帧 冗余 CAN + MOTOR_CAN_FAULT1 = 0x18EF1020u, // 支路一故障报警状态帧1 + MOTOR_CAN_FAULT1_RED = 0x18EF1021u, // 支路一 冗余 CAN + MOTOR_CAN_FAULT2 = 0x18EF1120u, // 支路二故障报警状态帧2 + MOTOR_CAN_FAULT2_RED = 0x18EF1121u, // 支路二 冗余 CAN + MOTOR_CAN_STATE3 = 0x18EF1220u, // 状态帧3（转速/母线电压） + MOTOR_CAN_STATE3_RED = 0x18EF1221u, // 状态帧3 冗余 CAN + MOTOR_CAN_STATE4 = 0x18EF1320u, // 状态帧4（温度） + MOTOR_CAN_STATE4_RED = 0x18EF1321u, // 状态帧4 冗余 CAN +}; + +// 指令动作（网页 /api/motor_cmd -> 队列 -> Iterate 应用） +// 与协议指令帧字段一一对应，独立下发、无联动： +enum MotorAction { + MOTOR_ACT_ENABLE = 1, // 使能命令=1（Byte0 bit0，保持当前期望转速） + MOTOR_ACT_DISABLE = 2, // 使能命令=0（Byte0 bit0，协议"停机"示例：期望转速清 0） + MOTOR_ACT_SPEED = 3, // 期望转速（Byte2/3，speed 参数，使能状态保持不变） + MOTOR_ACT_RESET = 4, // 复位命令=1（Byte0 bit1，保持高 resetHold 秒，仅故障态有效） +}; + +// 网页下发的控制指令 +struct MotorCmd { + int action = 0; // MotorAction + int16_t speed = 0; // MOTOR_ACT_SPEED / MOTOR_ACT_START 有效 +}; + +// 单条电机上行消息的新鲜度 + 原始数据 +struct MotorBranchState { + int branch = 0; // 1 / 2 + uint16_t word[4] = {0, 0, 0, 0}; // 4 个 16 位字（故障1/故障2/故障3/报警） + std::vector faults; // 当前置位的故障描述（word0~2） + std::vector alarms; // 当前置位的报警描述（word3） + bool valid = false; // 是否收到过该支路故障帧 + double lastRxTime = 0; // 最近收到时刻（MOOSTime，调用方设置） +}; + +// 电机最新合成状态（各帧部分更新合并） +struct MotorState { + // 状态帧3 0x18EF1220 + int16_t feedbackSpeed = 0; // 反馈转速 + uint16_t busVoltage = 0; // 母线电压 + bool state3Valid = false; + double state3LastRx = 0; + // 状态帧4 0x18EF1320（s8，偏移 0） + int8_t tempA = 0, tempB = 0, tempC = 0; // A/B/C 相绕组温度 + int8_t tempInv = 0, tempFilter = 0, tempAir = 0; // 逆变底板/滤波器/筒内空气 + bool state4Valid = false; + double state4LastRx = 0; + // 故障/报警 + MotorBranchState branch1; + MotorBranchState branch2; + // 计数 + uint32_t rxCount = 0; // 命中电机报文的帧计数 +}; + +// 是否为电机协议定义的 CAN ID（含冗余 CAN 变体） +inline bool isCanMotorId(uint32_t canId) { + switch (canId) { + case MOTOR_CAN_CMD: + case MOTOR_CAN_CMD_RED: + case MOTOR_CAN_LIMIT: + case MOTOR_CAN_LIMIT_RED: + case MOTOR_CAN_FAULT1: + case MOTOR_CAN_FAULT1_RED: + case MOTOR_CAN_FAULT2: + case MOTOR_CAN_FAULT2_RED: + case MOTOR_CAN_STATE3: + case MOTOR_CAN_STATE3_RED: + case MOTOR_CAN_STATE4: + case MOTOR_CAN_STATE4_RED: + return true; + default: + return false; + } +} + +// 故障/报警字索引（帧内）：0=byte0|1 1=byte2|3 2=byte4|5 3=byte6|7(报警) +extern const char* kMotorWordName[4]; + +// 按支路(1/2)与字索引查故障/报警描述表，把 bits 中置位的描述填入 out； +// 返回描述条数。wordIdx 非法或无置位返回 0。 +int motorFaultTexts(int branch, int wordIdx, uint16_t bits, + std::vector& out); + +// 编码指令帧数据域（8 字节，经 MOOS CAN_TX_0x%08X 下发 pCanBridge 发送）： +// [0] = enable(bit0) | reset(bit1)（支路通信状态位恒 0） +// [1] = 0 [2]/[3] = speed s16 Intel（停用时调用方传 0） [4..7] = 0 +void buildMotorCmdData(bool enable, bool reset, int16_t speed, uint8_t out[8]); + +// 解析一帧 CAN 数据域；命中电机上行报文返回 true 并部分更新 st： +// 故障帧 -> 对应支路 word[4] + 故障/报警描述 +// 状态帧3 -> feedbackSpeed / busVoltage +// 状态帧4 -> 6 路温度 +// 非电机报文或 data/dlc 非法返回 false。lastRxTime 由调用方设置。 +bool decodeMotorFrame(uint32_t canId, const uint8_t* data, int dlc, MotorState& st); + +} // namespace motor + +#endif // PMOTOR_CAN_H diff --git a/src/pMotor/web/WebServer.cpp b/src/pMotor/web/WebServer.cpp new file mode 100644 index 0000000..afaf5e4 --- /dev/null +++ b/src/pMotor/web/WebServer.cpp @@ -0,0 +1,144 @@ +#include "WebServer.h" +#include "pages/index.h" +#include + +namespace motor { + +//--------------------------------------------------------- +// 事件处理：HTTP 路由 + WebSocket 收发 + +void WebServer::handleEvent(struct mg_connection* c, int ev, void* ev_data) { + if (ev == MG_EV_HTTP_MSG) { + struct mg_http_message* hm = (struct mg_http_message*)ev_data; + + // WebSocket 升级 + if (mg_match(hm->uri, mg_str("/ws"), NULL)) { + mg_ws_upgrade(c, hm, NULL); + return; + } + + // /api/* 接口：交给宿主程序处理，返回 JSON + std::string path(hm->uri.buf, hm->uri.len); + if (path.rfind("/api/", 0) == 0) { + std::string body; + if (m_apiHandler) { + std::string query(hm->query.buf, hm->query.len); + body = m_apiHandler(path, query); + } + if (!body.empty()) { + mg_http_reply(c, 200, "Content-Type: application/json\r\n", + "%.*s", (int)body.size(), body.c_str()); + } else { + mg_http_reply(c, 404, "Content-Type: text/plain\r\n", "Not Found\n"); + } + return; + } + + // 电机控制页面 + if (path == "/" || path == "/index.html") { + mg_http_reply(c, 200, "Content-Type: text/html; charset=utf-8\r\n", + "%.*s", (int)INDEX_HTML.size(), INDEX_HTML.c_str()); + return; + } + + mg_http_reply(c, 404, "Content-Type: text/plain\r\n", "Not Found\n"); + } else if (ev == MG_EV_WS_OPEN) { + // Web 线程内：先构建欢迎快照，再登记连接 + std::string welcome; + if (m_onOpen) welcome = m_onOpen(); + m_wsConnections.push_back(c); + if (!welcome.empty()) { + mg_ws_send(c, welcome.c_str(), welcome.size(), WEBSOCKET_OP_TEXT); + } + } else if (ev == MG_EV_CLOSE || ev == MG_EV_ERROR) { + for (auto it = m_wsConnections.begin(); it != m_wsConnections.end(); ++it) { + if (*it == c) { + m_wsConnections.erase(it); + break; + } + } + } else if (ev == MG_EV_WS_MSG) { + // 忽略客户端消息（控制走 /api/*） + } +} + +WebServer::WebServer() { + mg_mgr_init(&m_mgr); +} + +WebServer::~WebServer() { + stop(); + mg_mgr_free(&m_mgr); +} + +void WebServer::setOnOpen(std::function handler) { + m_onOpen = std::move(handler); +} + +void WebServer::setApiHandler(std::function handler) { + m_apiHandler = std::move(handler); +} + +bool WebServer::start(int port) { + if (m_running) return true; + m_port = port; + + std::string addr = "http://0.0.0.0:" + std::to_string(port); + mg_http_listen(&m_mgr, addr.c_str(), [](mg_connection* c, int ev, void* ev_data) { + WebServer* server = static_cast(c->fn_data); + if (server) server->handleEvent(c, ev, ev_data); + }, this); + + m_running = true; + m_thread = std::thread([this]() { serverThreadFunc(); }); + + std::cout << "pMotor web UI listening on " << addr << std::endl; + return true; +} + +void WebServer::stop() { + if (!m_running) return; + m_running = false; + if (m_thread.joinable()) m_thread.join(); + + // Web 线程已退出，此处触碰连接列表安全 + for (auto c : m_wsConnections) { + mg_ws_send(c, "", 0, WEBSOCKET_OP_CLOSE); + } + m_wsConnections.clear(); +} + +bool WebServer::serverThreadCB(void* pParam) { + WebServer* pThis = static_cast(pParam); + return pThis->serverThreadFunc(); +} + +bool WebServer::serverThreadFunc() { + while (m_running) { + mg_mgr_poll(&m_mgr, 200); + // 仅在本线程（poll 间隙）发送，避免跨线程操作 mongoose 连接 + flushPending(); + } + return true; +} + +void WebServer::flushPending() { + std::string text; + { + std::lock_guard lock(m_pendingMutex); + text = std::move(m_pending); + m_pending.clear(); + } + if (text.empty()) return; + for (auto c : m_wsConnections) { + mg_ws_send(c, text.c_str(), text.size(), WEBSOCKET_OP_TEXT); + } +} + +void WebServer::broadcast(const std::string& text) { + // 只投递到队列，实际发送在 Web 线程 flushPending() 中完成 + std::lock_guard lock(m_pendingMutex); + m_pending = text; +} + +} // namespace motor diff --git a/src/pMotor/web/WebServer.h b/src/pMotor/web/WebServer.h new file mode 100644 index 0000000..928a188 --- /dev/null +++ b/src/pMotor/web/WebServer.h @@ -0,0 +1,66 @@ +#ifndef PMOTOR_WEB_SERVER_H +#define PMOTOR_WEB_SERVER_H + +#define UNIX +#include "mongoose.h" +#include +#include +#include +#include +#include +#include + +namespace motor { + +//============================================================================ +// WebServer：HTTP + WebSocket 控制/监控服务器（基于 mongoose）。 +// +// 路由： +// / -> 内嵌电机控制页面 +// /ws -> WebSocket，推送 JSON 快照（由 Motor 周期调用 broadcast） +// /api/* -> 控制指令接口（由 Motor 处理，返回 JSON） +// 采用与仓库内 pCCU/WebServer、pPowerMangerHost/WebServer 一致的回调模式。 +//============================================================================ + +class WebServer { +public: + WebServer(); + ~WebServer(); + + bool start(int port); + void stop(); + bool isRunning() const { return m_running; } + + // 向所有 WebSocket 客户端广播文本 + // 线程安全：mongoose 非线程安全，本接口只把文本放入待发队列， + // 由 Web 线程在 mg_mgr_poll 返回后统一发送（禁止跨线程直接 mg_ws_send） + void broadcast(const std::string& text); + + // 新客户端连接时调用，返回初始快照 + void setOnOpen(std::function handler); + + // 处理 /api/* 请求，返回 JSON 响应体（空串表示 404） + void setApiHandler(std::function handler); + +private: + static bool serverThreadCB(void* pParam); + bool serverThreadFunc(); + void handleEvent(struct mg_connection* c, int ev, void* ev_data); + // Web 线程内：把待发队列中的快照推给所有 WS 客户端（仅本线程触碰连接列表） + void flushPending(); + + struct mg_mgr m_mgr; + int m_port = 0; + std::atomic m_running{false}; + std::thread m_thread; + + std::mutex m_pendingMutex; // 保护 m_pending + std::string m_pending; // 最新待广播快照（新快照覆盖旧快照） + std::vector m_wsConnections; // 仅 Web 线程访问 + std::function m_onOpen; + std::function m_apiHandler; +}; + +} // namespace motor + +#endif // PMOTOR_WEB_SERVER_H diff --git a/src/pMotor/web/pages/index.h b/src/pMotor/web/pages/index.h new file mode 100644 index 0000000..0eb42fc --- /dev/null +++ b/src/pMotor/web/pages/index.h @@ -0,0 +1,271 @@ +#ifndef PMOTOR_PAGE_INDEX_H +#define PMOTOR_PAGE_INDEX_H + +#include + +//============================================================================ +// pMotor 推进电机控制页面（纯前端，内嵌 HTML）。 +// +// - 通过 WebSocket /ws 接收服务器推送的 JSON 快照（每秒） +// - 控制：按协议指令帧字段独立下发（无联动按钮） +// 使能命令(Byte0 bit0) / 复位命令(Byte0 bit1) / 期望转速(Byte2/3) +// （fetch /api/motor_cmd -> pMotor 入队 -> MOOS CAN_TX -> pCanBridge -> CAN1） +// - 展示：反馈转速/母线电压、温度、支路一/支路二故障与报警（按位解析中文） +// - 协议定义：docs/推进电机通信协议20260321.docx +//============================================================================ + +namespace motor { +const std::string INDEX_HTML = R"HTML( + + + + +pMotor 推进电机控制 + + + +

pMotor 推进电机控制 连接中...

+
加载中...
+ + + + +)HTML"; +} // namespace motor + +#endif // PMOTOR_PAGE_INDEX_H diff --git a/src/pPowerManger/PowerManger.h b/src/pPowerManger/PowerManger.h index c8087b5..72bb224 100644 --- a/src/pPowerManger/PowerManger.h +++ b/src/pPowerManger/PowerManger.h @@ -168,7 +168,7 @@ class PowerManger : public AppCastingMOOSApp double GetPublicLastIterateTime() { return GetLastIterateTime(); } int GetPublicIterateCount() { return GetIterateCount(); } string GetServerHost() { return m_sServerHost; } - int GetServerPort() { return m_lServerPort; } + int GetServerPort() { return m_lServerPort; } public: //故障注入 void injectFault(unsigned type,unsigned int faultCode); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f318581..35fb5d1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -161,3 +161,23 @@ target_link_libraries(pccuTest pthread dl ) + +################################################pmotorTest（pMotor 单元测试）#### +# pMotor 推进电机 CAN 协议编解码单元测试（独立可执行，无 gtest 依赖） +set(MOTOR_DIR ${CMAKE_SOURCE_DIR}/src/pMotor) + +add_executable(pmotorTest + pmotor/pmotorTest.cpp + ${MOTOR_DIR}/protocol/MotorCan.cpp +) +target_include_directories(pmotorTest PRIVATE + ${MOTOR_DIR} +) +set_target_properties(pmotorTest PROPERTIES + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON +) +target_link_libraries(pmotorTest + m + pthread +) diff --git a/test/pmotor/pmotorTest.cpp b/test/pmotor/pmotorTest.cpp new file mode 100644 index 0000000..75d3005 --- /dev/null +++ b/test/pmotor/pmotorTest.cpp @@ -0,0 +1,255 @@ +//============================================================================ +// pmotorTest：pMotor 推进电机 CAN 协议编解码单元测试 +// 无第三方测试库，独立可执行。参照 test/pccu/pccuTest.cpp 的轻量风格。 +// +// 协议依据：docs/推进电机通信协议20260321.docx +//============================================================================ + +#include +#include +#include +#include +#include + +#include "../../src/pMotor/protocol/MotorCan.h" + +using namespace motor; + +static int g_fail = 0; +static int g_pass = 0; + +#define CHECK(cond) do { \ + if (cond) { ++g_pass; } \ + else { ++g_fail; std::printf("FAIL %s:%d %s\n", __FILE__, __LINE__, #cond); } \ +} while (0) + +#define CHECK_STREQ(a, b) do { \ + if (std::string(a) == std::string(b)) { ++g_pass; } \ + else { ++g_fail; std::printf("FAIL %s:%d \"%s\" != \"%s\"\n", __FILE__, __LINE__, std::string(a).c_str(), std::string(b).c_str()); } \ +} while (0) + +//-------------------------------------------------------------------------- +// 1. 指令帧编码（对照文档示例） + +static void testCmdEncode() { + uint8_t d[8]; + + // 文档示例：启动并给定 1000 转速 -> 01 00 E8 03 00 00 00 00 + buildMotorCmdData(true, false, 1000, d); + uint8_t expect1[] = {0x01, 0x00, 0xE8, 0x03, 0x00, 0x00, 0x00, 0x00}; + CHECK(std::memcmp(d, expect1, 8) == 0); + + // 文档示例：停机 -> 00 00 00 00 00 00 00 00 + buildMotorCmdData(false, false, 0, d); + uint8_t expect0[] = {0, 0, 0, 0, 0, 0, 0, 0}; + CHECK(std::memcmp(d, expect0, 8) == 0); + + // 文档示例：复位 -> 02 00 00 00 00 00 00 00 + buildMotorCmdData(false, true, 0, d); + uint8_t expect2[] = {0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + CHECK(std::memcmp(d, expect2, 8) == 0); + + // 使能+复位 同时有效 -> bit0+bit1 = 0x03 + buildMotorCmdData(true, true, 0, d); + CHECK(d[0] == 0x03); + + // 负转速 s16 Intel：-100 = 0xFF9C -> byte2=0x9C byte3=0xFF + buildMotorCmdData(true, false, -100, d); + CHECK(d[0] == 0x01); + CHECK(d[2] == 0x9C && d[3] == 0xFF); + + // 边界转速：32767 / -32768 + buildMotorCmdData(true, false, 32767, d); + CHECK(d[2] == 0xFF && d[3] == 0x7F); + buildMotorCmdData(true, false, -32768, d); + CHECK(d[2] == 0x00 && d[3] == 0x80); + + // 支路通信状态位（bit2/bit3）恒 0（pMotor 不反馈通信状态） + buildMotorCmdData(true, false, 1, d); + CHECK((d[0] & 0x0C) == 0); +} + +//-------------------------------------------------------------------------- +// 2. CAN ID 识别 + +static void testCanId() { + // 12 条电机 ID 全部命中 + const uint32_t ids[] = { + MOTOR_CAN_CMD, MOTOR_CAN_CMD_RED, + MOTOR_CAN_LIMIT, MOTOR_CAN_LIMIT_RED, + MOTOR_CAN_FAULT1, MOTOR_CAN_FAULT1_RED, + MOTOR_CAN_FAULT2, MOTOR_CAN_FAULT2_RED, + MOTOR_CAN_STATE3, MOTOR_CAN_STATE3_RED, + MOTOR_CAN_STATE4, MOTOR_CAN_STATE4_RED, + }; + for (size_t i = 0; i < sizeof(ids) / sizeof(ids[0]); ++i) + CHECK(isCanMotorId(ids[i])); + + // 冗余 CAN 与主 CAN 仅末字节差 1 + CHECK(MOTOR_CAN_CMD_RED == MOTOR_CAN_CMD + 1); + CHECK(MOTOR_CAN_FAULT1_RED == MOTOR_CAN_FAULT1 + 1); + CHECK(MOTOR_CAN_FAULT2_RED == MOTOR_CAN_FAULT2 + 1); + CHECK(MOTOR_CAN_STATE3_RED == MOTOR_CAN_STATE3 + 1); + CHECK(MOTOR_CAN_STATE4_RED == MOTOR_CAN_STATE4 + 1); + + // 其他协议报文不命中（BCU 电池 / 配电） + CHECK(!isCanMotorId(0x10010000)); // BCU 概要 + CHECK(!isCanMotorId(0x103681FF)); // BCU 继电器控制 + CHECK(!isCanMotorId(0x1401A501)); // 配电控制指令 + CHECK(!isCanMotorId(0x18EF2012)); // 电机协议未定义的 ID +} + +//-------------------------------------------------------------------------- +// 3. 状态帧3 解码（0x18EF1220：反馈转速/母线电压） + +static void testState3Decode() { + MotorState st; + uint8_t d[8] = {0xE8, 0x03, 0xF4, 0x01, 0, 0, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_STATE3, d, 8, st)); + CHECK(st.state3Valid); + CHECK(st.feedbackSpeed == 1000); // 0x03E8 Intel + CHECK(st.busVoltage == 500); // 0x01F4 Intel + + // 负转速 + uint8_t d2[8] = {0x9C, 0xFF, 0, 0, 0, 0, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_STATE3, d2, 8, st)); + CHECK(st.feedbackSpeed == -100); + + // 冗余 CAN 同样可解码 + MotorState st2; + CHECK(decodeMotorFrame(MOTOR_CAN_STATE3_RED, d, 8, st2)); + CHECK(st2.feedbackSpeed == 1000 && st2.busVoltage == 500); + + // 非法 dlc + MotorState st3; + CHECK(!decodeMotorFrame(MOTOR_CAN_STATE3, d, 4, st3)); + CHECK(!decodeMotorFrame(MOTOR_CAN_STATE3, nullptr, 8, st3)); +} + +//-------------------------------------------------------------------------- +// 4. 状态帧4 解码（0x18EF1320：温度） + +static void testState4Decode() { + MotorState st; + uint8_t d[8] = {26, 44, 62, 79, 90, 107, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_STATE4, d, 8, st)); + CHECK(st.state4Valid); + CHECK(st.tempA == 26 && st.tempB == 44 && st.tempC == 62); + CHECK(st.tempInv == 79 && st.tempFilter == 90 && st.tempAir == 107); + + // 冗余 CAN + MotorState st2; + CHECK(decodeMotorFrame(MOTOR_CAN_STATE4_RED, d, 8, st2)); + CHECK(st2.tempA == 26); +} + +//-------------------------------------------------------------------------- +// 5. 故障帧解码（0x18EF1020 支路一 / 0x18EF1120 支路二） + +static void testFaultDecode() { + MotorState st; + + // 支路一：word0 置 bit8(超速) + bit9(紧急停机) -> byte0|byte1<<8 = 0x0300； + // word3 置 bit8(遥控CAN通信中断报警) -> 0x0100 + uint8_t d1[8] = {0x00, 0x03, 0, 0, 0, 0, 0x00, 0x01}; + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d1, 8, st)); + CHECK(st.branch1.valid); + CHECK(st.branch1.word[0] == 0x0300); + CHECK(st.branch1.word[3] == 0x0100); + CHECK(st.branch1.faults.size() == 2); + CHECK_STREQ(st.branch1.faults[0], "超速故障"); + CHECK_STREQ(st.branch1.faults[1], "紧急停机故障"); + CHECK(st.branch1.alarms.size() == 1); + CHECK_STREQ(st.branch1.alarms[0], "遥控CAN通信中断报警"); + + // 支路一 word0 bit7 = 筒内空气过温故障 + uint8_t d2[8] = {0x80, 0x00, 0, 0, 0, 0, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d2, 8, st)); + CHECK(st.branch1.faults.size() == 1); + CHECK_STREQ(st.branch1.faults[0], "筒内空气过温故障"); + + // 支路二：同 bit7 为预留，不应产生故障描述 + uint8_t d3[8] = {0x80, 0x00, 0, 0, 0, 0, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT2, d3, 8, st)); + CHECK(st.branch2.valid); + CHECK(st.branch2.faults.empty()); + CHECK(st.branch2.alarms.empty()); + + // 支路二 word1：bit0(逆变A1相驱动1上管) + word2 bit3(母线电压过压) + uint8_t d4[8] = {0, 0, 0x01, 0x00, 0x08, 0x00, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT2, d4, 8, st)); + CHECK(st.branch2.word[1] == 0x0001); + CHECK(st.branch2.word[2] == 0x0008); + CHECK(st.branch2.faults.size() == 2); + CHECK_STREQ(st.branch2.faults[0], "逆变A1相驱动1(上管)故障"); + CHECK_STREQ(st.branch2.faults[1], "母线电压过压故障"); + + // 冗余 CAN 同样可解码且归入对应支路 + MotorState st2; + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1_RED, d1, 8, st2)); + CHECK(st2.branch1.valid && !st2.branch2.valid); + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT2_RED, d4, 8, st2)); + CHECK(st2.branch2.valid && st2.branch2.faults.size() == 2); +} + +//-------------------------------------------------------------------------- +// 6. 故障描述查表 + +static void testFaultTable() { + std::vector out; + + // word1 bit12/13/14/15 + CHECK(motorFaultTexts(1, 1, (1u << 12) | (1u << 13) | (1u << 14) | (1u << 15), out) == 4); + CHECK_STREQ(out[0], "硬件总故障"); + CHECK_STREQ(out[1], "驱动总故障"); + CHECK_STREQ(out[2], "采样总故障"); + CHECK_STREQ(out[3], "24V欠压故障"); + + // word3 全位 16 条 + CHECK(motorFaultTexts(1, 3, 0xFFFF, out) == 16); + CHECK_STREQ(out[0], "直流母线1电压偏高报警"); + CHECK_STREQ(out[15], "协调光纤报警"); + + // 无置位 + CHECK(motorFaultTexts(1, 0, 0, out) == 0); + + // 非法参数 + CHECK(motorFaultTexts(0, 0, 1, out) == 0); + CHECK(motorFaultTexts(3, 0, 1, out) == 0); + CHECK(motorFaultTexts(1, 9, 1, out) == 0); +} + +//-------------------------------------------------------------------------- +// 7. 部分更新合并语义（故障帧重复上报，故障清除后描述随之清除） + +static void testPartialUpdate() { + MotorState st; + uint8_t d1[8] = {0x01, 0x00, 0, 0, 0, 0, 0, 0}; // 支路一 CPLD自检故障 (bit0) + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d1, 8, st)); + CHECK(st.branch1.faults.size() == 1); + CHECK_STREQ(st.branch1.faults[0], "CPLD自检故障标志"); + + // 复位后故障字清零 -> 描述清空 + uint8_t d2[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d2, 8, st)); + CHECK(st.branch1.faults.empty()); + + // 状态帧不影响支路有效标志 + uint8_t d3[8] = {1, 0, 0, 0, 0, 0, 0, 0}; + CHECK(decodeMotorFrame(MOTOR_CAN_STATE3, d3, 8, st)); + CHECK(st.feedbackSpeed == 1); + CHECK(st.branch1.valid); // 仍为 true（部分更新语义） +} + +int main() { + testCmdEncode(); + testCanId(); + testState3Decode(); + testState4Decode(); + testFaultDecode(); + testFaultTable(); + testPartialUpdate(); + + std::printf("\n==== pmotorTest: %d passed, %d failed ====\n", g_pass, g_fail); + return g_fail ? 1 : 0; +}