138 lines
4.4 KiB
C++
Executable File
138 lines
4.4 KiB
C++
Executable File
/************************************************************/
|
|
/* NAME: Xiaobin Zeng */
|
|
/* ORGN: MIT */
|
|
/* FILE: BoardSupportComm.h */
|
|
/* DATE: */
|
|
/************************************************************/
|
|
|
|
#ifndef BoardSupportComm_HEADER
|
|
#define BoardSupportComm_HEADER
|
|
|
|
#include "MOOS/libMOOS/MOOSLib.h"
|
|
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
|
|
#include <DUNE/DUNE.hpp>
|
|
#include <GeographicLib/LocalCartesian.hpp>
|
|
#include <json/json.h>
|
|
#include <thread>
|
|
#include <chrono>
|
|
|
|
#define CRC_COMPUTE_START_POS 5
|
|
#define CRC_COMPUTE_SUM_SIZE 13
|
|
|
|
struct AUVEmbedded
|
|
{
|
|
uint16_t header; //1:[0,1]
|
|
uint16_t count; //2:[2,3]
|
|
uint8_t size; //3:[4]
|
|
uint8_t drive_mode; //4:[5]
|
|
uint16_t height; //5:[6,7]
|
|
uint16_t depth; //6:[8,9]
|
|
uint16_t yaw; //7:[10,11]
|
|
int16_t pitch; //8:[12,13]
|
|
int16_t roll; //9:[14,15]
|
|
int16_t ins_vx; //10:[16,17]
|
|
int16_t ins_vy; //11:[18,19]
|
|
int16_t ins_vz; //12:[20,21]
|
|
int32_t lon; //13:[22,23,24,25]
|
|
int32_t lat; //14:[26,27,28,29]
|
|
int16_t alt; //15:[30,31]
|
|
int16_t dvl_vx; //16:[32,33]
|
|
int16_t dvl_vy; //17:[34,35]
|
|
int16_t dvl_vz; //18:[36,37]
|
|
int16_t rpm; //19:[38,39]
|
|
uint8_t light_enable; //20:[40]
|
|
uint16_t battery_voltage; //21:[41,42]
|
|
uint8_t battery_level; //22:[43]
|
|
uint16_t battery_temp; //23:[44,45]
|
|
uint32_t fault_leakSensor; //24:[46,47,48,49]
|
|
uint8_t fault_battery; //25:[50]
|
|
uint8_t fault_emergencyBattery; //26:[51]
|
|
uint8_t fault_thrust; //27:[52]
|
|
uint8_t iridium; //28:[53]
|
|
uint8_t throwing_load; //29:[54]
|
|
uint8_t dvl_status; //30:[55]
|
|
uint8_t crc; //31:[56]
|
|
uint16_t footer; //32:[57,58]
|
|
|
|
float refLon;
|
|
float refLat;
|
|
float refAlt;
|
|
float north;
|
|
float east;
|
|
};
|
|
|
|
struct AUVExecuteCommand
|
|
{
|
|
uint16_t header; //1:[0,1]
|
|
uint16_t count; //2:[2,3]
|
|
uint8_t size; //3:[4]
|
|
uint8_t drive_mode; //4:[5]
|
|
uint8_t thrust; //5:[6]
|
|
uint16_t yaw; //6:[7,8]
|
|
uint16_t depth; //7:[9,10]
|
|
uint8_t helm_top_angle; //8:[11]
|
|
uint8_t helm_bottom_angle;//9:[12]
|
|
uint8_t helm_left_angle; //10:[13]
|
|
uint8_t helm_right_angle;//11:[14]
|
|
uint8_t light_enable; //12:[15]
|
|
uint8_t dvl_enable; //13:[16]
|
|
uint8_t throwing_load_enable; //14:[17]
|
|
uint8_t crc; //15:[18]
|
|
uint16_t footer; //16:[19,20]
|
|
bool manual_mode;
|
|
};
|
|
|
|
class BoardSupportComm : public AppCastingMOOSApp
|
|
{
|
|
public:
|
|
BoardSupportComm();
|
|
~BoardSupportComm();
|
|
|
|
protected:
|
|
bool OnNewMail(MOOSMSG_LIST &NewMail);
|
|
bool Iterate();
|
|
bool OnConnectToServer();
|
|
bool OnStartUp();
|
|
void RegisterVariables();
|
|
bool buildReport();
|
|
|
|
private:
|
|
|
|
void ConvertLLAToENU(std::vector<double> init_lla,
|
|
std::vector<double> point_lla,
|
|
std::vector<double>& point_enu);
|
|
void ConvertENUToLLA(std::vector<double> init_lla,
|
|
std::vector<double> point_enu,
|
|
std::vector<double> &point_lla);
|
|
void ConvertLLAToNED(std::vector<double> init_lla,
|
|
std::vector<double> point_lla,
|
|
std::vector<double>& point_ned);
|
|
void ConvertNEDToLLA(std::vector<double> init_lla,
|
|
std::vector<double> point_ned,
|
|
std::vector<double> &point_lla);
|
|
|
|
void tcpProcessThread();
|
|
std::string convertEmbeddedFormat(Json::Value &embeddedState);
|
|
int tcpSockFD;
|
|
int tcpConnectRet;
|
|
char tcpReceiveBuffer[65535];
|
|
unsigned char tcpSendBuffer[65535];
|
|
int parseMessage(unsigned char* buffer, int size);
|
|
int serializeMessage(unsigned char* buffer);
|
|
|
|
template <typename Type> inline uint16_t deserialize(Type& t, const uint8_t* bfr, uint16_t& length);
|
|
template <typename Type> inline uint16_t serialize(const Type t, uint8_t* bfr, uint16_t& sumSize);
|
|
uint8_t convertIntToUchar(int src, int min, int max);
|
|
|
|
struct AUVEmbedded embeddedInfo;
|
|
struct AUVExecuteCommand executeCommand;
|
|
|
|
std::string llaOriginPath;
|
|
int recvLen;
|
|
std::string recvContent;
|
|
std::string castLogStream;
|
|
|
|
};
|
|
|
|
#endif
|