- 协议模块(protocol/MotorCan)：12 条 CAN ID、指令帧 0x18EF2010 编码、 状态帧(转速/母线电压/温度)与支路一/二故障报警帧解码，故障位按协议 逐位映射中文描述（docs/推进电机通信协议20260321.docx） - 主程序(MOOS 应用)：订阅 pCanBridge 透传 CAN_0x* 解码电机状态； 500ms 周期下发指令帧（CAN_TX_0x18EF2010，m_sSrcAux=CAN1）， 可选 red_channel 冗余下发 0x18EF2011；复位位保持 reset_hold_sec - 网页(18081)：按协议指令帧字段独立下发（使能命令 Byte0 bit0 / 复位命令 Byte0 bit1 / 期望转速 Byte2/3，无联动按钮）； WebSocket 1Hz 推送转速/电压/温度/故障告警/链路统计 - 配置接入：missions/h100.moos 增加 pMotor 配置块（can_channel=CAN1） - 测试：pmotorTest 92 项（指令帧字节级对照协议示例、解码/故障表） - 部署：scripts/build-board.sh 增加 pMotor 产物拷贝 + pMotor.service； .gitignore 补 bin/pMotor、bin/pmotorTest - 联调验证：MOOSDB+pCanBridge+假CANET 全链路，下行帧 01 00 E8 03 与协议文档示例一致，复位位 3s 保持行为正确
This commit is contained in:
@@ -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
|
||||
|
||||
# ============================================================
|
||||
# 数据库与日志文件（全局忽略，一律不入库）
|
||||
|
||||
Binary file not shown.
+28
-5
@@ -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
|
||||
}
|
||||
|
||||
+28
-4
@@ -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" <<EOF
|
||||
@@ -238,6 +239,29 @@ EOF
|
||||
${SSH} "${SSH_TARGET}" "systemctl daemon-reload && systemctl reset-failed pCanBridge 2>/dev/null || true"
|
||||
ok "pCanBridge.service 已安装"
|
||||
|
||||
#-------------------------------------------------------------------
|
||||
# 4.6 安装/刷新 pMotor systemd unit（幂等）
|
||||
#-------------------------------------------------------------------
|
||||
info "安装 pMotor.service ..."
|
||||
${SSH} "${SSH_TARGET}" "cat > /etc/systemd/system/pMotor.service" <<EOF
|
||||
[Unit]
|
||||
Description=pMotor Propulsion Motor for H100 Power Manager
|
||||
After=moosdb.service pCanBridge.service
|
||||
Requires=moosdb.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
WorkingDirectory=${BOARD_BIN}
|
||||
ExecStart=${BOARD_BIN}/pMotor --alias=pMotor /root/work/h100/missions/h100.moos
|
||||
Restart=on-failure
|
||||
RestartSec=5
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
EOF
|
||||
${SSH} "${SSH_TARGET}" "systemctl daemon-reload && systemctl reset-failed pMotor 2>/dev/null || true"
|
||||
ok "pMotor.service 已安装"
|
||||
|
||||
echo ""
|
||||
ok "产物: ${BOARD_BIN}/pPowerManger (aarch64)"
|
||||
${SSH} "${SSH_TARGET}" "file ${BOARD_BIN}/pPowerManger | cut -c1-60"
|
||||
|
||||
@@ -22,6 +22,7 @@ add_subdirectory(pPMtest)
|
||||
add_subdirectory(pPowerMangerHost)
|
||||
add_subdirectory(pCCU)
|
||||
add_subdirectory(pCanBridge)
|
||||
add_subdirectory(pMotor)
|
||||
##############################################################################
|
||||
# END of 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}
|
||||
)
|
||||
@@ -0,0 +1,467 @@
|
||||
#include "Motor.h"
|
||||
#include "MBUtils.h"
|
||||
#include "../pPowerManger/logc/loguru.hpp"
|
||||
#include "json/json.h"
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
|
||||
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<std::mutex> lock(m_mutex);
|
||||
if (!decodeMotorFrame(canId, d, static_cast<int>(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<int16_t>(atoi(speedStr.c_str()));
|
||||
} else {
|
||||
return "{\"ok\":false,\"error\":\"bad action (enable/disable/reset/speed)\"}";
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(m_mutex);
|
||||
m_txCount += static_cast<unsigned long>(okCount);
|
||||
m_txErr += static_cast<unsigned long>(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<Json::UInt64>(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<int>(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<std::mutex> 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<std::mutex> 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;
|
||||
}
|
||||
@@ -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 <mutex>
|
||||
|
||||
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<MotorCmd> 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
|
||||
@@ -0,0 +1,103 @@
|
||||
/****************************************************************/
|
||||
/* NAME: Motor_Info */
|
||||
/* FILE: Motor_Info.cpp */
|
||||
/****************************************************************/
|
||||
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#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","=<ProcessName> ");
|
||||
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);
|
||||
}
|
||||
@@ -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
|
||||
@@ -0,0 +1,54 @@
|
||||
/************************************************************/
|
||||
/* NAME: pMotor */
|
||||
/* FILE: main.cpp */
|
||||
/************************************************************/
|
||||
|
||||
#include <string>
|
||||
#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;
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -0,0 +1,219 @@
|
||||
#include "MotorCan.h"
|
||||
|
||||
#include <cstring>
|
||||
|
||||
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<uint16_t>(d[byteIdx] | (static_cast<uint16_t>(d[byteIdx + 1]) << 8));
|
||||
}
|
||||
|
||||
// 重建某支路的故障/报警描述列表（word 更新后调用）
|
||||
void rebuildBranchTexts(MotorBranchState& br) {
|
||||
br.faults.clear();
|
||||
br.alarms.clear();
|
||||
for (int w = 0; w < 3; ++w) {
|
||||
std::vector<std::string> 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<std::string>& 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<int>(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<uint8_t>((enable ? 0x01u : 0u) | (reset ? 0x02u : 0u));
|
||||
uint16_t sp = static_cast<uint16_t>(speed);
|
||||
out[2] = static_cast<uint8_t>(sp & 0xFFu);
|
||||
out[3] = static_cast<uint8_t>((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<int16_t>(sp);
|
||||
st.busVoltage = wordOf(data, 2);
|
||||
st.state3Valid = true;
|
||||
return true;
|
||||
}
|
||||
case MOTOR_CAN_STATE4:
|
||||
case MOTOR_CAN_STATE4_RED: {
|
||||
st.tempA = static_cast<int8_t>(data[0]);
|
||||
st.tempB = static_cast<int8_t>(data[1]);
|
||||
st.tempC = static_cast<int8_t>(data[2]);
|
||||
st.tempInv = static_cast<int8_t>(data[3]);
|
||||
st.tempFilter = static_cast<int8_t>(data[4]);
|
||||
st.tempAir = static_cast<int8_t>(data[5]);
|
||||
st.state4Valid = true;
|
||||
return true;
|
||||
}
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace motor
|
||||
@@ -0,0 +1,154 @@
|
||||
#ifndef PMOTOR_CAN_H
|
||||
#define PMOTOR_CAN_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
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<std::string> faults; // 当前置位的故障描述（word0~2）
|
||||
std::vector<std::string> 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<std::string>& 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
|
||||
@@ -0,0 +1,144 @@
|
||||
#include "WebServer.h"
|
||||
#include "pages/index.h"
|
||||
#include <iostream>
|
||||
|
||||
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<std::string()> handler) {
|
||||
m_onOpen = std::move(handler);
|
||||
}
|
||||
|
||||
void WebServer::setApiHandler(std::function<std::string(const std::string&, const std::string&)> 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<WebServer*>(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<WebServer*>(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<std::mutex> 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<std::mutex> lock(m_pendingMutex);
|
||||
m_pending = text;
|
||||
}
|
||||
|
||||
} // namespace motor
|
||||
@@ -0,0 +1,66 @@
|
||||
#ifndef PMOTOR_WEB_SERVER_H
|
||||
#define PMOTOR_WEB_SERVER_H
|
||||
|
||||
#define UNIX
|
||||
#include "mongoose.h"
|
||||
#include <string>
|
||||
#include <functional>
|
||||
#include <vector>
|
||||
#include <mutex>
|
||||
#include <atomic>
|
||||
#include <thread>
|
||||
|
||||
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<std::string()> handler);
|
||||
|
||||
// 处理 /api/* 请求，返回 JSON 响应体（空串表示 404）
|
||||
void setApiHandler(std::function<std::string(const std::string& uri, const std::string& query)> 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<bool> m_running{false};
|
||||
std::thread m_thread;
|
||||
|
||||
std::mutex m_pendingMutex; // 保护 m_pending
|
||||
std::string m_pending; // 最新待广播快照（新快照覆盖旧快照）
|
||||
std::vector<mg_connection*> m_wsConnections; // 仅 Web 线程访问
|
||||
std::function<std::string()> m_onOpen;
|
||||
std::function<std::string(const std::string&, const std::string&)> m_apiHandler;
|
||||
};
|
||||
|
||||
} // namespace motor
|
||||
|
||||
#endif // PMOTOR_WEB_SERVER_H
|
||||
@@ -0,0 +1,271 @@
|
||||
#ifndef PMOTOR_PAGE_INDEX_H
|
||||
#define PMOTOR_PAGE_INDEX_H
|
||||
|
||||
#include <string>
|
||||
|
||||
//============================================================================
|
||||
// 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(<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>pMotor 推进电机控制</title>
|
||||
<style>
|
||||
:root{--bg:#0f1420;--card:#1a2130;--line:#2a3447;--fg:#e6edf3;--dim:#8b98ab;--ok:#2ea043;--warn:#d29922;--bad:#f85149;}
|
||||
*{margin:0;padding:0;box-sizing:border-box;}
|
||||
body{background:var(--bg);color:var(--fg);font-family:'Segoe UI',system-ui,sans-serif;font-size:13px;padding:16px;}
|
||||
h1{font-size:18px;margin-bottom:12px;display:flex;align-items:center;gap:10px;}
|
||||
h2{font-size:13px;color:var(--dim);text-transform:uppercase;letter-spacing:.5px;margin-bottom:10px;border-bottom:1px solid var(--line);padding-bottom:6px;}
|
||||
.badge{font-size:11px;padding:2px 8px;border-radius:10px;background:var(--card);border:1px solid var(--line);}
|
||||
.badge.ok{color:var(--ok);border-color:var(--ok);}
|
||||
.badge.warn{color:var(--warn);border-color:var(--warn);}
|
||||
.badge.err{color:var(--bad);border-color:var(--bad);}
|
||||
.grid{display:grid;grid-template-columns:repeat(auto-fill,minmax(340px,1fr));gap:12px;}
|
||||
.card{background:var(--card);border:1px solid var(--line);border-radius:8px;padding:12px;}
|
||||
.row{display:flex;justify-content:space-between;padding:3px 0;border-bottom:1px dashed #222c3d;}
|
||||
.row:last-child{border-bottom:none;}
|
||||
.row .k{color:var(--dim);}
|
||||
.row .v{font-family:Consolas,monospace;color:var(--fg);text-align:right;}
|
||||
.sub{font-size:11px;color:#79c0ff;margin:10px 0 2px;border-bottom:1px solid #22304a;padding-bottom:2px;}
|
||||
.sub:first-child{margin-top:0;}
|
||||
.btn{padding:6px 14px;margin:0 8px 8px 0;border:1px solid var(--line);border-radius:6px;background:#222b3d;color:var(--fg);font-size:12px;cursor:pointer;}
|
||||
.btn:hover{border-color:#3b4a63;}
|
||||
.btn.on{background:#1f6feb;border-color:#1f6feb;color:#fff;font-weight:600;}
|
||||
.btn.off{background:#3d2226;border-color:#f85149;color:#f85149;}
|
||||
.btn.warn{background:#3a2e12;border-color:var(--warn);color:var(--warn);}
|
||||
.btn:disabled{opacity:.4;cursor:not-allowed;}
|
||||
input[type=number]{width:110px;background:#0f1420;border:1px solid var(--line);border-radius:6px;color:var(--fg);font-family:Consolas,monospace;padding:5px 8px;font-size:13px;}
|
||||
input[type=number]:focus{outline:none;border-color:#1f6feb;}
|
||||
.fc-bad{color:#f85149;font-weight:700;background:rgba(248,81,73,.12);padding:1px 6px;border-radius:4px;display:inline-block;margin:2px 4px 2px 0;}
|
||||
.fc-warn{color:#d29922;font-weight:600;background:rgba(210,153,34,.10);padding:1px 6px;border-radius:4px;display:inline-block;margin:2px 4px 2px 0;}
|
||||
.fc-ok{color:#3fb950;font-weight:600;}
|
||||
#status{color:var(--dim);font-size:12px;}
|
||||
.state-big{font-size:22px;font-family:Consolas,monospace;}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<h1>pMotor 推进电机控制 <span id="status" class="badge">连接中...</span></h1>
|
||||
<div class="grid" id="pages">加载中...</div>
|
||||
|
||||
<script>
|
||||
const $ = id => document.getElementById(id);
|
||||
const pagesEl = $('pages');
|
||||
const statusEl = $('status');
|
||||
|
||||
function row(k, v){ return '<div class="row"><span class="k">'+k+'</span><span class="v">'+v+'</span></div>'; }
|
||||
function sub(t){ return '<div class="sub">'+t+'</div>'; }
|
||||
function hex(v){ return '0x' + (v & 0xFF).toString(16).toUpperCase().padStart(2,'0'); }
|
||||
function hex16(v){ return '0x' + (v & 0xFFFF).toString(16).toUpperCase().padStart(4,'0'); }
|
||||
function card(title, html){ return '<div class="card"><h2>'+title+'</h2>'+html+'</div>'; }
|
||||
function ageText(a){
|
||||
if(typeof a !== 'number' || a < 0) return '从未收到';
|
||||
if(a < 90) return a.toFixed(1)+' 秒前';
|
||||
if(a < 3600) return Math.floor(a/60)+' 分 '+Math.floor(a%60)+' 秒前';
|
||||
return Math.floor(a/3600)+' 时 '+Math.floor((a%3600)/60)+' 分前';
|
||||
}
|
||||
function ageCell(d){
|
||||
if(!d || !d.valid) return '<span style="color:var(--dim)">从未收到</span>';
|
||||
const online = d.age < 3;
|
||||
return (online ? '<span class="fc-ok">' : '<span class="fc-bad">')
|
||||
+ ageText(d.age) + (online ? ' (在线)' : ' (超时)') + '</span>';
|
||||
}
|
||||
|
||||
//------------------ 控制指令（/api/motor_cmd） ------------------
|
||||
// 链路: 网页 fetch -> pMotor 入队 -> Iterate 周期编码 0x18EF2010
|
||||
// -> MOOS CAN_TX_0x18EF2010 -> pCanBridge -> CAN1 -> 电机控制器
|
||||
function sendCmd(action, speed, confirmMsg){
|
||||
if(confirmMsg && !confirm(confirmMsg)) return;
|
||||
let url = '/api/motor_cmd?action='+action;
|
||||
if(speed !== undefined && speed !== null) url += '&speed='+speed;
|
||||
fetch(url)
|
||||
.then(function(r){ return r.json(); })
|
||||
.then(function(j){
|
||||
if(j.ok) alert('指令已下发: '+j.action+(j.action==='speed' ? ' '+(j.speed||0)+' rpm' : ''));
|
||||
else alert('下发失败: '+(j.error||'未知错误'));
|
||||
})
|
||||
.catch(function(){ alert('网络错误，指令未下发'); });
|
||||
}
|
||||
function motorEnable(){
|
||||
sendCmd('enable', null, '确认启动推进电机（使能=1，IGBT 开管）？\n电机将按当前期望转速运行。');
|
||||
}
|
||||
function motorStop(){
|
||||
sendCmd('disable', null, '确认停机（使能=0，期望转速清零）？');
|
||||
}
|
||||
function motorReset(){
|
||||
sendCmd('reset', null, '确认下发故障复位指令？\n复位命令仅在故障状态下有效，故障支路将执行复位。');
|
||||
}
|
||||
function readSpeed(){
|
||||
const el = $('speedInput');
|
||||
const v = parseInt(el.value, 10);
|
||||
if(isNaN(v)){ alert('请输入有效整数转速'); return null; }
|
||||
if(v < -32767 || v > 32767){ alert('转速范围 -32767 ~ 32767'); return null; }
|
||||
return v;
|
||||
}
|
||||
function motorSetSpeed(){
|
||||
const v = readSpeed(); if(v === null) return;
|
||||
sendCmd('speed', v, '确认设定期望转速 '+v+' rpm？\n（对应指令帧 Byte2/3，使能状态保持不变）');
|
||||
}
|
||||
|
||||
//------------------ 控制卡片 ------------------
|
||||
function cmdCard(s){
|
||||
const c = s || {};
|
||||
const cmd = c.cmd || {};
|
||||
const en = cmd.enable;
|
||||
let h = '';
|
||||
h += row('使能状态', en
|
||||
? '<span class="fc-ok">已使能（运行）</span>'
|
||||
: '<span style="color:var(--dim)">未使能（停机）</span>');
|
||||
h += row('期望转速', (cmd.speed||0) + ' rpm');
|
||||
h += row('复位命令', cmd.resetActive ? '<span class="fc-warn">生效中（保持数秒）</span>' : '无效');
|
||||
if(cmd.lastCmdValid){
|
||||
const names = {enable:'使能命令=1', disable:'使能命令=0(停机)', reset:'复位命令=1', speed:'期望转速'};
|
||||
h += row('最近指令', (names[cmd.lastCmdAction]||cmd.lastCmdAction)
|
||||
+ (cmd.lastCmdAction==='speed' ? ' ('+(cmd.lastCmdSpeed||0)+' rpm)' : '')
|
||||
+ ' · ' + ageText(cmd.lastCmdAge));
|
||||
} else {
|
||||
h += row('最近指令', '<span style="color:var(--dim)">无</span>');
|
||||
}
|
||||
h += '<div class="sub">使能命令 (Byte0 bit0)</div>' +
|
||||
'<div>' +
|
||||
'<button class="btn on" onclick="motorEnable()">使能 (1)</button>' +
|
||||
'<button class="btn off" onclick="motorStop()">去使能 (0)</button></div>' +
|
||||
'<div class="sub">复位命令 (Byte0 bit1，仅故障状态下有效)</div>' +
|
||||
'<div>' +
|
||||
'<button class="btn warn" onclick="motorReset()">复位</button></div>' +
|
||||
'<div class="sub">期望转速 (Byte2/3，s16，单位 rpm)</div>' +
|
||||
'<div>' +
|
||||
'<input type="number" id="speedInput" value="1000" min="-32767" max="32767" step="100" title="期望转速 (rpm)">' +
|
||||
'<span style="color:var(--dim);margin-left:6px;">rpm</span> ' +
|
||||
'<button class="btn" onclick="motorSetSpeed()">设定</button></div>' +
|
||||
'<div class="sub" style="margin-top:8px;">指令帧 0x18EF2010 周期 ' + ((c.link&&c.link.periodMs)||500) +
|
||||
'ms 持续发送（协议要求）；停机=使能0且转速清0（协议示例 00 00 00 00 00 00 00 00）</div>';
|
||||
return card('电机指令 (下行 0x18EF2010)', h);
|
||||
}
|
||||
|
||||
//------------------ 状态帧3：转速 / 母线电压 ------------------
|
||||
function state3Card(d){
|
||||
if(!d) return '';
|
||||
let h = '';
|
||||
h += row('反馈转速', '<span class="state-big">'+(d.valid?(d.speed||0):'—')+'</span> rpm');
|
||||
h += row('母线电压', (d.valid?(d.voltage||0):'—') + (d.valid?' V':''));
|
||||
h += row('最后更新', ageCell(d));
|
||||
h += sub('来源 0x18EF1220（100ms，冗余 0x18EF1221）');
|
||||
return card('运行状态', h);
|
||||
}
|
||||
|
||||
//------------------ 状态帧4：温度 ------------------
|
||||
function state4Card(d){
|
||||
if(!d) return '';
|
||||
const v = d.valid;
|
||||
let h = '';
|
||||
const temps = [
|
||||
['A相绕组温度', d.tA], ['B相绕组温度', d.tB], ['C相绕组温度', d.tC],
|
||||
['逆变组件底板温度', d.tInv], ['输出滤波器温度', d.tFilter], ['筒内空气温度', d.tAir],
|
||||
];
|
||||
for(const t of temps){
|
||||
h += row(t[0], v ? (t[1]|0) + ' ℃' : '—');
|
||||
}
|
||||
h += row('最后更新', ageCell(d));
|
||||
h += sub('来源 0x18EF1320（100ms，冗余 0x18EF1321，Byte0~5 各 1 字节 s8）');
|
||||
return card('温度', h);
|
||||
}
|
||||
|
||||
//------------------ 支路故障/报警卡片 ------------------
|
||||
function branchCard(title, d){
|
||||
if(!d) return '';
|
||||
const words = d.words || [];
|
||||
let h = '';
|
||||
const faults = d.faults || [];
|
||||
const alarms = d.alarms || [];
|
||||
if(faults.length){
|
||||
h += row('故障(' + faults.length + '条)', faults.map(function(f){ return '<span class="fc-bad">'+f+'</span>'; }).join(''));
|
||||
} else {
|
||||
h += row('故障', d.valid ? '<span class="fc-ok">无</span>' : '—');
|
||||
}
|
||||
if(alarms.length){
|
||||
h += row('报警(' + alarms.length + '条)', alarms.map(function(a){ return '<span class="fc-warn">'+a+'</span>'; }).join(''));
|
||||
} else {
|
||||
h += row('报警', d.valid ? '<span class="fc-ok">无</span>' : '—');
|
||||
}
|
||||
h += row('故障字原始值', words.map(hex16).join(' '));
|
||||
h += row('最后更新', ageCell(d));
|
||||
return card(title, h);
|
||||
}
|
||||
|
||||
//------------------ 通信链路卡片 ------------------
|
||||
function linkCard(d){
|
||||
const l = (d && d.link) || {};
|
||||
let h = '';
|
||||
h += row('下行通道 (CAN1)', l.channel || 'CAN1');
|
||||
h += row('冗余通道', l.redChannel ? l.redChannel : '<span style="color:var(--dim)">未配置</span>');
|
||||
h += row('指令帧周期', (l.periodMs||0) + ' ms');
|
||||
h += row('指令帧累计', (l.txCount||0) + ' 成功 / ' + (l.txErr||0) + ' 失败');
|
||||
h += row('最近下发', ageCell({valid:!!l.lastTxValid, age:l.lastTxAge}));
|
||||
h += row('电机报文接收', (l.rxCount||0) + ' 帧');
|
||||
h += sub('链路: 电机 <-> CAN1 <-> CANET <-> pCanBridge <-> MOOS <-> pMotor');
|
||||
return card('通信链路 (pCanBridge)', h);
|
||||
}
|
||||
|
||||
//------------------ 协议参考卡片 ------------------
|
||||
function protoCard(){
|
||||
let h = '';
|
||||
h += row('0x18EF2010', '主控指令帧（下行 500ms，冗余 0x18EF2011）');
|
||||
h += row('0x18EF2110', '主控限制帧（下行，全预留，暂不使用）');
|
||||
h += row('0x18EF1020', '支路一故障报警状态帧1（上行 200ms，冗余 0x18EF1021）');
|
||||
h += row('0x18EF1120', '支路二故障报警状态帧2（上行 200ms，冗余 0x18EF1121）');
|
||||
h += row('0x18EF1220', '状态帧3：反馈转速/母线电压（上行 100ms，冗余 0x18EF1221）');
|
||||
h += row('0x18EF1320', '状态帧4：温度（上行 100ms，冗余 0x18EF1321）');
|
||||
h += row('帧格式', 'CAN2.0B 扩展帧 29 位 ID，数据域 8 字节，Intel 字节序');
|
||||
h += row('波特率', '250 kbps（默认，按客户可定制 125~1000）');
|
||||
return card('报文定义 (推进电机通信协议 20260321)', h);
|
||||
}
|
||||
|
||||
function render(snap){
|
||||
let html = '';
|
||||
html += cmdCard(snap);
|
||||
html += state3Card(snap.state3);
|
||||
html += state4Card(snap.state4);
|
||||
html += branchCard('支路一故障/报警 (0x18EF1020)', snap.fault1);
|
||||
html += branchCard('支路二故障/报警 (0x18EF1120)', snap.fault2);
|
||||
html += linkCard(snap);
|
||||
html += protoCard();
|
||||
pagesEl.innerHTML = html;
|
||||
// 保留转速输入值（render 会重建 DOM）
|
||||
const prev = window.__lastSpeed;
|
||||
if(prev !== undefined){ const el = $('speedInput'); if(el) el.value = prev; }
|
||||
}
|
||||
|
||||
function connect(){
|
||||
const proto = location.protocol==='https:'?'wss':'ws';
|
||||
const ws = new WebSocket(proto+'://'+location.host+'/ws');
|
||||
ws.onopen = ()=>{ statusEl.textContent='已连接'; statusEl.className='badge ok'; };
|
||||
ws.onclose = ()=>{ statusEl.textContent='已断开，重连中...'; statusEl.className='badge err'; setTimeout(connect, 2000); };
|
||||
ws.onerror = ()=>{ ws.close(); };
|
||||
ws.onmessage = (ev)=>{
|
||||
try{
|
||||
const snap = JSON.parse(ev.data);
|
||||
const el = $('speedInput');
|
||||
if(el && el === document.activeElement) window.__lastSpeed = el.value;
|
||||
render(snap);
|
||||
}catch(e){}
|
||||
};
|
||||
}
|
||||
connect();
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
)HTML";
|
||||
} // namespace motor
|
||||
|
||||
#endif // PMOTOR_PAGE_INDEX_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);
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -0,0 +1,255 @@
|
||||
//============================================================================
|
||||
// pmotorTest：pMotor 推进电机 CAN 协议编解码单元测试
|
||||
// 无第三方测试库，独立可执行。参照 test/pccu/pccuTest.cpp 的轻量风格。
|
||||
//
|
||||
// 协议依据：docs/推进电机通信协议20260321.docx
|
||||
//============================================================================
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<std::string> 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;
|
||||
}
|
||||
Reference in New Issue
Block a user