From f170b741da0a18e221c2701205ee7285057f5996 Mon Sep 17 00:00:00 2001 From: zjk <1553836110@qq.com> Date: Wed, 18 Jun 2025 11:15:01 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=BA=86bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/pAUV150/AUV150.cpp | 84 ++++++++++++++++++++++++++++-------------- src/pAUV150/AUV150.h | 6 +++ 2 files changed, 63 insertions(+), 27 deletions(-) diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp index 4d5c8da..3b63e4a 100644 --- a/src/pAUV150/AUV150.cpp +++ b/src/pAUV150/AUV150.cpp @@ -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); } diff --git a/src/pAUV150/AUV150.h b/src/pAUV150/AUV150.h index 18dab76..61ab5ce 100644 --- a/src/pAUV150/AUV150.h +++ b/src/pAUV150/AUV150.h @@ -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;