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

1
src/pEmulator/.LastOpenedMOOSLogDirectory Normal file
View File

@@ -0,0 +1 @@
LastOpenedLoggingDirectory=./MOOSLog_24_10_2023_____11_46_34

21
src/pEmulator/CMakeLists.txt Normal file
View File

@@ -0,0 +1,21 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pTaskManger
# Author(s): zjk
#--------------------------------------------------------
# set(INCLUDE_IVP /home/zjk/Software/moos-ivp/include/ivp)
FILE(GLOB SRC *.cpp *.hpp)
ADD_EXECUTABLE(pEmulator ${SRC})
#需要把被依赖的库放到依赖库的后面
TARGET_LINK_LIBRARIES(pEmulator
${MOOS_LIBRARIES}
${MOOSGeodesy_LIBRARIES}
contacts
geometry
apputil
mbutil
m
pthread
)

416
src/pEmulator/Emulator.cpp Normal file
View File

@@ -0,0 +1,416 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-21 15:28:32
* @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";
}
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;
}
}
}
}
}
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)1024; //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 = 0x80; //24:[46,47,48,49]
p_150server_1.embeddedInfoSrc.fault_battery = 0x2E; //25:[50]
p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0xAA; //26:[51]
p_150server_1.embeddedInfoSrc.fault_thrust = 0x76; //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]
}

97
src/pEmulator/Emulator.hpp Normal file
View File

@@ -0,0 +1,97 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 15:57:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-10 08:34:14
* @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef __EMULATOR_H
#define __EMULATOR_H
#define UNIX
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include"MOOS/libMOOS/Comms/MulticastNode.h"
#include <iostream>
#include<vector>
#include "VarDataPair.h"
#include "MBUtils.h"
#include "AngleUtils.h"
#include <list>
#include"MOOS/libMOOS/Comms/XPCUdpSocket.h"
#include "NodeRecord.h"
#include "MOOS/libMOOS/Utils/MOOSThread.h"
#include "_150server.hpp"
using namespace std;
typedef struct uuv
{
double u;
double v;
double w;
double p;
double q;
double r;
double x;
double y;
double z;
double roll;
double pitch;
double yaw;
double thrust;
double rudder;
double elevator;
double lon;
double lat;
double alt;
};
class Emulator : public AppCastingMOOSApp
{
public:
Emulator(){};
~Emulator(){delete receiveBuf;delete sendBuf;};
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void registerVariables();
bool buildReport();
void postNodeRecordUpdate(std::string, const NodeRecord&);
bool receiveUdpDate();
void set150Info();
bool _150Connect();
private:
long int local_port = 8080;
long int matalb_port = 8085;
string matlab_host = "127.0.0.1";
XPCUdpSocket* udp;
unsigned int receiveDateSize = 15;
unsigned int sendDataSize = 6;
unsigned int receiveBufSize;
unsigned int sendBufSize;
char *receiveBuf;
double *sendBuf;
NodeRecord record;
string prefix="NAV";
bool geoOk = false;
//UUV init state
double start_x = 0;
double start_y = 0;
double start_z = 0;
double start_heading = 0;
uuv remus100;
string isConnect = "";
//
CMOOSThread receiveThread;
CMOOSThread _150ServerThread;
CMOOSThread _150ConnectThread;
//150 Server
_150server p_150server_1;
};
#endif

206
src/pEmulator/_150server.cpp Normal file
View File

