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

集成了simulink的生成代码

Browse Source
This commit is contained in:
zjk
2025-06-17 23:55:57 +08:00
parent 1124f4ce69
commit 8b84d4c48f
10 changed files with 1238 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
src/pAUV150/AUV150.cpp
View File

@@ -17,6 +17,9 @@
using namespace std;
// 添加静态成员定义
Controler AUV150::m_control;
static bool TCP_ReadLoop(void *p)
{
AUV150 *pAUV150 = (AUV150 *)p;
@@ -49,6 +52,8 @@ AUV150::AUV150()
m_desiredVarTime = 0;
m_controlTime = 0;
m_samplingTime = 0;
m_control.initialize();
}
//---------------------------------------------------------
@@ -102,7 +107,19 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
{
m_CommandFrame.mainThruster = msg.GetDouble();
}
else if(key != "APPCAST_REQ") // handled by AppCastingMOOSApp
else if(key == "DESIRED_HEADING")
{
m_control.rtU.heading_cmd = msg.GetDouble();
}
else if(key == "DESIRED_SPEED")
{
m_control.rtU.speed_cmd = msg.GetDouble();
}
else if(key == "DESIRED_DEPTH")
{
m_control.rtU.depth_cmd = msg.GetDouble();
}
else if(key != "APPCAST_REQ")
reportRunWarning("Unhandled Mail: " + key);
}
@@ -205,6 +222,9 @@ void AUV150::registerVariables()
Register("DESIRED_RUDDER",0);
Register("DESIRED_THRUST",0);
Register("DESIRED_ELEVATOR",0);
Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0);
}
@@ -262,7 +282,16 @@ bool AUV150::ListenLoop()
updateStatus(*p);
postStatusUpdate("NAV");
// 计算控制量
// 修改航向角输入，解决360度临界问题
m_control.rtU.psi = std::fmod(m_status.trueHeading, 360.0);
m_control.rtU.theta = std::remainder(m_status.pitch, 360.0);
m_control.rtU.z = m_status.depthSensor;
m_control.step();
m_CommandFrame.rudderUp = -m_control.rtY.DirectUpperRudderServoAngleCmd;
m_CommandFrame.rudderDown = -m_control.rtY.DirectLowerRudderServoAngleCmd;
m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd;
m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd;
m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
// 发送消息
try
{
@@ -278,7 +307,6 @@ bool AUV150::ListenLoop()
<< e.sGetException() << "\n";
}
// 计算控制频率
m_samplingTime = MOOSTime();
message_count++;
@@ -411,6 +439,12 @@ bool AUV150::buildReport()
m_msgs << oss.str() << std::endl << std::endl;
};
//=================DEGUB==========================
m_msgs << "rudder_L" << m_control.rtY.DirectLowerRudderServoAngleCmd << std::endl;
m_msgs << "rudder_R" << m_control.rtY.DirectRightRudderServoAngleCmd << std::endl;
m_msgs << "thrust" << m_control.rtY.MainThrusterSpeedCmd << std::endl;
// =============== Navigation Info ===============
printTable(
{"Connect", "Read", "Writ" ,"Conter" ,"Server ip","Latitude", "Longitude","Gap"},
@@ -425,26 +459,12 @@ 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) + " ";
}
m_msgs << std::endl;
m_msgs << "Navigation Mode : " << uintToString(m_status.navModeFb) << std::endl;
// m_msgs << "Origin Latitude : " << m_OriginLatitude << endl;
// m_msgs << "Origin Longitude : " << m_OriginLongitude << endl;
// m_msgs << "---------------------------------\n";
// =============== Heading & Attitude ===============
printTable(
{"Heading", "Pitch", "Roll", "X", "Y", "Z", "H"},