190 lines
12 KiB
C++
190 lines
12 KiB
C++
/************************************************************/
|
||
/* 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 <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;
|
||
|
||
//控制器
|
||
static Controler m_control;
|
||
};
|
||
|
||
#endif
|