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

增加了岸基配置文件，修复了连接bug

Browse Source
This commit is contained in:
zjk
2025-06-29 14:15:33 +08:00
parent 805d663a76
commit 47d437abd5
7 changed files with 572 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
src/pAUV150/AUV150.cpp
View File

@@ -263,6 +263,7 @@ bool AUV150::ListenLoop()
try
{
if((m_TcpSocket!=NULL) && m_bConnected)
// if((m_TcpSocket!=NULL))
{
int count = m_TcpSocket->iReadMessageWithTimeOut(buffer, sizeof(FeedbackFrame_150AUV),3);
if(count > 0) {
@@ -304,40 +305,40 @@ bool AUV150::ListenLoop()
m_status.heading_error = m_control.rtY.heading_error;
m_status.pitch_error = m_control.rtY.pitch_error;
// }
// 发送消息
try
{
m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
m_controlGap = MOOSTime() - m_desiredVarTime;
m_controlTime = MOOSTime();
m_bConnected = true;
}
catch(XPCException &e)
{
m_bConnected = false;
std::cerr << "there was trouble sending message: "
<< e.sGetException() << "\n";
}
// 计算控制频率
m_samplingTime = MOOSTime();
message_count++;
double elapsed_time = m_samplingTime - start_time;
if(elapsed_time >= 1.0) { // 每秒更新一次频率
m_real_read_freq = message_count / elapsed_time;
message_count = 0;
start_time = MOOSTime();
}
//正常情况下清除错误码
m_faultCodes.erase(0x11);
m_faultCodes.erase(0x12);
m_faultCodes.erase(0x13);
}
else
{
m_faultCodes.insert(0x12); //无数据接收
}
}
// 发送消息
try
{
m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
m_controlGap = MOOSTime() - m_desiredVarTime;
m_controlTime = MOOSTime();
m_bConnected = true;
}
catch(XPCException &e)
{
m_bConnected = false;
std::cerr << "there was trouble sending message: "
<< e.sGetException() << "\n";
}
// 计算控制频率
m_samplingTime = MOOSTime();
message_count++;
double elapsed_time = m_samplingTime - start_time;
if(elapsed_time >= 1.0) { // 每秒更新一次频率
m_real_read_freq = message_count / elapsed_time;
message_count = 0;
start_time = MOOSTime();
}
//正常情况下清除错误码
m_faultCodes.erase(0x11);
m_faultCodes.erase(0x12);
m_faultCodes.erase(0x13);
}
else
{
m_faultCodes.insert(0x12); //无数据接收
}
}
catch (XPCException & e)
{