@@ -0,0 +1,206 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-11-07 14:59:47
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 09:19:46
* @FilePath: /moos-ivp-pi/src/pEmulator/_150server.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include "_150server.hpp"
uint16_t _150server::serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr)
{
memcpy(bfr, &(embeddedInfo.header), sizeof(embeddedInfo.header));
bfr += sizeof(embeddedInfo.header);
memcpy(bfr, &(embeddedInfo.count), sizeof(embeddedInfo.count));
bfr += sizeof(embeddedInfo.count);
memcpy(bfr, &(embeddedInfo.size), sizeof(embeddedInfo.size));
bfr += sizeof(embeddedInfo.size);
memcpy(bfr, &(embeddedInfo.drive_mode), sizeof(embeddedInfo.drive_mode));
bfr += sizeof(embeddedInfo.drive_mode);
memcpy(bfr, &(embeddedInfo.height), sizeof(embeddedInfo.height));
bfr += sizeof(embeddedInfo.height);
memcpy(bfr, &(embeddedInfo.depth), sizeof(embeddedInfo.depth));
bfr += sizeof(embeddedInfo.depth);
memcpy(bfr, &(embeddedInfo.yaw), sizeof(embeddedInfo.yaw));
bfr += sizeof(embeddedInfo.yaw);
memcpy(bfr, &(embeddedInfo.pitch), sizeof(embeddedInfo.pitch));
bfr += sizeof(embeddedInfo.pitch);
memcpy(bfr, &(embeddedInfo.roll), sizeof(embeddedInfo.roll));
bfr += sizeof(embeddedInfo.roll);
memcpy(bfr, &(embeddedInfo.ins_vx), sizeof(embeddedInfo.ins_vx));
bfr += sizeof(embeddedInfo.ins_vx);
memcpy(bfr, &(embeddedInfo.ins_vy), sizeof(embeddedInfo.ins_vy));
bfr += sizeof(embeddedInfo.ins_vy);
memcpy(bfr, &(embeddedInfo.ins_vz), sizeof(embeddedInfo.ins_vz));
bfr += sizeof(embeddedInfo.ins_vz);
memcpy(bfr, &(embeddedInfo.lon), sizeof(embeddedInfo.lon));
bfr += sizeof(embeddedInfo.lon);
memcpy(bfr, &(embeddedInfo.lat), sizeof(embeddedInfo.lat));
bfr += sizeof(embeddedInfo.lat);
memcpy(bfr, &(embeddedInfo.alt), sizeof(embeddedInfo.alt));
bfr += sizeof(embeddedInfo.alt);
memcpy(bfr, &(embeddedInfo.dvl_vx), sizeof(embeddedInfo.dvl_vx));
bfr += sizeof(embeddedInfo.dvl_vx);
memcpy(bfr, &(embeddedInfo.dvl_vy), sizeof(embeddedInfo.dvl_vy));
bfr += sizeof(embeddedInfo.dvl_vy);
memcpy(bfr, &(embeddedInfo.dvl_vz), sizeof(embeddedInfo.dvl_vz));
bfr += sizeof(embeddedInfo.dvl_vz);
memcpy(bfr, &(embeddedInfo.rpm), sizeof(embeddedInfo.rpm));
bfr += sizeof(embeddedInfo.rpm);
memcpy(bfr, &(embeddedInfo.lightEnable), sizeof(embeddedInfo.lightEnable));
bfr += sizeof(embeddedInfo.lightEnable);
memcpy(bfr, &(embeddedInfo.battery_voltage), sizeof(embeddedInfo.battery_voltage));
bfr += sizeof(embeddedInfo.battery_voltage);
memcpy(bfr, &(embeddedInfo.battery_level), sizeof(embeddedInfo.battery_level));
bfr += sizeof(embeddedInfo.battery_level);
memcpy(bfr, &(embeddedInfo.battery_temp), sizeof(embeddedInfo.battery_temp));
bfr += sizeof(embeddedInfo.battery_temp);
memcpy(bfr, &(embeddedInfo.fault_leakSensor), sizeof(embeddedInfo.fault_leakSensor));
bfr += sizeof(embeddedInfo.fault_leakSensor);
memcpy(bfr, &(embeddedInfo.fault_battery), sizeof(embeddedInfo.fault_battery));
bfr += sizeof(embeddedInfo.fault_battery);
memcpy(bfr, &(embeddedInfo.fault_emergencyBattery), sizeof(embeddedInfo.fault_emergencyBattery));
bfr += sizeof(embeddedInfo.fault_emergencyBattery);
memcpy(bfr, &(embeddedInfo.fault_thrust), sizeof(embeddedInfo.fault_thrust));
bfr += sizeof(embeddedInfo.fault_thrust);
memcpy(bfr, &(embeddedInfo.iridium), sizeof(embeddedInfo.iridium));
bfr += sizeof(embeddedInfo.iridium);
memcpy(bfr, &(embeddedInfo.throwing_load), sizeof(embeddedInfo.throwing_load));
bfr += sizeof(embeddedInfo.throwing_load);
memcpy(bfr, &(embeddedInfo.dvl_status), sizeof(embeddedInfo.dvl_status));
bfr += sizeof(embeddedInfo.dvl_status);
memcpy(bfr, &(embeddedInfo.crc), sizeof(embeddedInfo.crc));
bfr += sizeof(embeddedInfo.crc);
memcpy(bfr, &(embeddedInfo.footer), sizeof(embeddedInfo.footer));
bfr += sizeof(embeddedInfo.footer);
}
void _150server::postInfo()
{
bzero(embeddedBuffer, 0);
serializeFields(embeddedInfoSrc, embeddedBuffer);
write(cfd, embeddedBuffer, 59);
};
bool _150server::listenInfo()
{
while (1)
{
int lens = read(cfd, recvbuf, sizeof(recvbuf));
if(lens>0)
{
_150_recive++;
uint16_t* buf16;
printf("header: %u, %u\n", recvbuf[0], recvbuf[1]);
printf("count: %u\n", recvbuf[2], recvbuf[3]);
printf("size: %u\n", recvbuf[4]);
printf("drive_mode: %u\n", recvbuf[5]);
printf("thrust: %u\n", recvbuf[6]);
_150cmd.thrust = (double)recvbuf[6];
printf("yaw: %u, %u\n", recvbuf[7], recvbuf[8]);
memcpy(buf16, &recvbuf[7], 2*sizeof(recvbuf[7]));
_150cmd.yaw = *((double*)buf16);
printf("depth: %u, %u\n", recvbuf[9], recvbuf[10]);
memcpy(buf16, &recvbuf[9], 2*sizeof(recvbuf[9]));
_150cmd.depth = *((double*)buf16);
printf("helm_top_angle: %u\n", recvbuf[11]);
_150cmd.helm_top_angle = (double)recvbuf[11];
printf("helm_bottom_angle: %u\n", recvbuf[12]);
_150cmd.helm_bottom_angle = (double)recvbuf[12];
_150cmd.helm_left_angle = (double)recvbuf[13];
printf("helm_left_angle: %u\n", recvbuf[13]);
_150cmd.helm_right_angle = (double)recvbuf[14];
printf("helm_right_angle: %u\n", recvbuf[14]);
_150cmd.light_enable = (double)recvbuf[15];
printf("light_enable: %u\n", recvbuf[15]);
printf("dvl_enable: %u\n", recvbuf[16]);
_150cmd.dvl_enable = (double)recvbuf[16];
printf("throwing_load_enable: %u\n", recvbuf[17]);
_150cmd.throwing_load_enable = (double)recvbuf[16];
printf("crc: %u\n", recvbuf[18]);
_150cmd.crc = (double)recvbuf[18];
printf("footer: %u, %u\n", recvbuf[19], recvbuf[20]);
memcpy(buf16, &recvbuf[19], 2*sizeof(recvbuf[19]));
_150cmd.footer = *((double*)buf16);
}
}
return true;
}
void _150server::_150_startServer()
{
std::cout << "--------------------" << std::endl;
lfd = socket(AF_INET, SOCK_STREAM, 0);
if(lfd==-1)
{
perror("socket");
exit(-1);
}
//2.绑定
struct sockaddr_in saddr;
saddr.sin_family = PF_INET;
saddr.sin_addr.s_addr = INADDR_ANY; //0.0.0.0
saddr.sin_port = htons(8001);
int ret = bind(lfd, (struct sockaddr *)&saddr, sizeof(saddr));
if(ret == -1)
{
perror("bind");
exit(-1);
}
//3.监听
listen(lfd, 5);
if(ret==-1)
{
perror("listen");
exit(-1);
}
//4.接受客户端连接
struct sockaddr_in caddr;
socklen_t len = sizeof(caddr);
cfd = accept(lfd, (struct sockaddr *)&caddr, &len);
if(cfd==-1)
{
perror("accept");
exit(-1);
}
std::cout << "--------------------" << std::endl;
//输出客户端的信息
char cip[16];
inet_ntop(AF_INET, &caddr.sin_addr.s_addr, cip, sizeof(cip));
unsigned short cport = ntohs(caddr.sin_port);
printf("client ip is %s,port is %d\n", cip, cport);
}

