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-18 11:15:01 +08:00
parent 8b84d4c48f
commit f170b741da
2 changed files with 63 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
src/pAUV150/AUV150.cpp
View File

@@ -53,6 +53,7 @@ AUV150::AUV150()
m_controlTime = 0;
m_samplingTime = 0;
m_overrived = true; //手动控制
m_control.initialize();
}
@@ -79,7 +80,6 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p;
string key = msg.GetKey();
#if 0 // Keep these around just for template
string comm = msg.GetCommunity();
double dval = msg.GetDouble();
@@ -89,23 +89,41 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
bool mdbl = msg.IsDouble();
bool mstr = msg.IsString();
#endif
if(key == "FOO")
cout << "great!";
if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
{
string sval = msg.GetString();
if(sval == "false")
{
m_overrived == false;
}
else
{
m_overrived == true;
}
}
else if(key == "DESIRED_RUDDER")
{
m_CommandFrame.rudderUp = msg.GetDouble() + 30.0f;
m_CommandFrame.rudderDown = msg.GetDouble() + 30.0f;
m_desiredVarTime = msg.GetTime();
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")
{
m_CommandFrame.rudderLeft = msg.GetDouble()+ 30.0f;
m_CommandFrame.rudderRight = msg.GetDouble()+ 30.0f;
if(m_overrived)
{
m_CommandFrame.rudderLeft = msg.GetDouble()+ 30.0f;
m_CommandFrame.rudderRight = msg.GetDouble()+ 30.0f;
}
}
else if(key == "DESIRED_THRUST")
{
m_CommandFrame.mainThruster = msg.GetDouble();
if(m_overrived)
{
m_CommandFrame.mainThruster = msg.GetDouble();
}
}
else if(key == "DESIRED_HEADING")
{
@@ -225,6 +243,8 @@ void AUV150::registerVariables()
Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0);
Register("MOOS_MANUAL_OVERIDE", 0);
Register("MOOS_MANUAL_OVERRIDE", 0);
}
@@ -281,17 +301,28 @@ bool AUV150::ListenLoop()
m_counter = p->counter;
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;
// if(!m_overrived)
// {
// 计算控制量
m_control.rtU.u = m_status.dvlVelX;
m_control.rtU.v = m_status.dvlVelY;
m_control.rtU.w = m_status.dvlVelZ;
m_control.rtU.dx = m_status.velocityEast;
m_control.rtU.dy = m_status.velocityNorth;
m_control.rtU.dz = m_status.velocityDown;
m_control.rtU.phi = m_status.roll;
m_control.rtU.psi = (m_status.trueHeading);
m_control.rtU.theta = (m_status.pitch);
m_control.rtU.x = m_status.x;
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.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
// }
// 发送消息
try
{
@@ -441,9 +472,7 @@ bool AUV150::buildReport()
//=================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;
m_msgs << "MOOS_MANUAL_OVERIDE : " << m_overrived << std::endl;
// =============== Navigation Info ===============
printTable(
@@ -579,6 +608,10 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
m_status.x = m_pos_x;
m_status.y = m_pos_y;
m_status.z = m_status.depthSensor;
return true;
}
@@ -617,7 +650,4 @@ void AUV150::postStatusUpdate(std::string prefix)
if(m_status.insAltitude > 0) {
Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time);
}
// // 发布模拟模式标志
// Notify("SIMULATION_MODE","TRUE", m_curr_time);
}

6
src/pAUV150/AUV150.h
View File

@@ -112,6 +112,10 @@ struct AUV150_Status
unsigned char reserved; // 30. 预留 @55 Uint8 单位:—
unsigned char payloadStatus; // 28. 抛载状态反馈 @53 Uint8 单位:— 0xFF: 默认，0x00: 已抛载
unsigned char dvlStatus; // 29. DVL传感器状态反馈 @54 Uint8 单位:— 0x00: 关，0x01: 开
//以下为转换状态
double x;
double y;
double z;
};
class AUV150 : public AppCastingMOOSApp
@@ -181,6 +185,8 @@ private: // State variables
double m_controlTime;
double m_samplingTime;
bool m_overrived;
//控制器
static Controler m_control;