交互程序基本完成,需要增加锁相功能
This commit is contained in:
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);
|
||||
}
|
||||
Reference in New Issue
Block a user