141 lines
4.2 KiB
C++
141 lines
4.2 KiB
C++
/************************************************************/
|
|
/* NAME: Xiaobin Zeng */
|
|
/* ORGN: MIT */
|
|
/* FILE: SurfaceSupportComm.h */
|
|
/* DATE: */
|
|
/************************************************************/
|
|
|
|
#ifndef SurfaceSupportComm_HEADER
|
|
#define SurfaceSupportComm_HEADER
|
|
|
|
#include <stdio.h>
|
|
#include <sys/types.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <arpa/inet.h>
|
|
|
|
|
|
#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 "PeriodicUDPEvent.h"
|
|
|
|
struct Landmark {
|
|
float lon;
|
|
float lat;
|
|
float depth;
|
|
float speed;
|
|
};
|
|
|
|
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;
|
|
};
|
|
|
|
struct DeviceStatus {
|
|
unsigned int batteryVoltage;
|
|
unsigned int batteryLevel;
|
|
float batteryTemp;
|
|
unsigned int controllerTemp;
|
|
int thrustRpm;
|
|
unsigned int lightEnable;
|
|
unsigned int throwingLoadEnable;
|
|
unsigned int dvlStatus;
|
|
};
|
|
|
|
|
|
class SurfaceSupportComm : public CMOOSApp
|
|
// class SurfaceSupportComm : public AppCastingMOOSApp
|
|
{
|
|
public:
|
|
SurfaceSupportComm();
|
|
~SurfaceSupportComm();
|
|
|
|
protected: // Standard MOOSApp functions to overload
|
|
bool OnNewMail(MOOSMSG_LIST &NewMail);
|
|
bool Iterate();
|
|
bool OnConnectToServer();
|
|
bool OnStartUp();
|
|
void RegisterVariables();
|
|
// bool buildReport();
|
|
|
|
private:
|
|
char recv_buf[2048];
|
|
DUNE::Network::UDPSocket sock_udp_send;
|
|
DUNE::Network::TCPSocket sock_tcp_receive;
|
|
DUNE::IO::Poll m_poll;
|
|
uint16_t header_src, header_dst;
|
|
uint8_t header_src_ent, header_dst_ent;
|
|
std::string planConfigPath;
|
|
struct EstimatedState estimatedState;
|
|
struct DeviceStatus deviceStatus;
|
|
struct Client
|
|
{
|
|
DUNE::Network::TCPSocket* socket; // Socket handle.
|
|
DUNE::Network::Address address; // Client address.
|
|
uint16_t port; // Client port.
|
|
DUNE::IMC::Parser parser; // Parser handle
|
|
};
|
|
|
|
typedef std::list<Client> ClientList;
|
|
ClientList m_clients;
|
|
|
|
bool udpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
|
|
void tcpReceiveFromClient(char* buf, unsigned int cap, double timeout);
|
|
void tcpSendToClient(char* buf, unsigned int cap);
|
|
void acceptNewClient(void);
|
|
void handleClients(char* buf, unsigned int cap);
|
|
void closeConnection(Client& c, std::exception& e);
|
|
void processMessage(DUNE::IMC::Message * message);
|
|
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 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 BatchConvertCoordinate(Json::Value &object);
|
|
void ParsePlanConfig(Json::Value inputJsonValue);
|
|
int simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value);
|
|
int retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList);
|
|
template <typename Type>
|
|
inline int getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value);
|
|
|
|
int testFlag = 0;
|
|
double TimeLast;
|
|
int sendCount = 0;
|
|
|
|
PeriodicUDPEvent udpEvent;
|
|
// PeriodicTCPEvent tcpEvent;
|
|
std::string missionStatusString;
|
|
};
|
|
|
|
#endif
|