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:
zengxiaobin
2023-11-24 17:09:26 +08:00
commit a6919ec672
158 changed files with 30851 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

892
src/pBoardSupportComm/BoardSupportComm.cpp Normal file
View File

@@ -0,0 +1,892 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: BoardSupportComm.cpp */
/* DATE: */
/************************************************************/
#include <iterator>
#include "BoardSupportComm.h"
#include <cmath>
#define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS "127.0.0.1"
#define MOOS_AUV_SIM
// #define MATLAB_AUV_SIM
// #ifdef 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;
embeddedInfo.size = 0;
embeddedInfo.drive_mode = 0xFF;
embeddedInfo.height = 0;
embeddedInfo.depth = 0;
embeddedInfo.yaw = 0;
embeddedInfo.pitch = 0;
embeddedInfo.roll = 0;
embeddedInfo.ins_vx = 0;
embeddedInfo.ins_vy = 0;
embeddedInfo.ins_vz = 0;
embeddedInfo.lon = 0;
embeddedInfo.lat = 0;
embeddedInfo.alt = 0;
embeddedInfo.dvl_vx = 0;
embeddedInfo.dvl_vy = 0;
embeddedInfo.dvl_vz = 0;
embeddedInfo.rpm = 0;
embeddedInfo.light_enable = 0;
embeddedInfo.battery_voltage = 0;
embeddedInfo.battery_level = 0;
embeddedInfo.battery_temp = 0;
embeddedInfo.fault_leakSensor = 0;
embeddedInfo.fault_battery = 0;
embeddedInfo.fault_emergencyBattery = 0;
embeddedInfo.fault_thrust = 0;
embeddedInfo.iridium = 0xFF;
embeddedInfo.throwing_load = 0xFF;
embeddedInfo.dvl_status = 0;
embeddedInfo.footer = 0xEE1A;
executeCommand.header = 0xEBA2; //1:[0,1]
executeCommand.count = 16; //2:[2,3]
executeCommand.size = 21; //3:[4]
executeCommand.drive_mode = 0xFF; //4:[5]
executeCommand.thrust = 0; //5:[6]
executeCommand.yaw = 0; //6:[7,8]
executeCommand.depth = 0; //7:[9,10]
executeCommand.helm_top_angle = 0; //8:[11]
executeCommand.helm_bottom_angle = 0; //9:[12]
executeCommand.helm_left_angle = 0; //10:[13]
executeCommand.helm_right_angle = 0; //11:[14]
executeCommand.light_enable = 0; //12:[15]
executeCommand.dvl_enable = 0; //13:[16]
executeCommand.throwing_load_enable = 0; //14:[17]
executeCommand.crc = 0; //15:[18]
executeCommand.footer = 0xEE2A; //16:[19,20]
executeCommand.manual_mode = false;
tcpSockFD = -1;
tcpConnectRet = -1;
}
//---------------------------------------------------------
// Destructor
BoardSupportComm::~BoardSupportComm()
{
// delete tcpReceiveBuffer;
close(tcpSockFD);
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{
// AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
string key = msg.GetKey();
string comm = msg.GetCommunity();
double dval = msg.GetDouble();
string sval = msg.GetString();
string msrc = msg.GetSource();
double mtime = msg.GetTime();
bool mdbl = msg.IsDouble();
bool mstr = msg.IsString();
#ifdef MOOS_AUV_SIM
if(key == "NAV_X")
{
//E->N
estimatedState.offsetEast = msg.GetDouble();
}
if(key == "NAV_Y")
{
//N->E
estimatedState.offsetNorth = msg.GetDouble();
}
if(key == "NAV_Z")
{
//U->D
estimatedState.offsetDown = -msg.GetDouble();
}
if(key == "NAV_YAW")
{
double yawTemp = msg.GetDouble();
if (std::abs(yawTemp) <= M_PI)
{
if (yawTemp <= 0)
{
estimatedState.yaw = -yawTemp * 180 / M_PI;
}
else
{
estimatedState.yaw = (2 * M_PI - yawTemp) * 180 / M_PI;
}
}
}
if(key == "NAV_PITCH")
{
estimatedState.pitch = msg.GetDouble();
}
if(key == "NAV_LAT")
{
estimatedState.currentLat = msg.GetDouble();
}
if(key == "NAV_LONG")
{
estimatedState.currentLon = msg.GetDouble();
}
if(key == "NAV_SPEED")
{
estimatedState.linearVelocityNorth = msg.GetDouble() * cos(estimatedState.yaw);
estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw);
estimatedState.linearVelocityDown = 0;
}
if(key == "NAV_DEPTH")
{
estimatedState.depth = msg.GetDouble();
}
if(key == "Fault_LeakSensor")
{
embeddedInfo.fault_leakSensor = (uint32_t)msg.GetDouble();
}
if(key == "Fault_Battery")
{
embeddedInfo.fault_battery = (uint8_t)msg.GetDouble();
}
if(key == "Fault_EmergencyBattery")
{
embeddedInfo.fault_emergencyBattery = (uint8_t)(msg.GetDouble());
}
if(key == "Fault_Thrust")
{
embeddedInfo.fault_thrust = (uint8_t)(msg.GetDouble());
}
#endif
if(key == "uManual_enable_cmd")
{
if (msg.GetDouble() == 1.0)
{
executeCommand.drive_mode = 0x02;
executeCommand.manual_mode = true;
}
else
{
executeCommand.drive_mode = 0xFF;
executeCommand.manual_mode = false;
}
}
if(key == "uManual_drive_cmd")
{
if (executeCommand.manual_mode)
{
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
{
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
int thrust = recvCommand["Thrust"].asInt();
if ((thrust < -100) or (thrust > 100))
{
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;
}
executeCommand.depth = 0;
executeCommand.helm_top_angle = 0;
executeCommand.helm_bottom_angle = 0;
executeCommand.helm_left_angle = 0;
executeCommand.helm_right_angle = 0;
int serializeResult = serializeMessage(tcpSendBuffer);
if ((serializeResult == 0) && (tcpSockFD != -1))
{
try
{
write(tcpSockFD, tcpSendBuffer, executeCommand.size);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
}
}
}
if(key == "uMotion_control_cmd")
{
if (executeCommand.manual_mode)
{
return false;
}
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
{
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
executeCommand.thrust = convertIntToUchar(recvCommand["thrust"].asInt(), -100, 100);
executeCommand.helm_top_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
executeCommand.helm_bottom_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
executeCommand.helm_left_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);
executeCommand.helm_right_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);
int serializeResult = serializeMessage(tcpSendBuffer);
// std::cout << "serializeResult: " << serializeResult << std::endl;
// printf("header: %u, %u\n", tcpSendBuffer[0], tcpSendBuffer[1]);
// printf("count: %u\n", tcpSendBuffer[2], tcpSendBuffer[3]);
// printf("size: %u\n", tcpSendBuffer[4]);
// printf("drive_mode: %u\n", tcpSendBuffer[5]);
// printf("thrust: %u\n", tcpSendBuffer[6]);
// printf("yaw: %u, %u\n", tcpSendBuffer[7], tcpSendBuffer[8]);
// printf("depth: %u, %u\n", tcpSendBuffer[9], tcpSendBuffer[10]);
// printf("helm_top_angle: %u\n", tcpSendBuffer[11]);
// printf("helm_bottom_angle: %u\n", tcpSendBuffer[12]);
// printf("helm_left_angle: %u\n", tcpSendBuffer[13]);
// printf("helm_right_angle: %u\n", tcpSendBuffer[14]);
// printf("light_enable: %u\n", tcpSendBuffer[15]);
// printf("dvl_enable: %u\n", tcpSendBuffer[16]);
// printf("throwing_load_enable: %u\n", tcpSendBuffer[17]);
// printf("crc: %u\n", tcpSendBuffer[18]);
// printf("footer: %u, %u\n", tcpSendBuffer[19], tcpSendBuffer[20]);
if ((serializeResult == 0) && (tcpSockFD != -1))
{
try
{
write(tcpSockFD, tcpSendBuffer, executeCommand.size);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
}
}
if(key == "uMission_origin_cmd")
{
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
{
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();
struct stat info;
if (stat(llaOriginPath.c_str(), &info) == 0)
{
std::ifstream ifs;
ifs.open(llaOriginPath, std::ios::in);
Json::Reader reader;
Json::Value originJsonValue;
reader.parse(ifs, originJsonValue);
ifs.close();
originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat();
originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat();
originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat();
originJsonValue["TaskName"] = recvCommand["task"].asString();
Json::StreamWriterBuilder builder;
std::ofstream ofs;
ofs.open(llaOriginPath, std::ios::out);
ofs << Json::writeString(builder, originJsonValue) << std::endl;
ofs.close();
}
else
{
Json::Value originJsonValue;
originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat();
originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat();
originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat();
originJsonValue["TaskName"] = recvCommand["task"].asString();
Json::StreamWriterBuilder builder;
std::ofstream ofs;
ofs.open(llaOriginPath, std::ios::out);
ofs << Json::writeString(builder, originJsonValue) << std::endl;
ofs.close();
}
}
}
return(true);
}
uint8_t BoardSupportComm::convertIntToUchar(int src, int min, int max)
{
uint8_t dst;
if (src < 0)
{
if (src < min)
{
dst = std::abs(min) + 0B10000000;
}
else
{
dst = std::abs(src) + 0B10000000;
}
}
else
{
if (src > max)
{
dst = std::abs(max);
}
else
{
dst = std::abs(src);
}
}
return dst;
}
bool BoardSupportComm::OnConnectToServer()
{
RegisterVariables();
return(true);
}
std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
{
#ifdef MOOS_AUV_SIM
embeddedState["driveMode"] = 0xFF;
embeddedState["height"] = 0;
embeddedState["depth"] = 0;
embeddedState["yaw"] = estimatedState.yaw;
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["dvlVX"] = 0;
embeddedState["dvlVY"] = 0;
embeddedState["dvlVZ"] = 0;
embeddedState["rpm"] = 0;
embeddedState["lightEnable"] = 0;
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;
embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
embeddedState["iridium"] = 0xFF;
embeddedState["throwingLoadEnable"] = 0xFF;
embeddedState["dvlStatus"] = 0;
#endif
#ifdef MATLAB_AUV_SIM
embeddedState["driveMode"] = embeddedInfo.drive_mode;
embeddedState["height"] = embeddedInfo.height * 0.01;
embeddedState["depth"] = embeddedInfo.depth * 0.01;
embeddedState["roll"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->N
embeddedState["pitch"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->E
embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D
embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01; //N->N
embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01; //E->E
embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01; //D->D
embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
embeddedState["dvlVX"] = embeddedInfo.dvl_vx * 0.01; //N->N
embeddedState["dvlVY"] = embeddedInfo.dvl_vy * 0.01; //E->E
embeddedState["dvlVZ"] = embeddedInfo.dvl_vz * 0.01; //D->D
embeddedState["rpm"] = embeddedInfo.rpm;
embeddedState["lightEnable"] = embeddedInfo.light_enable;
embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
embeddedState["batteryLevel"] = embeddedInfo.battery_level;
embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
embeddedState["faultBattery"] = embeddedInfo.fault_battery;
embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
embeddedState["iridium"] = embeddedInfo.iridium;
embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
#endif
#ifdef TRUE_AUV
embeddedState["driveMode"] = embeddedInfo.drive_mode;
embeddedState["height"] = embeddedInfo.height * 0.01;
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["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
embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
embeddedState["dvlVX"] = embeddedInfo.dvl_vy * 0.01; //E->N
embeddedState["dvlVY"] = embeddedInfo.dvl_vx * 0.01; //N->E
embeddedState["dvlVZ"] = -embeddedInfo.dvl_vz * 0.01; //U->D
embeddedState["rpm"] = embeddedInfo.rpm;
embeddedState["lightEnable"] = embeddedInfo.light_enable;
embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
embeddedState["batteryLevel"] = embeddedInfo.battery_level;
embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
embeddedState["faultBattery"] = embeddedInfo.fault_battery;
embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
embeddedState["iridium"] = embeddedInfo.iridium;
embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
#endif
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> 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;
Json::StreamWriterBuilder builder;
std::string embeddedStateString = Json::writeString(builder, embeddedState);
return embeddedStateString;
}
void BoardSupportComm::tcpProcessThread()
{
while(1)
{
bzero(tcpReceiveBuffer, 0);
int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
if(lens>0)
{
#ifndef MOOS_AUV_SIM
parseMessage((unsigned char* )tcpReceiveBuffer, lens);
#endif
Json::Value embeddedState;
std::string embeddedStateString = convertEmbeddedFormat(embeddedState);
Notify("uDevice_monitor_fb", embeddedStateString);
#ifndef MOOS_AUV_SIM
Notify("NAV_X", embeddedState["north"].asDouble());
Notify("NAV_Y", embeddedState["east"].asDouble());
Notify("NAV_Z", embeddedState["depth"].asDouble());
Notify("NAV_LAT", embeddedState["currentLat"].asDouble());
Notify("NAV_LONG", embeddedState["currentLon"].asDouble());
Notify("NAV_HEADING", embeddedState["yaw"].asDouble() * 180 / M_PI);
double driveSpeed = std::sqrt(std::pow((double)embeddedState["dvlVX"].asDouble(), 2)
+ std::pow((double)embeddedState["dvlVY"].asDouble(), 2)
+ std::pow((double)embeddedState["dvlVZ"].asDouble(), 2));
Notify("NAV_SPEED", driveSpeed);
Notify("NAV_DEPTH", embeddedState["depth"].asDouble());
Notify("NAV_ROLL", embeddedState["roll"].asDouble() * 180 / M_PI);
Notify("NAV_PITCH", embeddedState["pitch"].asDouble() * 180 / M_PI);
Notify("NAV_YAW", embeddedState["yaw"].asDouble() * 180 / M_PI);
#endif
}
}
}
// bool BoardSupportComm::buildReport()
// {
// m_msgs << "buildReport:" << embeddedStateString << endl;
// return true;
// }
bool BoardSupportComm::Iterate()
{
// AppCastingMOOSApp::Iterate();
if(tcpSockFD == -1)
{
tcpSockFD = socket(AF_INET, SOCK_STREAM, 0);
if(tcpSockFD == -1)
{
return false;
}
}
if(tcpConnectRet == -1)
{
struct sockaddr_in saddr;
//inet_pton(AF_INET, TCP_SERVER_ADDRESS, &saddr.sin_addr.s_addr);
saddr.sin_addr.s_addr = inet_addr(TCP_SERVER_ADDRESS);
saddr.sin_family = AF_INET;
saddr.sin_port = htons(TCP_RECEIVE_PORT);
tcpConnectRet = connect(tcpSockFD, (struct sockaddr *)&saddr, sizeof(saddr));
if(tcpConnectRet == -1)
{
return false;
}
}
if ((tcpSockFD != -1) && (tcpConnectRet != -1))
{
std::thread t1(&BoardSupportComm::tcpProcessThread, this);
t1.detach();
}
#if 0
printf("header: %hx\n", embeddedInfo.header);
printf("count: %hu\n", embeddedInfo.count);
printf("size: %u\n", embeddedInfo.size);
printf("drive_mode: %u\n", embeddedInfo.drive_mode);
printf("height: %hu\n", embeddedInfo.height);
printf("depth: %hu\n", embeddedInfo.depth);
printf("yaw: %hu\n", embeddedInfo.yaw);
printf("pitch: %hd\n", embeddedInfo.pitch);
printf("roll: %hd\n", embeddedInfo.roll);
printf("ins_vx: %hd\n", embeddedInfo.ins_vx);
printf("ins_vy: %hd\n", embeddedInfo.ins_vy);
printf("ins_vz: %hd\n", embeddedInfo.ins_vz);
printf("lon: %d\n", embeddedInfo.lon);
printf("lat: %d\n", embeddedInfo.lat);
printf("alt: %hd\n", embeddedInfo.alt);
printf("dvl_vx: %hd\n", embeddedInfo.dvl_vx);
printf("dvl_vy: %hd\n", embeddedInfo.dvl_vy);
printf("dvl_vz: %hd\n", embeddedInfo.dvl_vz);
printf("rpm: %hd\n", embeddedInfo.rpm);
printf("light_enable: %u\n", embeddedInfo.light_enable);
printf("battery_voltage: %u\n", embeddedInfo.battery_voltage);
printf("battery_level: %u\n", embeddedInfo.battery_level);
printf("battery_temp: %u\n", embeddedInfo.battery_temp);
printf("fault_leakSensor: %hd\n", embeddedInfo.fault_leakSensor);
printf("fault_battery: %u\n", embeddedInfo.fault_battery);
printf("fault_emergencyBattery: %u\n", embeddedInfo.fault_emergencyBattery);
printf("fault_thrust: %u\n", embeddedInfo.fault_thrust);
printf("iridium: %u\n", embeddedInfo.iridium);
printf("throwing_load: %u\n", embeddedInfo.throwing_load);
printf("dvl_status: %u\n", embeddedInfo.dvl_status);
printf("crc: %u\n", embeddedInfo.crc);
printf("footer: %u\n", embeddedInfo.footer);
#endif
#ifdef MOOS_AUV_SIM
executeCommand.header = 0xEBA2; //1:[0,1]
executeCommand.count = 16; //2:[2,3]
executeCommand.size = 21; //3:[4]
executeCommand.drive_mode = 0x02; //4:[5]
executeCommand.thrust = 178; //5:[6],赋值-50
executeCommand.yaw = 45.3 * 10; //6:[7,8],赋值45.3
executeCommand.depth = 298.6 * 10; //7:[9,10],赋值298.6
executeCommand.helm_top_angle = 145; //8:[11],赋值-17
executeCommand.helm_bottom_angle = 154; //9:[12],赋值-26
executeCommand.helm_left_angle = 9; //10:[13],赋值9
executeCommand.helm_right_angle = 158; //11:[14],赋值-30
executeCommand.light_enable = 1; //12:[15],赋值1
executeCommand.dvl_enable = 1; //13:[16],赋值1
executeCommand.throwing_load_enable = 1; //14:[17],赋值1
executeCommand.crc = 0; //15:[18]
executeCommand.footer = 0xEE2A; //16:[19,20]
int serializeResult = serializeMessage(tcpSendBuffer);
if (serializeResult == 0)
{
try
{
write(tcpSockFD, tcpSendBuffer, executeCommand.size);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
}
#endif
//AppCastingMOOSApp::PostReport();
return true;
}
template <typename Type>
inline uint16_t BoardSupportComm::serialize(const Type t, uint8_t* bfr, uint16_t& sumSize)
{
uint16_t size = sizeof(Type);
std::memcpy(bfr, &t, size);
sumSize += size;
return size;
}
int BoardSupportComm::serializeMessage(unsigned char* buffer)
{
uint8_t* ptr = buffer;
uint16_t sumSize = 0;
ptr += serialize(executeCommand.header, ptr, sumSize);
ptr += serialize(executeCommand.count, ptr, sumSize);
ptr += serialize(executeCommand.size, ptr, sumSize);
ptr += serialize(executeCommand.drive_mode, ptr, sumSize);
ptr += serialize(executeCommand.thrust, ptr, sumSize);
ptr += serialize(executeCommand.yaw, ptr, sumSize);
ptr += serialize(executeCommand.depth, ptr, sumSize);
ptr += serialize(executeCommand.helm_top_angle, ptr, sumSize);
ptr += serialize(executeCommand.helm_bottom_angle, ptr, sumSize);
ptr += serialize(executeCommand.helm_left_angle, ptr, sumSize);
ptr += serialize(executeCommand.helm_right_angle, ptr, sumSize);
ptr += serialize(executeCommand.light_enable, ptr, sumSize);
ptr += serialize(executeCommand.dvl_enable, ptr, sumSize);
ptr += serialize(executeCommand.throwing_load_enable, ptr, sumSize);
DUNE::Algorithms::CRC8 crc(0x07);
crc.putArray(buffer+CRC_COMPUTE_START_POS, CRC_COMPUTE_SUM_SIZE);
executeCommand.crc = crc.get();
ptr += serialize(executeCommand.crc, ptr, sumSize);
ptr += serialize(executeCommand.footer, ptr, sumSize);
if (sumSize != (uint16_t)executeCommand.size)
{
return -1;
}
return 0;
}
template <typename Type>
inline uint16_t BoardSupportComm::deserialize(Type& t, const uint8_t* bfr, uint16_t& length)
{
uint16_t size = sizeof(Type);
std::memcpy(&t, bfr, size);
length -= size;
return size;
}
int BoardSupportComm::parseMessage(unsigned char* buffer, int size)
{
const uint8_t* ptr = buffer;
uint16_t size__ = size;
uint16_t header;
ptr += deserialize(embeddedInfo.header, ptr, size__);
ptr += deserialize(embeddedInfo.count, ptr, size__);
ptr += deserialize(embeddedInfo.size, ptr, size__);
ptr += deserialize(embeddedInfo.drive_mode, ptr, size__);
ptr += deserialize(embeddedInfo.height, ptr, size__);
ptr += deserialize(embeddedInfo.depth, ptr, size__);
ptr += deserialize(embeddedInfo.yaw, ptr, size__);
ptr += deserialize(embeddedInfo.pitch, ptr, size__);
ptr += deserialize(embeddedInfo.roll, ptr, size__);
ptr += deserialize(embeddedInfo.ins_vx, ptr, size__);
ptr += deserialize(embeddedInfo.ins_vy, ptr, size__);
ptr += deserialize(embeddedInfo.ins_vz, ptr, size__);
ptr += deserialize(embeddedInfo.lon, ptr, size__);
ptr += deserialize(embeddedInfo.lat, ptr, size__);
ptr += deserialize(embeddedInfo.alt, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_vx, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_vy, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_vz, ptr, size__);
ptr += deserialize(embeddedInfo.rpm, ptr, size__);
ptr += deserialize(embeddedInfo.light_enable, ptr, size__);
ptr += deserialize(embeddedInfo.battery_voltage, ptr, size__);
ptr += deserialize(embeddedInfo.battery_level, ptr, size__);
ptr += deserialize(embeddedInfo.battery_temp, ptr, size__);
ptr += deserialize(embeddedInfo.fault_leakSensor, ptr, size__);
ptr += deserialize(embeddedInfo.fault_battery, ptr, size__);
ptr += deserialize(embeddedInfo.fault_emergencyBattery, ptr, size__);
ptr += deserialize(embeddedInfo.fault_thrust, ptr, size__);
ptr += deserialize(embeddedInfo.iridium, ptr, size__);
ptr += deserialize(embeddedInfo.throwing_load, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_status, ptr, size__);
ptr += deserialize(embeddedInfo.crc, ptr, size__);
ptr += deserialize(embeddedInfo.footer, ptr, size__);
return 0;
}
bool BoardSupportComm::OnStartUp()
{
// AppCastingMOOSApp::OnStartUp();
m_MissionReader.GetValue("llaOriginPath", llaOriginPath);
try
{
struct stat info;
if (stat(llaOriginPath.c_str(), &info) == 0)
{
std::ifstream ifs;
ifs.open(llaOriginPath, std::ios::in);
Json::Reader reader;
Json::Value originJsonValue;
reader.parse(ifs, originJsonValue);
ifs.close();
if (originJsonValue.isMember("LongOrigin") &&
originJsonValue.isMember("LatOrigin") &&
originJsonValue.isMember("AltOrigin"))
{
estimatedState.referenceLon = originJsonValue["LongOrigin"].asFloat();
estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat();
estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat();
}
else
{
throw -1;
}
}
else
{
throw -1;
}
}
catch(...)
{
m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon);
m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat);
m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude);
}
// std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", "
// << estimatedState.referenceLat << ", "
// << estimatedState.referenceAltitude << std::endl;
RegisterVariables();
return(true);
}
void BoardSupportComm::RegisterVariables()
{
// AppCastingMOOSApp::RegisterVariables();
#ifdef MOOS_AUV_SIM
Register("NAV_X", 0);
Register("NAV_Y", 0);
Register("NAV_Z", 0);
Register("NAV_LAT", 0);
Register("NAV_LONG", 0);
Register("NAV_SPEED", 0);
Register("NAV_DEPTH", 0);
Register("NAV_YAW", 0);
Register("NAV_PITCH", 0);
Register("Fault_LeakSensor", 0);
Register("Fault_Battery", 0);
Register("Fault_EmergencyBattery", 0);
Register("Fault_Thrust", 0);
#endif
Register("uManual_drive_cmd", 0);
Register("uMotion_control_cmd", 0);
Register("uManual_enable_cmd", 0);
Register("uMission_origin_cmd", 0);
}
void BoardSupportComm::ConvertLLAToENU(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_enu)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
point_enu.at(0), point_enu.at(1), point_enu.at(2));
}
void BoardSupportComm::ConvertENUToLLA(std::vector<double> init_lla,
std::vector<double> point_enu,
std::vector<double> &point_lla)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
local_cartesian.Reverse(point_enu.at(0), point_enu.at(1), point_enu.at(2),
point_lla.at(1), point_lla.at(0), point_lla.at(2));
}
void BoardSupportComm::ConvertLLAToNED(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_ned)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
double point_e = 0;
double point_n = 0;
double point_u = 0;
local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
point_e, point_n, point_u);
point_ned.at(0) = point_n;
point_ned.at(1) = point_e;
point_ned.at(2) = -point_u;
}
void BoardSupportComm::ConvertNEDToLLA(std::vector<double> init_lla,
std::vector<double> point_ned,
std::vector<double> &point_lla)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
double point_e = point_ned.at(1);
double point_n = point_ned.at(0);
double point_u = -point_ned.at(2);
local_cartesian.Reverse(point_e, point_n, point_u,
point_lla.at(1), point_lla.at(0), point_lla.at(2));
}

