迁移分支
This commit is contained in:
zengxiaobin
892
src/pBoardSupportComm/BoardSupportComm.cpp
Normal file
892
src/pBoardSupportComm/BoardSupportComm.cpp
Normal 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));
|
||||
}
|
||||
Reference in New Issue
Block a user