From dbf35f6ba3292289d32fc6b515f8a41abae5e9d0 Mon Sep 17 00:00:00 2001 From: zjk <1553836110@qq.com> Date: Thu, 26 Jun 2025 21:00:14 +0800 Subject: [PATCH] =?UTF-8?q?0626=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ...XLOG_SHORESIDE_16_6_2025_____22_24_59.slog | 507 ------------------ missions/alder/alder.moos | 4 +- src/pAUV150/AUV150.cpp | 66 +-- src/pAUV150/AUV150.h | 4 + 4 files changed, 25 insertions(+), 556 deletions(-) delete mode 100644 missions/alder/XLOG_SHORESIDE_16_6_2025_____22_24_59/XLOG_SHORESIDE_16_6_2025_____22_24_59.slog diff --git a/missions/alder/XLOG_SHORESIDE_16_6_2025_____22_24_59/XLOG_SHORESIDE_16_6_2025_____22_24_59.slog b/missions/alder/XLOG_SHORESIDE_16_6_2025_____22_24_59/XLOG_SHORESIDE_16_6_2025_____22_24_59.slog deleted file mode 100644 index 087c2a5..0000000 --- a/missions/alder/XLOG_SHORESIDE_16_6_2025_____22_24_59/XLOG_SHORESIDE_16_6_2025_____22_24_59.slog +++ /dev/null @@ -1,507 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% LOG FILE: ./XLOG_SHORESIDE_16_6_2025_____22_24_59/XLOG_SHORESIDE_16_6_2025_____22_24_59.slog -%% FILE OPENED ON Thu Jan 1 08:00:00 1970 -%% LOGSTART 1750083899.229048 -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% (1) TIME -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% TIME -%% TIME -0.753351 -%% TIME -%% TIME -0.9541769 -1.154654 -1.355344 -1.555892 -1.756309 -1.957507 -2.159124 -2.359773 -2.56044 -2.761579 -2.962356 -3.163624 -3.364308 -3.564977 -3.765584 -3.966173 -4.166899 -4.367947 -4.56872 -4.769442 -4.970057 -5.170623 -5.3714 -5.572141 -5.772642 -5.973307 -6.173816 -6.374367 -6.575039 -6.775772 -%% TIME -%% TIME -6.976394 -7.176887 -7.377504 -7.578273 -7.779018 -7.979556 -8.180116 -8.380661 -8.581375 -8.782269 -8.982922 -9.183596 -9.384163 -9.585023 -9.785771 -9.986298 -10.18705 -10.38767 -10.58915 -10.78978 -10.99054 -11.19117 -11.39183 -11.59261 -11.79331 -11.99397 -12.19466 -12.39533 -12.59607 -12.79691 -%% TIME -%% TIME -12.99757 -13.19839 -13.40052 -13.60162 -13.80216 -14.00297 -14.20373 -14.40436 -14.60503 -14.80572 -15.00716 -15.20774 -15.40825 -15.60892 -15.80956 -16.01028 -16.21093 -16.41163 -16.61239 -16.81314 -17.01373 -17.21427 -17.41495 -17.61559 -17.81621 -18.01677 -18.21732 -18.41789 -18.61861 -18.82008 -%% TIME -%% TIME -19.02093 -19.22154 -19.4222 -19.62307 -19.82388 -20.02439 -20.22731 -20.42817 -20.6296 -20.83033 -21.03077 -21.23151 -21.43203 -21.63292 -21.83371 -22.03494 -22.23552 -22.43613 -22.63677 -22.83751 -23.03813 -23.23886 -23.43951 -23.64017 -23.84067 -24.04121 -24.24189 -24.44258 -24.64337 -24.844 -%% TIME -%% TIME -25.04463 -25.2452 -25.44573 -25.64626 -25.847 -26.04751 -26.24819 -26.44885 -26.64966 -26.85041 -27.05103 -27.25183 -27.45259 -27.65336 -27.85387 -28.05447 -28.25517 -28.45659 -28.65718 -28.85778 -29.05839 -29.25912 -29.45987 -29.66105 -29.86174 -30.06238 -30.26313 -30.46388 -30.66472 -30.86538 -%% TIME -%% TIME -31.06595 -31.26655 -31.46723 -31.66805 -31.86872 -32.06911 -32.26977 -32.47035 -32.67114 -32.87194 -33.0725 -33.27309 -33.47374 -33.67441 -33.87498 -34.07561 -34.27614 -34.47669 -34.67736 -34.87791 -35.07867 -35.27921 -35.4797 -35.68037 -35.88092 -36.0815 -36.28229 -36.48282 -36.68356 -36.88418 -%% TIME -%% TIME -37.08485 -37.28559 -37.48616 -37.68695 -37.88758 -38.08825 -38.28895 -38.48963 -38.69037 -38.89095 -39.09159 -39.2921 -39.49272 -39.69319 -39.894 -40.09477 -40.2954 -40.49589 -40.69654 -40.89731 -41.0981 -41.29871 -41.49917 -41.69975 -41.90029 -42.10097 -42.30172 -42.50249 -42.70328 -42.904 -%% TIME -%% TIME -43.10473 -43.30688 -43.50759 -43.70835 -43.90911 -44.10994 -44.31045 -44.51092 -44.71182 -44.91255 -45.11345 -45.31411 -45.51459 -45.7152 -45.9157 -46.11637 -46.31697 -46.51752 -46.71834 -46.91902 -47.11984 -47.32065 -47.52125 -47.7226 -47.92329 -48.12404 -48.32464 -48.52522 -48.72574 -48.92664 -%% TIME -%% TIME -49.1275 -49.32804 -49.52874 -49.7295 -49.93018 -50.13099 -50.33162 -50.53217 -50.73272 -50.93353 -51.13435 -51.33578 -51.53632 -51.73706 -51.93772 -52.13849 -52.33924 -52.53995 -52.74131 -52.94199 -53.14274 -53.34349 -53.54407 -53.74475 -53.94528 -54.14598 -54.34656 -54.547 -54.74763 -54.94835 -%% TIME -%% TIME -55.14907 -55.34962 -55.55006 -55.75082 -55.95134 -56.15204 -56.35268 -56.55323 -56.75382 -56.95457 -57.15536 -57.35608 -57.55659 -57.75765 -57.95828 -58.15882 -58.35953 -58.55999 -58.76058 -58.96128 -59.16228 -59.36293 -59.56335 -59.764 -59.9645 -60.16511 -60.36572 -60.56624 -60.76684 -60.96766 -%% TIME -%% TIME -61.16849 -61.36899 -61.5695 -61.77005 -61.9706 -62.17123 -62.3718 -62.57225 -62.77296 -62.97364 -63.1745 -63.37505 -63.5755 -63.77626 -63.97674 -64.17739 -64.37793 -64.57839 -64.77893 -64.97951 -65.18021 -65.3808 -65.58128 -65.782 -65.98256 -66.18323 -66.38409 -66.58549 -66.78604 -66.98836 -%% TIME -%% TIME -67.18904 -67.38967 -67.59016 -67.79091 -67.99137 -68.19202 -68.39277 -68.59335 -68.794 -68.99486 -69.19559 -69.39615 -69.59659 -69.79731 -69.99776 -70.19827 -70.39908 -70.59954 -70.80009 -71.00086 -71.20138 -71.40195 -71.60251 -71.80303 -72.00341 -72.20387 -72.40451 -72.60503 -72.8055 -73.0064 -%% TIME -%% TIME -73.20704 -73.40755 -73.60801 -73.80876 -74.00935 -74.20996 -74.41052 -74.61089 -74.81154 -75.0123 -75.21298 -75.41346 -75.61402 -75.81472 -76.0154 -76.21598 -76.4164 -76.61682 -76.81735 -77.01857 -77.21921 -77.41978 -77.62023 -77.82092 -78.02147 -78.22216 -78.42275 -78.62348 -78.82409 -79.02489 -%% TIME -%% TIME -79.22562 -79.42619 -79.6267 -79.82731 -80.02774 -80.2284 -80.42915 -80.63017 -80.83089 -81.03153 -81.23204 -81.43256 -81.63315 -81.83399 -82.03449 -82.23539 -82.43602 -82.63671 -82.83746 -83.03824 -83.23917 -83.4398 -83.64045 -83.84125 -84.04179 -84.24239 -84.44314 -84.64379 -84.84455 -85.04534 -%% TIME -%% TIME -85.24831 -85.54721 -85.74793 -85.94861 -86.14924 -86.35003 -86.55062 -86.7512 -86.95184 -87.15264 -87.35327 -87.55388 -87.75448 -87.95504 -88.1557 -88.35626 -88.55692 -88.75752 -88.95807 -89.159 -89.35971 -89.56033 -89.76098 -89.96161 -90.16245 -90.36322 -90.564 -90.76487 -90.9655 -91.16648 -%% TIME -%% TIME -91.36718 -91.56772 -91.76833 -91.96926 -92.17004 -92.37056 -92.57115 -92.77188 -92.9725 -93.17327 -93.37389 -93.57453 -93.77508 -93.97559 -94.17622 diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos index b3c1a41..71f7cec 100644 --- a/missions/alder/alder.moos +++ b/missions/alder/alder.moos @@ -16,12 +16,12 @@ ProcessConfig = ANTLER Run = MOOSDB @ NewConsole = false //Run = uSimMarineV22 @ NewConsole = false Run = pNodeReporter @ NewConsole = false - Run = pLogger @ NewConsole = false + //Run = pLogger @ NewConsole = false //Run = pMarinePIDV22 @ NewConsole = false Run = pMarineViewer @ NewConsole = false Run = uProcessWatch @ NewConsole = false Run = pHelmIvP @ NewConsole = false - Run = pAUV150 @ NewConsole = false + Run = pAUV150 @ NewConsole = true //Run = pMotionControler @ NewConsole = false } diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp index 5528b3d..e948d17 100644 --- a/src/pAUV150/AUV150.cpp +++ b/src/pAUV150/AUV150.cpp @@ -13,7 +13,7 @@ #include #include -// #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(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); } diff --git a/src/pAUV150/AUV150.h b/src/pAUV150/AUV150.h index 4542334..37a19bf 100644 --- a/src/pAUV150/AUV150.h +++ b/src/pAUV150/AUV150.h @@ -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