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

调整了控制指令发送的位置

Browse Source
This commit is contained in:
zjk
2025-06-16 21:33:31 +08:00
parent c1afe465eb
commit 1124f4ce69
1 changed files with 32 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
src/pAUV150/AUV150.cpp
View File

@@ -261,7 +261,25 @@ bool AUV150::ListenLoop()
m_counter = p->counter;
updateStatus(*p);
postStatusUpdate("NAV");
// 计算控制量
// 发送消息
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;
@@ -280,7 +298,6 @@ bool AUV150::ListenLoop()
m_faultCodes.insert(0x12); //无数据接收
}
}
}
catch (XPCException & e)
{
@@ -294,37 +311,8 @@ bool AUV150::ListenLoop()
}
bool AUV150::WriteLoop()
{
double start_time = MOOSTime();
int message_count = 0;
double last_send_time = MOOSTime();
double gap;
//5Hz发送
{
while(!WritingThread_.IsQuitRequested()){
if((MOOSTime() - last_send_time) >= (m_controlCycle))
{
try
{
m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
m_controlGap = MOOSTime() - m_desiredVarTime;
m_bConnected = true;
}
catch(XPCException &e)
{
m_bConnected = false;
std::cerr << "there was trouble sending message: "
<< e.sGetException() << "\n";
}
message_count++;
double elapsed_time = MOOSTime() - start_time;
if(elapsed_time > 1.0) { // 每秒更新一次频率
m_real_write_freq = message_count / elapsed_time;
message_count = 0;
start_time = MOOSTime();
}
last_send_time = MOOSTime();
}
if(!m_bConnected)
{
m_real_read_freq = 0;
@@ -437,6 +425,17 @@ bool AUV150::buildReport()
doubleToString(m_controlGap, 5) + "s"
}
);
// printTable(
// {"Connect", "Read", "Writ" ,"Conter" ,"Gap","Gap2"},
// {
// (isConnected() ? "Yes" : "No"),
// doubleToString(getRealReadFreq(), 4) + " Hz",
// doubleToString(getRealWriteFreq(), 4) + " Hz",
// ulintToString(getCounter()),
// doubleToString(m_controlGap, 5) + "s",
// doubleToString((m_controlTime-m_samplingTime),5)
// }
// );
m_msgs << "Fault Codes: ";
for (uint8_t code : getFaultCodes()) {
m_msgs << uintToString(code) + " ";
@@ -539,8 +538,8 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
m_status.velocityEast = feedbackFrame.velocityEast / 100.0f;
m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0f;
m_status.velocityDown = feedbackFrame.velocityDown / 100.0f;
m_status.insLongitude = feedbackFrame.insLongitude / 100000.0f - 180.0f;
m_status.insLatitude = feedbackFrame.insLatitude / 100000.0f - 90.0f;
m_status.insLongitude = feedbackFrame.insLongitude / 1e7 - 180.0f;
m_status.insLatitude = feedbackFrame.insLatitude / 1e7 - 90.0f;
m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;