迁移分支

src/pBoardSupportComm/BoardSupportComm.h

@@ -0,0 +1,156 @@
+/************************************************************/
+/*    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]
+};
+
+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;
+};
+
+struct EstimatedState {
+  float referenceLon;
+  float referenceLat;
+  float referenceAltitude;  
+  float currentLon;
+  float currentLat;
+  float currentAltitude;
+  float offsetNorth;
+  float offsetEast;
+  float offsetDown;	
+  float roll;
+  float pitch;
+  float yaw;
+  float linearVelocityNorth;
+  float linearVelocityEast;
+  float linearVelocityDown;
+  float height;	        
+  float depth;
+};
+
+class BoardSupportComm : public CMOOSApp
+// class BoardSupportComm : public AppCastingMOOSApp
+{
+ public:
+   BoardSupportComm();
+   ~BoardSupportComm();
+
+protected: // Standard MOOSApp functions to overload  
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void RegisterVariables();
+  // bool buildReport();
+
+private:
+
+  struct EstimatedState estimatedState;
+
+  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();
+  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;
+  int testFlag = 0;
+  bool testCount = false;
+	int sockfd = -1;
+	int connectFlg = -1;
+  std::string llaOriginPath;
+
+
+};
+
+#endif