96
src/pEmulator/_150server.hpp Normal file
View File

@@ -0,0 +1,96 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-11-07 14:59:36
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-07 17:12:30
* @FilePath: /moos-ivp-pi/src/pEmulator/_150server.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef __150SERVER_H
#define __150SERVER_H
#include <stdio.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <sstream>
#include <iostream>
struct AUVEmbedded
{
uint16_t header; //1:[0,1] hx
uint16_t count; //2:[2,3] hu
uint8_t size; //3:[4] u
uint8_t drive_mode; //4:[5] u
uint16_t height; //5:[6,7] hu
uint16_t depth; //6:[8,9] hu
uint16_t yaw; //7:[10,11] hu
int16_t pitch; //8:[12,13] hd
int16_t roll; //9:[14,15] hd
int16_t ins_vx; //10:[16,17] hd
int16_t ins_vy; //11:[18,19] hd
int16_t ins_vz; //12:[20,21] hd
int32_t lon; //13:[22,23,24,25] d
int32_t lat; //14:[26,27,28,29] d
int16_t alt; //15:[30,31] hd
int16_t dvl_vx; //16:[32,33] hd
int16_t dvl_vy; //17:[34,35] hd
int16_t dvl_vz; //18:[36,37] hd
int16_t rpm; //19:[38,39] hd
uint8_t lightEnable; //20:[40] u
uint16_t battery_voltage; //21:[41,42] hu
uint8_t battery_level; //22:[43] u
uint16_t battery_temp; //23:[44,45] hu
uint32_t fault_leakSensor; //24:[46,47,48,49] u
uint8_t fault_battery; //25:[50] u
uint8_t fault_emergencyBattery; //26:[51] u
uint8_t fault_thrust; //27:[52] u
uint8_t iridium; //28:[53] u
uint8_t throwing_load; //29:[54] u
uint8_t dvl_status; //30:[55] u
uint8_t crc; //31:[56] u
uint16_t footer; //32:[57,58] hu
};
struct AUVCmd
{
uint16_t header = 0xEBA2;
uint16_t count;
uint8_t size;
uint8_t mode = 0XFF;
uint8_t thrust;
uint16_t yaw;
uint16_t depth;
uint8_t helm_top_angle;
uint8_t helm_bottom_angle;
uint8_t helm_left_angle;
uint8_t helm_right_angle;
uint8_t light_enable;
uint8_t dvl_enable = 0XFF;
uint8_t throwing_load_enable = 0XFF;
uint8_t crc;
uint16_t footer = 0XEE2A;
};
class _150server
{
private:
/* data */
int lfd;
int cfd;
public:
_150server(/* args */){};
~_150server(){};
uint16_t serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr);
bool listenInfo();
void postInfo();
void _150_startServer();
AUVEmbedded embeddedInfoSrc;
AUVCmd _150cmd;
unsigned char embeddedBuffer[59];
unsigned char recvbuf[65535] = {0};
unsigned int _150_recive = 0;
};
#endif

