425 lines
12 KiB
C++
425 lines
12 KiB
C++
#ifndef MAVLINKNODE_H
|
|
#define MAVLINKNODE_H
|
|
|
|
#include <QObject>
|
|
#include "QThread"
|
|
#include "QFile"
|
|
#include "QDebug"
|
|
|
|
#include "mavlink.h"
|
|
#include "replay.h"
|
|
#include "missionprocess.h"
|
|
#include "parameterprocess.h"
|
|
#include "commandprocess.h"
|
|
#include "statusprocess.h"
|
|
#include "Terminal.h"
|
|
#include "sbusparser.h"
|
|
//#include "QAxObject"
|
|
|
|
#include "rcprocess.h"
|
|
#include "rtkprocess.h"
|
|
#include "DataStream.h"
|
|
|
|
#include "ThreadTemplet.h"
|
|
|
|
#include "ParsePack.h"
|
|
#include "QTime"
|
|
#include "QDateTime"
|
|
#include <QTimer>
|
|
|
|
#ifdef QtMavlinkNode
|
|
#include <mavlinknodeglobal.h>
|
|
class MAVLINKNODESHARED_EXPORT MavLinkNode : public ThreadTemplet {
|
|
#else
|
|
class MavLinkNode : public ThreadTemplet
|
|
{
|
|
#endif
|
|
Q_OBJECT
|
|
public:
|
|
|
|
typedef struct
|
|
{
|
|
quint16 year;
|
|
quint8 mon;
|
|
quint8 day;
|
|
|
|
quint8 hour;
|
|
quint8 min;
|
|
quint8 sec;
|
|
quint16 ms;
|
|
}_gpsTimer;
|
|
|
|
|
|
typedef struct {
|
|
int sysid = 0; /* ID of message sender system/aircraft */
|
|
int compid = 0; /* ID of the message sender component */
|
|
|
|
qint64 timebase;
|
|
|
|
|
|
mavlink_autopilot_version_t autopilot_version = {0};
|
|
mavlink_sys_status_t sys_status = {0};
|
|
mavlink_heartbeat_t heartbeat = {0};
|
|
mavlink_ping_t ping = {0};
|
|
mavlink_attitude_t attitude = {0};
|
|
mavlink_ins1_t ins1 = {0};
|
|
mavlink_ins2_t ins2 = {0};
|
|
mavlink_gps_raw_int_t gps_raw_int = {0};
|
|
mavlink_global_position_int_t global_position_int = {0};
|
|
mavlink_servo_output_raw_t servo_output_raw = {0};
|
|
mavlink_rc_channels_raw_t rc_channels_raw = {0};
|
|
mavlink_nav_controller_output_t nav_controller_output = {0};
|
|
mavlink_airspeed_autocal_t airspeed_autocal = {0};
|
|
mavlink_rpm_t rpm = {0};
|
|
mavlink_scaled_pressure_t scaled_pressure = {0};
|
|
mavlink_extended_sys_state_t extended_sys_state = {0};
|
|
mavlink_battery_status_t battery_status = {0};
|
|
mavlink_vibration_t vibration = {0};
|
|
mavlink_enginestate_t enginestate = {0};
|
|
mavlink_vfr_hud_t vfr_hud = {0};
|
|
mavlink_aoa_ssa_t aoa_ssa = {0};
|
|
mavlink_emb_atmo_com_t emb_atom_com = {0};
|
|
mavlink_turbinestate_t turbinstate = {0};
|
|
mavlink_bmustate_t bmustate = {0};
|
|
mavlink_ccmstate_t ccmstate = {0};
|
|
mavlink_serial_control_t serial_control = {0};
|
|
|
|
mavlink_uvwdstate_t uvwdstate = {0};
|
|
mavlink_rwrstate_t rwrstate = {0};
|
|
mavlink_dlsstate_t dlsstate = {0};
|
|
mavlink_payloadalarmstate_t payloadalarmstate = {0};
|
|
|
|
|
|
}_vehicle;
|
|
|
|
// typedef struct
|
|
// {
|
|
// int16_t flag;// 标识
|
|
// int32_t time;//(second+min*60+hour*3600)*1000
|
|
// int16_t num; //飞机数量，默认值为1
|
|
// int16_t id;//飞机编号，默认值为1
|
|
// int16_t t; //
|
|
// int32_t pe; //相对参考点的东向位置，实际值*8，单位为米
|
|
// int32_t pu; //相对参考点的天向位置，实际值*8，单位为米
|
|
// int32_t ps; //相对参考点的南向位置，实际值*8，单位为米
|
|
// int32_t ve; //相对参考点的东向速度，实际值*1024，单位为米/秒
|
|
// int32_t vu; //相对参考点的天向速度，实际值*1024，单位为米/秒
|
|
// int32_t vs; //相对参考点的南向速度，实际值*1024，单位为米/秒
|
|
// int32_t vt; //相对参考点的合速度，实际值*1024，单位为米/秒
|
|
// int32_t rcs; //飞机RCS值，默认值为0
|
|
// int32_t reserve; //预留位，默认值为0
|
|
// }_showinfo;//192.168.5.72:10049
|
|
|
|
|
|
typedef struct
|
|
{
|
|
int16_t Flag; // 0x23f3 标识（每个厂家不一样）
|
|
int16_t Src; // 飞机编号
|
|
int32_t GpsTime; // (Second + Minute * 60 + Hour * 3600) * 1000 + msec
|
|
int32_t Lan; // 经度 * 10000000 度
|
|
int32_t Lat; // 纬度 * 10000000 度
|
|
int32_t Alt; // 高度 * 100 m
|
|
int32_t VE; // 东向速度 * 100 m/s
|
|
int32_t VN; // 北向速度 * 100 m/s
|
|
int32_t VU; // 天向速度 * 100 m/s
|
|
int32_t V; // 东北天合速度 * 100 m/s
|
|
int32_t K; // 航向 * 100 度
|
|
int32_t Reserve1; // 保留字节
|
|
int32_t Reserve2; // 保留字节
|
|
}_showinfo;
|
|
|
|
typedef struct
|
|
{
|
|
double lon; //参考点经度
|
|
double lat; //参考点纬度
|
|
double hight; //参考点高度
|
|
}ReferencePoint;
|
|
|
|
explicit MavLinkNode(QObject *parent = nullptr);
|
|
~MavLinkNode();
|
|
|
|
_gpsTimer gpsTimer;
|
|
_vehicle vehicle;//没有初始化，所以会有野值
|
|
QHash<int,_vehicle> vehicleList;
|
|
|
|
Replay *replay = nullptr;
|
|
|
|
MissionProcess *Mission = nullptr;
|
|
ParameterProcess *Parameter = nullptr;
|
|
commandprocess *Commander = nullptr;
|
|
statusprocess *Status = nullptr;
|
|
terminal *Terminal = nullptr;
|
|
rcprocess *RC = nullptr;
|
|
rtkprocess *rtk = nullptr;
|
|
DataStream *datstream = nullptr;
|
|
|
|
bool isCommunicationLost = false;
|
|
|
|
uint64_t rate_in = 0;
|
|
uint64_t count_in = 0;
|
|
|
|
uint64_t rate_out = 0;
|
|
uint64_t count_out = 0;
|
|
|
|
|
|
|
|
uint64_t bittotal = 0;
|
|
|
|
uint64_t parserSuccess = 0;
|
|
uint64_t parserFailure = 0;
|
|
|
|
|
|
uint64_t TotalFrame_1s = 0;
|
|
uint64_t LossFrame_1s = 0;
|
|
|
|
float rssi = 100;
|
|
|
|
|
|
|
|
QByteArray rtkrawdata;
|
|
|
|
QFile * autopilot_version_file = nullptr;
|
|
QFile * sys_status_file = nullptr;
|
|
QFile * heartbeat_file = nullptr;
|
|
QFile * ping_file = nullptr;
|
|
QFile * attitude_file = nullptr;
|
|
QFile * ins1_file = nullptr;
|
|
QFile * ins2_file = nullptr;
|
|
QFile * gps_raw_int_file = nullptr;
|
|
QFile * global_position_int_file = nullptr;
|
|
QFile * servo_output_raw_file = nullptr;
|
|
QFile * rc_channels_raw_file = nullptr;
|
|
QFile * nav_controller_output_file = nullptr;
|
|
QFile * airspeed_autocal_file = nullptr;
|
|
QFile * rpm_file = nullptr;
|
|
QFile * scaled_pressure_file = nullptr;
|
|
QFile * extended_sys_state_file = nullptr;
|
|
QFile * battery_status_file = nullptr;
|
|
QFile * vibration_file = nullptr;
|
|
QFile * enginestate_file = nullptr;
|
|
QFile * vfr_hud_file = nullptr;
|
|
QFile * aoa_ssa_file = nullptr;
|
|
QFile * emb_atom_com_file = nullptr;
|
|
QFile * turbinstate_file = nullptr;
|
|
QFile * bmustate_file = nullptr;
|
|
QFile * ccmstate_file = nullptr;
|
|
QFile * serial_control_file = nullptr;
|
|
|
|
int infoExportID = 1;
|
|
signals:
|
|
|
|
void setMa(qint64 time, QVariant Ma);
|
|
|
|
void signal_autopilot_version(mavlink_autopilot_version_t);
|
|
void signal_sys_status(mavlink_sys_status_t);
|
|
void signal_heartbeat(mavlink_heartbeat_t);
|
|
void signal_ping(mavlink_ping_t);
|
|
void signal_attitude(mavlink_attitude_t);
|
|
void signal_ins1(mavlink_ins1_t ins);
|
|
void signal_ins2(mavlink_ins2_t ins);
|
|
void signal_gps_raw_int(mavlink_gps_raw_int_t);
|
|
void signal_global_position_int(mavlink_global_position_int_t);
|
|
void signal_servo_output_raw(mavlink_servo_output_raw_t servo);
|
|
void signal_rc_channels_raw(mavlink_rc_channels_raw_t);
|
|
void signal_nav_controller_output(mavlink_nav_controller_output_t);
|
|
void signal_airspeed_autocal(mavlink_airspeed_autocal_t);
|
|
void signal_rpm(mavlink_rpm_t);
|
|
void signal_scaled_pressure(mavlink_scaled_pressure_t);
|
|
void signal_extended_sys_state(mavlink_extended_sys_state_t);
|
|
void signal_battery_status(mavlink_battery_status_t);
|
|
void signal_vibration(mavlink_vibration_t);
|
|
void signal_enginestate(mavlink_enginestate_t);
|
|
void signal_vfr_hud(mavlink_vfr_hud_t);
|
|
void signal_aoa_ssa(mavlink_aoa_ssa_t);
|
|
void signal_emb_atom_com(mavlink_emb_atmo_com_t);
|
|
void signal_turbinstate(mavlink_turbinestate_t);
|
|
void signal_bmustate(mavlink_bmustate_t);
|
|
void signal_ccmstate(mavlink_ccmstate_t);
|
|
void signal_serial_control(mavlink_serial_control_t);
|
|
|
|
void signal_uvwdstate(mavlink_uvwdstate_t);
|
|
void signal_rwrstate(mavlink_rwrstate_t);
|
|
void signal_dlsstate(mavlink_dlsstate_t);
|
|
void signal_payloadalarmstate(mavlink_payloadalarmstate_t);
|
|
|
|
void signal_vehicle(MavLinkNode::_vehicle vehicle);
|
|
|
|
void updateDlink(float rssi,uint64_t in,uint64_t out);
|
|
void CommuniationLost(bool);
|
|
|
|
void beep(int);
|
|
|
|
void addVehicles(int sysid,int compid);
|
|
|
|
void recievemsg(mavlink_message_t msg);
|
|
|
|
void state_updated();
|
|
void parameter_updated();
|
|
//void mission_updated();
|
|
|
|
void setCurrentID(int sysid,int compid);
|
|
|
|
|
|
|
|
void setAltitude(int sysid,int compid,double roll,double pitch,double yaw);
|
|
void setPos(int sysid,int compid,double x,double y,double z);
|
|
void setHeading(int sysid,int compid,double x,double y,double z);
|
|
|
|
void Recieve(QString msg);
|
|
|
|
void heartbeatTimeStamp(quint32 time);
|
|
|
|
|
|
void SendMessageToExport(quint8 ch, quint8 *data,quint16 len);
|
|
|
|
|
|
|
|
void recievePayloadData(QByteArray);
|
|
|
|
void enCapData(uint16_t,QByteArray);
|
|
|
|
|
|
public slots:
|
|
/**
|
|
* @brief 设置厂家标识
|
|
*/
|
|
void setManufacturerIdentification(quint16 id);
|
|
|
|
//线程对外接口
|
|
void CreateCSV(void);
|
|
void CloseCSV(void);
|
|
|
|
bool setFileData(QFile *file,const QByteArray &data);
|
|
|
|
|
|
//缓存对外接口
|
|
void readPendingDatagramsReplay(void);
|
|
|
|
void setCurrentSelected(int sysid,int compid);
|
|
|
|
void setLogfile(QString file);
|
|
|
|
void setGCSID(int id);
|
|
|
|
void setHeartbeat(QVariant state, QVariant frq );
|
|
|
|
void serial_control(void);
|
|
void Transmit(QString msg);
|
|
|
|
void infoExport(_showinfo info);
|
|
|
|
void setReferencePoint(double lon, double lat, double h);
|
|
|
|
private slots:
|
|
//线程私有接口
|
|
void process();
|
|
|
|
//缓存私有接口
|
|
|
|
|
|
void TimerOut(void);
|
|
void LogTimerOut(void);
|
|
void timer_1s_Out(void);
|
|
//解析
|
|
void Mavlinkparse(quint32 src,QByteArray datagram);
|
|
|
|
void MAVLinkRcv_Handler(mavlink_message_t msg);
|
|
|
|
void StatusParse(mavlink_message_t msg);
|
|
void CommandParse(mavlink_message_t msg);
|
|
//
|
|
|
|
//check for new vehicle
|
|
void CheckVehicle(int sysid, int compid);
|
|
|
|
bool setLogData(mavlink_message_t msg);
|
|
|
|
void heartbeat(uint32_t custom_mode,
|
|
uint8_t type,
|
|
uint8_t autopilot,
|
|
uint8_t base_mode,
|
|
uint8_t system_status,
|
|
uint8_t mavlink_version);
|
|
|
|
protected:
|
|
|
|
int Current_sysID = 0xF1;
|
|
int Current_CompID = MAV_COMP_ID_MISSIONPLANNER;
|
|
|
|
enum SourceType{
|
|
c_sock = 0,
|
|
s_port = 1
|
|
};
|
|
|
|
bool hasConneted = false;
|
|
int32_t CommucationOverCount = 5;
|
|
uint64_t CommucationOverTimer = 5000;
|
|
|
|
uint8_t GCS_System_ID = 0xFF;
|
|
|
|
|
|
QDateTime startuptime;
|
|
|
|
QTime *LocationTime;
|
|
|
|
|
|
QTimer *gdt_timer = nullptr;
|
|
QTimer *timer = nullptr;
|
|
|
|
|
|
QByteArray databuff;
|
|
|
|
_buffdef serial_buff;
|
|
_buffdef client_buff;
|
|
|
|
QString mavlogFileName;
|
|
QFile *mavLogFile = NULL;
|
|
|
|
QString HeartBeatTimeName;
|
|
QFile *HeartBeatTimeFile = NULL;
|
|
|
|
|
|
|
|
QTimer *logTimer = nullptr;
|
|
|
|
|
|
|
|
QTimer *timer_1s = nullptr;
|
|
|
|
|
|
Parser2_t parser;
|
|
Packer2_t packer;
|
|
|
|
mutable QReadWriteLock RWlock;
|
|
|
|
|
|
qreal heartbeatFrq = 1;
|
|
bool isSendHeartBeat = false;
|
|
|
|
mavlink_heartbeat_t m_heartbeat;
|
|
|
|
QTimer *heartbeatTimer = nullptr;
|
|
|
|
|
|
bool isSendTerminal = false;
|
|
QString SerialData;
|
|
|
|
bool use_ins1 = false;
|
|
|
|
QDateTime *gpstimebase = nullptr;
|
|
|
|
_showinfo info;
|
|
|
|
QPointer<QTimer> showInfoTimer = nullptr;
|
|
QThread showInfoThread;
|
|
|
|
quint16 ManufacturerID;
|
|
|
|
ReferencePoint refPoint;
|
|
|
|
private slots:
|
|
void showInfoTimerTimeout();
|
|
};
|
|
|
|
#endif // MAVLINKNODE_H
|