diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..a988b70 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,56 @@ +{ + "files.associations": { + "numeric": "cpp", + "streambuf": "cpp", + "iostream": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "compare": "cpp", + "concepts": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "random": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp" + } +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..cb8b75d --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,28 @@ +{ + "tasks": [ + { + "type": "cppbuild", + "label": "C/C++: g++ 生成活动文件", + "command": "/usr/bin/g++", + "args": [ + "-fdiagnostics-color=always", + "-g", + "${file}", + "-o", + "${fileDirname}/${fileBasenameNoExtension}" + ], + "options": { + "cwd": "${fileDirname}" + }, + "problemMatcher": [ + "$gcc" + ], + "group": { + "kind": "build", + "isDefault": true + }, + "detail": "调试器生成的任务。" + } + ], + "version": "2.0.0" +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 10002ca..03a4b63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0) PROJECT( IVP_EXTEND ) set (CMAKE_CXX_STANDARD 11) +set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/project/lib/moos-ivp") #======================================================================= # Set the output directories for the binary and library files diff --git a/doc/a.md b/doc/a.md new file mode 100644 index 0000000..b5de242 --- /dev/null +++ b/doc/a.md @@ -0,0 +1,17 @@ +# 150 auv 软件结构 + + +# 150 auv 适配接口 + +## 发布变量 + +根据pNodeReporter模块订阅的变量，需要发布的变量如下 + +NAV_X: The ownship vehicle position on the x axis of local coordinates. +NAV_Y: The ownship vehicle position on the y axis of local coordinates. +NAV_LAT: The ownship vehicle position on the y axis of global coordinates. +NAV_LONG: The ownship vehicle position on the x axis of global coordinates. +NAV_HEADING: The ownship vehicle heading in degrees. +NAV_YAW: The ownship vehicle yaw in radians. +NAV_SPEED: The ownship vehicle speed in meters per second. +NAV_DEPTH: The ownship vehicle depth in meters. \ No newline at end of file diff --git a/missions/alder/alder.bhv b/missions/alder/alder.bhv index 08f2ddc..c63240f 100644 --- a/missions/alder/alder.bhv +++ b/missions/alder/alder.bhv @@ -1,33 +1,94 @@ -//-------- FILE: alder.bhv ------------- +//-------- FILE: alpha.bhv ------------- initialize DEPLOY = false initialize RETURN = false - + //---------------------------------------------- -Behavior = BHV_SimpleWaypoint +Behavior = BHV_Waypoint { - name = waypt_to_point + name = waypt_survey pwt = 100 condition = RETURN = false condition = DEPLOY = true endflag = RETURN = true - speed = 2.0 // meters per second - radius = 8.0 - ptx = 100 - pty = -50 + configflag = CRUISE_SPD = $[SPEED] + //configflag = OSPOS = $[OSX],$[OSY] + + activeflag = INFO=$[OWNSHIP] + activeflag = INFO=$[BHVNAME] + activeflag = INFO=$[BHVTYPE] + + cycleflag = CINFO=$[OSX],$[OSY] + + 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 + + speed_alt = 1.2 + use_alt_speed = true + lead = 8 + lead_damper = 1 + lead_to_start = true + speed = 12 // meters per second + capture_line = true + capture_radius = 5.0 + slip_radius = 15.0 + efficiency_measure = all + + // points = pts={107.5,-53.5:112,-43.8:112,-46.1:111.7,-49.3:108.3,-52.7:107.7,-53.3} + polygon = 60,-40 : 60,-160 : 150,-160 : 180,-100 : 150,-40 + + order = normal + //repeat = 100000 + 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_Waypoint +Behavior=BHV_Waypoint { name = waypt_return pwt = 100 - condition = (RETURN = true) - condition = (DEPLOY = true) + condition = RETURN = true + condition = DEPLOY = true + condition = ((DEPLoy = true) or (alpha = one)) or (bravo = two) + perpetual = true + updates = RETURN_UPDATE + endflag = RETURN = false + endflag = DEPLOY = false + endflag = MISSION = complete + + speed = 2.0 + capture_radius = 2.0 + slip_radius = 8.0 + points = 0,-20 + } - speed = 2.0 - radius = 8.0 - point = 0,0 +//---------------------------------------------- +Behavior=BHV_ConstantSpeed +{ + name = const_speed + pwt = 200 + condition = SPD=true + condition = DEPLOY = true + perpetual = true + updates = SPEED_UPDATE + endflag = SPD = false + + speed = 0.5 + duration = 10 + duration_reset = CONST_SPD_RESET=true + } diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos index 63c437d..e328b37 100644 --- a/missions/alder/alder.moos +++ b/missions/alder/alder.moos @@ -14,13 +14,13 @@ ProcessConfig = ANTLER MSBetweenLaunches = 200 Run = MOOSDB @ NewConsole = false - Run = uSimMarineV22 @ NewConsole = false + //Run = uSimMarineV22 @ NewConsole = false Run = pNodeReporter @ NewConsole = false Run = pMarinePIDV22 @ NewConsole = false Run = pMarineViewer @ NewConsole = false Run = uProcessWatch @ NewConsole = false Run = pHelmIvP @ NewConsole = false - //Run = pOdometry @ NewConsole = false + Run = pAUV150 @ NewConsole = false } //------------------------------------------ @@ -80,8 +80,8 @@ ProcessConfig = pHelmIvP ProcessConfig = pMarinePIDV22 { - AppTick = 20 - CommsTick = 20 + AppTick = 5 + CommsTick = 5 VERBOSE = true DEPTH_CONTROL = false @@ -148,5 +148,22 @@ ProcessConfig = pNodeReporter AppTick = 2 CommsTick = 2 - vessel_type = kayak + platform_length = 1.5 // meters + //platform_beam = 0.2 // meters + platform_type = auv + platform_color = dodger_blue + //vessel_type = auv +} + +ProcessConfig = pAUV150 +{ + AppTick = 4 + CommsTick = 4 + + server_host = 10.127.0.18 + LatOrigin = 43.825300 + LongOrigin = -70.330400 + + ControlFrequency = 5 + } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 23fedd4..241cb01 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,7 +18,7 @@ INCLUDE_DIRECTORIES(${LOCAL_LIBRARY_DIRS}) ADD_SUBDIRECTORY(lib_behaviors-test) ADD_SUBDIRECTORY(pExampleApp) ADD_SUBDIRECTORY(pXRelayTest) - +add_subdirectory(pAUV150) ############################################################################## # END of CMakeLists.txt ############################################################################## diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp new file mode 100644 index 0000000..e8c80c1 --- /dev/null +++ b/src/pAUV150/AUV150.cpp @@ -0,0 +1,600 @@ +/************************************************************/ +/* NAME: */ +/* ORGN: MIT, Cambridge MA */ +/* FILE: AUV150.cpp */ +/* DATE: December 29th, 1963 */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "ACTable.h" +#include "AUV150.h" +#include +#include +#include + +// #define DEBUG_ + +using namespace std; + +static bool TCP_ReadLoop(void *p) +{ + AUV150 *pAUV150 = (AUV150 *)p; + return pAUV150->ListenLoop(); +} + +static bool TCP_WriteLoop(void *p) +{ + AUV150 *pAUV150 = (AUV150 *)p; + return pAUV150->WriteLoop(); +} + +//--------------------------------------------------------- +// Constructor() + +AUV150::AUV150() +{ + m_TcpSocket = new XPCTcpSocket(8150L); + m_real_read_freq = 0; + m_real_write_freq = 0; + m_counter = 0; + m_bConnected = false; + m_status = {0}; + m_faultCodes.clear(); + m_pos_x = 0; + m_pos_y = 0; + m_OriginLatitude = 39.9042; + m_OriginLongitude = 116.4074; + m_controlGap = 0; + m_desiredVarTime = 0; + m_controlTime = 0; + m_samplingTime = 0; +} + +//--------------------------------------------------------- +// Destructor + +AUV150::~AUV150() +{ + if(m_TcpSocket) + { + m_TcpSocket->vCloseSocket(); + } + delete m_TcpSocket; +} + +//--------------------------------------------------------- +// Procedure: OnNewMail() + +bool AUV150::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(); + +#if 0 // Keep these around just for template + 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(); +#endif + + if(key == "FOO") + cout << "great!"; + else if(key == "DESIRED_RUDDER") + { + m_CommandFrame.rudderUp = msg.GetDouble(); + m_CommandFrame.rudderDown = msg.GetDouble(); + m_desiredVarTime = msg.GetTime(); + } + else if(key == "DESIRED_ELEVATOR") + { + m_CommandFrame.rudderLeft = msg.GetDouble(); + m_CommandFrame.rudderRight = msg.GetDouble(); + } + else if(key == "DESIRED_THRUST") + { + m_CommandFrame.mainThruster = msg.GetDouble(); + } + else if(key != "APPCAST_REQ") // handled by AppCastingMOOSApp + reportRunWarning("Unhandled Mail: " + key); + } + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnConnectToServer() + +bool AUV150::OnConnectToServer() +{ + registerVariables(); + return(true); +} + +//--------------------------------------------------------- +// Procedure: Iterate() +// happens AppTick times per second + +bool AUV150::Iterate() +{ + AppCastingMOOSApp::Iterate(); + // Do your thing here! + #ifdef DEBUG_ + m_status.insLatitude += 0.000001; + m_status.insLongitude += 0.000001; + m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y); + postStatusUpdate("NAV"); + #endif + AppCastingMOOSApp::PostReport(); + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnStartUp() +// happens before connection is open + +bool AUV150::OnStartUp() +{ + AppCastingMOOSApp::OnStartUp(); + + STRING_LIST sParams; + m_MissionReader.EnableVerbatimQuoting(false); + if(!m_MissionReader.GetConfiguration(GetAppName(), sParams)) + reportConfigWarning("No config block found for " + GetAppName()); + + STRING_LIST::iterator p; + for(p=sParams.begin(); p!=sParams.end(); p++) { + string orig = *p; + string line = *p; + string param = tolower(biteStringX(line, '=')); + string value = line; + + bool handled = false; + if(param == "foo") { + handled = true; + } + else if(param == "bar") { + handled = true; + } + else if(param == "server_host") + { + setServerHost(value); + handled = true; + } + else if(param == "latorigin") + { + m_OriginLatitude = stod(value); + handled = true; + } + else if(param == "longorigin") + { + m_OriginLongitude = stod(value); + handled = true; + } + else if(param == "controlfrequency") + { + m_controlCycle = 1.0 / stod(value); + handled = true; + } + if(!handled) + reportUnhandledConfigWarning(orig); + } + m_geo_ok = m_geodesy.Initialise(m_OriginLatitude, m_OriginLongitude); + Start(); + registerVariables(); +#ifdef DEBUG_ + m_status.insLatitude = m_OriginLatitude; + m_status.insLongitude = m_OriginLongitude; +#endif + return(true); +} + +//--------------------------------------------------------- +// Procedure: registerVariables() + +void AUV150::registerVariables() +{ + AppCastingMOOSApp::RegisterVariables(); + Register("DESIRED_RUDDER",0); + Register("DESIRED_THRUST",0); + Register("DESIRED_ELEVATOR",0); + // Register("FOOBAR", 0); +} + + +bool AUV150::Start() +{ + //初始化TcpSocket + try + { + m_TcpSocket->vSetRecieveBuf(m_nReceiveBufferSizeKB * 1024); + m_TcpSocket->vSetSendBuf(m_nSendBufferSizeKB * 1024); + } + catch (XPCException & e) + { + std::cerr << "there was trouble configuring socket buffers: " + << e.sGetException() << "\n"; + } + + //启动线程 + WritingThread_.Initialise(TCP_ReadLoop,this); + WritingThread_.Start(); + ReadingThread_.Initialise(TCP_WriteLoop,this); + ReadingThread_.Start(); + return true; +} +void AUV150::setServerHost(std::string serverHost) +{ + m_serverHost = serverHost; +} + +bool AUV150::ListenLoop() +{ + double start_time = MOOSTime(); + int message_count = 0; + + while(!ReadingThread_.IsQuitRequested()) + { + char buffer[sizeof(FeedbackFrame_150AUV)] = {0}; + try + { + if((m_TcpSocket!=NULL) && m_bConnected) + { + int count = m_TcpSocket->iRecieveMessage(buffer, sizeof(FeedbackFrame_150AUV)); + if(count > 0) { + std::cout << "Received " << count << " bytes" << std::endl; + FeedbackFrame_150AUV *p = reinterpret_cast(buffer); + // 处理接受到的数据 + if(p->frameHeader != 0xEBA1) + { + std::cout << "Received data is not a valid frame" << std::endl; + m_faultCodes.insert(0x11); + continue; + } + // 解析数据 + m_counter = p->counter; + updateStatus(*p); + postStatusUpdate("NAV"); + + m_samplingTime = MOOSTime(); + message_count++; + double elapsed_time = m_samplingTime - start_time; + if(elapsed_time >= 1.0) { // 每秒更新一次频率 + m_real_read_freq = message_count / elapsed_time; + message_count = 0; + start_time = MOOSTime(); + } + //正常情况下清除错误码 + m_faultCodes.erase(0x11); + m_faultCodes.erase(0x12); + m_faultCodes.erase(0x13); + } + else + { + m_faultCodes.insert(0x12); //无数据接收 + } + } + + } + catch (XPCException & e) + { + m_faultCodes.insert(0x13); + std::cerr << "there was trouble recieving message: " + << e.sGetException() << "\n"; + return false; + } + } + return true; +} + +bool AUV150::WriteLoop() +{ + double start_time = MOOSTime(); + int message_count = 0; + double last_send_time = MOOSTime(); + + //5Hz发送 + while(!WritingThread_.IsQuitRequested()){ + + if(MOOSTime() - last_send_time >= m_controlCycle) + { + try + { + m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV)); + m_controlTime = MOOSTime(); + m_controlGap = m_controlTime - m_desiredVarTime; + m_bConnected = true; + } + catch(XPCException &e) + { + m_bConnected = false; + std::cerr << "there was trouble sending message: " + << e.sGetException() << "\n"; + } + message_count++; + double elapsed_time = MOOSTime() - start_time; + if(elapsed_time > 1.0) { // 每秒更新一次频率 + m_real_write_freq = message_count / elapsed_time; + message_count = 0; + start_time = MOOSTime(); + } + last_send_time = MOOSTime(); + } + if(!m_bConnected) + { + m_real_read_freq = 0; + m_real_write_freq = 0; + try + { + if(m_TcpSocket) { + m_TcpSocket->vCloseSocket(); + delete m_TcpSocket; + } + // m_TcpSocket->vCloseSocket(); + m_TcpSocket = new XPCTcpSocket(8150L); + m_TcpSocket->vSetRecieveBuf(m_nReceiveBufferSizeKB * 1024); + m_TcpSocket->vSetSendBuf(m_nSendBufferSizeKB * 1024); + m_TcpSocket->vConnect(m_serverHost.c_str()); + m_bConnected = true; + } catch(XPCException &e) + { + std::cerr << "连接失败: " << e.sGetException() << std::endl; + MOOSTime(1.0); + } + } + } + return true; +} + + +bool AUV150::buildReport() +{ + auto displayWidth = [&](const std::string& str) { + std::mbstate_t state; + std::memset(&state, 0, sizeof(state)); + const char* ptr = str.c_str(); + size_t remaining = str.size(); + int count = 0; + while (remaining > 0) { + wchar_t wc; + size_t ret = std::mbrtowc(&wc, ptr, remaining, &state); + if (ret == (size_t)-1 || ret == (size_t)-2 || ret == 0) { + // invalid or null or incomplete, treat a byte as a char + ptr++; + remaining--; + } else { + ptr += ret; + remaining -= ret; + } + count++; + } + return count; + }; + + // Helper to center text within a given display width + auto centerText = [&](const std::string& text, int width) { + int len = displayWidth(text); + int pad = width - len; + int padLeft = pad / 2; + int padRight = pad - padLeft; + return std::string(padLeft, ' ') + text + std::string(padRight, ' '); + }; + + // Helper to print a formatted table + auto printTable = [&](const std::vector& headers, + const std::vector& values) + { + size_t cols = headers.size(); + std::vector colWidths(cols); + // Compute max display width per column and add padding + for (size_t i = 0; i < cols; ++i) { + int h = displayWidth(headers[i]); + int v = displayWidth(values[i]); + colWidths[i] = std::max(h, v) + 2; + } + + std::ostringstream oss; + // Header row + for (size_t i = 0; i < cols; ++i) { + oss << centerText(headers[i], colWidths[i]); + if (i + 1 < cols) oss << " | "; + } + m_msgs << oss.str() << std::endl; + + // Divider row + oss.str(""); oss.clear(); + for (size_t i = 0; i < cols; ++i) { + oss << std::string(colWidths[i], '-'); + if (i + 1 < cols) oss << " | "; + } + m_msgs << oss.str() << std::endl; + + // Value row + oss.str(""); oss.clear(); + for (size_t i = 0; i < cols; ++i) { + oss << centerText(values[i], colWidths[i]); + if (i + 1 < cols) oss << " | "; + } + m_msgs << oss.str() << std::endl << std::endl; + }; + + // =============== Navigation Info =============== + printTable( + {"Connect", "Read", "Writ" ,"Conter" ,"Server ip","Latitude", "Longitude","Gap"}, + { + (isConnected() ? "Yes" : "No"), + doubleToString(getRealReadFreq(), 2) + " Hz", + doubleToString(getRealWriteFreq(), 2) + " Hz", + ulintToString(getCounter()), + m_serverHost, + doubleToString(m_OriginLatitude, 5) + "deg", + doubleToString(m_OriginLongitude, 5) + "deg", + doubleToString(m_controlGap, 5) + "s" + } + ); + m_msgs << "Fault Codes: "; + for (uint8_t code : getFaultCodes()) { + m_msgs << uintToString(code) + " "; + } + m_msgs << std::endl; + m_msgs << "Navigation Mode : " << uintToString(m_status.navModeFb) << std::endl; + // m_msgs << "Origin Latitude : " << m_OriginLatitude << endl; + // m_msgs << "Origin Longitude : " << m_OriginLongitude << endl; + // m_msgs << "---------------------------------\n"; + // =============== Heading & Attitude =============== + printTable( + {"Heading", "Pitch", "Roll", "X", "Y", "Z", "H"}, + { + doubleToString(m_status.trueHeading, 2) + "deg", + doubleToString(m_status.pitch, 2) + "deg", + doubleToString(m_status.roll, 2) + "deg", + doubleToString(m_pos_x, 4) + "m", + doubleToString(m_pos_y, 4) + "m", + doubleToString(m_status.depthSensor, 2) + "m", + doubleToString(m_status.altimeterHeight, 2) + "m" + } + ); + m_msgs << "---------------INS------------------\n"; + + // =============== Velocity Info =============== + printTable( + {"Latitude", "Longitude", "Altitude","East Velocity","North Velocity","Down Velocity"}, + { + doubleToString(m_status.insLatitude, 4) + "deg", + doubleToString(m_status.insLongitude, 4) + "deg", + doubleToString(m_status.insAltitude, 2) + "m", + doubleToString(m_status.velocityEast, 2) + "m/s", + doubleToString(m_status.velocityNorth, 2) + "m/s", + doubleToString(m_status.velocityDown, 2) + "m/s" + } + ); + m_msgs << "----------------DVL-----------------\n"; + printTable( + {"DVL X Velocity", "DVL Y Velocity", "DVL Z Velocity"}, + { + doubleToString(m_status.dvlVelX, 2) + "m/s", + doubleToString(m_status.dvlVelY, 2) + "m/s", + doubleToString(m_status.dvlVelZ, 2) + "m/s" + } + ); + m_msgs << "---------------------------------\n"; + + // =============== Propulsion System =============== + printTable( + {"Thruster RPM"}, + { + doubleToString(m_status.thrusterRPM, 1) + "rpm", + } + ); + // m_msgs << "---------------------------------\n"; + + // =============== Power System =============== + printTable( + {"Battery Voltage", "Battery Level", "Battery Temperature"}, + { + doubleToString(m_status.batteryVoltage / 1000.0, 2) + "V", + uintToString(m_status.batteryLevel) + "%", + doubleToString(m_status.batteryTemp / 10.0, 1) + "°C" + } + ); + m_msgs << "---------------------------------\n"; + + // =============== System Status =============== + printTable( + {"Light", "Leak", "Power" , "Emergency power" , "Payload", "DVL Status","Thruster Status"}, + { + m_status.ledSwitch ? "ON" : "OFF", + m_status.leakStatus ? "LEAK" : "NORMAL", + uintToString(m_status.powerModule), + uintToString(m_status.backupPower), + m_status.payloadStatus ?"DROPPED" : "NORMAL", + m_status.dvlStatus?"ON" : "OFF", + uintToString(m_status.thrusterStatus) + } + ); + // m_msgs << "---------------------------------\n"; + + return true; +} + +bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame) +{ + m_status.navModeFb = feedbackFrame.navModeFb; + m_status.altimeterHeight = feedbackFrame.altimeterHeight / 100.0; + m_status.depthSensor = feedbackFrame.depthSensor / 100.0; + m_status.trueHeading = feedbackFrame.trueHeading / 100.0; + m_status.pitch = feedbackFrame.pitch / 100.0; + m_status.roll = feedbackFrame.roll / 100.0; + m_status.velocityEast = feedbackFrame.velocityEast / 100.0; + m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0; + m_status.velocityDown = feedbackFrame.velocityDown / 100.0; + m_status.insLongitude = feedbackFrame.insLongitude * 360.0 / (1ULL << 32); + m_status.insLatitude = feedbackFrame.insLatitude * 180.0 / (1ULL << 32) - 90.0; + m_status.insAltitude = feedbackFrame.insAltitude / 100.0; + m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0; + m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0; + m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0; + m_status.thrusterRPM = feedbackFrame.thrusterRPM; + m_status.ledSwitch = feedbackFrame.ledSwitch; + m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0; + m_status.batteryLevel = feedbackFrame.batteryLevel; + m_status.batteryTemp = feedbackFrame.batteryTemp / 10.0; + m_status.leakStatus = feedbackFrame.leakStatus; + m_status.powerModule = feedbackFrame.powerModule; + m_status.backupPower = feedbackFrame.backupPower; + m_status.thrusterStatus = feedbackFrame.thrusterStatus; + m_status.reserved = feedbackFrame.reserved; + m_status.payloadStatus = feedbackFrame.payloadStatus; + m_status.dvlStatus = feedbackFrame.dvlStatus; + + m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y); + + return true; +} + +void AUV150::postStatusUpdate(std::string prefix) +{ + m_curr_time = MOOSTime(); + + // 发布位置信息 + Notify(prefix+"_X", m_pos_x, m_curr_time); + Notify(prefix+"_Y", m_pos_y, m_curr_time); + + if(m_geo_ok) { + double lat, lon; + m_geodesy.LocalGrid2LatLong(m_pos_x, m_pos_y, lat, lon); + Notify(prefix+"_LAT", lat, m_curr_time); + Notify(prefix+"_LONG", lon, m_curr_time); + } + + // 发布运动状态 + Notify(prefix+"_HEADING", m_status.trueHeading, m_curr_time); + Notify(prefix+"_SPEED", m_status.dvlVelX, m_curr_time); + Notify(prefix+"_DEPTH", m_status.depthSensor, m_curr_time); + + // 发布姿态信息 + Notify(prefix+"_Z", -m_status.depthSensor, m_curr_time); + Notify(prefix+"_PITCH", m_status.pitch, m_curr_time); + Notify(prefix+"_YAW", m_status.trueHeading*3.1415926/180, m_curr_time); + Notify("TRUE_X", m_pos_x, m_curr_time); + Notify("TRUE_Y", m_pos_y, m_curr_time); + + // 发布对地速度 + Notify(prefix+"_HEADING_OVER_GROUND", m_status.trueHeading, m_curr_time); + Notify(prefix+"_SPEED_OVER_GROUND", m_status.dvlVelX, m_curr_time); + + // 发布高度信息 + if(m_status.insAltitude > 0) { + Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time); + } + + // // 发布模拟模式标志 + // Notify("SIMULATION_MODE","TRUE", m_curr_time); +} diff --git a/src/pAUV150/AUV150.h b/src/pAUV150/AUV150.h new file mode 100644 index 0000000..6135473 --- /dev/null +++ b/src/pAUV150/AUV150.h @@ -0,0 +1,184 @@ +/************************************************************/ +/* NAME: */ +/* ORGN: MIT, Cambridge MA */ +/* FILE: AUV150.h */ +/* DATE: December 29th, 1963 */ +/************************************************************/ + +#ifndef AUV150_HEADER +#define AUV150_HEADER +#define UNIX +// #include "AUV150_Tcp.h" +#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +#include "MOOS/libMOOS/Comms/XPCTcpSocket.h" +#include "MOOS/libMOOS/Utils/MOOSThread.h" +#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h" +#include "MOOS/libMOOSGeodesy/MOOSGeodesy.h" + +#include +#include +#include +#include +#pragma pack(push, 1) + +struct CommandFrame_150AUV { + uint16_t frameHeader = 0xEBA2; // 1. 帧头 @0 Uint16 单位:EBA2 固定值 0xEBA2 + uint16_t counter; // 2. 计数值 @2 Uint16 单位:— 0~65535，满后重置 + uint8_t dataLength; // 3. 数据长度 @4 Uint8 单位:字节 从“航行模式设置”到“控制抛载”的字节长度 + uint8_t navMode; // 4. 航行模式设置 @5 Uint8 单位:— 0xFF: 默认基本控制响应 + // 0x02: 客户直接控制推进器、舵机 + uint8_t mainThruster; // 5. 主推进器航速指令 @6 Uint8 单位:占空比 首位符号位，-100~100 (%) + uint16_t heading; // 6. 航向指令 @7 Uint16 单位:0.1° 无符号，0~3600 对应 0~360° + uint16_t depth; // 7. 深度指令 @9 Uint16 单位:0.1m 无符号，0~500 对应 0~50.0m + uint8_t rudderUp; // 8. 直操上舵机角度指令 @11 Uint8 单位:占空比 首位符号位，-30~30 (°) + uint8_t rudderDown; // 9. 直操下舵机角度指令 @12 Uint8 单位:占空比 首位符号位，-30~30 (°) + uint8_t rudderLeft; // 10. 直操左舵机角度指令 @13 Uint8 单位:占空比 首位符号位，-30~30 (°) + uint8_t rudderRight; // 11. 直操右舵机角度指令 @14 Uint8 单位:占空比 首位符号位，-30~30 (°) + uint8_t led; // 12. LED灯 @15 Uint8 单位:— 0x00: 关，0x01: 开 + uint8_t dvlSwitch; // 13. 传感器开关:DVL开关 @16 Uint8 单位:— 0xFF: 默认，0x00: 关，0x01: 开 + uint8_t payloadCtrl; // 14. 控制抛载 @17 Uint8 单位:— 0xFF: 默认，0x01: 执行抛载 + uint8_t crc; // 15. CRC校验位 @18 Uint8 单位:— 对“航行模式”到“控制抛载”区间进行求和校验 + uint16_t frameTail = 0xEE2A; // 16. 帧尾 @19 Uint16 单位:EE2A 固定值 0xEE2A +}; + +struct FeedbackFrame_150AUV { + uint16_t frameHeader = 0xEBA1; // 1. 帧头 @0 Uint16 单位:EBA1 固定值 0xEBA1 + uint16_t counter; // 2. 计数值 @2 Uint16 单位:— 0~65535，最大后重置 + uint8_t dataLength; // 3. 数据长度 @4 Uint8 单位:字节 从“航行模式反馈”到“DVL传感器状态反馈”的字节长度 + uint8_t navModeFb; // 4. 航行模式反馈 @5 Uint8 单位:— 0xFF: 默认 + // 0x02: 直接控制（客户直接控制推进器、舵机） + uint16_t altimeterHeight;// 5. 高度计：高度 @6 Uint16 单位:0.01m 无符号，0~100 对应 0~1.00m，取小值 + uint16_t depthSensor; // 6. 深度计：深度 @8 Uint16 单位:0.01m 无符号，0~600 对应 0~6.00m + uint16_t trueHeading; // 7. 真航向 @10 Uint16 单位:0.01° 无符号，0~36000 对应 0~360°，惯导无符号 + int16_t pitch; // 8. 俯仰角 @12 int16 单位:0.01° -18000~18000 对应 -180~180°，惯导 + int16_t roll; // 9. 横滚角 @14 int16 单位:0.01° -18000~18000 对应 -180~180°，惯导 + int16_t velocityEast; // 10. 东向速度 @16 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + int16_t velocityNorth; // 11. 北向速度 @18 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + int16_t velocityDown; // 12. 垂向速度 @20 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + uint32_t insLongitude; // 13. 惯导：经度 @22 Uint32 单位:°，精度:360/2^32 -180~180，惯导 + uint32_t insLatitude; // 14. 惯导：纬度 @26 Uint32 单位:°，精度:180/2^32 -90~90，惯导 + int16_t insAltitude; // 15. 惯导：高度 @30 int16 单位:0.01m 无符号，惯导 + int16_t dvlVelX; // 16. DVL:横向速度 @32 int16 单位:0.01m/s DVL + int16_t dvlVelY; // 17. DVL:纵向速度 @34 int16 单位:0.01m/s DVL + int16_t dvlVelZ; // 18. DVL:天向速度 @36 int16 单位:0.01m/s DVL + int16_t thrusterRPM; // 19. 主推进器实际转速(x1) @38 int16 单位:rpm -4000~4000，推进器 + uint8_t ledSwitch; // 20. 灯的开关 @40 Uint8 单位:— 0x00: 关，0x01: 开，灯 + uint16_t batteryVoltage; // 21. 电池电压反馈 @41 Uint16 单位:mV 约24000，电池状态 + uint8_t batteryLevel; // 22. 电池电量反馈 @43 Uint8 单位:% 0~100，电池状态 + uint16_t batteryTemp; // 23. 电池温度反馈 @44 Uint16 单位:0.1°C 0~1000 对应 0~100.0°C，电池状态 + uint32_t leakStatus; // 24. 漏水传感器反馈 @46 Uint32 单位:— 漏水传感器状态 + uint8_t powerModule; // 25. 电源模块状态反馈 @50 Uint8 单位:— 电源模块状态 + uint8_t backupPower; // 26. 应急电源模块状态反馈 @51 Uint8 单位:— 应急电源模块状态 + uint8_t thrusterStatus; // 27. 推进器状态反馈 @52 Uint8 单位:— 推进器状态 + uint8_t reserved; // 30. 预留 @55 Uint8 单位:— + uint8_t payloadStatus; // 28. 抛载状态反馈 @53 Uint8 单位:— 0xFF: 默认，0x00: 已抛载 + uint8_t dvlStatus; // 29. DVL传感器状态反馈 @54 Uint8 单位:— 0x00: 关，0x01: 开 + uint8_t crc; // 31. CRC校验位 @56 Uint8 单位:— 从“航行模式反馈”到“DVL传感器状态反馈”区间求和校验 + uint16_t frameTail = 0xEE1A; // 32. 帧尾 @57 Uint16 单位:EE1A 固定值 0xEE1A +}; + +// static_assert(sizeof(FeedbackFrame_150AUV) == 59, "FeedbackFrame size must be 59 bytes"); +// static_assert(sizeof(CommandFrame_150AUV) == 21, "CommandFrame size must be 21 bytes"); +#pragma pack(pop) + +struct AUV150_Status +{ + unsigned char navModeFb; // 4. 航行模式反馈 @5 Uint8 单位:— 0xFF: 默认 + double altimeterHeight;// 5. 高度计：高度 @6 Uint16 单位:0.01m 无符号，0~100 对应 0~1.00m，取小值 + double depthSensor; // 6. 深度计：深度 @8 Uint16 单位:0.01m 无符号，0~600 对应 0~6.00m + double trueHeading; // 7. 真航向 @10 Uint16 单位:0.01° 无符号，0~36000 对应 0~360°，惯导无符号 + double pitch; // 8. 俯仰角 @12 int16 单位:0.01° -18000~18000 对应 -180~180°，惯导 + double roll; // 9. 横滚角 @14 int16 单位:0.01° -18000~18000 对应 -180~180°，惯导 + double velocityEast; // 10. 东向速度 @16 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + double velocityNorth; // 11. 北向速度 @18 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + double velocityDown; // 12. 垂向速度 @20 int16 单位:0.01m/s -10000~10000 对应 -100~100m/s，惯导 + double insLongitude; // 13. 惯导：经度 @22 Uint32 单位:°，精度:360/2^32 -180~180，惯导 + double insLatitude; // 14. 惯导：纬度 @26 Uint32 单位:°，精度:180/2^32 -90~90，惯导 + double insAltitude; // 15. 惯导：高度 @30 int16 单位:0.01m 无符号，惯导 + double dvlVelX; // 16. DVL:横向速度 @32 int16 单位:0.01m/s DVL + double dvlVelY; // 17. DVL:纵向速度 @34 int16 单位:0.01m/s DVL + double dvlVelZ; // 18. DVL:天向速度 @36 int16 单位:0.01m/s DVL + double thrusterRPM; // 19. 主推进器实际转速(x1) @38 int16 单位:rpm -4000~4000，推进器 + unsigned char ledSwitch; // 20. 灯的开关 @40 Uint8 单位:— 0x00: 关，0x01: 开，灯 + double batteryVoltage; // 21. 电池电压反馈 @41 Uint16 单位:mV 约24000，电池状态 + unsigned char batteryLevel; // 22. 电池电量反馈 @43 Uint8 单位:% 0~100，电池状态 + unsigned short batteryTemp; // 23. 电池温度反馈 @44 Uint16 单位:0.1°C 0~1000 对应 0~100.0°C，电池状态 + int leakStatus; // 24. 漏水传感器反馈 @46 Uint32 单位:— 漏水传感器状态 + unsigned char powerModule; // 25. 电源模块状态反馈 @50 Uint8 单位:— 电源模块状态 + unsigned char backupPower; // 26. 应急电源模块状态反馈 @51 Uint8 单位:— 应急电源模块状态 + unsigned char thrusterStatus; // 27. 推进器状态反馈 @52 Uint8 单位:— 推进器状态 + unsigned char reserved; // 30. 预留 @55 Uint8 单位:— + unsigned char payloadStatus; // 28. 抛载状态反馈 @53 Uint8 单位:— 0xFF: 默认，0x00: 已抛载 + unsigned char dvlStatus; // 29. DVL传感器状态反馈 @54 Uint8 单位:— 0x00: 关，0x01: 开 +}; + +class AUV150 : public AppCastingMOOSApp +{ + public: + AUV150(); + ~AUV150(); + + protected: // Standard MOOSApp functions to overload + bool OnNewMail(MOOSMSG_LIST &NewMail); + bool Iterate(); + bool OnConnectToServer(); + bool OnStartUp(); + + protected: // Standard AppCastingMOOSApp function to overload + bool buildReport(); + + protected: + void registerVariables(); + + private: // Configuration variables + +public: + bool Start(); + void setServerHost(std::string serverHost); + CommandFrame_150AUV m_CommandFrame; + FeedbackFrame_150AUV m_FeedbackFrame; + bool ListenLoop(); + bool WriteLoop(); + bool updateStatus(FeedbackFrame_150AUV &feedbackFrame); + bool isConnected() { return m_bConnected; } + double getRealReadFreq() { return m_real_read_freq; } + double getRealWriteFreq() { return m_real_write_freq; } + unsigned long int getCounter() { return m_counter; } + std::set getFaultCodes() { return m_faultCodes; } + + // 状态发布函数 + void postStatusUpdate(std::string prefix); + +private: // State variables + XPCTcpSocket* m_TcpSocket; + CMOOSThread WritingThread_; + CMOOSThread ReadingThread_; + AUV150_Status m_status; + std::string m_serverHost; + int m_nReceiveBufferSizeKB = 1024; + int m_nSendBufferSizeKB = 1024; + double m_real_read_freq; + double m_real_write_freq; + bool m_bConnected; + std::set m_faultCodes; + unsigned long int m_counter; + + //导航相关 + CMOOSGeodesy m_geodesy; + bool m_geo_ok; + double m_pos_x; + double m_pos_y; + double m_OriginLatitude; + double m_OriginLongitude; + double m_curr_time; // 当前时间戳 + + //控制相关 + double m_controlGap; + double m_desiredVarTime; + double m_controlCycle; + + double m_controlTime; + double m_samplingTime; +}; + +#endif diff --git a/src/pAUV150/AUV150_Info.cpp b/src/pAUV150/AUV150_Info.cpp new file mode 100644 index 0000000..087199a --- /dev/null +++ b/src/pAUV150/AUV150_Info.cpp @@ -0,0 +1,115 @@ +/****************************************************************/ +/* NAME: */ +/* ORGN: MIT, Cambridge MA */ +/* FILE: AUV150_Info.cpp */ +/* DATE: December 29th, 1963 */ +/****************************************************************/ + +#include +#include +#include "AUV150_Info.h" +#include "ColorParse.h" +#include "ReleaseInfo.h" + +using namespace std; + +//---------------------------------------------------------------- +// Procedure: showSynopsis + +void showSynopsis() +{ + blk("SYNOPSIS: "); + blk("------------------------------------ "); + blk(" The pAUV150 application is used for "); + blk(" "); + blk(" "); + blk(" "); + blk(" "); +} + +//---------------------------------------------------------------- +// Procedure: showHelpAndExit + +void showHelpAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("Usage: pAUV150 file.moos [OPTIONS] "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("Options: "); + mag(" --alias","= "); + blk(" Launch pAUV150 with the given process name "); + blk(" rather than pAUV150. "); + mag(" --example, -e "); + blk(" Display example MOOS configuration block. "); + mag(" --help, -h "); + blk(" Display this help message. "); + mag(" --interface, -i "); + blk(" Display MOOS publications and subscriptions. "); + mag(" --version,-v "); + blk(" Display the release version of pAUV150. "); + blk(" "); + blk("Note: If argv[2] does not otherwise match a known option, "); + blk(" then it will be interpreted as a run alias. This is "); + blk(" to support pAntler launching conventions. "); + blk(" "); + exit(0); +} + +//---------------------------------------------------------------- +// Procedure: showExampleConfigAndExit + +void showExampleConfigAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("pAUV150 Example MOOS Configuration "); + blu("=============================================================== "); + blk(" "); + blk("ProcessConfig = pAUV150 "); + blk("{ "); + blk(" AppTick = 4 "); + blk(" CommsTick = 4 "); + blk(" "); + blk("} "); + blk(" "); + exit(0); +} + + +//---------------------------------------------------------------- +// Procedure: showInterfaceAndExit + +void showInterfaceAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("pAUV150 INTERFACE "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("SUBSCRIPTIONS: "); + blk("------------------------------------ "); + blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, "); + blk(" string_val=BAR "); + blk(" "); + blk("PUBLICATIONS: "); + blk("------------------------------------ "); + blk(" Publications are determined by the node message content. "); + blk(" "); + exit(0); +} + +//---------------------------------------------------------------- +// Procedure: showReleaseInfoAndExit + +void showReleaseInfoAndExit() +{ + showReleaseInfo("pAUV150", "gpl"); + exit(0); +} + diff --git a/src/pAUV150/AUV150_Info.h b/src/pAUV150/AUV150_Info.h new file mode 100644 index 0000000..22cd37d --- /dev/null +++ b/src/pAUV150/AUV150_Info.h @@ -0,0 +1,18 @@ +/****************************************************************/ +/* NAME: */ +/* ORGN: MIT, Cambridge MA */ +/* FILE: AUV150_Info.h */ +/* DATE: Dececmber 29th, 1963 */ +/****************************************************************/ + +#ifndef AUV150_INFO_HEADER +#define AUV150_INFO_HEADER + +void showSynopsis(); +void showHelpAndExit(); +void showExampleConfigAndExit(); +void showInterfaceAndExit(); +void showReleaseInfoAndExit(); + +#endif + diff --git a/src/pAUV150/CMakeLists.txt b/src/pAUV150/CMakeLists.txt new file mode 100644 index 0000000..eab7a55 --- /dev/null +++ b/src/pAUV150/CMakeLists.txt @@ -0,0 +1,30 @@ +#-------------------------------------------------------- +# The CMakeLists.txt for: pAUV150 +# Author(s): +#-------------------------------------------------------- + +# Replace the find_package section with direct path specification +set(MOOSGEODESY_INCLUDE_DIRS "/usr/local/include") +set(MOOSGEODESY_LIBRARIES "/usr/local/lib/libMOOSGeodesy.so") +include_directories(${MOOSGEODESY_INCLUDE_DIRS}) + +message(STATUS "Manually set MOOSGeodesy paths:") +message(STATUS " - Include dirs: ${MOOSGEODESY_INCLUDE_DIRS}") +message(STATUS " - Libraries: ${MOOSGEODESY_LIBRARIES}") + +SET(SRC + AUV150.cpp + AUV150_Info.cpp + main.cpp +) + +ADD_EXECUTABLE(pAUV150 ${SRC}) + +TARGET_LINK_LIBRARIES(pAUV150 + ${MOOS_LIBRARIES} + ${MOOSGEODESY_LIBRARIES} + apputil + mbutil + m + pthread) + diff --git a/src/pAUV150/main.cpp b/src/pAUV150/main.cpp new file mode 100644 index 0000000..0698521 --- /dev/null +++ b/src/pAUV150/main.cpp @@ -0,0 +1,52 @@ +/************************************************************/ +/* NAME: */ +/* ORGN: MIT, Cambridge MA */ +/* FILE: main.cpp, Cambridge MA */ +/* DATE: December 29th, 1963 */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "ColorParse.h" +#include "AUV150.h" +#include "AUV150_Info.h" + +using namespace std; + +int main(int argc, char *argv[]) +{ + string mission_file; + string run_command = argv[0]; + + for(int i=1; i