249
src/pEmulator/a.moos Normal file
View File

@@ -0,0 +1,249 @@
//-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = alpha
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pEmulator @ NewConsole = true
//Run = pLogger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
}
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
}
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
matlab_host = 192.168.0.11
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 10
start_y = 9
start_z = 1
start_heading = 30
}
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
//Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
//Log = REPORT @ 0 NOSYNC
//Log = BHV_SETTINGS @ 0 NOSYNC
Log = OPREGION_RESET @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
}
//------------------------------------------
// uSimMarineV22 config block
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMotionControler
{
AppTick = 10
CommsTick = 10
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 10
yaw_pid_kd = 0.0
yaw_pid_ki = 0.01
yaw_pid_integral_limit = 10
// Speed PID controller
speed_pid_kp = 20.0
speed_pid_kd = 0.0
speed_pid_ki = 1
speed_pid_integral_limit = 100
maxpitch = 15
maxelevator = 30
pitch_pid_kp = 1.5
pitch_pid_kd = 0
pitch_pid_ki = 1.0
pitch_pid_integral_limit = 0
z_to_pitch_pid_kp = 0.12
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.004
z_to_pitch_pid_integral_limit = 0.05
//MAXIMUMS MAXRUDDER
maxrudder = 30
maxthrust = 1525
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
//button_one = START # START=true
//button_one = MOOS_MANUAL_OVERRIDE=false
button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
button_two = STOP # uMission_action_cmd={"taskName":"east_waypt_survey","action":"stop"}
//button_two = MOOS_MANUAL_OVERRIDE=true
button_three = FaultClear # ClearFalut = true
button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
//platform_type = kayak
//更改显示形状为uuv
platform_type = UUV
platform_color = red
platform_length = 4
}

181
src/pEmulator/alpha.bhv Normal file
View File