156
src/pBoardSupportComm/BoardSupportComm.h Normal file
View File

@@ -0,0 +1,156 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: BoardSupportComm.h */
/* DATE: */
/************************************************************/
#ifndef BoardSupportComm_HEADER
#define BoardSupportComm_HEADER
#include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <DUNE/DUNE.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <json/json.h>
#include <thread>
#include <chrono>
#define CRC_COMPUTE_START_POS 5
#define CRC_COMPUTE_SUM_SIZE 13
struct AUVEmbedded
{
uint16_t header; //1:[0,1]
uint16_t count; //2:[2,3]
uint8_t size; //3:[4]
uint8_t drive_mode; //4:[5]
uint16_t height; //5:[6,7]
uint16_t depth; //6:[8,9]
uint16_t yaw; //7:[10,11]
int16_t pitch; //8:[12,13]
int16_t roll; //9:[14,15]
int16_t ins_vx; //10:[16,17]
int16_t ins_vy; //11:[18,19]
int16_t ins_vz; //12:[20,21]
int32_t lon; //13:[22,23,24,25]
int32_t lat; //14:[26,27,28,29]
int16_t alt; //15:[30,31]
int16_t dvl_vx; //16:[32,33]
int16_t dvl_vy; //17:[34,35]
int16_t dvl_vz; //18:[36,37]
int16_t rpm; //19:[38,39]
uint8_t light_enable; //20:[40]
uint16_t battery_voltage; //21:[41,42]
uint8_t battery_level; //22:[43]
uint16_t battery_temp; //23:[44,45]
uint32_t fault_leakSensor; //24:[46,47,48,49]
uint8_t fault_battery; //25:[50]
uint8_t fault_emergencyBattery; //26:[51]
uint8_t fault_thrust; //27:[52]
uint8_t iridium; //28:[53]
uint8_t throwing_load; //29:[54]
uint8_t dvl_status; //30:[55]
uint8_t crc; //31:[56]
uint16_t footer; //32:[57,58]
};
struct AUVExecuteCommand
{
uint16_t header; //1:[0,1]
uint16_t count; //2:[2,3]
uint8_t size; //3:[4]
uint8_t drive_mode; //4:[5]
uint8_t thrust; //5:[6]
uint16_t yaw; //6:[7,8]
uint16_t depth; //7:[9,10]
uint8_t helm_top_angle; //8:[11]
uint8_t helm_bottom_angle;//9:[12]
uint8_t helm_left_angle; //10:[13]
uint8_t helm_right_angle;//11:[14]
uint8_t light_enable; //12:[15]
uint8_t dvl_enable; //13:[16]
uint8_t throwing_load_enable; //14:[17]
uint8_t crc; //15:[18]
uint16_t footer; //16:[19,20]
bool manual_mode;
};
struct EstimatedState {
float referenceLon;
float referenceLat;
float referenceAltitude;
float currentLon;
float currentLat;
float currentAltitude;
float offsetNorth;
float offsetEast;
float offsetDown;
float roll;
float pitch;
float yaw;
float linearVelocityNorth;
float linearVelocityEast;
float linearVelocityDown;
float height;
float depth;
};
class BoardSupportComm : public CMOOSApp
// class BoardSupportComm : public AppCastingMOOSApp
{
public:
BoardSupportComm();
~BoardSupportComm();
protected: // Standard MOOSApp functions to overload
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void RegisterVariables();
// bool buildReport();
private:
struct EstimatedState estimatedState;
void ConvertLLAToENU(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_enu);
void ConvertENUToLLA(std::vector<double> init_lla,
std::vector<double> point_enu,
std::vector<double> &point_lla);
void ConvertLLAToNED(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_ned);
void ConvertNEDToLLA(std::vector<double> init_lla,
std::vector<double> point_ned,
std::vector<double> &point_lla);
void tcpProcessThread();
// std::string convertEmbeddedFormat();
std::string convertEmbeddedFormat(Json::Value &embeddedState);
int tcpSockFD;
int tcpConnectRet;
char tcpReceiveBuffer[65535];
unsigned char tcpSendBuffer[65535];
int parseMessage(unsigned char* buffer, int size);
int serializeMessage(unsigned char* buffer);
template <typename Type> inline uint16_t deserialize(Type& t, const uint8_t* bfr, uint16_t& length);
template <typename Type> inline uint16_t serialize(const Type t, uint8_t* bfr, uint16_t& sumSize);
uint8_t convertIntToUchar(int src, int min, int max);
struct AUVEmbedded embeddedInfo;
struct AUVExecuteCommand executeCommand;
int testFlag = 0;
bool testCount = false;
int sockfd = -1;
int connectFlg = -1;
std::string llaOriginPath;
};
#endif

