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

0626提交

Browse Source
This commit is contained in:
zjk
2025-06-26 21:00:14 +08:00
parent 8bad51bc5b
commit dbf35f6ba3
4 changed files with 25 additions and 556 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
src/pAUV150/AUV150.cpp
View File

@@ -13,7 +13,7 @@
#include <string>
#include <iostream>
// #define DEBUG_
#define DEBUG_
using namespace std;
@@ -55,10 +55,6 @@ 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;
}
//---------------------------------------------------------
@@ -98,37 +94,15 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
string sval = msg.GetString();
if(sval == "false")
{
std::cout << "======================" << std::endl;
m_overrived == false;
}
else
{
std::cout << "=============//=========" << std::endl;
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_HEADING")
{
m_control.rtU.heading_cmd = msg.GetDouble();
@@ -171,7 +145,7 @@ bool AUV150::Iterate()
m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
postStatusUpdate("NAV");
#endif
AppCastingMOOSApp::PostReport();
// AppCastingMOOSApp::PostReport();
return(true);
}
@@ -241,9 +215,9 @@ bool AUV150::OnStartUp()
void AUV150::registerVariables()
{
AppCastingMOOSApp::RegisterVariables();
Register("DESIRED_RUDDER",0);
Register("DESIRED_THRUST",0);
Register("DESIRED_ELEVATOR",0);
// Register("DESIRED_RUDDER",0);
// Register("DESIRED_THRUST",0);
// Register("DESIRED_ELEVATOR",0);
Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0);
@@ -290,7 +264,7 @@ bool AUV150::ListenLoop()
{
if((m_TcpSocket!=NULL) && m_bConnected)
{
int count = m_TcpSocket->iRecieveMessage(buffer, sizeof(FeedbackFrame_150AUV));
int count = m_TcpSocket->iReadMessageWithTimeOut(buffer, sizeof(FeedbackFrame_150AUV),3);
if(count > 0) {
std::cout << "Received " << count << " bytes" << std::endl;
FeedbackFrame_150AUV *p = reinterpret_cast<FeedbackFrame_150AUV *>(buffer);
@@ -478,25 +452,18 @@ 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"},
{"Connect", "Fre","Cont" ,"ip","OLat", "OLon","Gap"},
{
(isConnected() ? "Yes" : "No"),
doubleToString(getRealReadFreq(), 4) + " Hz",
doubleToString(getRealWriteFreq(), 4) + " Hz",
ulintToString(getCounter()),
m_serverHost,
doubleToString(m_OriginLatitude, 5) + "deg",
@@ -527,7 +494,7 @@ bool AUV150::buildReport()
// =============== Velocity Info ===============
printTable(
{"Latitude", "Longitude", "Altitude","East Velocity","North Velocity","Down Velocity"},
{"Lat", "Lon", "Alt","East Vel","North Vel","Down Vel"},
{
doubleToString(m_status.insLatitude, 4) + "deg",
doubleToString(m_status.insLongitude, 4) + "deg",
@@ -539,7 +506,7 @@ bool AUV150::buildReport()
);
m_msgs << "----------------DVL-----------------\n";
printTable(
{"DVL X Velocity", "DVL Y Velocity", "DVL Z Velocity"},
{"DVL X Vel", "DVL Y Vel", "DVL Z Vel"},
{
doubleToString(m_status.dvlVelX, 2) + "m/s",
doubleToString(m_status.dvlVelY, 2) + "m/s",
@@ -575,7 +542,7 @@ bool AUV150::buildReport()
// =============== System Status ===============
printTable(
{"Light", "Leak", "Power" , "Emergency power" , "Payload", "DVL Status","Thruster Status"},
{"Light", "Leak", "Power" , "Emergency power" , "PZ", "DVL","Thruster Status"},
{
m_status.ledSwitch ? "ON" : "OFF",
m_status.leakStatus ? "LEAK" : "NORMAL",
@@ -623,7 +590,7 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
m_status.dvlStatus = feedbackFrame.dvlStatus;
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;
@@ -667,8 +634,13 @@ void AUV150::postStatusUpdate(std::string prefix)
Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time);
}
// 发布误差信息
// 发布AUV信息
prefix = "AUV150";
Notify(prefix+"_HEADING_ERROR", m_status.heading_error, m_curr_time);
Notify(prefix+"_PITCH_ERROR", m_status.pitch_error, m_curr_time);
Notify(prefix+"_DEPTH_ERROR", m_status.depth_error, m_curr_time);
Notify(prefix+"_DESIRED_RUDDER",m_status.desired_rudder);
Notify(prefix+"_DESIRED_EVEVATOR",m_status.desired_elevator);
Notify(prefix+"_DESIRED_THRUST",m_status.desired_thrust);
}

4
src/pAUV150/AUV150.h
View File

@@ -120,6 +120,10 @@ struct AUV150_Status
double heading_error;
double pitch_error;
double depth_error;
double desired_rudder;
double desired_elevator;
double desired_thrust;
};
class AUV150 : public AppCastingMOOSApp