418 lines
13 KiB
C++
Executable File
418 lines
13 KiB
C++
Executable File
/*
|
||
* @Author: zjk 1553836110@qq.com
|
||
* @Date: 2023-10-12 09:52:27
|
||
* @LastEditors: zhaojingkui 1553836110@qq.com
|
||
* @LastEditTime: 2023-11-28 11:32:48
|
||
* @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp
|
||
* @Description:
|
||
*
|
||
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
|
||
*/
|
||
// #define DEBUG
|
||
#include"Emulator.hpp"
|
||
#include <exception>
|
||
#include"ACTable.h"
|
||
|
||
bool _ListenCB(void * pParam)
|
||
{
|
||
Emulator* pMe = (Emulator* )pParam;
|
||
return pMe->receiveUdpDate();
|
||
}
|
||
|
||
bool _ListenBSC(void* pParam)
|
||
{
|
||
_150server* pMe = (_150server*)pParam;
|
||
return pMe->listenInfo();
|
||
}
|
||
|
||
bool _Connect(void* pParam)
|
||
{
|
||
Emulator* pMe = (Emulator* )pParam;
|
||
return pMe->_150Connect();
|
||
}
|
||
|
||
bool Emulator::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.m_sKey;
|
||
string sval = msg.m_sVal;
|
||
double dval = msg.m_dfVal;
|
||
double dfT;
|
||
msg.IsSkewed(m_curr_time, &dfT);
|
||
// if(key == "DESIRED_RUDDER")
|
||
// {
|
||
// sendBuf[0] = dval;
|
||
// remus100.rudder = dval;
|
||
// }
|
||
// else if(key == "DESIRED_ELEVATOR")
|
||
// {
|
||
// sendBuf[1] = dval;
|
||
// remus100.elevator = dval;
|
||
// }
|
||
// else if(key == "DESIRED_THRUST")
|
||
// {
|
||
// sendBuf[2] = dval;
|
||
// remus100.thrust = dval;
|
||
// }
|
||
// else
|
||
if(key == "DESIRED_DEPTH")
|
||
sendBuf[3] = dval;
|
||
else if(key == "DESIRED_HEADING")
|
||
sendBuf[4] = dval;
|
||
else if(key == "DESIRED_SPEED")
|
||
sendBuf[5] = dval;
|
||
// reportRunWarning("UnKown var:" + key);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
bool Emulator::Iterate()
|
||
{
|
||
AppCastingMOOSApp::Iterate();
|
||
if(_150ServerThread.IsThreadRunning())
|
||
{ // 这里需要转换数据
|
||
if(p_150server_1._150cmd.helm_top_angle > 128)
|
||
sendBuf[0] = -(p_150server_1._150cmd.helm_top_angle - 128);
|
||
else
|
||
sendBuf[0] = p_150server_1._150cmd.helm_top_angle;
|
||
if(p_150server_1._150cmd.helm_right_angle > 128)
|
||
sendBuf[1] = -(p_150server_1._150cmd.helm_right_angle - 128);
|
||
else
|
||
sendBuf[1] = p_150server_1._150cmd.helm_right_angle;
|
||
sendBuf[2] = (uint8_t)p_150server_1._150cmd.thrust * 1525 / 100;
|
||
set150Info();
|
||
p_150server_1.postInfo();
|
||
isConnect = " 150 Server...";
|
||
}
|
||
else
|
||
isConnect = "No 150 Connect...";
|
||
udp->iSendMessageTo((void*)sendBuf, sendBufSize, matalb_port, matlab_host);
|
||
//postNodeRecordUpdate(prefix, record);
|
||
AppCastingMOOSApp::PostReport();
|
||
return true;
|
||
}
|
||
|
||
bool Emulator::OnConnectToServer()
|
||
{
|
||
registerVariables();
|
||
return true;
|
||
}
|
||
|
||
bool Emulator::OnStartUp()
|
||
{
|
||
AppCastingMOOSApp::OnStartUp();
|
||
//TODO: 添加初始状态设置
|
||
STRING_LIST sParams;
|
||
m_MissionReader.GetConfiguration(GetAppName(), sParams);
|
||
STRING_LIST::iterator p;
|
||
|
||
for(p=sParams.begin(); p!=sParams.end(); p++)
|
||
{
|
||
bool unhandled = false;
|
||
string orig = *p;
|
||
string line = *p;
|
||
string param = toupper(biteStringX(line, '='));
|
||
string value = line;
|
||
double dval = atof(value.c_str());
|
||
cout << param << endl;
|
||
if(param == "MATLAB_HOST")
|
||
{
|
||
matlab_host = value;
|
||
}
|
||
else if(param == "MATLAB_PORT")
|
||
{
|
||
matalb_port = (long int)dval;
|
||
}
|
||
else if(param == "LOCAL_PORT")
|
||
{
|
||
local_port = (long int)dval;
|
||
}
|
||
else if(param == "PREFIX")
|
||
{
|
||
setNonWhiteVarOnString(prefix, value);
|
||
}
|
||
else if(param == "START_X")
|
||
{
|
||
start_x = dval;
|
||
}
|
||
else if(param == "START_Y")
|
||
{
|
||
start_y = dval;
|
||
}
|
||
else if(param == "START_Z")
|
||
{
|
||
start_z = dval;
|
||
}
|
||
else if(param == "START_HEADING")
|
||
{
|
||
start_heading = dval;
|
||
}
|
||
else
|
||
unhandled = true;
|
||
if(unhandled)
|
||
reportUnhandledConfigWarning(orig);
|
||
}
|
||
//添加UDP 设置
|
||
//TODO：添加try
|
||
try
|
||
{
|
||
udp = new XPCUdpSocket(local_port);
|
||
udp->vSetBroadcast(true);
|
||
udp->vBindSocket();
|
||
}
|
||
catch(const std::exception& e)
|
||
{
|
||
std::cerr << e.what() << '\n';
|
||
reportRunWarning("Local port Open Field");
|
||
}
|
||
//初始化数据
|
||
receiveBufSize = receiveDateSize*sizeof(double);
|
||
receiveBuf = new char[receiveBufSize];
|
||
sendBufSize = sendDataSize*sizeof(double);
|
||
sendBuf = new double[sendDataSize];
|
||
for(int i=0; i<sendDataSize; i++)
|
||
sendBuf[i] = 0;
|
||
for(int i=0; i<receiveBufSize; i++)
|
||
receiveBuf[i] = 0;
|
||
// sendBuf[0] = 0.1;
|
||
// sendBuf[1] = 0.1;
|
||
// sendBuf[2] = 1525;
|
||
|
||
record.setX(start_x);
|
||
record.setY(start_y);
|
||
record.setDepth(start_z);
|
||
record.setHeading(start_heading);
|
||
|
||
receiveThread.Initialise(_ListenCB,this);
|
||
receiveThread.Start();
|
||
|
||
_150ConnectThread.Initialise(_Connect, this);
|
||
_150ConnectThread.Start();
|
||
//启动150 server
|
||
//
|
||
//
|
||
return true;
|
||
|
||
}
|
||
bool Emulator::_150Connect()
|
||
{
|
||
cout << "Connect ... " << endl;
|
||
isConnect = "Connect...";
|
||
p_150server_1._150_startServer();
|
||
_150ServerThread.Initialise(_ListenBSC,&(this->p_150server_1));
|
||
_150ServerThread.Start();
|
||
_150ConnectThread.RequestQuit();
|
||
isConnect = "Connected and start server";
|
||
return true;
|
||
}
|
||
void Emulator::registerVariables()
|
||
{
|
||
AppCastingMOOSApp::RegisterVariables();
|
||
Register("DESIRED_RUDDER",0);
|
||
Register("DESIRED_THRUST",0);
|
||
Register("DESIRED_ELEVATOR",0);
|
||
Register("DESIRED_DEPTH");
|
||
Register("DESIRED_HEADING");
|
||
Register("DESIRED_SPEED");
|
||
}
|
||
bool Emulator::buildReport()
|
||
{
|
||
// for(int i=0; i<sendDataSize; i++)
|
||
// {
|
||
// m_msgs << sendBuf[i] << "!#";
|
||
// }
|
||
m_msgs << "MATLAB : " << matlab_host << " : " << matalb_port << endl;
|
||
m_msgs << "LOCAL : " << local_port << endl;
|
||
m_msgs << "_150 Connect: " << isConnect << endl;
|
||
m_msgs << "_150 Cmd Recive: " << p_150server_1._150_recive << endl;
|
||
m_msgs << "=============================================" << endl;
|
||
double *s = (double*)receiveBuf;
|
||
m_msgs << "DESIRED_RUDDER : " << sendBuf[0] << " (deg)" << endl;
|
||
m_msgs << "DESIRED_ELEVATOR : " << sendBuf[1] << " (deg)" << endl;
|
||
m_msgs << "DESIRED_THRUST : " << sendBuf[2] << " (rpm)" << endl;
|
||
|
||
m_msgs << "Pose : " << "[ " << doubleToStringX(s[6],1) << " , " << doubleToStringX(s[7],1) << " ]" << endl;
|
||
m_msgs << "Depth : " << doubleToStringX(s[8],1) << " (m)" << endl;
|
||
|
||
m_msgs << "u : " << doubleToStringX(s[0],2) << " (m/s)" << endl;
|
||
m_msgs << "v : " << doubleToStringX(s[1],2) << " (m/s)" << endl;
|
||
m_msgs << "w : " << doubleToStringX(s[2],2) << " (m/s)" << endl;
|
||
|
||
m_msgs << "p : " << doubleToStringX(s[3],1) << " (deg/s)" << endl;
|
||
m_msgs << "q : " << doubleToStringX(s[4],1) << " (deg/s)" << endl;
|
||
m_msgs << "r : " << doubleToStringX(s[5],1) << " (deg/s)" << endl;
|
||
|
||
m_msgs << "roll : " << doubleToStringX(s[9],1) << " (deg)" << endl;
|
||
m_msgs << "pitch : " << doubleToStringX(s[10],1) << " (deg)" << endl;
|
||
m_msgs << "yaw : " << doubleToStringX(s[11],1) << " (deg)" << endl;
|
||
|
||
m_msgs << "Lat : " << doubleToStringX(s[12],5) << " (du)" << endl;
|
||
m_msgs << "Lon : " << doubleToStringX(s[13],5) << " (du)" << endl;
|
||
m_msgs << "Alt : " << doubleToStringX(s[14],5) << " (m)" << endl;
|
||
|
||
|
||
return(true);
|
||
}
|
||
|
||
void Emulator::postNodeRecordUpdate(std::string, const NodeRecord& record)
|
||
{
|
||
double nav_x = record.getX();
|
||
double nav_y = record.getY();
|
||
|
||
Notify(prefix+"_X", nav_x, m_curr_time);
|
||
Notify(prefix+"_Y", nav_y, m_curr_time);
|
||
|
||
if(geoOk) {
|
||
double nav_lat = record.getLat();
|
||
double nav_lon = record.getLon();
|
||
Notify(prefix+"_LAT", nav_lat, m_curr_time);
|
||
Notify(prefix+"_LONG", nav_lon, m_curr_time);
|
||
}
|
||
|
||
double new_speed = record.getSpeed();
|
||
new_speed = snapToStep(new_speed, 0.01);
|
||
|
||
Notify(prefix+"_HEADING", record.getHeading(), m_curr_time);
|
||
Notify(prefix+"_SPEED", new_speed, m_curr_time);
|
||
Notify(prefix+"_DEPTH", record.getDepth(), m_curr_time);
|
||
|
||
// Added by HS 120124 to make it work ok with iHuxley
|
||
// Notify("SIMULATION_MODE","TRUE", m_curr_time);
|
||
Notify(prefix+"_Z", -record.getDepth(), m_curr_time);
|
||
Notify(prefix+"_PITCH", record.getPitch(), m_curr_time);
|
||
Notify(prefix+"_YAW", record.getYaw(), m_curr_time);
|
||
Notify("TRUE_X", nav_x, m_curr_time);
|
||
Notify("TRUE_Y", nav_y, m_curr_time);
|
||
|
||
double hog = angle360(record.getHeadingOG());
|
||
double sog = record.getSpeedOG();
|
||
|
||
Notify(prefix+"_HEADING_OVER_GROUND", hog, m_curr_time);
|
||
Notify(prefix+"_SPEED_OVER_GROUND", sog, m_curr_time);
|
||
|
||
if(record.isSetAltitude())
|
||
Notify(prefix+"_ALTITUDE", record.getAltitude(), m_curr_time);
|
||
}
|
||
|
||
bool Emulator::receiveUdpDate()
|
||
{
|
||
double u,v,w;
|
||
double U;
|
||
while(!receiveThread.IsQuitRequested())
|
||
{
|
||
int iRx = udp->iRecieveMessage(receiveBuf,receiveBufSize);
|
||
if(iRx)
|
||
{
|
||
for(int i=0; i<receiveDateSize; i++)
|
||
{
|
||
// MOOSTrace(doubleToString(*((double*)receiveBuf+i))+"\n");
|
||
double data = *((double*)receiveBuf+i);
|
||
switch (i)
|
||
{
|
||
case 0: //u
|
||
u = data;
|
||
remus100.u = u*100;
|
||
break;
|
||
case 1: //v
|
||
// record.set
|
||
v = data;
|
||
remus100.v = v*100;
|
||
break;
|
||
case 2: //w
|
||
// record
|
||
w = data;
|
||
remus100.w = w*100;
|
||
U = sqrt(u*u + v*v + w*w);
|
||
record.setSpeed(U);
|
||
break;
|
||
case 3: //p
|
||
remus100.p = data*100;
|
||
break;
|
||
case 4: //q
|
||
remus100.q = data*100;
|
||
break;
|
||
case 5: //r
|
||
remus100.r = data*100;
|
||
break;
|
||
case 6: //x
|
||
remus100.x = data*100;
|
||
record.setY(data);
|
||
break;
|
||
case 7: //y
|
||
remus100.y = data*100;
|
||
record.setX(data);
|
||
break;
|
||
case 8: //z
|
||
remus100.z = data*100;
|
||
record.setDepth(data);
|
||
break;
|
||
case 9: //phi
|
||
remus100.roll = data*100;
|
||
break;
|
||
case 10: //theta
|
||
remus100.pitch = data*100;
|
||
record.setPitch(data);
|
||
break;
|
||
case 11: //psi
|
||
remus100.yaw = data*100;
|
||
record.setHeading(data);
|
||
break;
|
||
case 12:
|
||
remus100.lat = data;
|
||
record.setLat(data);
|
||
break;
|
||
case 13:
|
||
remus100.lon = data;
|
||
record.setLon(data);
|
||
break;
|
||
case 14:
|
||
remus100.alt = data;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void Emulator::set150Info()
|
||
{
|
||
p_150server_1.embeddedInfoSrc.header = 0xEBA1; //0:[0,1]
|
||
p_150server_1.embeddedInfoSrc.count = (uint16_t)m_iteration; //2:[2,3]
|
||
p_150server_1.embeddedInfoSrc.size = (uint8_t)51; //3:[4]
|
||
p_150server_1.embeddedInfoSrc.drive_mode = (uint8_t)0xFF; //4:[5]
|
||
p_150server_1.embeddedInfoSrc.height = (uint16_t)(150-remus100.z); //5:[6,7]
|
||
p_150server_1.embeddedInfoSrc.depth = (uint16_t)remus100.z; //6:[8,9]
|
||
p_150server_1.embeddedInfoSrc.yaw = (uint16_t)remus100.yaw; //7:[10,11]
|
||
p_150server_1.embeddedInfoSrc.pitch = (int16_t)remus100.pitch; //8:[12,13]
|
||
p_150server_1.embeddedInfoSrc.roll = (int16_t)remus100.roll; //9:[14,15]
|
||
p_150server_1.embeddedInfoSrc.ins_vx = (int16_t)remus100.u; //10:[16,17]
|
||
p_150server_1.embeddedInfoSrc.ins_vy = (int16_t)remus100.v; //11:[18,19]
|
||
p_150server_1.embeddedInfoSrc.ins_vz = (int16_t)remus100.w; //12:[20,21]
|
||
p_150server_1.embeddedInfoSrc.lon = (int32_t)(remus100.lon * 1000000); //13:[22,23,24,25]
|
||
p_150server_1.embeddedInfoSrc.lat = (int32_t)(remus100.lat * 1000000) ; //14:[26,27,28,29]
|
||
p_150server_1.embeddedInfoSrc.alt = (int16_t)(remus100.alt * 100);//15:[30,31]
|
||
p_150server_1.embeddedInfoSrc.dvl_vx = (int16_t)remus100.u; //16:[32,33]
|
||
p_150server_1.embeddedInfoSrc.dvl_vy = (int16_t)remus100.v; //17:[34,35]
|
||
p_150server_1.embeddedInfoSrc.dvl_vz = (int16_t)remus100.w; //18:[36,37]
|
||
p_150server_1.embeddedInfoSrc.rpm = (int16_t)remus100.thrust; //19:[38,39]
|
||
p_150server_1.embeddedInfoSrc.lightEnable = (uint8_t)0x01; //20:[40]
|
||
p_150server_1.embeddedInfoSrc.battery_voltage = (uint16_t)30; //21:[41,42]
|
||
p_150server_1.embeddedInfoSrc.battery_level = 60; //22:[43]
|
||
p_150server_1.embeddedInfoSrc.battery_temp = 21.3 / 0.1; //23:[44,45]
|
||
p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x0; //24:[46,47,48,49]
|
||
p_150server_1.embeddedInfoSrc.fault_battery = 0x0; //25:[50]
|
||
p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0x0; //26:[51]
|
||
p_150server_1.embeddedInfoSrc.fault_thrust = 0x0; //27:[52]
|
||
p_150server_1.embeddedInfoSrc.iridium = 0xFF; //28:[53]
|
||
p_150server_1.embeddedInfoSrc.throwing_load = 0xFF; //29:[54]
|
||
p_150server_1.embeddedInfoSrc.dvl_status = 0x00; //30:[55]
|
||
p_150server_1.embeddedInfoSrc.crc = 0XFF; //31:[56]
|
||
p_150server_1.embeddedInfoSrc.footer = 0XEE1A; //32:[57,58]
|
||
} |