863 lines
30 KiB
C++
Executable File
863 lines
30 KiB
C++
Executable File
/************************************************************/
|
|
/* 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
|
|
//#define TRUE_AUV
|
|
|
|
using namespace std;
|
|
|
|
BoardSupportComm::BoardSupportComm()
|
|
{
|
|
|
|
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 = 0x02; //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;
|
|
|
|
embeddedInfo.header = 0xEBA1;
|
|
embeddedInfo.count = 0;
|
|
tcpConnectRet = -1;
|
|
tcpSockFD = -1;
|
|
recvLen = -1;
|
|
}
|
|
|
|
//---------------------------------------------------------
|
|
// Destructor
|
|
BoardSupportComm::~BoardSupportComm()
|
|
{
|
|
close(tcpSockFD);
|
|
}
|
|
|
|
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
|
|
embeddedInfo.east = (int)dval;
|
|
}
|
|
if(key == "NAV_Y")
|
|
{
|
|
//N->E
|
|
embeddedInfo.north = (int)dval;
|
|
}
|
|
if(key == "NAV_Z")
|
|
{
|
|
//U->D
|
|
embeddedInfo.depth = -1 * dval * 100;
|
|
}
|
|
if(key == "NAV_YAW")
|
|
{
|
|
double yawTemp = dval; //radian
|
|
if (std::abs(yawTemp) <= M_PI)
|
|
{
|
|
if (yawTemp <= 0)
|
|
{
|
|
embeddedInfo.yaw = -yawTemp * 180 / M_PI * 100;
|
|
}
|
|
else
|
|
{
|
|
embeddedInfo.yaw = (2 * M_PI - yawTemp) * 180 / M_PI * 100;
|
|
}
|
|
}
|
|
}
|
|
if(key == "NAV_PITCH")
|
|
{
|
|
embeddedInfo.pitch = dval * 100;
|
|
}
|
|
if(key == "NAV_LAT")
|
|
{
|
|
embeddedInfo.lat = dval * 1000000;
|
|
}
|
|
if(key == "NAV_LONG")
|
|
{
|
|
embeddedInfo.lon = dval * 1000000;
|
|
}
|
|
if(key == "NAV_SPEED")
|
|
{
|
|
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")
|
|
{
|
|
embeddedInfo.depth = dval * 100;
|
|
}
|
|
#endif
|
|
if(key == "Fault_LeakSensor")
|
|
{
|
|
embeddedInfo.fault_leakSensor = (uint32_t)dval;
|
|
}
|
|
if(key == "Fault_Battery")
|
|
{
|
|
embeddedInfo.fault_battery = (uint8_t)dval;
|
|
}
|
|
if(key == "Fault_EmergencyBattery")
|
|
{
|
|
embeddedInfo.fault_emergencyBattery = (uint8_t)dval;
|
|
}
|
|
if(key == "Fault_Thrust")
|
|
{
|
|
embeddedInfo.fault_thrust = (uint8_t)dval;
|
|
}
|
|
|
|
if(key == "uManual_enable_cmd")
|
|
{
|
|
if (sval == "true")
|
|
{
|
|
executeCommand.manual_mode = true;
|
|
}
|
|
else
|
|
{
|
|
executeCommand.manual_mode = false;
|
|
|
|
}
|
|
}
|
|
|
|
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(sval);
|
|
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);
|
|
uint8_t helm_top_bottom_angle = convertIntToUchar((int)(recvCommand["Heading"].asFloat()), -30, 30);
|
|
executeCommand.depth = 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
|
|
{
|
|
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(sval);
|
|
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(sval);
|
|
Json::CharReaderBuilder builder;
|
|
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
|
|
if (!parsingResult)
|
|
{
|
|
std::cerr << "Failed to parse JSON string." << std::endl;
|
|
return false;
|
|
}
|
|
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)
|
|
{
|
|
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"] = embeddedInfo.yaw * 0.01 * M_PI / 180;
|
|
embeddedState["pitch"] = 0;
|
|
embeddedState["roll"] = 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;
|
|
embeddedState["rpm"] = 0;
|
|
embeddedState["lightEnable"] = 0;
|
|
embeddedState["batteryVoltage"] = 15;
|
|
embeddedState["batteryLevel"] = 60;
|
|
embeddedState["batteryTemp"] = 25;
|
|
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 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
|
|
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 = {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"]= embeddedInfo.refLon;
|
|
embeddedState["referenceLat"]= embeddedInfo.refLat;
|
|
embeddedState["referenceAltitude"]= embeddedInfo.refAlt;
|
|
|
|
Json::StreamWriterBuilder builder;
|
|
return Json::writeString(builder, embeddedState);
|
|
}
|
|
|
|
|
|
void BoardSupportComm::tcpProcessThread()
|
|
{
|
|
while(1)
|
|
{
|
|
bzero(tcpReceiveBuffer, 0);
|
|
recvLen = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
|
|
if(recvLen > 0)
|
|
{
|
|
#ifndef MOOS_AUV_SIM
|
|
parseMessage((unsigned char* )tcpReceiveBuffer, recvLen);
|
|
#endif
|
|
Json::Value embeddedState;
|
|
recvContent = convertEmbeddedFormat(embeddedState);
|
|
Notify("uDevice_monitor_fb", recvContent);
|
|
#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 << 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;
|
|
}
|
|
|
|
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"))
|
|
{
|
|
embeddedInfo.refLon = originJsonValue["LongOrigin"].asFloat();
|
|
embeddedInfo.refLat = originJsonValue["LatOrigin"].asFloat();
|
|
embeddedInfo.refAlt = originJsonValue["AltOrigin"].asFloat();
|
|
}
|
|
else
|
|
{
|
|
throw -1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
throw -1;
|
|
}
|
|
}
|
|
catch(...)
|
|
{
|
|
m_MissionReader.GetValue("LongOrigin", embeddedInfo.refLon);
|
|
m_MissionReader.GetValue("LatOrigin", embeddedInfo.refLat);
|
|
m_MissionReader.GetValue("AltOrigin", embeddedInfo.refAlt);
|
|
}
|
|
|
|
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));
|
|
}
|