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

no comment

Browse Source
This commit is contained in:
zengxiaobin
2023-11-28 09:06:15 +08:00
parent 61c67ff7e2
commit 70008ee3d8
16 changed files with 506 additions and 1319 deletions
Download Patch File Download Diff File Expand all files Collapse all files

174
src/pBoardSupportComm/BoardSupportComm.cpp
View File

@@ -13,31 +13,14 @@
#define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS "127.0.0.1"
#define MOOS_AUV_SIM
// #define MATLAB_AUV_SIM
// #ifdef TRUE_AUV
// #define MOOS_AUV_SIM
//#define MATLAB_AUV_SIM
#define TRUE_AUV
using namespace std;
BoardSupportComm::BoardSupportComm()
{
estimatedState.currentLon = 0;
estimatedState.currentLat = 0;
estimatedState.currentAltitude = 0;
estimatedState.referenceLon = 0;
estimatedState.referenceLat = 0;
estimatedState.referenceAltitude = 0;
estimatedState.offsetNorth = 0;
estimatedState.offsetEast = 0;
estimatedState.offsetDown = 0;
estimatedState.roll = 0;
estimatedState.pitch = 0;
estimatedState.yaw = 0;
estimatedState.linearVelocityNorth = 0;
estimatedState.linearVelocityEast = 0;
estimatedState.linearVelocityDown = 0;
estimatedState.height = 0;
estimatedState.depth = 0;
embeddedInfo.header = 0xEBA1;
embeddedInfo.count = 0;
@@ -89,26 +72,23 @@ BoardSupportComm::BoardSupportComm()
executeCommand.footer = 0xEE2A; //16:[19,20]
executeCommand.manual_mode = false;
tcpSockFD = -1;
embeddedInfo.header = 0xEBA1;
embeddedInfo.count = 0;
tcpConnectRet = -1;
tcpSockFD = -1;
recvLen = -1;
}
//---------------------------------------------------------
// Destructor
BoardSupportComm::~BoardSupportComm()
{
// delete tcpReceiveBuffer;
close(tcpSockFD);
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{
// AppCastingMOOSApp::OnNewMail(NewMail);
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
@@ -127,77 +107,78 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
if(key == "NAV_X")
{
//E->N
estimatedState.offsetEast = msg.GetDouble();
embeddedInfo.east = (int)dval;
}
if(key == "NAV_Y")
{
//N->E
estimatedState.offsetNorth = msg.GetDouble();
embeddedInfo.north = (int)dval;
}
if(key == "NAV_Z")
{
//U->D
estimatedState.offsetDown = -msg.GetDouble();
embeddedInfo.depth = -1 * dval * 100;
}
if(key == "NAV_YAW")
{
double yawTemp = msg.GetDouble();
double yawTemp = dval; //radian
if (std::abs(yawTemp) <= M_PI)
{
if (yawTemp <= 0)
{
estimatedState.yaw = -yawTemp * 180 / M_PI;
embeddedInfo.yaw = -yawTemp * 180 / M_PI * 100;
}
else
{
estimatedState.yaw = (2 * M_PI - yawTemp) * 180 / M_PI;
embeddedInfo.yaw = (2 * M_PI - yawTemp) * 180 / M_PI * 100;
}
}
}
if(key == "NAV_PITCH")
{
estimatedState.pitch = msg.GetDouble();
embeddedInfo.pitch = dval * 100;
}
if(key == "NAV_LAT")
{
estimatedState.currentLat = msg.GetDouble();
embeddedInfo.lat = dval * 1000000;
}
if(key == "NAV_LONG")
{
estimatedState.currentLon = msg.GetDouble();
embeddedInfo.lon = dval * 1000000;
}
if(key == "NAV_SPEED")
{
estimatedState.linearVelocityNorth = msg.GetDouble() * cos(estimatedState.yaw);
estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw);
estimatedState.linearVelocityDown = 0;
float tempYaw = (float)(embeddedInfo.yaw) / 100.0;
embeddedInfo.ins_vx = dval * cos(tempYaw) * 100;
embeddedInfo.ins_vy = -dval * sin(tempYaw) * 100;
embeddedInfo.ins_vz = 0;
}
if(key == "NAV_DEPTH")
{
estimatedState.depth = msg.GetDouble();
embeddedInfo.depth = dval * 100;
}
#endif
if(key == "Fault_LeakSensor")
{
embeddedInfo.fault_leakSensor = (uint32_t)msg.GetDouble();
embeddedInfo.fault_leakSensor = (uint32_t)dval;
}
if(key == "Fault_Battery")
{
embeddedInfo.fault_battery = (uint8_t)msg.GetDouble();
embeddedInfo.fault_battery = (uint8_t)dval;
}
if(key == "Fault_EmergencyBattery")
{
embeddedInfo.fault_emergencyBattery = (uint8_t)(msg.GetDouble());
embeddedInfo.fault_emergencyBattery = (uint8_t)dval;
}
if(key == "Fault_Thrust")
{
embeddedInfo.fault_thrust = (uint8_t)(msg.GetDouble());
embeddedInfo.fault_thrust = (uint8_t)dval;
}
#endif
if(key == "uManual_enable_cmd")
{
if (msg.GetDouble() == 1.0)
if (sval == "true")
{
executeCommand.drive_mode = 0x02;
executeCommand.manual_mode = true;
@@ -212,11 +193,15 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
if(key == "uManual_drive_cmd")
{
#ifdef MOOS_AUV_SIM
if (executeCommand.manual_mode)
#else
if (embeddedInfo.drive_mode == 0x02)
#endif
{
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
std::istringstream iss(sval);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
@@ -271,7 +256,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
}
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
std::istringstream iss(sval);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
@@ -319,7 +304,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
std::istringstream iss(sval);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
@@ -327,9 +312,9 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
estimatedState.referenceAltitude = recvCommand["alt"].asFloat();
estimatedState.referenceLat = recvCommand["lat"].asFloat();
estimatedState.referenceLon = recvCommand["lon"].asFloat();
embeddedInfo.refLon = recvCommand["lon"].asFloat();
embeddedInfo.refLat = recvCommand["lat"].asFloat();
embeddedInfo.refAlt = recvCommand["alt"].asFloat();
struct stat info;
if (stat(llaOriginPath.c_str(), &info) == 0)
@@ -366,9 +351,6 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
ofs.close();
}
}
}
return(true);
@@ -415,15 +397,15 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
embeddedState["driveMode"] = 0xFF;
embeddedState["height"] = 0;
embeddedState["depth"] = 0;
embeddedState["yaw"] = estimatedState.yaw;
embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180;
embeddedState["pitch"] = 0;
embeddedState["roll"] = 0;
embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01;
embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01;
embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01;
embeddedState["currentLon"] = estimatedState.currentLon;
embeddedState["currentLat"] = estimatedState.currentLat;
embeddedState["currentAltitude"] = 0;
embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01;
embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01;
embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01;
embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
embeddedState["dvlVX"] = 0;
embeddedState["dvlVY"] = 0;
embeddedState["dvlVZ"] = 0;
@@ -432,10 +414,6 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
embeddedState["batteryVoltage"] = 15;
embeddedState["batteryLevel"] = 60;
embeddedState["batteryTemp"] = 25;
// embeddedState["faultLeakSensor"] = 0;
// embeddedState["faultBattery"] = 0;
// embeddedState["faultEmergencyBattery"] = 0;
// embeddedState["faultThrust"] = 0;
embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
embeddedState["faultBattery"] = embeddedInfo.fault_battery;
embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
@@ -482,7 +460,7 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
embeddedState["depth"] = embeddedInfo.depth * 0.01;
embeddedState["roll"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->N
embeddedState["pitch"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->E
embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D
embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D radian<-degree
embeddedState["insVX"] = embeddedInfo.ins_vy * 0.01; //E->N
embeddedState["insVY"] = embeddedInfo.ins_vx * 0.01; //N->E
embeddedState["insVZ"] = -embeddedInfo.ins_vz * 0.01; //U->D
@@ -509,19 +487,18 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
double currentLon = embeddedInfo.lon * 0.000001;
double currentLat = embeddedInfo.lat * 0.000001;
double currentAlt = embeddedInfo.alt * 0.01;
std::vector<double> reference = {estimatedState.referenceLon, estimatedState.referenceLat, estimatedState.referenceAltitude};
std::vector<double> reference = {embeddedInfo.refLon, embeddedInfo.refLat, embeddedInfo.refAlt};
std::vector<double> current = {currentLon, currentLat, currentAlt};
std::vector<double> ned = {0, 0, 0};
ConvertLLAToNED(reference, current, ned);
embeddedState["north"] = ned.at(0);
embeddedState["east"]= ned.at(1);
embeddedState["referenceLon"]= estimatedState.referenceLon;
embeddedState["referenceLat"]= estimatedState.referenceLat;
embeddedState["referenceAltitude"]= estimatedState.referenceAltitude;
embeddedState["referenceLon"]= embeddedInfo.refLon;
embeddedState["referenceLat"]= embeddedInfo.refLat;
embeddedState["referenceAltitude"]= embeddedInfo.refAlt;
Json::StreamWriterBuilder builder;
std::string embeddedStateString = Json::writeString(builder, embeddedState);
return embeddedStateString;
return Json::writeString(builder, embeddedState);
}
@@ -530,15 +507,15 @@ void BoardSupportComm::tcpProcessThread()
while(1)
{
bzero(tcpReceiveBuffer, 0);
int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
if(lens>0)
recvLen = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
if(recvLen > 0)
{
#ifndef MOOS_AUV_SIM
parseMessage((unsigned char* )tcpReceiveBuffer, lens);
parseMessage((unsigned char* )tcpReceiveBuffer, recvLen);
#endif
Json::Value embeddedState;
std::string embeddedStateString = convertEmbeddedFormat(embeddedState);
Notify("uDevice_monitor_fb", embeddedStateString);
recvContent = convertEmbeddedFormat(embeddedState);
Notify("uDevice_monitor_fb", recvContent);
#ifndef MOOS_AUV_SIM
Notify("NAV_X", embeddedState["north"].asDouble());
Notify("NAV_Y", embeddedState["east"].asDouble());
@@ -559,15 +536,16 @@ void BoardSupportComm::tcpProcessThread()
}
}
// bool BoardSupportComm::buildReport()
// {
// m_msgs << "buildReport:" << embeddedStateString << endl;
// return true;
// }
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;
return true;
}
bool BoardSupportComm::Iterate()
{
// AppCastingMOOSApp::Iterate();
AppCastingMOOSApp::Iterate();
if(tcpSockFD == -1)
{
@@ -591,8 +569,10 @@ bool BoardSupportComm::Iterate()
return false;
}
}
if ((tcpSockFD != -1) && (tcpConnectRet != -1))
{
std::thread t1(&BoardSupportComm::tcpProcessThread, this);
t1.detach();
}
@@ -665,7 +645,7 @@ bool BoardSupportComm::Iterate()
}
#endif
//AppCastingMOOSApp::PostReport();
AppCastingMOOSApp::PostReport();
return true;
}
@@ -763,7 +743,7 @@ int BoardSupportComm::parseMessage(unsigned char* buffer, int size)
bool BoardSupportComm::OnStartUp()
{
// AppCastingMOOSApp::OnStartUp();
AppCastingMOOSApp::OnStartUp();
m_MissionReader.GetValue("llaOriginPath", llaOriginPath);
try
{
@@ -782,9 +762,9 @@ bool BoardSupportComm::OnStartUp()
originJsonValue.isMember("LatOrigin") &&
originJsonValue.isMember("AltOrigin"))
{
estimatedState.referenceLon = originJsonValue["LongOrigin"].asFloat();
estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat();
estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat();
embeddedInfo.refLon = originJsonValue["LongOrigin"].asFloat();
embeddedInfo.refLat = originJsonValue["LatOrigin"].asFloat();
embeddedInfo.refAlt = originJsonValue["AltOrigin"].asFloat();
}
else
{
@@ -798,22 +778,18 @@ bool BoardSupportComm::OnStartUp()
}
catch(...)
{
m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon);
m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat);
m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude);
m_MissionReader.GetValue("LongOrigin", embeddedInfo.refLon);
m_MissionReader.GetValue("LatOrigin", embeddedInfo.refLat);
m_MissionReader.GetValue("AltOrigin", embeddedInfo.refAlt);
}
// std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", "
// << estimatedState.referenceLat << ", "
// << estimatedState.referenceAltitude << std::endl;
RegisterVariables();
return(true);
}
void BoardSupportComm::RegisterVariables()
{
// AppCastingMOOSApp::RegisterVariables();
AppCastingMOOSApp::RegisterVariables();
#ifdef MOOS_AUV_SIM
Register("NAV_X", 0);