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-18 22:30:06 +08:00
parent f170b741da
commit 44dd1f0470
11 changed files with 726 additions and 269 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
src/pAUV150/AUV150.cpp
View File

@@ -55,6 +55,10 @@ AUV150::AUV150()
m_overrived = true; //手动控制
m_control.initialize();
// Controler::rtP.pid_heading.P = 10.0f;
// Controler::rtP.pid_heading.I = 0.1;
// Controler::rtP.pid_heading.D = 0.1f;
}
//---------------------------------------------------------
@@ -101,30 +105,30 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
m_overrived == true;
}
}
else if(key == "DESIRED_RUDDER")
{
if(m_overrived)
{
m_CommandFrame.rudderUp = msg.GetDouble() + 30.0f;
m_CommandFrame.rudderDown = msg.GetDouble() + 30.0f;
m_desiredVarTime = msg.GetTime();
}
}
else if(key == "DESIRED_ELEVATOR")
{
if(m_overrived)
{
m_CommandFrame.rudderLeft = msg.GetDouble()+ 30.0f;
m_CommandFrame.rudderRight = msg.GetDouble()+ 30.0f;
}
}
else if(key == "DESIRED_THRUST")
{
if(m_overrived)
{
m_CommandFrame.mainThruster = msg.GetDouble();
}
}
// else if(key == "DESIRED_RUDDER")
// {
// if(m_overrived)
// {
// m_CommandFrame.rudderUp = msg.GetDouble() + 30.0f;
// m_CommandFrame.rudderDown = msg.GetDouble() + 30.0f;
// m_desiredVarTime = msg.GetTime();
// }
// }
// else if(key == "DESIRED_ELEVATOR")
// {
// if(m_overrived)
// {
// m_CommandFrame.rudderLeft = msg.GetDouble()+ 30.0f;
// m_CommandFrame.rudderRight = msg.GetDouble()+ 30.0f;
// }
// }
// else if(key == "DESIRED_THRUST")
// {
// if(m_overrived)
// {
// m_CommandFrame.mainThruster = msg.GetDouble();
// }
// }
else if(key == "DESIRED_HEADING")
{
m_control.rtU.heading_cmd = msg.GetDouble();
@@ -317,10 +321,10 @@ bool AUV150::ListenLoop()
m_control.rtU.y = m_status.y;
m_control.rtU.z = m_status.z;
m_control.step();
m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 30.0f;
m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 30.0f;
m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 30.0f;
m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 30.0f;
m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f;
m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
// }
// 发送消息
@@ -473,7 +477,16 @@ bool AUV150::buildReport()
//=================DEGUB==========================
m_msgs << "MOOS_MANUAL_OVERIDE : " << m_overrived << std::endl;
m_msgs << "DirectUpperRudderServoAngleCmd : " << m_control.rtY.DirectUpperRudderServoAngleCmd << std::endl;
m_msgs << "DirectLowerRudderServoAngleCmd : " << m_control.rtY.DirectLowerRudderServoAngleCmd << std::endl;
m_msgs << "DirectLeftRudderServoAngleCmd : " << m_control.rtY.DirectLeftRudderServoAngleCmd << std::endl;
m_msgs << "DirectRightRudderServoAngleCmd : " << m_control.rtY.DirectRightRudderServoAngleCmd << std::endl;
m_msgs << "mainThruster : " << m_control.rtY.MainThrusterSpeedCmd << std::endl;
// m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
// m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
// =============== Navigation Info ===============
printTable(
{"Connect", "Read", "Writ" ,"Conter" ,"Server ip","Latitude", "Longitude","Gap"},
@@ -538,10 +551,10 @@ bool AUV150::buildReport()
{
doubleToString(m_status.thrusterRPM, 1) + "rpm",
doubleToString(m_CommandFrame.mainThruster, 1) + "%",
doubleToString(m_CommandFrame.rudderUp-30.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderDown-30.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderLeft-30.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderRight-30.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderUp-35.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderDown-35.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderLeft-35.0f, 1) + "deg",
doubleToString(m_CommandFrame.rudderRight-35.0f, 1) + "deg",
}
);
// m_msgs << "---------------------------------\n";
@@ -587,8 +600,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 / 1e7 - 180.0f;
m_status.insLatitude = feedbackFrame.insLatitude / 1e7 - 90.0f;
m_status.insLongitude = feedbackFrame.insLongitude / 1e5 - 180.0f;
m_status.insLatitude = feedbackFrame.insLatitude / 1e5 - 90.0f;
m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;