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

init

Browse Source
This commit is contained in:
zjk
2025-06-16 11:36:21 +08:00
parent 2e9214ee4e
commit f7c8f108b3
18 changed files with 2280 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
src/pAUV150/AUV150.cpp
View File

@@ -13,7 +13,7 @@
#include <string>
#include <iostream>
// #define DEBUG_
#define DEBUG_
using namespace std;
@@ -89,14 +89,14 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
cout << "great!";
else if(key == "DESIRED_RUDDER")
{
m_CommandFrame.rudderUp = msg.GetDouble();
m_CommandFrame.rudderDown = msg.GetDouble();
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();
m_CommandFrame.rudderRight = msg.GetDouble();
m_CommandFrame.rudderLeft = msg.GetDouble()+ 30.0f;
m_CommandFrame.rudderRight = msg.GetDouble()+ 30.0f;
}
else if(key == "DESIRED_THRUST")
{
@@ -205,7 +205,6 @@ void AUV150::registerVariables()
Register("DESIRED_RUDDER",0);
Register("DESIRED_THRUST",0);
Register("DESIRED_ELEVATOR",0);
// Register("FOOBAR", 0);
}
@@ -299,17 +298,16 @@ bool AUV150::WriteLoop()
double start_time = MOOSTime();
int message_count = 0;
double last_send_time = MOOSTime();
double gap;
//5Hz发送
while(!WritingThread_.IsQuitRequested()){
if(MOOSTime() - last_send_time >= m_controlCycle)
if((MOOSTime() - last_send_time) >= (m_controlCycle))
{
try
{
m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
m_controlTime = MOOSTime();
m_controlGap = m_controlTime - m_desiredVarTime;
m_controlGap = MOOSTime() - m_desiredVarTime;
m_bConnected = true;
}
catch(XPCException &e)
@@ -488,9 +486,14 @@ bool AUV150::buildReport()
// =============== Propulsion System ===============
printTable(
{"Thruster RPM"},
{"Thruster RPM", "Thruster CMD" ,"up", "down", "left", "right"},
{
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",
}
);
// m_msgs << "---------------------------------\n";
@@ -527,25 +530,26 @@ bool AUV150::buildReport()
bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
{
m_status.navModeFb = feedbackFrame.navModeFb;
m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0;
m_status.depthSensor = feedbackFrame.depthSensor / 100.0;
m_status.trueHeading = feedbackFrame.trueHeading / 100.0;
m_status.pitch = feedbackFrame.pitch / 100.0;
m_status.roll = feedbackFrame.roll / 100.0;
m_status.velocityEast = feedbackFrame.velocityEast / 100.0;
m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0;
m_status.velocityDown = feedbackFrame.velocityDown / 100.0;
m_status.insLongitude = feedbackFrame.insLongitude * 360.0 / (1ULL << 32);
m_status.insLatitude = feedbackFrame.insLatitude * 180.0 / (1ULL << 32) - 90.0;
m_status.insAltitude = feedbackFrame.insAltitude / 100.0;
m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0;
m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0;
m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0;
m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0f;
m_status.depthSensor = feedbackFrame.depthSensor / 100.0f;
m_status.trueHeading = feedbackFrame.trueHeading / 100.0f;
m_status.pitch = feedbackFrame.pitch / 100.0f;
m_status.roll = feedbackFrame.roll / 100.0f;
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 / 100000.0f - 180.0f;
m_status.insLatitude = feedbackFrame.insLatitude / 100000.0f - 90.0f;
m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;
m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0f;
m_status.thrusterRPM = feedbackFrame.thrusterRPM;
m_status.ledSwitch = feedbackFrame.ledSwitch;
m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0;
m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0f;
m_status.batteryLevel = feedbackFrame.batteryLevel;
m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0;
m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0f;
m_status.leakStatus = feedbackFrame.leakStatus;
m_status.powerModule = feedbackFrame.powerModule;
m_status.backupPower = feedbackFrame.backupPower;