交互程序基本完成,需要增加锁相功能
This commit is contained in:
56
.vscode/settings.json
vendored
Normal file
56
.vscode/settings.json
vendored
Normal file
@@ -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"
|
||||||
|
}
|
||||||
|
}
|
||||||
28
.vscode/tasks.json
vendored
Normal file
28
.vscode/tasks.json
vendored
Normal file
@@ -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"
|
||||||
|
}
|
||||||
@@ -10,6 +10,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
|
|||||||
PROJECT( IVP_EXTEND )
|
PROJECT( IVP_EXTEND )
|
||||||
|
|
||||||
set (CMAKE_CXX_STANDARD 11)
|
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
|
# Set the output directories for the binary and library files
|
||||||
|
|||||||
17
doc/a.md
Normal file
17
doc/a.md
Normal file
@@ -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.
|
||||||
@@ -1,33 +1,94 @@
|
|||||||
//-------- FILE: alder.bhv -------------
|
//-------- FILE: alpha.bhv -------------
|
||||||
|
|
||||||
initialize DEPLOY = false
|
initialize DEPLOY = false
|
||||||
initialize RETURN = false
|
initialize RETURN = false
|
||||||
|
|
||||||
//----------------------------------------------
|
//----------------------------------------------
|
||||||
Behavior = BHV_SimpleWaypoint
|
Behavior = BHV_Waypoint
|
||||||
{
|
{
|
||||||
name = waypt_to_point
|
name = waypt_survey
|
||||||
pwt = 100
|
pwt = 100
|
||||||
condition = RETURN = false
|
condition = RETURN = false
|
||||||
condition = DEPLOY = true
|
condition = DEPLOY = true
|
||||||
endflag = RETURN = true
|
endflag = RETURN = true
|
||||||
|
|
||||||
speed = 2.0 // meters per second
|
configflag = CRUISE_SPD = $[SPEED]
|
||||||
radius = 8.0
|
//configflag = OSPOS = $[OSX],$[OSY]
|
||||||
ptx = 100
|
|
||||||
pty = -50
|
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
|
name = waypt_return
|
||||||
pwt = 100
|
pwt = 100
|
||||||
condition = (RETURN = true)
|
condition = RETURN = true
|
||||||
condition = (DEPLOY = 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
|
Behavior=BHV_ConstantSpeed
|
||||||
point = 0,0
|
{
|
||||||
|
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
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -14,13 +14,13 @@ ProcessConfig = ANTLER
|
|||||||
MSBetweenLaunches = 200
|
MSBetweenLaunches = 200
|
||||||
|
|
||||||
Run = MOOSDB @ NewConsole = false
|
Run = MOOSDB @ NewConsole = false
|
||||||
Run = uSimMarineV22 @ NewConsole = false
|
//Run = uSimMarineV22 @ NewConsole = false
|
||||||
Run = pNodeReporter @ NewConsole = false
|
Run = pNodeReporter @ NewConsole = false
|
||||||
Run = pMarinePIDV22 @ NewConsole = false
|
Run = pMarinePIDV22 @ NewConsole = false
|
||||||
Run = pMarineViewer @ NewConsole = false
|
Run = pMarineViewer @ NewConsole = false
|
||||||
Run = uProcessWatch @ NewConsole = false
|
Run = uProcessWatch @ NewConsole = false
|
||||||
Run = pHelmIvP @ NewConsole = false
|
Run = pHelmIvP @ NewConsole = false
|
||||||
//Run = pOdometry @ NewConsole = false
|
Run = pAUV150 @ NewConsole = false
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------
|
//------------------------------------------
|
||||||
@@ -80,8 +80,8 @@ ProcessConfig = pHelmIvP
|
|||||||
|
|
||||||
ProcessConfig = pMarinePIDV22
|
ProcessConfig = pMarinePIDV22
|
||||||
{
|
{
|
||||||
AppTick = 20
|
AppTick = 5
|
||||||
CommsTick = 20
|
CommsTick = 5
|
||||||
|
|
||||||
VERBOSE = true
|
VERBOSE = true
|
||||||
DEPTH_CONTROL = false
|
DEPTH_CONTROL = false
|
||||||
@@ -148,5 +148,22 @@ ProcessConfig = pNodeReporter
|
|||||||
AppTick = 2
|
AppTick = 2
|
||||||
CommsTick = 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
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -18,7 +18,7 @@ INCLUDE_DIRECTORIES(${LOCAL_LIBRARY_DIRS})
|
|||||||
ADD_SUBDIRECTORY(lib_behaviors-test)
|
ADD_SUBDIRECTORY(lib_behaviors-test)
|
||||||
ADD_SUBDIRECTORY(pExampleApp)
|
ADD_SUBDIRECTORY(pExampleApp)
|
||||||
ADD_SUBDIRECTORY(pXRelayTest)
|
ADD_SUBDIRECTORY(pXRelayTest)
|
||||||
|
add_subdirectory(pAUV150)
|
||||||
##############################################################################
|
##############################################################################
|
||||||
# END of CMakeLists.txt
|
# END of CMakeLists.txt
|
||||||
##############################################################################
|
##############################################################################
|
||||||
|
|||||||
600
src/pAUV150/AUV150.cpp
Normal file
600
src/pAUV150/AUV150.cpp
Normal file
@@ -0,0 +1,600 @@
|
|||||||
|
/************************************************************/
|
||||||
|
/* NAME: */
|
||||||
|
/* ORGN: MIT, Cambridge MA */
|
||||||
|
/* FILE: AUV150.cpp */
|
||||||
|
/* DATE: December 29th, 1963 */
|
||||||
|
/************************************************************/
|
||||||
|
|
||||||
|
#include <iterator>
|
||||||
|
#include "MBUtils.h"
|
||||||
|
#include "ACTable.h"
|
||||||
|
#include "AUV150.h"
|
||||||
|
#include <iomanip>
|
||||||
|
#include <string>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
// #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<FeedbackFrame_150AUV *>(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<std::string>& headers,
|
||||||
|
const std::vector<std::string>& values)
|
||||||
|
{
|
||||||
|
size_t cols = headers.size();
|
||||||
|
std::vector<int> 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);
|
||||||
|
}
|
||||||
184
src/pAUV150/AUV150.h
Normal file
184
src/pAUV150/AUV150.h
Normal file
@@ -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 <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
#include <set>
|
||||||
|
#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<unsigned char> 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<unsigned char> 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
|
||||||
115
src/pAUV150/AUV150_Info.cpp
Normal file
115
src/pAUV150/AUV150_Info.cpp
Normal file
@@ -0,0 +1,115 @@
|
|||||||
|
/****************************************************************/
|
||||||
|
/* NAME: */
|
||||||
|
/* ORGN: MIT, Cambridge MA */
|
||||||
|
/* FILE: AUV150_Info.cpp */
|
||||||
|
/* DATE: December 29th, 1963 */
|
||||||
|
/****************************************************************/
|
||||||
|
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <iostream>
|
||||||
|
#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","=<ProcessName> ");
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
18
src/pAUV150/AUV150_Info.h
Normal file
18
src/pAUV150/AUV150_Info.h
Normal file
@@ -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
|
||||||
|
|
||||||
30
src/pAUV150/CMakeLists.txt
Normal file
30
src/pAUV150/CMakeLists.txt
Normal file
@@ -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)
|
||||||
|
|
||||||
52
src/pAUV150/main.cpp
Normal file
52
src/pAUV150/main.cpp
Normal file
@@ -0,0 +1,52 @@
|
|||||||
|
/************************************************************/
|
||||||
|
/* NAME: */
|
||||||
|
/* ORGN: MIT, Cambridge MA */
|
||||||
|
/* FILE: main.cpp, Cambridge MA */
|
||||||
|
/* DATE: December 29th, 1963 */
|
||||||
|
/************************************************************/
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#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<argc; i++) {
|
||||||
|
string argi = argv[i];
|
||||||
|
if((argi=="-v") || (argi=="--version") || (argi=="-version"))
|
||||||
|
showReleaseInfoAndExit();
|
||||||
|
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
|
||||||
|
showExampleConfigAndExit();
|
||||||
|
else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
|
||||||
|
showHelpAndExit();
|
||||||
|
else if((argi == "-i") || (argi == "--interface"))
|
||||||
|
showInterfaceAndExit();
|
||||||
|
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 << "pAUV150 launching as " << run_command << endl;
|
||||||
|
cout << termColor() << endl;
|
||||||
|
|
||||||
|
AUV150 AUV150;
|
||||||
|
|
||||||
|
AUV150.Run(run_command.c_str(), mission_file.c_str());
|
||||||
|
|
||||||
|
return(0);
|
||||||
|
}
|
||||||
|
|
||||||
9
src/pAUV150/pAUV150.moos
Normal file
9
src/pAUV150/pAUV150.moos
Normal file
@@ -0,0 +1,9 @@
|
|||||||
|
//------------------------------------------------
|
||||||
|
// pAUV150 config block
|
||||||
|
|
||||||
|
ProcessConfig = pAUV150
|
||||||
|
{
|
||||||
|
AppTick = 4
|
||||||
|
CommsTick = 4
|
||||||
|
}
|
||||||
|
|
||||||
Reference in New Issue
Block a user