/************************************************************/ /* 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 "Controler/Controler.h" #include "AngleUtils.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: 开 //以下为转换状态 double x; double y; double z; double heading_error; double pitch_error; double depth_error; double desired_rudder; double desired_elevator; double desired_thrust; }; 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; bool m_overrived; //控制器 static Controler m_control; }; #endif