zjk/AUV_150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: zjk:796d64256383f66b05ac9a11d05209009c0d0004
Branches Tags
zjk:main
...
compare: zjk:6f6139934b5da951fb43f6970b44ef3d58c8129c
Branches Tags
zjk:main

2 Commits
796d642563 ... 6f6139934b

Author SHA1 Message Date
zjk
6f6139934b .. 2025-08-07 22:40:43 +08:00
zjk
6193725822 增加了校验码等 2025-08-07 22:40:32 +08:00
2 changed files with 97 additions and 28 deletions
Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -10,7 +10,8 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0)
PROJECT( IVP_EXTEND )
set (CMAKE_CXX_STANDARD 11)
set (MOOSIVP_SOURCE_TREE_BASE "/home/jima/moos-ivp")
# Resolved conflict: using relative path for MOOSIVP_SOURCE_TREE_BASE
set (MOOSIVP_SOURCE_TREE_BASE "../moos-ivp")
#=======================================================================
# Set the output directories for the binary and library files

116
src/pAUV150/AUV150.cpp
View File

@@ -16,7 +16,65 @@
// #define DEBUG_
using namespace std;
// until函数
/**
* @brief 解析int16数值，除以100并根据高位判断正负
* @param value 待解析的int16数值
* @return 解析后的浮点数值
*/
float parseInt16WithSign(int16_t value) {
// 检查最高位是否为1(负数)
if (value & 0x8000) {
// 负数：取补码并除以100后取负
return -((~value + 1) & 0x7FFF);
}
// 正数
return value;
}
/**
* @brief 将double编码为uint8，第一位为符号位
* @param value 要编码的double值
* @param limit 限幅值，输入将被限制在[-limit, limit]范围内
* @return 编码后的uint8值，第一位为符号位(0正1负)，后7位为数值
*/
uint8_t encodeDoubleToUint8WithSign(double value, double limit) {
// 限幅处理
if (value > limit) value = limit;
if (value < -limit) value = -limit;
// 直接取整数值
uint8_t magnitude = static_cast<uint8_t>(round(fabs(value)));
// 确保不会超过127
magnitude = magnitude > 127 ? 127 : magnitude;
// 设置符号位
if (value < 0) {
magnitude |= 0x80; // 设置最高位为1表示负数
}
return magnitude;
}
static uint8_t calculateCRC(const FeedbackFrame_150AUV& frame) {
const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navModeFb);
const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.dvlStatus) + sizeof(frame.dvlStatus);
uint8_t crc = 0;
for (const uint8_t* p = start; p < end; ++p) {
crc += *p;
}
return crc;
}
static uint8_t calculateCRC(const CommandFrame_150AUV& frame) {
const uint8_t* start = reinterpret_cast<const uint8_t*>(&frame.navMode);
const uint8_t* end = reinterpret_cast<const uint8_t*>(&frame.payloadCtrl) + sizeof(frame.payloadCtrl);
uint8_t crc = 0;
for (const uint8_t* p = start; p < end; ++p) {
crc += *p;
}
return crc;
}
// 添加静态成员定义
Controler AUV150::m_control;
@@ -99,7 +157,7 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
}
else
{
std::cout << "=============//=========" << std::endl;
std::cout << "======================" << std::endl;
m_overrived == true;
}
}
@@ -191,11 +249,11 @@ bool AUV150::OnStartUp()
m_OriginLongitude = stod(value);
handled = true;
}
else if(param == "controlfrequency")
{
m_controlCycle = 1.0 / stod(value);
handled = true;
}
// else if(param == "controlfrequency")
// {
// m_controlCycle = 1.0 / stod(value);
// handled = true;
// }
if(!handled)
reportUnhandledConfigWarning(orig);
}
@@ -215,9 +273,6 @@ bool AUV150::OnStartUp()
void AUV150::registerVariables()
{
AppCastingMOOSApp::RegisterVariables();
// Register("DESIRED_RUDDER",0);
// Register("DESIRED_THRUST",0);
// Register("DESIRED_ELEVATOR",0);
Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0);
@@ -276,6 +331,12 @@ bool AUV150::ListenLoop()
m_faultCodes.insert(0x11);
continue;
}
if(p->crc != calculateCRC(*p))
{
std::cout << "Received data is not a valid frame" << std::endl;
m_faultCodes.insert(0x14); // 0x14 校验错误
continue;
}
// 解析数据
m_counter = p->counter;
updateStatus(*p);
@@ -296,11 +357,16 @@ bool AUV150::ListenLoop()
m_control.rtU.y = m_status.y;
m_control.rtU.z = m_status.z;
m_control.step();
m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f;
m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
// m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
// m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
m_CommandFrame.rudderUp = encodeDoubleToUint8WithSign(m_control.rtY.DirectUpperRudderServoAngleCmd,30);
m_CommandFrame.rudderDown = encodeDoubleToUint8WithSign(m_control.rtY.DirectLowerRudderServoAngleCmd,30);
m_CommandFrame.rudderLeft = encodeDoubleToUint8WithSign(m_control.rtY.DirectLeftRudderServoAngleCmd,30);
m_CommandFrame.rudderRight = encodeDoubleToUint8WithSign(m_control.rtY.DirectRightRudderServoAngleCmd,30);
m_CommandFrame.mainThruster = encodeDoubleToUint8WithSign(m_control.rtY.MainThrusterSpeedCmd,100);
m_status.depth_error = m_control.rtY.depth_error;
m_status.heading_error = m_control.rtY.heading_error;
m_status.pitch_error = m_control.rtY.pitch_error;
@@ -309,6 +375,7 @@ bool AUV150::ListenLoop()
// 发送消息
try
{
m_CommandFrame.crc = calculateCRC(m_CommandFrame);
m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV));
m_controlGap = MOOSTime() - m_desiredVarTime;
m_controlTime = MOOSTime();
@@ -566,17 +633,18 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
m_status.depthSensor = feedbackFrame.depthSensor / 100.0f;
m_status.trueHeading = feedbackFrame.trueHeading / 100.0f;
m_status.pitch = feedbackFrame.pitch / 100.0f;
m_status.roll = feedbackFrame.roll / 100.0f;
m_status.velocityEast = feedbackFrame.velocityEast / 100.0f;
m_status.velocityNorth = feedbackFrame.velocityNorth / 100.0f;
m_status.velocityDown = feedbackFrame.velocityDown / 100.0f;
m_status.insLongitude = feedbackFrame.insLongitude / 1e5 - 180.0f;
m_status.insLatitude = feedbackFrame.insLatitude / 1e5 - 90.0f;
// m_status.pitch = feedbackFrame.pitch / 10.0f;
m_status.pitch = parseInt16WithSign(feedbackFrame.pitch)/10.0f;
m_status.roll = parseInt16WithSign(feedbackFrame.roll)/10.0f;
m_status.velocityEast = parseInt16WithSign(feedbackFrame.velocityEast)/100.0f;
m_status.velocityNorth = parseInt16WithSign(feedbackFrame.velocityNorth)/100.0f;
m_status.velocityDown = parseInt16WithSign(feedbackFrame.velocityDown)/100.0f;
m_status.insLongitude = feedbackFrame.insLongitude / 1e5;
m_status.insLatitude = feedbackFrame.insLatitude / 1e5;
m_status.insAltitude = feedbackFrame.insAltitude / 100.0f;
m_status.dvlVelX = feedbackFrame.dvlVelX / 100.0f;
m_status.dvlVelY = feedbackFrame.dvlVelY / 100.0f;
m_status.dvlVelZ = feedbackFrame.dvlVelZ / 100.0f;
m_status.dvlVelX = parseInt16WithSign(feedbackFrame.dvlVelX) / 100.0f;
m_status.dvlVelY = parseInt16WithSign(feedbackFrame.dvlVelY) / 100.0f;
m_status.dvlVelZ = parseInt16WithSign(feedbackFrame.dvlVelZ) / 100.0f;
m_status.thrusterRPM = feedbackFrame.thrusterRPM;
m_status.ledSwitch = feedbackFrame.ledSwitch;
m_status.batteryVoltage = feedbackFrame.batteryVoltage / 1000.0f;