zjk/H100PowerManger
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

新增 pMotor 推进电机操作程序：经 pCanBridge(CAN1) 与电机控制器交互
build-test-deploy / ci (push) Failing after 32m37s
Details

Browse Source 
- 协议模块(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:
zjk
2026-09-02 10:50:28 +08:00
parent 67e732045a
commit 873a1a9b09
20 changed files with 2007 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/pMotor/Motor.cpp
+467
View File
@@ -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;
}