@@ -0,0 +1,181 @@
//-------- FILE: alpha.bhv -------------
initialize RUN = false
initialize TaskNum=t1
initialize SendTask=false
//--------------模式判断------------------------
set MODE = ACTIVE{
RUN=true
} INACTIVE
set MODE = T1{
MODE=ACTIVE
TaskNum = t1
}
//----------路径点任务----------------------------
Behavior = BHV_Waypoint
{
name = waypt_survey
pwt = 100 //优先权重
condition = MODE==T1
//endflag = START=false
endflag = END_WayPoint=true
configflag = CRUISE_SPD = $[SPEED]
//configflag = OSPOS = $[OSX],$[OSY]
activeflag = INFO=$[OWNSHIP]
activeflag = INFO=$[BHVNAME]
activeflag = INFO=$[BHVTYPE]
//cycleflag = CINFO=$[OSX],$[OSY]
wptflag = CurrentPointComplete=true
wptflag = PREV=$(PX),$(PY)
wptflag = NEXT=$(NX),$(NY)
wptflag = TEST=$(X),$(Y)
wptflag = OSPOS=$(OSX),$(OSY)
//wptflag_on_start = true
updates = WPT_UPDATE
//perpetual = true
templating = spawn
// speed_alt = 1.2
//use_alt_speed = true
lead = 8
lead_damper = 1
lead_to_start = false
speed = 1 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
polygon = 60,-40
order = normal
//repeat = 3
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//--------------定深任务------------------
Behavior=BHV_ConstantDepth
{
name = const_depth
pwt = 100
//condition = DEPLOY = true
condition = MODE==T1
duration = no-time-limit
updates = DEPTH_UPDATE
depth = 0
}
//--------------定向任务--------------------
Behavior=BHV_ConstantHeading
{
name = const_heading
pwt = 100
//condition = START_TURN = true
//condition = DEPLOY = true
condition = MODE==T3
perpetual = true
activeflag = TURN = started
//endflag = TURN = done
//endflag = RETURN = true
//endflag = START_TURN = false
endflag = START=false
heading = 225
complete_thresh = 5
duration = no-time-limit
}
//--------------定速任务--------------------
Behavior=BHV_ConstantSpeed
{
name = const_speed
pwt = 1000
condition = MODE==T1
perpetual = true
updates = SPEED_UPDATE
//endflag = START=false
speed = 5
duration = no-time-limit
//peakwidth = 0.5
//basewidth = 0.5
}
//----------------安全模式-----------------------
//----------------计时器---------------------
Behavior = BHV_Timer
{
name = mtime
condition = MODE==T1
pwt = 100
templating = spawn
//duration_status = MSTATUS
//duration = 10
endflag = TIME_OUT=true
updates = TIMER_UPDATES
//perpetual = true
}
//-------------最大深度限制--------------------------
Behavior = BHV_MaxDepth
{
name = maxdepth
pwt = 200
condition = MODE==ACTIVE
updates = MAXDEEP_UPDATES
max_depth = 20
tolerance = 0
duration = no-time-limit
}
//--------------安全区域设置-----------------------
Behavior = BHV_OpRegion
{
// General Behavior Parameters
// ---------------------------
name = op_region // example
pwt = 300 // default
condition = MODE==TN
updates = OPREGION_UPDATES // example
// Parameters specific to this behavior
// ------------------------------------
max_time = 20 // default (seconds)
max_depth = 25 // default (meters)
min_altitude = 0 // default (meters)
reset_var = OPREGION_RESET // example
trigger_entry_time = 1 // default (seconds)
trigger_exit_time = 0.5 // default (seconds)
polygon = pts={-80,-00:-30,-175:150,-100:95,25}
breached_altitude_flag = TaskFault = AltitudeOut
breached_depth_flag = TaskFault = DepthOut
breached_poly_flag = TaskFault = RegionOut
breached_time_flag = TaskFault = TimeOut
visual_hints = vertex_color = brown // default
visual_hints = vertex_size = 3 // default
visual_hints = edge_color = aqua // default
visual_hints = edge_size = 1 // default
}

63
src/pEmulator/main.cpp Normal file
View File

@@ -0,0 +1,63 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:06
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-10-20 16:52:52
* @FilePath: /moos-ivp-extend/src/pEmulator/main.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include<iostream>
using namespace std;
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
// #include "VarDataPair.h"
#include"Emulator.hpp"
#include <iostream>
#include<string>
#include "MBUtils.h"
#include "ColorParse.h"
// #include "MBUtils.h"
// #include "ColorParse.h"
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();
cout << " Version : 0.01" << endl;
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
// showExampleConfigAndExit();
cout << " example : NULL" << endl;
else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
// showHelpAndExit();
cout << " Low leave Control for UUV " << endl;
else if((argi=="-i") || (argi == "--interface"))
// showInterfaceAndExit();
cout << " UUV Motion Control " << endl;
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 << "Emulator launching as " << run_command << endl;
cout << termColor() << endl;
Emulator s;
s.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
return(0);
return(0);
}

15
src/pEmulator/pEmulator.moos Normal file
View File

@@ -0,0 +1,15 @@
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
//matlab_host = 192.168.0.11
matlab_host = 10.25.0.206
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 0
start_y = 0
start_z = 0
start_heading = 30
}