115
src/pBoardSupportComm/BoardSupportComm_Info.cpp Normal file
View File

@@ -0,0 +1,115 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: BoardSupportComm_Info.cpp */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#include <cstdlib>
#include <iostream>
#include "BoardSupportComm_Info.h"
#include "ColorParse.h"
#include "ReleaseInfo.h"
using namespace std;
//----------------------------------------------------------------
// Procedure: showSynopsis
void showSynopsis()
{
blk("SYNOPSIS: ");
blk("------------------------------------ ");
blk(" The pBoardSupportComm application is used for ");
blk(" ");
blk(" ");
blk(" ");
blk(" ");
}
//----------------------------------------------------------------
// Procedure: showHelpAndExit
void showHelpAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("Usage: pBoardSupportComm file.moos [OPTIONS] ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("Options: ");
mag(" --alias","=<ProcessName> ");
blk(" Launch pBoardSupportComm with the given process name ");
blk(" rather than pBoardSupportComm. ");
mag(" --example, -e ");
blk(" Display example MOOS configuration block. ");
mag(" --help, -h ");
blk(" Display this help message. ");
mag(" --interface, -i ");
blk(" Display MOOS publications and subscriptions. ");
mag(" --version,-v ");
blk(" Display the release version of pBoardSupportComm. ");
blk(" ");
blk("Note: If argv[2] does not otherwise match a known option, ");
blk(" then it will be interpreted as a run alias. This is ");
blk(" to support pAntler launching conventions. ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showExampleConfigAndExit
void showExampleConfigAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pBoardSupportComm Example MOOS Configuration ");
blu("=============================================================== ");
blk(" ");
blk("ProcessConfig = pBoardSupportComm ");
blk("{ ");
blk(" AppTick = 4 ");
blk(" CommsTick = 4 ");
blk(" ");
blk("} ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showInterfaceAndExit
void showInterfaceAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pBoardSupportComm INTERFACE ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("SUBSCRIPTIONS: ");
blk("------------------------------------ ");
blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, ");
blk(" string_val=BAR ");
blk(" ");
blk("PUBLICATIONS: ");
blk("------------------------------------ ");
blk(" Publications are determined by the node message content. ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showReleaseInfoAndExit
void showReleaseInfoAndExit()
{
showReleaseInfo("pBoardSupportComm", "gpl");
exit(0);
}

18
src/pBoardSupportComm/BoardSupportComm_Info.h Normal file
View File

@@ -0,0 +1,18 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: BoardSupportComm_Info.h */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#ifndef BoardSupportComm_INFO_HEADER
#define BoardSupportComm_INFO_HEADER
void showSynopsis();
void showHelpAndExit();
void showExampleConfigAndExit();
void showInterfaceAndExit();
void showReleaseInfoAndExit();
#endif

47
src/pBoardSupportComm/CMakeLists.txt Normal file
View File

@@ -0,0 +1,47 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pBoardSupportComm
# Author(s): Xiaobin Zeng
#--------------------------------------------------------
SET(SRC
BoardSupportComm.cpp
BoardSupportComm_Info.cpp
main.cpp
)
FIND_LIBRARY(DUNE_LIB dune-core /usr/local/lib /usr/local/lib/DUNE)
FIND_PATH(DUNE_INCLUDE DUNE/IMC.hpp /usr/local/include /usr/local/include/DUNE)
include_directories(${DUNE_INCLUDE})
# include(FindProtobuf)
# find_package(Protobuf REQUIRED)
# include_directories(${Protobuf_INCLUDE_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
#find_package (jsoncpp REQUIRED)
find_package (GeographicLib REQUIRED)
include_directories(${GeographicLib_INCLUDE_DIRS})
ADD_EXECUTABLE(pBoardSupportComm ${SRC})
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
TARGET_LINK_LIBRARIES(pBoardSupportComm
${MOOS_LIBRARIES}
${DUNE_LIB}
${GeographicLib_LIBRARIES}
mbutil
m
pthread
jsoncpp
# jsoncpp_lib_static
# protobuf
# protoc
# proto
# ${PROTOBUF_LIBRARY}
)

53
src/pBoardSupportComm/main.cpp Normal file
View File

@@ -0,0 +1,53 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: main.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "MBUtils.h"
#include "ColorParse.h"
#include "BoardSupportComm.h"
#include "BoardSupportComm_Info.h"
using namespace std;
int main(int argc, char *argv[])
{
string mission_file;
string run_command = argv[0];
for(int i=1; i<argc; i++) {
string argi = argv[i];
if((argi=="-v") || (argi=="--version") || (argi=="-version"))
showReleaseInfoAndExit();
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
showExampleConfigAndExit();
else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
showHelpAndExit();
else if((argi == "-i") || (argi == "--interface"))
showInterfaceAndExit();
else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
mission_file = argv[i];
else if(strBegins(argi, "--alias="))
run_command = argi.substr(8);
else if(i==2)
run_command = argi;
}
if(mission_file == "")
showHelpAndExit();
cout << termColor("green");
cout << "pBoardSupportComm launching as " << run_command << endl;
cout << termColor() << endl;
BoardSupportComm BoardSupportComm;
BoardSupportComm.Run(run_command.c_str(), mission_file.c_str());
return(0);
}

9
src/pBoardSupportComm/pBoardSupportComm.moos Normal file
View File

@@ -0,0 +1,9 @@
//------------------------------------------------
// pBoardSupportComm config block
ProcessConfig = pBoardSupportComm
{
AppTick = 4
CommsTick = 4
}