zjk
873a1a9b09
build-test-deploy / ci (push) Failing after 32m37s
- 协议模块(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 保持行为正确
468 lines
16 KiB
C++
468 lines
16 KiB
C++
#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;
|
||
}
|