zjk/moos-ivp-pi
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

手操控制测试完毕

Browse Source
This commit is contained in:
jhl
2023-11-29 17:47:03 +08:00
parent c71064fcb4
commit 9db47019cc
7 changed files with 57 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
src/pBoardSupportComm/BoardSupportComm.cpp
View File

@@ -13,9 +13,9 @@
#define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS "127.0.0.1"
// #define MOOS_AUV_SIM
//#define MATLAB_AUV_SIM
#define TRUE_AUV
//#define MOOS_AUV_SIM
#define MATLAB_AUV_SIM
//#define TRUE_AUV
using namespace std;
@@ -57,7 +57,7 @@ BoardSupportComm::BoardSupportComm()
executeCommand.header = 0xEBA2; //1:[0,1]
executeCommand.count = 16; //2:[2,3]
executeCommand.size = 21; //3:[4]
executeCommand.drive_mode = 0xFF; //4:[5]
executeCommand.drive_mode = 0x02; //4:[5]
executeCommand.thrust = 0; //5:[6]
executeCommand.yaw = 0; //6:[7,8]
executeCommand.depth = 0; //7:[9,10]
@@ -180,15 +180,13 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{
if (sval == "true")
{
executeCommand.drive_mode = 0x02;
executeCommand.manual_mode = true;
}
else
{
executeCommand.drive_mode = 0xFF;
executeCommand.manual_mode = false;
}
}
}
if(key == "uManual_drive_cmd")
@@ -215,26 +213,21 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
return false;
}
executeCommand.thrust = convertIntToUchar(recvCommand["Thrust"].asInt(), -100, 100);
float heading = recvCommand["Heading"].asFloat();
if ((heading >= 0) && (heading <= 180))
{
executeCommand.yaw = heading * 10;
}
if ((heading < 0) && (heading >= -180))
{
executeCommand.yaw = (360 + heading) * 10;
}
if ((heading > 180) or (heading < -180))
{
executeCommand.yaw = 180 * 10;
}
uint8_t helm_top_bottom_angle = convertIntToUchar((int)(recvCommand["Heading"].asFloat()), -30, 30);
executeCommand.depth = 0;
executeCommand.helm_top_angle = 0;
executeCommand.helm_bottom_angle = 0;
executeCommand.helm_top_angle = helm_top_bottom_angle;
executeCommand.helm_bottom_angle = helm_top_bottom_angle;
executeCommand.helm_left_angle = 0;
executeCommand.helm_right_angle = 0;
executeCommand.yaw = 0;
int serializeResult = serializeMessage(tcpSendBuffer);
std::stringstream ss;
ss << tcpSockFD;
ss << ", ";
ss << serializeResult;
castLogStream = ss.str();
if ((serializeResult == 0) && (tcpSockFD != -1))
{
try
@@ -540,6 +533,7 @@ bool BoardSupportComm::buildReport()
{
m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvLen:" << recvLen << endl;
m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvContent:" << recvContent << endl;
m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", sendContent:" << castLogStream << endl;
return true;
}