mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
Merged HiTLv2 into HiTLnew. AeroSimRC support in HiTLnew is currently untested.
This commit is contained in:
Laura Sebesta
Kenz Dale
parent
a99b5454eb
commit
d4a1fb28c7
@ -0,0 +1,10 @@
|
|||||||
|
TEMPLATE = subdirs
|
||||||
|
|
||||||
|
win32 {
|
||||||
|
SUBDIRS += plugin
|
||||||
|
}
|
||||||
|
|
||||||
|
SUBDIRS += udptest
|
||||||
|
|
||||||
|
plugin.file = src/plugin.pro
|
||||||
|
udptest.file = src/udptest.pro
|
||||||
@ -0,0 +1,215 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file aerosimrcdatastruct.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef AEROSIMRCDATASTRUCT_H
|
||||||
|
#define AEROSIMRCDATASTRUCT_H
|
||||||
|
|
||||||
|
#include <QtCore>
|
||||||
|
|
||||||
|
const quint8 AEROSIMRC_MAX_CHANNELS = 39;
|
||||||
|
const quint16 DBG_BUFFER_MAX_SIZE = 4096;
|
||||||
|
|
||||||
|
#define MAX_DLL_USER_MENU_ITEMS 16
|
||||||
|
#define OBSOLETE_MIT_COMMAND (1 << 0)
|
||||||
|
#define OBSOLETE_MIT_CHECKBOX (1 << 1)
|
||||||
|
#define OBSOLETE_MIT_SEPARATOR (1 << 7)
|
||||||
|
|
||||||
|
#if defined(Q_CC_MSVC)
|
||||||
|
#define PACK_STRUCT
|
||||||
|
#define MAX_PATH 260
|
||||||
|
#pragma pack (push, r1, 1)
|
||||||
|
#elif defined(Q_CC_GNU)
|
||||||
|
#define PACK_STRUCT __attribute__((packed))
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct simToPlugin
|
||||||
|
{
|
||||||
|
quint16 structSize;
|
||||||
|
float simTimeStep;
|
||||||
|
float chSimTX[AEROSIMRC_MAX_CHANNELS];
|
||||||
|
float chSimRX[AEROSIMRC_MAX_CHANNELS];
|
||||||
|
uchar *OSDVideoBuf;
|
||||||
|
quint32 simMenuStatus;
|
||||||
|
float initPosX;
|
||||||
|
float initPosY;
|
||||||
|
float initPosZ;
|
||||||
|
float initHeading;
|
||||||
|
float initPitch;
|
||||||
|
float initRoll;
|
||||||
|
float wpHomeX;
|
||||||
|
float wpHomeY;
|
||||||
|
float wpHomeLat;
|
||||||
|
float wpHomeLong;
|
||||||
|
const char *wpHomeDesc; // (m, deg, string)
|
||||||
|
float wpAX;
|
||||||
|
float wpAY;
|
||||||
|
float wpALat;
|
||||||
|
float wpALong;
|
||||||
|
const char *wpADesc; // (m, deg, string)
|
||||||
|
float wpBX;
|
||||||
|
float wpBY;
|
||||||
|
float wpBLat;
|
||||||
|
float wpBLong;
|
||||||
|
const char *wpBDesc; // (m, deg, string)
|
||||||
|
float wpCX;
|
||||||
|
float wpCY;
|
||||||
|
float wpCLat;
|
||||||
|
float wpCLong;
|
||||||
|
const char *wpCDesc; // (m, deg, string)
|
||||||
|
float wpDX;
|
||||||
|
float wpDY;
|
||||||
|
float wpDLat;
|
||||||
|
float wpDLong;
|
||||||
|
const char *wpDDesc; // (m, deg, string)
|
||||||
|
float posX;
|
||||||
|
float posY;
|
||||||
|
float posZ;
|
||||||
|
float velX;
|
||||||
|
float velY;
|
||||||
|
float velZ;
|
||||||
|
float angVelX;
|
||||||
|
float angVelY;
|
||||||
|
float angVelZ;
|
||||||
|
float accelX;
|
||||||
|
float accelY;
|
||||||
|
float accelZ;
|
||||||
|
qreal latitude;
|
||||||
|
qreal longitude;
|
||||||
|
float AGL;
|
||||||
|
float heading;
|
||||||
|
float pitch;
|
||||||
|
float roll;
|
||||||
|
float windVelX;
|
||||||
|
float windVelY;
|
||||||
|
float windVelZ;
|
||||||
|
float eng1RPM;
|
||||||
|
float eng2RPM;
|
||||||
|
float eng3RPM;
|
||||||
|
float eng4RPM;
|
||||||
|
float voltage; // V
|
||||||
|
float current; // A
|
||||||
|
float consumedCharge; // Ah
|
||||||
|
float capacity; // Ah
|
||||||
|
float fuelConsumed; // l
|
||||||
|
float fuelTankCapacity; // l
|
||||||
|
// ver 3.83
|
||||||
|
qint16 screenW;
|
||||||
|
qint16 screenH;
|
||||||
|
// Model Orientation Matrix (X=Right, Y=Front, Z=Up)
|
||||||
|
float axisXx;
|
||||||
|
float axisXy;
|
||||||
|
float axisXz;
|
||||||
|
float axisYx;
|
||||||
|
float axisYy;
|
||||||
|
float axisYz;
|
||||||
|
float axisZx;
|
||||||
|
float axisZy;
|
||||||
|
float axisZz;
|
||||||
|
// Model data in body frame coordinates (X=Right, Y=Front, Z=Up)
|
||||||
|
float velXm; // m/s Model velocity in body coordinates
|
||||||
|
float velYm;
|
||||||
|
float velZm;
|
||||||
|
float angVelXm; // rad/s Model angular velocity in body coordinates
|
||||||
|
float angVelYm;
|
||||||
|
float angVelZm;
|
||||||
|
float accelXm; // m/s/s Model acceleration in body coordinates
|
||||||
|
float accelYm;
|
||||||
|
float accelZm;
|
||||||
|
// ver 3.90
|
||||||
|
quint32 OSDVideoBufSize;
|
||||||
|
} PACK_STRUCT ; // normal - 592, packed - 582 OK (3.81)
|
||||||
|
// normal - ???, packed - 658 OK (3.83)
|
||||||
|
// normal - ???, packed - 662 OK (3.90)
|
||||||
|
|
||||||
|
struct pluginToSim
|
||||||
|
{
|
||||||
|
quint16 structSize;
|
||||||
|
const char *dbgInfoText;
|
||||||
|
uchar chOverTX[AEROSIMRC_MAX_CHANNELS];
|
||||||
|
float chNewTX[AEROSIMRC_MAX_CHANNELS];
|
||||||
|
uchar chOverRX[AEROSIMRC_MAX_CHANNELS];
|
||||||
|
float chNewRX[AEROSIMRC_MAX_CHANNELS];
|
||||||
|
float newPosX; // m
|
||||||
|
float newPosY;
|
||||||
|
float newPosZ;
|
||||||
|
float newVelX; // m/s
|
||||||
|
float newVelY;
|
||||||
|
float newVelZ;
|
||||||
|
float newAngVelX; // rad/s
|
||||||
|
float newAngVelY;
|
||||||
|
float newAngVelZ;
|
||||||
|
float newHeading; // rad
|
||||||
|
float newPitch;
|
||||||
|
float newRoll;
|
||||||
|
quint32 modelOverrideFlags;
|
||||||
|
quint32 newMenuStatus;
|
||||||
|
quint8 isOSDShow;
|
||||||
|
quint8 isOSDChanged;
|
||||||
|
quint16 OSDWindow_DX;
|
||||||
|
quint16 OSDWindow_DY;
|
||||||
|
float OSDScale;
|
||||||
|
float newWindVelX;
|
||||||
|
float newWindVelY;
|
||||||
|
float newWindVelZ;
|
||||||
|
float newEng1RPM;
|
||||||
|
float newEng2RPM;
|
||||||
|
float newEng3RPM;
|
||||||
|
float newEng4RPM;
|
||||||
|
float newVoltage;
|
||||||
|
float newCurrent;
|
||||||
|
float newConsumedCharge;
|
||||||
|
float newFuelConsumed;
|
||||||
|
quint8 modelCrashInhibit;
|
||||||
|
// ver 3.83
|
||||||
|
qint16 newScreenW; // Simulator window position and size on screen
|
||||||
|
qint16 newScreenH;
|
||||||
|
qint16 newScreenX;
|
||||||
|
qint16 newScreenY;
|
||||||
|
} PACK_STRUCT ; // normal 516, packed 507 OK (3.81)
|
||||||
|
// normal ???, packed 515 OK (3.83 & 3.90)
|
||||||
|
|
||||||
|
struct TPluginMenuItem
|
||||||
|
{
|
||||||
|
quint32 OBSOLETE_eType;
|
||||||
|
char *OBSOLETE_strName;
|
||||||
|
} PACK_STRUCT ;
|
||||||
|
|
||||||
|
struct pluginInit
|
||||||
|
{
|
||||||
|
quint32 nStructSize;
|
||||||
|
char *OBSOLETE_strMenuTitle;
|
||||||
|
TPluginMenuItem OBSOLETE_atMenuItem[MAX_DLL_USER_MENU_ITEMS];
|
||||||
|
const char *strPluginFolder;
|
||||||
|
const char *strOutputFolder;
|
||||||
|
} PACK_STRUCT ; // normal - 144, packed - 144 OK (3.81 & 3.83 & 3.90)
|
||||||
|
|
||||||
|
#ifdef Q_CC_MSVC
|
||||||
|
#pragma pack (pop, r1)
|
||||||
|
#endif
|
||||||
|
#undef PACK_STRUCT
|
||||||
|
|
||||||
|
#endif // AEROSIMRCDATASTRUCT_H
|
||||||
102
ground/openpilotgcs/src/plugins/hitlnew/aerosimrc/src/enums.h
Normal file
102
ground/openpilotgcs/src/plugins/hitlnew/aerosimrc/src/enums.h
Normal file
@ -0,0 +1,102 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file enums.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef ENUMS_H
|
||||||
|
#define ENUMS_H
|
||||||
|
|
||||||
|
// Custom Menu Item masks
|
||||||
|
enum MenuMasks {
|
||||||
|
MenuEnable = (1 << 0),
|
||||||
|
MenuTx = (1 << 1),
|
||||||
|
MenuRx = (1 << 2),
|
||||||
|
MenuScreen = (1 << 3),
|
||||||
|
MenuNextWpt = (1 << 4),
|
||||||
|
MenuCmdReset = (1 << 5),
|
||||||
|
MenuLedBlue = (1 << 6),
|
||||||
|
MenuLedGreen = (1 << 7),
|
||||||
|
MenuFMode1 = (1 << 8),
|
||||||
|
MenuFMode2 = (1 << 9),
|
||||||
|
MenuFMode3 = (1 << 10)
|
||||||
|
};
|
||||||
|
|
||||||
|
enum EOverrideFlags
|
||||||
|
{
|
||||||
|
OVR_POS = (1 << 0),
|
||||||
|
OVR_VEL = (1 << 1),
|
||||||
|
OVR_ANG_VEL = (1 << 2),
|
||||||
|
OVR_HPR = (1 << 3), // Override Heading, Pitch and Roll
|
||||||
|
OVR_WIND_VEL = (1 << 4), // Override Wind velocity at model
|
||||||
|
OVR_ENGINE_RPM = (1 << 5), // Override RPM of all Engines or Motors
|
||||||
|
OVR_BAT_VOLT = (1 << 6), // Override motor Battery Voltage
|
||||||
|
OVR_BAT_AMP = (1 << 7), // Override motor Battery current
|
||||||
|
OVR_BAT_AH_CONSUMED = (1 << 8), // Override motor Battery AmpsHour consumed
|
||||||
|
OVR_FUEL_CONSUMED = (1 << 9) // Override Fuel consumed (gas & jet engines)
|
||||||
|
};
|
||||||
|
|
||||||
|
enum Channels {
|
||||||
|
Ch1Aileron,
|
||||||
|
Ch2Elevator,
|
||||||
|
Ch3Throttle,
|
||||||
|
Ch4Rudder,
|
||||||
|
Ch5,
|
||||||
|
Ch6,
|
||||||
|
Ch7,
|
||||||
|
Ch8,
|
||||||
|
Ch9,
|
||||||
|
Ch10Retracts,
|
||||||
|
Ch11Flaps,
|
||||||
|
Ch12FPVCamPan,
|
||||||
|
Ch13FPVCamTilt,
|
||||||
|
Ch14Brakes,
|
||||||
|
Ch15Spoilers,
|
||||||
|
Ch16Smoke,
|
||||||
|
Ch17Fire,
|
||||||
|
Ch18FlightMode,
|
||||||
|
Ch19ALTHold,
|
||||||
|
Ch20FPVTiltHold,
|
||||||
|
Ch21ResetModel,
|
||||||
|
Ch22MouseTX,
|
||||||
|
Ch23Plugin1,
|
||||||
|
Ch24Plugin2,
|
||||||
|
Ch25ThrottleHold,
|
||||||
|
Ch26CareFree,
|
||||||
|
Ch27FPVCamRoll,
|
||||||
|
Ch28LMotorDual,
|
||||||
|
Ch29RMotorDual,
|
||||||
|
Ch30Mix,
|
||||||
|
Ch31Mix,
|
||||||
|
Ch32Mix,
|
||||||
|
Ch33Mix,
|
||||||
|
Ch34Mix,
|
||||||
|
Ch35Mix,
|
||||||
|
Ch36Mix,
|
||||||
|
Ch37Mix,
|
||||||
|
Ch38Mix,
|
||||||
|
Ch39Mix
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // ENUMS_H
|
||||||
394
ground/openpilotgcs/src/plugins/hitlnew/aerosimrc/src/plugin.cpp
Normal file
394
ground/openpilotgcs/src/plugins/hitlnew/aerosimrc/src/plugin.cpp
Normal file
@ -0,0 +1,394 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file plugin.cpp
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "plugin.h"
|
||||||
|
#include "udpconnect.h"
|
||||||
|
#include "qdebughandler.h"
|
||||||
|
#include "enums.h"
|
||||||
|
#include "settings.h"
|
||||||
|
|
||||||
|
bool isFirstRun = true;
|
||||||
|
QString debugInfo(DBG_BUFFER_MAX_SIZE, ' ');
|
||||||
|
QString pluginFolder(MAX_PATH, ' ');
|
||||||
|
QString outputFolder(MAX_PATH, ' ');
|
||||||
|
|
||||||
|
QList<quint16> videoModes;
|
||||||
|
QTime ledTimer;
|
||||||
|
|
||||||
|
UdpSender *sndr;
|
||||||
|
UdpReceiver *rcvr;
|
||||||
|
|
||||||
|
const float RAD2DEG = (float)(180.0 / M_PI);
|
||||||
|
const float DEG2RAD = (float)(M_PI / 180.0);
|
||||||
|
|
||||||
|
//extern "C" int __stdcall DllMain(HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved)
|
||||||
|
extern "C" int __stdcall DllMain(void*, quint32 fdwReason, void*)
|
||||||
|
{
|
||||||
|
switch (fdwReason) {
|
||||||
|
case 0:
|
||||||
|
// qDebug() << hinstDLL << "DLL_PROCESS_DETACH " << lpvReserved;
|
||||||
|
// free resources here
|
||||||
|
rcvr->stop();
|
||||||
|
rcvr->wait(500);
|
||||||
|
delete rcvr;
|
||||||
|
delete sndr;
|
||||||
|
qDebug("------");
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
// qDebug() << hinstDLL << " DLL_PROCESS_ATTACH " << lpvReserved;
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
// qDebug() << hinstDLL << "DLL_THREAD_ATTACH " << lpvReserved;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
// qDebug() << hinstDLL << "DLL_THREAD_DETACH " << lpvReserved;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes(quint32 *sizeSimToPlugin,
|
||||||
|
quint32 *sizePluginToSim,
|
||||||
|
quint32 *sizePluginInit)
|
||||||
|
{
|
||||||
|
// debug redirection
|
||||||
|
qInstallMsgHandler(myQDebugHandler);
|
||||||
|
|
||||||
|
qDebug() << "AeroSIMRC_Plugin_ReportStructSizes";
|
||||||
|
*sizeSimToPlugin = sizeof(simToPlugin);
|
||||||
|
*sizePluginToSim = sizeof(pluginToSim);
|
||||||
|
*sizePluginInit = sizeof(pluginInit);
|
||||||
|
qDebug() << "sizeSimToPlugin = " << *sizeSimToPlugin;
|
||||||
|
qDebug() << "sizePluginToSim = " << *sizePluginToSim;
|
||||||
|
qDebug() << "sizePluginInit = " << *sizePluginInit;
|
||||||
|
}
|
||||||
|
|
||||||
|
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(pluginInit *p)
|
||||||
|
{
|
||||||
|
qDebug() << "AeroSIMRC_Plugin_Init begin";
|
||||||
|
|
||||||
|
pluginFolder = p->strPluginFolder;
|
||||||
|
outputFolder = p->strOutputFolder;
|
||||||
|
|
||||||
|
ledTimer.restart();
|
||||||
|
|
||||||
|
Settings *ini = new Settings(pluginFolder);
|
||||||
|
ini->read();
|
||||||
|
|
||||||
|
videoModes = ini->getVideoModes();
|
||||||
|
|
||||||
|
sndr = new UdpSender(ini->getOutputMap(), ini->isFromTX());
|
||||||
|
sndr->init(ini->remoteHost(), ini->remotePort());
|
||||||
|
|
||||||
|
rcvr = new UdpReceiver(ini->getInputMap(), ini->isToRX());
|
||||||
|
rcvr->init(ini->localHost(), ini->localPort());
|
||||||
|
|
||||||
|
// run thread
|
||||||
|
rcvr->start();
|
||||||
|
|
||||||
|
delete ini;
|
||||||
|
|
||||||
|
qDebug() << "AeroSIMRC_Plugin_Init done";
|
||||||
|
}
|
||||||
|
|
||||||
|
//-----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
void Run_Command_Reset(/*const simToPlugin *stp,
|
||||||
|
pluginToSim *pts*/)
|
||||||
|
{
|
||||||
|
// Print some debug info, although it will only be seen during one frame
|
||||||
|
debugInfo.append("\nRESET");
|
||||||
|
}
|
||||||
|
|
||||||
|
void Run_Command_WindowSizeAndPos(const simToPlugin *stp,
|
||||||
|
pluginToSim *pts)
|
||||||
|
{
|
||||||
|
static quint8 snSequence = 0;
|
||||||
|
quint8 idx = snSequence * 4;
|
||||||
|
|
||||||
|
if (snSequence >= videoModes.at(0)) { // set fullscreen
|
||||||
|
pts->newScreenX = 0;
|
||||||
|
pts->newScreenY = 0;
|
||||||
|
pts->newScreenW = stp->screenW;
|
||||||
|
pts->newScreenH = stp->screenH;
|
||||||
|
snSequence = 0;
|
||||||
|
} else { // set video mode from config
|
||||||
|
pts->newScreenX = videoModes.at(idx + 1);
|
||||||
|
pts->newScreenY = videoModes.at(idx + 2);
|
||||||
|
pts->newScreenW = videoModes.at(idx + 3);
|
||||||
|
pts->newScreenH = videoModes.at(idx + 4);
|
||||||
|
snSequence++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Run_Command_MoveToNextWaypoint(const simToPlugin *stp,
|
||||||
|
pluginToSim *pts)
|
||||||
|
{
|
||||||
|
static quint8 snSequence = 0;
|
||||||
|
|
||||||
|
switch(snSequence) {
|
||||||
|
case 0:
|
||||||
|
pts->newPosX = stp->wpAX;
|
||||||
|
pts->newPosY = stp->wpAY;
|
||||||
|
pts->newPosZ = 100.0;
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
pts->newPosX = stp->wpBX;
|
||||||
|
pts->newPosY = stp->wpBY;
|
||||||
|
pts->newPosZ = 100.0;
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
pts->newPosX = stp->wpCX;
|
||||||
|
pts->newPosY = stp->wpCY;
|
||||||
|
pts->newPosZ = 100.0;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
pts->newPosX = stp->wpDX;
|
||||||
|
pts->newPosY = stp->wpDY;
|
||||||
|
pts->newPosZ = 100.0;
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
pts->newPosX = stp->wpHomeX;
|
||||||
|
pts->newPosY = stp->wpHomeY;
|
||||||
|
pts->newPosZ = 100.0;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
qFatal("Run_Command_MoveToNextWaypoint switch error");
|
||||||
|
}
|
||||||
|
pts->modelOverrideFlags = 0;
|
||||||
|
pts->modelOverrideFlags |= OVR_POS;
|
||||||
|
|
||||||
|
snSequence++;
|
||||||
|
if(snSequence > 4)
|
||||||
|
snSequence = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Run_BlinkLEDs(const simToPlugin *stp,
|
||||||
|
pluginToSim *pts)
|
||||||
|
{
|
||||||
|
if ((stp->simMenuStatus & MenuEnable) != 0) {
|
||||||
|
pts->newMenuStatus |= MenuLedGreen;
|
||||||
|
int timeout;
|
||||||
|
quint8 armed;
|
||||||
|
quint8 mode;
|
||||||
|
rcvr->getFlighStatus(armed, mode);
|
||||||
|
debugInfo.append(QString("armed: %1, mode: %2\n").arg(armed).arg(mode));
|
||||||
|
|
||||||
|
if (armed == 0) // disarm
|
||||||
|
timeout = 1000;
|
||||||
|
else if (armed == 1) // arming
|
||||||
|
timeout = 40;
|
||||||
|
else if (armed == 2) // armed
|
||||||
|
timeout = 100;
|
||||||
|
else // unknown
|
||||||
|
timeout = 2000;
|
||||||
|
if (ledTimer.elapsed() > timeout) {
|
||||||
|
ledTimer.restart();
|
||||||
|
pts->newMenuStatus ^= MenuLedBlue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mode == 6) {
|
||||||
|
pts->newMenuStatus |= MenuFMode3;
|
||||||
|
pts->newMenuStatus |= MenuFMode2;
|
||||||
|
pts->newMenuStatus |= MenuFMode1;
|
||||||
|
} else if (mode == 5) {
|
||||||
|
pts->newMenuStatus |= MenuFMode3;
|
||||||
|
pts->newMenuStatus |= MenuFMode2;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode1;
|
||||||
|
} else if (mode == 4) {
|
||||||
|
pts->newMenuStatus |= MenuFMode3;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode2;
|
||||||
|
pts->newMenuStatus |= MenuFMode1;
|
||||||
|
} else if (mode == 3) {
|
||||||
|
pts->newMenuStatus |= MenuFMode3;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode2;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode1;
|
||||||
|
} else if (mode == 2) {
|
||||||
|
pts->newMenuStatus &= ~MenuFMode3;
|
||||||
|
pts->newMenuStatus |= MenuFMode2;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode1;
|
||||||
|
} else if (mode == 1) {
|
||||||
|
pts->newMenuStatus &= ~MenuFMode3;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode2;
|
||||||
|
pts->newMenuStatus |= MenuFMode1;
|
||||||
|
} else /*(mode == 0)*/ {
|
||||||
|
pts->newMenuStatus &= ~MenuFMode3;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode2;
|
||||||
|
pts->newMenuStatus &= ~MenuFMode1;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
pts->newMenuStatus = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void InfoText(const simToPlugin *stp,
|
||||||
|
pluginToSim *pts)
|
||||||
|
{
|
||||||
|
debugInfo.append(
|
||||||
|
QString(
|
||||||
|
"Plugin Folder = %1\n"
|
||||||
|
"Output Folder = %2\n"
|
||||||
|
"nStructSize = %3 "
|
||||||
|
"fIntegrationTimeStep = %4\n"
|
||||||
|
"\n"
|
||||||
|
"Aileron TX = %5 RX = %6 RCMD TX = %7 RX = %8\n"
|
||||||
|
"Elevator TX = %9 RX = %10 RCMD TX = %11 RX = %12\n"
|
||||||
|
"Throttle TX = %13 RX = %14 RCMD TX = %15 RX = %16\n"
|
||||||
|
"Rudder TX = %17 RX = %18 RCMD TX = %19 RX = %20\n"
|
||||||
|
"Channel5 TX = %21 RX = %22 RCMD TX = %23 RX = %24\n"
|
||||||
|
"Channel6 TX = %25 RX = %26 RCMD TX = %27 RX = %28\n"
|
||||||
|
"Channel7 TX = %29 RX = %30 RCMD TX = %31 RX = %32\n"
|
||||||
|
"PluginCh1 TX = %33 RX = %34 RCMD TX = %35 RX = %36\n"
|
||||||
|
"PluginCh2 TX = %37 RX = %38 RCMD TX = %39 RX = %40\n"
|
||||||
|
"FPVCamPan TX = %41 RX = %42 RCMD TX = %43 RX = %44\n"
|
||||||
|
"FPVCamTil TX = %45 RX = %46 RCMD TX = %47 RX = %48\n"
|
||||||
|
"\n"
|
||||||
|
"MenuItems = %49\n"
|
||||||
|
// Model data
|
||||||
|
"\n"
|
||||||
|
"fPosX,Y,Z = (%50, %51, %52)\n"
|
||||||
|
"fVelX,Y,Z = (%53, %54, %55)\n"
|
||||||
|
"fAngVelX,Y,Z = (%56, %57, %58)\n"
|
||||||
|
"fAccelX,Y,Z = (%59, %60, %61)\n"
|
||||||
|
"\n"
|
||||||
|
"Lat, Long = %62, %63\n"
|
||||||
|
"fHeightAboveTerrain = %64\n"
|
||||||
|
"\n"
|
||||||
|
"fHeading = %65 fPitch = %66 fRoll = %67\n"
|
||||||
|
)
|
||||||
|
.arg(pluginFolder)
|
||||||
|
.arg(outputFolder)
|
||||||
|
.arg(stp->structSize)
|
||||||
|
.arg(1.0 / stp->simTimeStep, 4, 'f', 1)
|
||||||
|
.arg(stp->chSimTX[Ch1Aileron], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch1Aileron], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch1Aileron], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch1Aileron], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch2Elevator], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch2Elevator], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch2Elevator], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch2Elevator], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch3Throttle], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch3Throttle], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch3Throttle], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch3Throttle], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch4Rudder], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch4Rudder], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch4Rudder], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch4Rudder], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch5], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch5], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch5], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch5], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch6], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch6], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch6], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch6], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch7], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch7], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch7], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch7], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch23Plugin1], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch23Plugin1], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch23Plugin1], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch23Plugin1], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch24Plugin2], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch24Plugin2], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch24Plugin2], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch24Plugin2], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch12FPVCamPan], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch12FPVCamPan], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch12FPVCamPan], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch12FPVCamPan], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimTX[Ch13FPVCamTilt], 5, 'f', 2)
|
||||||
|
.arg(stp->chSimRX[Ch13FPVCamTilt], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewTX[Ch13FPVCamTilt], 5, 'f', 2)
|
||||||
|
.arg(pts->chNewRX[Ch13FPVCamTilt], 5, 'f', 2)
|
||||||
|
.arg(stp->simMenuStatus)
|
||||||
|
.arg(stp->posX, 5, 'f', 2)
|
||||||
|
.arg(stp->posY, 5, 'f', 2)
|
||||||
|
.arg(stp->posZ, 5, 'f', 2)
|
||||||
|
.arg(stp->velX, 5, 'f', 2)
|
||||||
|
.arg(stp->velY, 5, 'f', 2)
|
||||||
|
.arg(stp->velZ, 5, 'f', 2)
|
||||||
|
.arg(stp->angVelXm, 5, 'f', 2)
|
||||||
|
.arg(stp->angVelYm, 5, 'f', 2)
|
||||||
|
.arg(stp->angVelZm, 5, 'f', 2)
|
||||||
|
.arg(stp->accelXm, 5, 'f', 2)
|
||||||
|
.arg(stp->accelYm, 5, 'f', 2)
|
||||||
|
.arg(stp->accelZm, 5, 'f', 2)
|
||||||
|
.arg(stp->latitude, 5, 'f', 2)
|
||||||
|
.arg(stp->longitude, 5, 'f', 2)
|
||||||
|
.arg(stp->AGL, 5, 'f', 2)
|
||||||
|
.arg(stp->heading*RAD2DEG, 5, 'f', 2)
|
||||||
|
.arg(stp->pitch*RAD2DEG, 5, 'f', 2)
|
||||||
|
.arg(stp->roll*RAD2DEG, 5, 'f', 2)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp,
|
||||||
|
pluginToSim *pts)
|
||||||
|
{
|
||||||
|
debugInfo = "---\n";
|
||||||
|
// By default do not change the Menu Items of type CheckBox
|
||||||
|
pts->newMenuStatus = stp->simMenuStatus;
|
||||||
|
// Extract Menu Commands from Flags
|
||||||
|
bool isReset = (stp->simMenuStatus & MenuCmdReset) != 0;
|
||||||
|
bool isEnable = (stp->simMenuStatus & MenuEnable) != 0;
|
||||||
|
bool isTxON = (stp->simMenuStatus & MenuTx) != 0;
|
||||||
|
bool isRxON = (stp->simMenuStatus & MenuRx) != 0;
|
||||||
|
bool isScreen = (stp->simMenuStatus & MenuScreen) != 0;
|
||||||
|
bool isNextWp = (stp->simMenuStatus & MenuNextWpt) != 0;
|
||||||
|
// Run commands
|
||||||
|
if (isReset) {
|
||||||
|
Run_Command_Reset(/*stp, pts*/);
|
||||||
|
} else if (isScreen) {
|
||||||
|
Run_Command_WindowSizeAndPos(stp, pts);
|
||||||
|
} else if (isNextWp) {
|
||||||
|
Run_Command_MoveToNextWaypoint(stp, pts);
|
||||||
|
} else {
|
||||||
|
Run_BlinkLEDs(stp, pts);
|
||||||
|
if (isEnable) {
|
||||||
|
if (isTxON)
|
||||||
|
sndr->sendDatagram(stp);
|
||||||
|
if (isRxON)
|
||||||
|
rcvr->setChannels(pts);
|
||||||
|
}
|
||||||
|
|
||||||
|
// network lag
|
||||||
|
debugInfo.append(QString("out: %1, inp: %2, delta: %3\n")
|
||||||
|
.arg(sndr->pcks() - 1)
|
||||||
|
.arg(rcvr->pcks())
|
||||||
|
.arg(sndr->pcks() - rcvr->pcks() - 1)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
// debug info is shown on the screen
|
||||||
|
InfoText(stp, pts);
|
||||||
|
pts->dbgInfoText = debugInfo.toAscii();
|
||||||
|
isFirstRun = false;
|
||||||
|
}
|
||||||
@ -0,0 +1,53 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file plugin.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef PLUGIN_H
|
||||||
|
#define PLUGIN_H
|
||||||
|
|
||||||
|
#include <QtCore>
|
||||||
|
#include <QTime>
|
||||||
|
#include <QList>
|
||||||
|
#include "aerosimrcdatastruct.h"
|
||||||
|
|
||||||
|
#define SIM_DLL_EXPORT extern "C" __declspec(dllexport)
|
||||||
|
|
||||||
|
SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes(
|
||||||
|
quint32 *sizeSimToPlugin,
|
||||||
|
quint32 *sizePluginToSim,
|
||||||
|
quint32 *sizePluginInit
|
||||||
|
);
|
||||||
|
|
||||||
|
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(
|
||||||
|
pluginInit *p
|
||||||
|
);
|
||||||
|
|
||||||
|
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(
|
||||||
|
const simToPlugin *stp,
|
||||||
|
pluginToSim *pts
|
||||||
|
);
|
||||||
|
|
||||||
|
#endif // PLUGIN_H
|
||||||
@ -0,0 +1,71 @@
|
|||||||
|
!win32 {
|
||||||
|
error("AeroSimRC plugin is only available for win32 platform")
|
||||||
|
}
|
||||||
|
|
||||||
|
include(../../../../../openpilotgcs.pri)
|
||||||
|
|
||||||
|
QT += network
|
||||||
|
QT -= gui
|
||||||
|
|
||||||
|
TEMPLATE = lib
|
||||||
|
TARGET = plugin_AeroSIMRC
|
||||||
|
|
||||||
|
RES_DIR = $${PWD}/resources
|
||||||
|
SIM_DIR = $$GCS_BUILD_TREE/../AeroSIM-RC
|
||||||
|
PLUGIN_DIR = $$SIM_DIR/Plugin/CopterControl
|
||||||
|
DLLDESTDIR = $$PLUGIN_DIR
|
||||||
|
|
||||||
|
# Don't depend on MSVRT*.dll
|
||||||
|
win32-msvc* {
|
||||||
|
QMAKE_CXXFLAGS_RELEASE -= -MD
|
||||||
|
QMAKE_CXXFLAGS_MT_DLL += -MT
|
||||||
|
}
|
||||||
|
|
||||||
|
HEADERS = \
|
||||||
|
aerosimrcdatastruct.h \
|
||||||
|
enums.h \
|
||||||
|
plugin.h \
|
||||||
|
qdebughandler.h \
|
||||||
|
udpconnect.h \
|
||||||
|
settings.h
|
||||||
|
|
||||||
|
SOURCES = \
|
||||||
|
qdebughandler.cpp \
|
||||||
|
plugin.cpp \
|
||||||
|
udpconnect.cpp \
|
||||||
|
settings.cpp
|
||||||
|
|
||||||
|
# Resemble the AeroSimRC directory structure and copy plugin files and resources
|
||||||
|
equals(copydata, 1) {
|
||||||
|
|
||||||
|
# Windows release only
|
||||||
|
win32:CONFIG(release, debug|release) {
|
||||||
|
|
||||||
|
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$PLUGIN_DIR\") $$addNewline()
|
||||||
|
|
||||||
|
# resources and sample configuration
|
||||||
|
PLUGIN_RESOURCES = \
|
||||||
|
cc_off.tga \
|
||||||
|
cc_off_hover.tga \
|
||||||
|
cc_on.tga \
|
||||||
|
cc_on_hover.tga \
|
||||||
|
cc_plugin.ini \
|
||||||
|
plugin.txt
|
||||||
|
for(res, PLUGIN_RESOURCES) {
|
||||||
|
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$RES_DIR/$$res\") $$targetPath(\"$$PLUGIN_DIR/$$res\") $$addNewline()
|
||||||
|
}
|
||||||
|
|
||||||
|
# Qt DLLs
|
||||||
|
QT_DLLS = \
|
||||||
|
libgcc_s_dw2-1.dll \
|
||||||
|
mingwm10.dll \
|
||||||
|
QtCore4.dll \
|
||||||
|
QtNetwork4.dll
|
||||||
|
for(dll, QT_DLLS) {
|
||||||
|
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_BINS]/$$dll\") $$targetPath(\"$$SIM_DIR/$$dll\") $$addNewline()
|
||||||
|
}
|
||||||
|
|
||||||
|
data_copy.target = FORCE
|
||||||
|
QMAKE_EXTRA_TARGETS += data_copy
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,64 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file qdebughandler.cpp
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "qdebughandler.h"
|
||||||
|
|
||||||
|
void myQDebugHandler(QtMsgType type, const char *msg)
|
||||||
|
{
|
||||||
|
static bool firstRun = true;
|
||||||
|
QString txt;
|
||||||
|
|
||||||
|
switch (type) {
|
||||||
|
case QtDebugMsg:
|
||||||
|
txt = QString("Debug: %1").arg(msg);
|
||||||
|
break;
|
||||||
|
case QtWarningMsg:
|
||||||
|
txt = QString("Warning: %1").arg(msg);
|
||||||
|
break;
|
||||||
|
case QtCriticalMsg:
|
||||||
|
txt = QString("Critical: %1").arg(msg);
|
||||||
|
break;
|
||||||
|
case QtFatalMsg:
|
||||||
|
txt = QString("Fatal: %1").arg(msg);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
QFile outFile("dbglog.txt");
|
||||||
|
outFile.open(QIODevice::WriteOnly | QIODevice::Append);
|
||||||
|
QTextStream ts(&outFile);
|
||||||
|
QTime time;
|
||||||
|
|
||||||
|
if (firstRun) {
|
||||||
|
ts << endl << endl;
|
||||||
|
firstRun = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
ts << time.currentTime().toString("hh:mm:ss.zzz") << " " << txt << endl;
|
||||||
|
|
||||||
|
if (type == QtFatalMsg)
|
||||||
|
abort();
|
||||||
|
}
|
||||||
@ -0,0 +1,37 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file qdebughandler.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef QDEBUGHANDLER_H
|
||||||
|
#define QDEBUGHANDLER_H
|
||||||
|
|
||||||
|
#include <QDebug>
|
||||||
|
#include <QFile>
|
||||||
|
#include <QTime>
|
||||||
|
|
||||||
|
void myQDebugHandler(QtMsgType type, const char *msg);
|
||||||
|
|
||||||
|
#endif // QDEBUGHANDLER_H
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,55 @@
|
|||||||
|
[General]
|
||||||
|
|
||||||
|
; Network settings
|
||||||
|
listen_on_host = 127.0.0.1
|
||||||
|
listen_on_port = 40200
|
||||||
|
send_to_host = 127.0.0.1
|
||||||
|
send_to_port = 40100
|
||||||
|
|
||||||
|
; Channels enumerator, applicable for the AeroSIM RC version 3.90+
|
||||||
|
all_channels = Ch1-Aileron Ch2-Elevator Ch3-Throttle Ch4-Rudder Ch5 Ch6 Ch7 Ch8 Ch9 Ch10-Retracts Ch11-Flaps Ch12-FPV-Pan Ch13-FPV-Tilt Ch14-Brakes Ch15-Spoilers Ch16-Smoke Ch17-Fire Ch18-Flight-Mode Ch19-ALT-Hold Ch20-FPV-Tilt-Hold Ch21-Reset-Model Ch22-MouseTX Ch23-Plugin-1 Ch24-Plugin-2 Ch25-Throttle-Hold Ch26-CareFree Ch27-FPV-Roll Ch28-L-Motor-Dual Ch29-R-Motor-Dual Ch30-Mix Ch31-Mix Ch32-Mix Ch33-Mix Ch34-Mix Ch35-Mix Ch36-Mix Ch37-Mix Ch38-Mix Ch39-Mix
|
||||||
|
|
||||||
|
[Input]
|
||||||
|
|
||||||
|
; Map CopterControl channels to simulator channels
|
||||||
|
; To use internal simulator channels just comment the mapping here
|
||||||
|
cc_channel_1 = Ch1-Aileron
|
||||||
|
cc_channel_2 = Ch2-Elevator
|
||||||
|
cc_channel_3 = Ch3-Throttle
|
||||||
|
cc_channel_4 = Ch4-Rudder
|
||||||
|
;cc_channel_5 = Ch27-FPV-Roll
|
||||||
|
cc_channel_6 = Ch13-FPV-Tilt
|
||||||
|
;cc_channel_7 = Ch12-FPV-Pan
|
||||||
|
;cc_channel_8 =
|
||||||
|
;cc_channel_9 =
|
||||||
|
;cc_channel_10=
|
||||||
|
|
||||||
|
; Control TX or RX (before or after mixes)
|
||||||
|
send_to = RX
|
||||||
|
|
||||||
|
[Output]
|
||||||
|
|
||||||
|
; Map simulator channels to GCS HiTL/CopterControl channels
|
||||||
|
; Only mapped channels will be sent to the GCS
|
||||||
|
sim_channel_1 = Ch1-Aileron
|
||||||
|
sim_channel_2 = Ch2-Elevator
|
||||||
|
sim_channel_3 = Ch3-Throttle
|
||||||
|
sim_channel_4 = Ch4-Rudder
|
||||||
|
sim_channel_5 = Ch23-Plugin-1
|
||||||
|
;sim_channel_6 = Ch27-FPV-Roll
|
||||||
|
sim_channel_7 = Ch13-FPV-Tilt
|
||||||
|
;sim_channel_8 = Ch12-FPV-Pan
|
||||||
|
|
||||||
|
; take values from TX or RX (before or after mixes)
|
||||||
|
take_from = TX
|
||||||
|
|
||||||
|
[Video]
|
||||||
|
|
||||||
|
; Windowed simulator mode configurations
|
||||||
|
; Each resolution defines the upper left corner and width/hight.
|
||||||
|
; User can cycle through all resolutions using the menu command.
|
||||||
|
number_of_resolutions = 2
|
||||||
|
|
||||||
|
; x, y, width, height
|
||||||
|
resolution_1 = 50 50 640 720
|
||||||
|
resolution_2 = 0 0 800 480
|
||||||
@ -0,0 +1,86 @@
|
|||||||
|
.NAME "CopterControl"
|
||||||
|
|
||||||
|
.HELP_EN "\
|
||||||
|
OpenPilot CopterControl HiTL plugin for AeroSIM RC"
|
||||||
|
|
||||||
|
.IMAGE_OFF "cc_off.tga"
|
||||||
|
.IMAGE_ON "cc_on.tga"
|
||||||
|
.IMAGE_OFF_MOUSE_HOVER "cc_off_hover.tga"
|
||||||
|
.IMAGE_ON_MOUSE_HOVER "cc_on_hover.tga"
|
||||||
|
|
||||||
|
.MENU_ITEM_00_NAME "Enable"
|
||||||
|
.MENU_ITEM_00_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_00_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_00_MOUSE_RECT_XY 89 91
|
||||||
|
.MENU_ITEM_00_MOUSE_RECT_DXDY 48 48
|
||||||
|
.MENU_ITEM_00_HIDE_MENU_ITEM 0
|
||||||
|
|
||||||
|
.MENU_ITEM_01_NAME "Transmit data"
|
||||||
|
.MENU_ITEM_01_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_01_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_01_MOUSE_RECT_XY 110 166
|
||||||
|
.MENU_ITEM_01_MOUSE_RECT_DXDY 52 34
|
||||||
|
.MENU_ITEM_01_HIDE_MENU_ITEM 0
|
||||||
|
|
||||||
|
.MENU_ITEM_02_NAME "Receive data"
|
||||||
|
.MENU_ITEM_02_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_02_HAS_SEPARATOR 1
|
||||||
|
.MENU_ITEM_02_MOUSE_RECT_XY 36 166
|
||||||
|
.MENU_ITEM_02_MOUSE_RECT_DXDY 52 34
|
||||||
|
.MENU_ITEM_02_HIDE_MENU_ITEM 0
|
||||||
|
|
||||||
|
.MENU_ITEM_03_NAME "Change window size and position"
|
||||||
|
.MENU_ITEM_03_TYPE COMMAND
|
||||||
|
.MENU_ITEM_03_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_03_MOUSE_RECT_XY 0 0
|
||||||
|
.MENU_ITEM_03_MOUSE_RECT_DXDY 0 0
|
||||||
|
.MENU_ITEM_03_HIDE_MENU_ITEM 0
|
||||||
|
|
||||||
|
.MENU_ITEM_04_NAME "Move to next waypoint"
|
||||||
|
.MENU_ITEM_04_TYPE COMMAND
|
||||||
|
.MENU_ITEM_04_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_04_MOUSE_RECT_XY 0 0
|
||||||
|
.MENU_ITEM_04_MOUSE_RECT_DXDY 0 0
|
||||||
|
.MENU_ITEM_04_HIDE_MENU_ITEM 0
|
||||||
|
|
||||||
|
.MENU_ITEM_05_NAME "no action"
|
||||||
|
.MENU_ITEM_05_TYPE COMMAND
|
||||||
|
.MENU_ITEM_05_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_05_MOUSE_RECT_XY 0 0
|
||||||
|
.MENU_ITEM_05_MOUSE_RECT_DXDY 0 0
|
||||||
|
.MENU_ITEM_05_HIDE_MENU_ITEM 0
|
||||||
|
|
||||||
|
.MENU_ITEM_06_NAME "Blue LED"
|
||||||
|
.MENU_ITEM_06_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_06_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_06_MOUSE_RECT_XY 6 40
|
||||||
|
.MENU_ITEM_06_MOUSE_RECT_DXDY 15 12
|
||||||
|
.MENU_ITEM_06_HIDE_MENU_ITEM 1
|
||||||
|
|
||||||
|
.MENU_ITEM_07_NAME "Green LED"
|
||||||
|
.MENU_ITEM_07_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_07_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_07_MOUSE_RECT_XY 6 52
|
||||||
|
.MENU_ITEM_07_MOUSE_RECT_DXDY 15 12
|
||||||
|
.MENU_ITEM_07_HIDE_MENU_ITEM 1
|
||||||
|
|
||||||
|
.MENU_ITEM_08_NAME "FlightMode 1"
|
||||||
|
.MENU_ITEM_08_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_08_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_08_MOUSE_RECT_XY 40 19
|
||||||
|
.MENU_ITEM_08_MOUSE_RECT_DXDY 38 38
|
||||||
|
.MENU_ITEM_08_HIDE_MENU_ITEM 1
|
||||||
|
|
||||||
|
.MENU_ITEM_09_NAME "FlightMode 2"
|
||||||
|
.MENU_ITEM_09_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_09_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_09_MOUSE_RECT_XY 78 19
|
||||||
|
.MENU_ITEM_09_MOUSE_RECT_DXDY 38 38
|
||||||
|
.MENU_ITEM_09_HIDE_MENU_ITEM 1
|
||||||
|
|
||||||
|
.MENU_ITEM_10_NAME "FlightMode 3"
|
||||||
|
.MENU_ITEM_10_TYPE CHECKBOX
|
||||||
|
.MENU_ITEM_10_HAS_SEPARATOR 0
|
||||||
|
.MENU_ITEM_10_MOUSE_RECT_XY 115 19
|
||||||
|
.MENU_ITEM_10_MOUSE_RECT_DXDY 38 38
|
||||||
|
.MENU_ITEM_10_HIDE_MENU_ITEM 1
|
||||||
@ -0,0 +1,98 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file settings.cpp
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "settings.h"
|
||||||
|
|
||||||
|
Settings::Settings(QString settingsPath, QObject *parent) :
|
||||||
|
QObject(parent)
|
||||||
|
{
|
||||||
|
settings = new QSettings(settingsPath + "/cc_plugin.ini", QSettings::IniFormat);
|
||||||
|
// default settings
|
||||||
|
sendToHost = "127.0.0.1";
|
||||||
|
sendToPort = 40100;
|
||||||
|
listenOnHost = "127.0.0.1";
|
||||||
|
listenOnPort = 40200;
|
||||||
|
channels.reserve(60);
|
||||||
|
for (quint8 i = 0; i < 10; ++i)
|
||||||
|
inputMap << 255;
|
||||||
|
for (quint8 i = 0; i < 8; ++i)
|
||||||
|
outputMap << 255;
|
||||||
|
sendToRX = true;
|
||||||
|
takeFromTX = true;
|
||||||
|
videoModes << 1 << 50 << 50 << 800 << 600;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Settings::read()
|
||||||
|
{
|
||||||
|
// network
|
||||||
|
listenOnHost = settings->value("listen_on_host", listenOnHost).toString();
|
||||||
|
listenOnPort = settings->value("listen_on_port", listenOnPort).toInt();
|
||||||
|
sendToHost = settings->value("send_to_host", sendToHost).toString();
|
||||||
|
sendToPort = settings->value("send_to_port", sendToPort).toInt();
|
||||||
|
|
||||||
|
QString allChannels = settings->value("all_channels").toString();
|
||||||
|
QString chan;
|
||||||
|
int i = 0;
|
||||||
|
foreach (chan, allChannels.split(" "))
|
||||||
|
channels.insert(chan, i++);
|
||||||
|
|
||||||
|
// inputs
|
||||||
|
QString num = "";
|
||||||
|
QString map = "";
|
||||||
|
for (quint8 i = 0; i < 10; ++i) {
|
||||||
|
num = QString::number(i+1);
|
||||||
|
map = settings->value("Input/cc_channel_" + num).toString();
|
||||||
|
inputMap[i] = channels.value(map, inputMap.at(i));
|
||||||
|
}
|
||||||
|
|
||||||
|
QString sendTo = settings->value("Input/send_to", "RX").toString();
|
||||||
|
sendToRX = (sendTo == "RX") ? true : false;
|
||||||
|
|
||||||
|
// outputs
|
||||||
|
for (quint8 i = 0; i < 8; ++i) {
|
||||||
|
num = QString::number(i+1);
|
||||||
|
map = settings->value("Output/sim_channel_" + num).toString();
|
||||||
|
outputMap[i] = channels.value(map, outputMap.at(i));
|
||||||
|
}
|
||||||
|
|
||||||
|
QString takeFrom = settings->value("Output/take_from", "TX").toString();
|
||||||
|
takeFromTX = (takeFrom == "TX") ? true : false;
|
||||||
|
|
||||||
|
// video
|
||||||
|
quint8 resolutionNum = settings->value("Video/number_of_resolutions", 0).toInt();
|
||||||
|
if (resolutionNum > 0) {
|
||||||
|
videoModes.clear();
|
||||||
|
videoModes << resolutionNum;
|
||||||
|
for (quint8 i = 0; i < resolutionNum; ++i) {
|
||||||
|
num = QString::number(i+1);
|
||||||
|
QString modes = settings->value("Video/resolution_" + num, "0, 0, 640, 480").toString();
|
||||||
|
QString mode;
|
||||||
|
foreach (mode, modes.split(" "))
|
||||||
|
videoModes << mode.toInt();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,67 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file settings.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef SETTINGS_H
|
||||||
|
#define SETTINGS_H
|
||||||
|
|
||||||
|
#include <QObject>
|
||||||
|
#include <QSettings>
|
||||||
|
#include <QHash>
|
||||||
|
#include <QList>
|
||||||
|
#include <QStringList>
|
||||||
|
#include <QDebug>
|
||||||
|
|
||||||
|
class Settings : public QObject
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
explicit Settings(QString settingsPath, QObject *parent = 0);
|
||||||
|
void read();
|
||||||
|
QString remoteHost() { return sendToHost; }
|
||||||
|
quint16 remotePort() { return sendToPort; }
|
||||||
|
QString localHost() { return listenOnHost; }
|
||||||
|
quint16 localPort() { return listenOnPort; }
|
||||||
|
QList<quint8> getInputMap() { return inputMap; }
|
||||||
|
QList<quint8> getOutputMap() { return outputMap; }
|
||||||
|
bool isToRX() { return sendToRX; }
|
||||||
|
bool isFromTX() { return takeFromTX; }
|
||||||
|
QList<quint16> getVideoModes() { return videoModes; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
QHash<QString, quint8> channels;
|
||||||
|
QSettings *settings;
|
||||||
|
QString sendToHost;
|
||||||
|
quint16 sendToPort;
|
||||||
|
QString listenOnHost;
|
||||||
|
quint16 listenOnPort;
|
||||||
|
QList<quint8> inputMap;
|
||||||
|
QList<quint8> outputMap;
|
||||||
|
bool sendToRX;
|
||||||
|
bool takeFromTX;
|
||||||
|
QList<quint16> videoModes;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // SETTINGS_H
|
||||||
@ -0,0 +1,228 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file udpconnect.cpp
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "udpconnect.h"
|
||||||
|
#include "enums.h"
|
||||||
|
|
||||||
|
UdpSender::UdpSender(const QList<quint8> map,
|
||||||
|
bool isTX,
|
||||||
|
QObject *parent)
|
||||||
|
: QObject(parent)
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpSender::UdpSender thread:" << thread();
|
||||||
|
outSocket = NULL;
|
||||||
|
for (int i = 0; i < 8; ++i)
|
||||||
|
channels << 0.0;
|
||||||
|
channelsMap = map;
|
||||||
|
takeFromTX = isTX;
|
||||||
|
packetsSended = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
UdpSender::~UdpSender()
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpSender::~UdpSender";
|
||||||
|
if (outSocket)
|
||||||
|
delete outSocket;
|
||||||
|
}
|
||||||
|
|
||||||
|
// public
|
||||||
|
void UdpSender::init(const QString &remoteHost, quint16 remotePort)
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpSender::init";
|
||||||
|
outHost = remoteHost;
|
||||||
|
outPort = remotePort;
|
||||||
|
outSocket = new QUdpSocket();
|
||||||
|
}
|
||||||
|
|
||||||
|
void UdpSender::sendDatagram(const simToPlugin *stp)
|
||||||
|
{
|
||||||
|
QByteArray data;
|
||||||
|
data.resize(188);
|
||||||
|
QDataStream out(&data, QIODevice::WriteOnly);
|
||||||
|
out.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
||||||
|
|
||||||
|
// magic header, "AERO"
|
||||||
|
out << quint32(0x4153494D);
|
||||||
|
// simulation step
|
||||||
|
out << stp->simTimeStep;
|
||||||
|
// home location
|
||||||
|
out << stp->initPosX << stp->initPosY << stp->initPosZ;
|
||||||
|
out << stp->wpHomeX << stp->wpHomeY << stp->wpHomeLat << stp->wpHomeLong;
|
||||||
|
// position
|
||||||
|
out << stp->posX << stp->posY << stp->posZ;
|
||||||
|
// velocity (world)
|
||||||
|
out << stp->velX << stp->velY << stp->velZ;
|
||||||
|
// angular velocity (model)
|
||||||
|
out << stp->angVelXm << stp->angVelYm << stp->angVelZm;
|
||||||
|
// acceleration (model)
|
||||||
|
out << stp->accelXm << stp->accelYm << stp->accelZm;
|
||||||
|
// coordinates
|
||||||
|
out << stp->latitude << stp->longitude;
|
||||||
|
// sonar
|
||||||
|
out << stp->AGL;
|
||||||
|
// attitude
|
||||||
|
out << stp->heading << stp->pitch << stp->roll;
|
||||||
|
// electric
|
||||||
|
out << stp->voltage << stp->current;
|
||||||
|
// matrix
|
||||||
|
out << stp->axisXx << stp->axisXy << stp->axisXz;
|
||||||
|
out << stp->axisYx << stp->axisYy << stp->axisYz;
|
||||||
|
out << stp->axisZx << stp->axisZy << stp->axisZz;
|
||||||
|
// channels
|
||||||
|
for (int i = 0; i < 8; ++i) {
|
||||||
|
quint8 mapTo = channelsMap.at(i);
|
||||||
|
if (mapTo == 255) // unused channel
|
||||||
|
out << 0.0;
|
||||||
|
else if (takeFromTX) // use values from simulators transmitter
|
||||||
|
out << stp->chSimTX[mapTo];
|
||||||
|
else // direct use values from ESC/motors/ailerons/etc
|
||||||
|
out << stp->chSimRX[mapTo];
|
||||||
|
}
|
||||||
|
|
||||||
|
// packet counter
|
||||||
|
out << packetsSended;
|
||||||
|
|
||||||
|
outSocket->writeDatagram(data, outHost, outPort);
|
||||||
|
++packetsSended;
|
||||||
|
}
|
||||||
|
|
||||||
|
//-----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
UdpReceiver::UdpReceiver(const QList<quint8> map,
|
||||||
|
bool isRX,
|
||||||
|
QObject *parent)
|
||||||
|
: QThread(parent)
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpReceiver::UdpReceiver thread:" << thread();
|
||||||
|
|
||||||
|
stopped = false;
|
||||||
|
inSocket = NULL;
|
||||||
|
for (int i = 0; i < 10; ++i)
|
||||||
|
channels << -1.0;
|
||||||
|
channelsMap = map;
|
||||||
|
sendToRX = isRX;
|
||||||
|
armed = 0;
|
||||||
|
mode = 0;
|
||||||
|
packetsRecived = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
UdpReceiver::~UdpReceiver()
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpReceiver::~UdpReceiver";
|
||||||
|
if (inSocket)
|
||||||
|
delete inSocket;
|
||||||
|
}
|
||||||
|
|
||||||
|
// public
|
||||||
|
void UdpReceiver::init(const QString &localHost, quint16 localPort)
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpReceiver::init";
|
||||||
|
|
||||||
|
inSocket = new QUdpSocket();
|
||||||
|
qDebug() << this << "inSocket constructed" << inSocket->thread();
|
||||||
|
|
||||||
|
inSocket->bind(QHostAddress(localHost), localPort);
|
||||||
|
}
|
||||||
|
|
||||||
|
void UdpReceiver::run()
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpReceiver::run start";
|
||||||
|
while (!stopped)
|
||||||
|
onReadyRead();
|
||||||
|
qDebug() << this << "UdpReceiver::run ended";
|
||||||
|
}
|
||||||
|
|
||||||
|
void UdpReceiver::stop()
|
||||||
|
{
|
||||||
|
qDebug() << this << "UdpReceiver::stop";
|
||||||
|
stopped = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void UdpReceiver::setChannels(pluginToSim *pts)
|
||||||
|
{
|
||||||
|
QMutexLocker locker(&mutex);
|
||||||
|
|
||||||
|
for (int i = 0; i < 10; ++i) {
|
||||||
|
quint8 mapTo = channelsMap.at(i);
|
||||||
|
if (mapTo != 255) {
|
||||||
|
float channelValue = qBound(-1.0f, channels.at(i), 1.0f);
|
||||||
|
if (sendToRX) {
|
||||||
|
// direct connect to ESC/motors/ailerons/etc
|
||||||
|
pts->chNewRX[mapTo] = channelValue;
|
||||||
|
pts->chOverRX[mapTo] = true;
|
||||||
|
} else {
|
||||||
|
// replace simulators transmitter
|
||||||
|
pts->chNewTX[mapTo] = channelValue;
|
||||||
|
pts->chOverTX[mapTo] = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void UdpReceiver::getFlighStatus(quint8 &arm, quint8 &mod)
|
||||||
|
{
|
||||||
|
QMutexLocker locker(&mutex);
|
||||||
|
|
||||||
|
arm = armed;
|
||||||
|
mod = mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
// private
|
||||||
|
void UdpReceiver::onReadyRead()
|
||||||
|
{
|
||||||
|
if (!inSocket->waitForReadyRead(8)) // 1/60fps ~= 16.7ms, 1/120fps ~= 8.3ms
|
||||||
|
return;
|
||||||
|
// TODO: add failsafe
|
||||||
|
// if a command not recieved then slowly return all channel to neutral
|
||||||
|
//
|
||||||
|
while (inSocket->hasPendingDatagrams()) {
|
||||||
|
QByteArray datagram;
|
||||||
|
datagram.resize(inSocket->pendingDatagramSize());
|
||||||
|
quint64 datagramSize;
|
||||||
|
datagramSize = inSocket->readDatagram(datagram.data(), datagram.size());
|
||||||
|
|
||||||
|
processDatagram(datagram);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void UdpReceiver::processDatagram(QByteArray &datagram)
|
||||||
|
{
|
||||||
|
QDataStream stream(datagram);
|
||||||
|
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
||||||
|
// check magic header
|
||||||
|
quint32 magic;
|
||||||
|
stream >> magic;
|
||||||
|
if (magic != 0x52434D44) // "RCMD"
|
||||||
|
return;
|
||||||
|
// read channels
|
||||||
|
for (int i = 0; i < 10; ++i)
|
||||||
|
stream >> channels[i];
|
||||||
|
// read flight mode
|
||||||
|
stream >> armed >> mode;
|
||||||
|
// read counter
|
||||||
|
stream >> packetsRecived;
|
||||||
|
}
|
||||||
@ -0,0 +1,90 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file udpconnect.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef UDPCONNECT_H
|
||||||
|
#define UDPCONNECT_H
|
||||||
|
|
||||||
|
#include <QObject>
|
||||||
|
#include <QUdpSocket>
|
||||||
|
#include <QList>
|
||||||
|
#include <QTime>
|
||||||
|
#include <QMutex>
|
||||||
|
#include <QMutexLocker>
|
||||||
|
#include "aerosimrcdatastruct.h"
|
||||||
|
|
||||||
|
class UdpSender : public QObject
|
||||||
|
{
|
||||||
|
// Q_OBJECT
|
||||||
|
public:
|
||||||
|
explicit UdpSender(const QList<quint8> map, bool isTX, QObject *parent = 0);
|
||||||
|
~UdpSender();
|
||||||
|
void init(const QString &remoteHost, quint16 remotePort);
|
||||||
|
void sendDatagram(const simToPlugin *stp);
|
||||||
|
quint32 pcks() { return packetsSended; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
QUdpSocket *outSocket;
|
||||||
|
QHostAddress outHost;
|
||||||
|
quint16 outPort;
|
||||||
|
QList<float> channels;
|
||||||
|
QList<quint8> channelsMap;
|
||||||
|
bool takeFromTX;
|
||||||
|
quint32 packetsSended;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class UdpReceiver : public QThread
|
||||||
|
{
|
||||||
|
// Q_OBJECT
|
||||||
|
public:
|
||||||
|
explicit UdpReceiver(const QList<quint8> map, bool isRX, QObject *parent = 0);
|
||||||
|
~UdpReceiver();
|
||||||
|
void init(const QString &localHost, quint16 localPort);
|
||||||
|
void run();
|
||||||
|
void stop();
|
||||||
|
// function getChannels for other threads
|
||||||
|
void setChannels(pluginToSim *pts);
|
||||||
|
void getFlighStatus(quint8 &arm, quint8 &mod);
|
||||||
|
quint8 getArmed() { return armed; }
|
||||||
|
quint8 getMode() { return mode; }
|
||||||
|
quint32 pcks() { return packetsRecived; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
volatile bool stopped;
|
||||||
|
QMutex mutex;
|
||||||
|
QUdpSocket *inSocket;
|
||||||
|
QList<float> channels;
|
||||||
|
QList<quint8> channelsMap;
|
||||||
|
bool sendToRX;
|
||||||
|
quint8 armed;
|
||||||
|
quint8 mode;
|
||||||
|
quint32 packetsRecived;
|
||||||
|
void onReadyRead();
|
||||||
|
void processDatagram(QByteArray &datagram);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // UDPCONNECT_H
|
||||||
@ -0,0 +1,17 @@
|
|||||||
|
include(../../../../../openpilotgcs.pri)
|
||||||
|
|
||||||
|
QT += core gui network
|
||||||
|
|
||||||
|
TEMPLATE = app
|
||||||
|
TARGET = udp_test
|
||||||
|
DESTDIR = $$GCS_APP_PATH
|
||||||
|
|
||||||
|
HEADERS += \
|
||||||
|
udptestwidget.h
|
||||||
|
|
||||||
|
SOURCES += \
|
||||||
|
udptestmain.cpp \
|
||||||
|
udptestwidget.cpp
|
||||||
|
|
||||||
|
FORMS += \
|
||||||
|
udptestwidget.ui
|
||||||
@ -0,0 +1,38 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file udptestmain.cpp
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin test utility
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <QApplication>
|
||||||
|
#include "udptestwidget.h"
|
||||||
|
|
||||||
|
int main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
QApplication a(argc, argv);
|
||||||
|
Widget w;
|
||||||
|
w.show();
|
||||||
|
|
||||||
|
return a.exec();
|
||||||
|
}
|
||||||
@ -0,0 +1,537 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file udptestwidget.cpp
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin test utility
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "udptestwidget.h"
|
||||||
|
#include "ui_udptestwidget.h"
|
||||||
|
|
||||||
|
Widget::Widget(QWidget *parent) :
|
||||||
|
QWidget(parent),
|
||||||
|
ui(new Ui::Widget)
|
||||||
|
{
|
||||||
|
ui->setupUi(this);
|
||||||
|
|
||||||
|
inSocket = NULL;
|
||||||
|
outSocket = NULL;
|
||||||
|
screenTimeout.start();
|
||||||
|
packetCounter = 0;
|
||||||
|
|
||||||
|
autoSendTimer = new QTimer(this);
|
||||||
|
connect(autoSendTimer, SIGNAL(timeout()), this, SLOT(sendDatagram()), Qt::DirectConnection);
|
||||||
|
}
|
||||||
|
|
||||||
|
Widget::~Widget()
|
||||||
|
{
|
||||||
|
if(outSocket) {
|
||||||
|
delete outSocket;
|
||||||
|
}
|
||||||
|
if(inSocket) {
|
||||||
|
delete inSocket;
|
||||||
|
}
|
||||||
|
delete ui;
|
||||||
|
}
|
||||||
|
|
||||||
|
// private slots //////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
void Widget::on_btReciveStart_clicked()
|
||||||
|
{
|
||||||
|
on_btReciveStop_clicked();
|
||||||
|
|
||||||
|
inSocket = new QUdpSocket();
|
||||||
|
QString host = ui->localHost->text();
|
||||||
|
quint16 port = ui->localPort->text().toInt();
|
||||||
|
|
||||||
|
if (inSocket->bind(QHostAddress(host), port)) {
|
||||||
|
connect(inSocket, SIGNAL(readyRead()),
|
||||||
|
this, SLOT(readDatagram()), Qt::DirectConnection);
|
||||||
|
|
||||||
|
ui->listWidget->addItem("bind ok");
|
||||||
|
ui->btReciveStop->setEnabled(1);
|
||||||
|
ui->localHost->setDisabled(1);
|
||||||
|
ui->localPort->setDisabled(1);
|
||||||
|
ui->btReciveStart->setDisabled(1);
|
||||||
|
} else {
|
||||||
|
ui->listWidget->addItem("bind error: " + inSocket->errorString());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::on_btReciveStop_clicked()
|
||||||
|
{
|
||||||
|
if(inSocket) {
|
||||||
|
delete inSocket;
|
||||||
|
inSocket = NULL;
|
||||||
|
ui->listWidget->addItem("unbind ok");
|
||||||
|
} else {
|
||||||
|
ui->listWidget->addItem("socket not found");
|
||||||
|
}
|
||||||
|
ui->btReciveStart->setEnabled(1);
|
||||||
|
ui->localHost->setEnabled(1);
|
||||||
|
ui->localPort->setEnabled(1);
|
||||||
|
ui->btReciveStop->setDisabled(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::on_btTransmitStart_clicked()
|
||||||
|
{
|
||||||
|
on_btTransmitStop_clicked();
|
||||||
|
|
||||||
|
outSocket = new QUdpSocket();
|
||||||
|
outHost = ui->simHost->text();
|
||||||
|
outPort = ui->simPort->text().toInt();
|
||||||
|
|
||||||
|
ui->listWidget->addItem("transmit started");
|
||||||
|
ui->btTransmitStop->setEnabled(1);
|
||||||
|
ui->simHost->setDisabled(1);
|
||||||
|
ui->simPort->setDisabled(1);
|
||||||
|
ui->btTransmitStart->setDisabled(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::on_btTransmitStop_clicked()
|
||||||
|
{
|
||||||
|
if(outSocket) {
|
||||||
|
delete outSocket;
|
||||||
|
outSocket = NULL;
|
||||||
|
ui->listWidget->addItem("transmit stopped");
|
||||||
|
} else {
|
||||||
|
ui->listWidget->addItem("transmit socket not found");
|
||||||
|
}
|
||||||
|
ui->btTransmitStart->setEnabled(1);
|
||||||
|
ui->simHost->setEnabled(1);
|
||||||
|
ui->simPort->setEnabled(1);
|
||||||
|
ui->btTransmitStop->setDisabled(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::readDatagram()
|
||||||
|
{
|
||||||
|
while (inSocket->hasPendingDatagrams()) {
|
||||||
|
QByteArray datagram;
|
||||||
|
datagram.resize(inSocket->pendingDatagramSize());
|
||||||
|
QHostAddress sender;
|
||||||
|
quint16 senderPort;
|
||||||
|
quint64 datagramSize = inSocket->readDatagram(datagram.data(), datagram.size(),
|
||||||
|
&sender, &senderPort);
|
||||||
|
Q_UNUSED(datagramSize);
|
||||||
|
|
||||||
|
processDatagram(datagram);
|
||||||
|
if (ui->autoAnswer->isChecked())
|
||||||
|
sendDatagram();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// private ////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
void Widget::processDatagram(const QByteArray &data)
|
||||||
|
{
|
||||||
|
QByteArray buf = data;
|
||||||
|
QDataStream stream(&buf, QIODevice::ReadOnly);
|
||||||
|
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
||||||
|
|
||||||
|
// check magic header
|
||||||
|
quint32 magic;
|
||||||
|
stream >> magic;
|
||||||
|
if (magic == 0x4153494D) { // "AERO"
|
||||||
|
float timeStep,
|
||||||
|
homeX, homeY, homeZ,
|
||||||
|
WpHX, WpHY, WpLat, WpLon,
|
||||||
|
posX, posY, posZ,
|
||||||
|
velX, velY, velZ,
|
||||||
|
angX, angY, angZ,
|
||||||
|
accX, accY, accZ,
|
||||||
|
lat, lon, alt,
|
||||||
|
head, pitch, roll,
|
||||||
|
volt, curr,
|
||||||
|
rx, ry, rz, fx, fy, fz, ux, uy, uz,
|
||||||
|
chAil, chEle, chThr, chRud, chPlg1, chPlg2, chFpv1, chFpv2;
|
||||||
|
|
||||||
|
stream >> timeStep;
|
||||||
|
stream >> homeX >> homeY >> homeZ;
|
||||||
|
stream >> WpHX >> WpHY >> WpLat >> WpLon;
|
||||||
|
stream >> posX >> posY >> posZ;
|
||||||
|
stream >> velX >> velY >> velZ;
|
||||||
|
stream >> angX >> angY >> angZ;
|
||||||
|
stream >> accX >> accY >> accZ;
|
||||||
|
stream >> lat >> lon >> alt;
|
||||||
|
stream >> head >> pitch >> roll;
|
||||||
|
stream >> volt >> curr;
|
||||||
|
stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
|
||||||
|
stream >> chAil >> chEle >> chThr >> chRud >> chPlg1 >> chPlg2 >> chFpv1 >> chFpv2;
|
||||||
|
stream >> packetCounter;
|
||||||
|
|
||||||
|
if(ui->tabWidget->currentIndex() != 0)
|
||||||
|
return;
|
||||||
|
|
||||||
|
if (screenTimeout.elapsed() < 200)
|
||||||
|
return;
|
||||||
|
|
||||||
|
ui->listWidget->clear();
|
||||||
|
/*
|
||||||
|
ui->listWidget->addItem("time step (s)");
|
||||||
|
ui->listWidget->addItem(QString("%1")
|
||||||
|
.arg(timeStep);
|
||||||
|
ui->listWidget->addItem("home location (m)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(homeX, 7, 'f', 4)
|
||||||
|
.arg(homeY, 7, 'f', 4)
|
||||||
|
.arg(homeZ, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("home waypoint");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3, %4")
|
||||||
|
.arg(WpHX, 7, 'f', 4)
|
||||||
|
.arg(WpHY, 7, 'f', 4)
|
||||||
|
.arg(WpLat, 7, 'f', 4)
|
||||||
|
.arg(WpLon, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("model position (m)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(posX, 7, 'f', 4)
|
||||||
|
.arg(posY, 7, 'f', 4)
|
||||||
|
.arg(posZ, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("model velocity (m/s)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(velX, 7, 'f', 4)
|
||||||
|
.arg(velY, 7, 'f', 4)
|
||||||
|
.arg(velZ, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("model angular velocity (rad/s)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(angX, 7, 'f', 4)
|
||||||
|
.arg(angY, 7, 'f', 4)
|
||||||
|
.arg(angZ, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("model acceleration (m/s/s)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(accX, 7, 'f', 4)
|
||||||
|
.arg(accY, 7, 'f', 4)
|
||||||
|
.arg(accZ, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("model coordinates (deg, deg, m)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(lat, 7, 'f', 4)
|
||||||
|
.arg(lon, 7, 'f', 4)
|
||||||
|
.arg(alt, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("model electrics");
|
||||||
|
ui->listWidget->addItem(QString("%1V, %2A")
|
||||||
|
.arg(volt, 7, 'f', 4)
|
||||||
|
.arg(curr, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("channels");
|
||||||
|
ui->listWidget->addItem(QString("%1 %2 %3 %4 %5 %6 %7 %8")
|
||||||
|
.arg(chAil, 6, 'f', 3)
|
||||||
|
.arg(chEle, 6, 'f', 3)
|
||||||
|
.arg(chThr, 6, 'f', 3)
|
||||||
|
.arg(chRud, 6, 'f', 3)
|
||||||
|
.arg(chPlg1, 6, 'f', 3)
|
||||||
|
.arg(chPlg2, 6, 'f', 3)
|
||||||
|
.arg(chFpv1, 6, 'f', 3)
|
||||||
|
.arg(chFpv2, 6, 'f', 3));
|
||||||
|
ui->listWidget->addItem("datagram size (bytes), packet counter");
|
||||||
|
ui->listWidget->addItem(QString("%1 %2")
|
||||||
|
.arg(data.size())
|
||||||
|
.arg(packetCounter));
|
||||||
|
*/
|
||||||
|
|
||||||
|
// matrix calculation start
|
||||||
|
// model matrix
|
||||||
|
QMatrix4x4 m = QMatrix4x4( fy, fx, -fz, 0.0,
|
||||||
|
ry, rx, -rz, 0.0,
|
||||||
|
-uy, -ux, uz, 0.0,
|
||||||
|
0.0, 0.0, 0.0, 1.0);
|
||||||
|
m.optimize();
|
||||||
|
|
||||||
|
// world matrix
|
||||||
|
QMatrix4x4 w = m.inverted();
|
||||||
|
|
||||||
|
// model quat
|
||||||
|
QQuaternion q;
|
||||||
|
asMatrix2Quat(m, q);
|
||||||
|
|
||||||
|
// model roll, pitch, yaw
|
||||||
|
QVector3D rpy;
|
||||||
|
asMatrix2RPY(m, rpy);
|
||||||
|
|
||||||
|
// sonar
|
||||||
|
float sAlt = 5.0;
|
||||||
|
if ((alt < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) {
|
||||||
|
float x = alt * qTan(roll);
|
||||||
|
float y = alt * qTan(pitch);
|
||||||
|
float h = QVector3D(x, y, alt).length();
|
||||||
|
sAlt = qMin(h, sAlt);
|
||||||
|
}
|
||||||
|
|
||||||
|
ui->listWidget->addItem("sonar altitude");
|
||||||
|
ui->listWidget->addItem(QString("%1")
|
||||||
|
.arg(sAlt, 8, 'f', 5));
|
||||||
|
ui->listWidget->addItem("vectors");
|
||||||
|
ui->listWidget->addItem(QString(" X Y Z"));
|
||||||
|
ui->listWidget->addItem(QString("R: %1 %2 %3\nF: %4 %5 %6\nU: %7 %8 %9")
|
||||||
|
.arg(rx, 8, 'f', 5).arg(ry, 8, 'f', 5).arg(rz, 8, 'f', 5)
|
||||||
|
.arg(fx, 8, 'f', 5).arg(fy, 8, 'f', 5).arg(fz, 8, 'f', 5)
|
||||||
|
.arg(ux, 8, 'f', 5).arg(uy, 8, 'f', 5).arg(uz, 8, 'f', 5));
|
||||||
|
ui->listWidget->addItem("CC model matrix");
|
||||||
|
ui->listWidget->addItem(QString(" %1 %2 %3\n %4 %5 %6\n %7 %8 %9")
|
||||||
|
.arg(m(0, 0), 8, 'f', 5).arg(m(0, 1), 8, 'f', 5).arg(m(0, 2), 8, 'f', 5)
|
||||||
|
.arg(m(1, 0), 8, 'f', 5).arg(m(1, 1), 8, 'f', 5).arg(m(1, 2), 8, 'f', 5)
|
||||||
|
.arg(m(2, 0), 8, 'f', 5).arg(m(2, 1), 8, 'f', 5).arg(m(2, 2), 8, 'f', 5));
|
||||||
|
ui->listWidget->addItem("CC world matrix");
|
||||||
|
ui->listWidget->addItem(QString(" %1 %2 %3\n %4 %5 %6\n %7 %8 %9")
|
||||||
|
.arg(w(0, 0), 8, 'f', 5).arg(w(0, 1), 8, 'f', 5).arg(w(0, 2), 8, 'f', 5)
|
||||||
|
.arg(w(1, 0), 8, 'f', 5).arg(w(1, 1), 8, 'f', 5).arg(w(1, 2), 8, 'f', 5)
|
||||||
|
.arg(w(2, 0), 8, 'f', 5).arg(w(2, 1), 8, 'f', 5).arg(w(2, 2), 8, 'f', 5));
|
||||||
|
ui->listWidget->addItem("CC quaternion");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3, %4")
|
||||||
|
.arg(q.x(), 7, 'f', 4)
|
||||||
|
.arg(q.y(), 7, 'f', 4)
|
||||||
|
.arg(q.z(), 7, 'f', 4)
|
||||||
|
.arg(q.scalar(), 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("model attitude (deg)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(roll*RAD2DEG, 7, 'f', 4)
|
||||||
|
.arg(pitch*RAD2DEG, 7, 'f', 4)
|
||||||
|
.arg(head*RAD2DEG, 7, 'f', 4));
|
||||||
|
ui->listWidget->addItem("CC attitude calculated (deg)");
|
||||||
|
ui->listWidget->addItem(QString("%1, %2, %3")
|
||||||
|
.arg(rpy.x(), 7, 'f', 4)
|
||||||
|
.arg(rpy.y(), 7, 'f', 4)
|
||||||
|
.arg(rpy.z(), 7, 'f', 4));
|
||||||
|
|
||||||
|
screenTimeout.restart();
|
||||||
|
|
||||||
|
} else if (magic == 0x52434D44) { // "RCMD"
|
||||||
|
qreal ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, ch9, ch10;
|
||||||
|
stream >> ch1 >> ch2 >> ch3 >> ch4 >> ch5 >> ch6 >> ch7 >> ch8 >> ch9 >> ch10;
|
||||||
|
quint8 armed, mode;
|
||||||
|
stream >> armed >> mode;
|
||||||
|
|
||||||
|
if(ui->tabWidget->currentIndex() == 0) {
|
||||||
|
if (screenTimeout.elapsed() < 200)
|
||||||
|
return;
|
||||||
|
ui->listWidget->clear();
|
||||||
|
ui->listWidget->addItem("channels");
|
||||||
|
ui->listWidget->addItem("CH1: " + QString::number(ch1));
|
||||||
|
ui->listWidget->addItem("CH2: " + QString::number(ch2));
|
||||||
|
ui->listWidget->addItem("CH3: " + QString::number(ch3));
|
||||||
|
ui->listWidget->addItem("CH4: " + QString::number(ch4));
|
||||||
|
ui->listWidget->addItem("CH5: " + QString::number(ch5));
|
||||||
|
ui->listWidget->addItem("CH6: " + QString::number(ch6));
|
||||||
|
ui->listWidget->addItem("CH7: " + QString::number(ch7));
|
||||||
|
ui->listWidget->addItem("CH8: " + QString::number(ch8));
|
||||||
|
ui->listWidget->addItem("CH9: " + QString::number(ch9));
|
||||||
|
ui->listWidget->addItem("CH10:" + QString::number(ch10));
|
||||||
|
ui->listWidget->addItem("armed:" + QString::number(armed));
|
||||||
|
ui->listWidget->addItem("fmode:" + QString::number(mode));
|
||||||
|
}
|
||||||
|
screenTimeout.restart();
|
||||||
|
} else {
|
||||||
|
qDebug() << "unknown magic:" << magic;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::sendDatagram()
|
||||||
|
{
|
||||||
|
if(!outSocket)
|
||||||
|
return;
|
||||||
|
|
||||||
|
float ch[10]; // = {0,0,0,0,0,0,0,0,0,0};
|
||||||
|
quint8 armed;
|
||||||
|
quint8 fmode;
|
||||||
|
const float coeff = 1.0 / 512.0;
|
||||||
|
|
||||||
|
ch[0] = ui->ch1->value() * coeff;
|
||||||
|
ch[1] = ui->ch2->value() * coeff;
|
||||||
|
ch[2] = ui->ch3->value() * coeff;
|
||||||
|
ch[3] = ui->ch4->value() * coeff;
|
||||||
|
ch[4] = ui->ch5->value() * coeff;
|
||||||
|
ch[5] = ui->ch6->value() * coeff;
|
||||||
|
ch[6] = ui->ch7->value() * coeff;
|
||||||
|
ch[7] = ui->ch8->value() * coeff;
|
||||||
|
ch[8] = ui->ch9->value() * coeff;
|
||||||
|
ch[9] = ui->ch10->value() * coeff;
|
||||||
|
|
||||||
|
armed = (ui->disarmed->isChecked()) ? 0 : (ui->arming->isChecked()) ? 1 : 2;
|
||||||
|
fmode = ui->flightMode->value();
|
||||||
|
|
||||||
|
QByteArray data;
|
||||||
|
// 50 - current size of values, 4(quint32) + 10*4(float) + 2*1(quint8) + 4(quint32)
|
||||||
|
data.resize(50);
|
||||||
|
QDataStream stream(&data, QIODevice::WriteOnly);
|
||||||
|
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
||||||
|
|
||||||
|
// magic header, "RCMD"
|
||||||
|
stream << quint32(0x52434D44);
|
||||||
|
// send channels
|
||||||
|
for (int i = 0; i < 10; ++i) {
|
||||||
|
stream << ch[i];
|
||||||
|
}
|
||||||
|
// send armed and mode
|
||||||
|
stream << armed << fmode;
|
||||||
|
// send readed counter
|
||||||
|
stream << packetCounter;
|
||||||
|
|
||||||
|
if (outSocket->writeDatagram(data, outHost, outPort) == -1) {
|
||||||
|
qDebug() << "write failed: outHost" << outHost << " "
|
||||||
|
<< "outPort " << outPort << " "
|
||||||
|
<< outSocket->errorString();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::on_autoSend_clicked()
|
||||||
|
{
|
||||||
|
autoSendTimer->start(100);
|
||||||
|
qDebug() << "timer start";
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::on_autoAnswer_clicked()
|
||||||
|
{
|
||||||
|
autoSendTimer->stop();
|
||||||
|
qDebug() << "timer stop";
|
||||||
|
}
|
||||||
|
|
||||||
|
// transfomations
|
||||||
|
|
||||||
|
void Widget::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q)
|
||||||
|
{
|
||||||
|
qreal w, x, y, z;
|
||||||
|
|
||||||
|
// w always >= 0
|
||||||
|
w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0;
|
||||||
|
x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0;
|
||||||
|
y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0;
|
||||||
|
z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0;
|
||||||
|
|
||||||
|
x = copysign(x, (m(1, 2) - m(2, 1)));
|
||||||
|
y = copysign(y, (m(2, 0) - m(0, 2)));
|
||||||
|
z = copysign(z, (m(0, 1) - m(1, 0)));
|
||||||
|
|
||||||
|
q.setScalar(w);
|
||||||
|
q.setX(x);
|
||||||
|
q.setY(y);
|
||||||
|
q.setZ(z);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::asQuat2RPY(const QQuaternion &q, QVector3D &rpy)
|
||||||
|
{
|
||||||
|
qreal roll;
|
||||||
|
qreal pitch;
|
||||||
|
qreal yaw;
|
||||||
|
|
||||||
|
const qreal d2 = 2.0;
|
||||||
|
const qreal qss = q.scalar() * q.scalar();
|
||||||
|
const qreal qxx = q.x() * q.x();
|
||||||
|
const qreal qyy = q.y() * q.y();
|
||||||
|
const qreal qzz = q.z() * q.z();
|
||||||
|
|
||||||
|
qreal test = -d2 * (q.x() * q.z() - q.scalar() * q.y());
|
||||||
|
if (qFabs(test) > 0.998) {
|
||||||
|
// ~86.3°, gimbal lock
|
||||||
|
qreal R10 = d2 * (q.x() * q.y() - q.scalar() * q.z());
|
||||||
|
qreal R11 = qss - qxx + qyy - qzz;
|
||||||
|
|
||||||
|
roll = 0.0;
|
||||||
|
pitch = copysign(M_PI_2, test);
|
||||||
|
yaw = qAtan2(-R10, R11);
|
||||||
|
} else {
|
||||||
|
qreal R12 = d2 * (q.y() * q.z() + q.scalar() * q.x());
|
||||||
|
qreal R22 = qss - qxx - qyy + qzz;
|
||||||
|
qreal R01 = d2 * (q.x() * q.y() + q.scalar() * q.z());
|
||||||
|
qreal R00 = qss + qxx - qyy - qzz;
|
||||||
|
|
||||||
|
roll = qAtan2(R12, R22);
|
||||||
|
pitch = qAsin(test);
|
||||||
|
yaw = qAtan2(R01, R00);
|
||||||
|
}
|
||||||
|
rpy.setX(RAD2DEG * roll);
|
||||||
|
rpy.setY(RAD2DEG * pitch);
|
||||||
|
rpy.setZ(RAD2DEG * yaw);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy)
|
||||||
|
{
|
||||||
|
qreal roll;
|
||||||
|
qreal pitch;
|
||||||
|
qreal yaw;
|
||||||
|
|
||||||
|
if (qFabs(m(0, 2)) > 0.998) {
|
||||||
|
// ~86.3°, gimbal lock
|
||||||
|
roll = 0.0;
|
||||||
|
pitch = copysign(M_PI_2, -m(0, 2));
|
||||||
|
yaw = qAtan2(-m(1, 0), m(1, 1));
|
||||||
|
} else {
|
||||||
|
roll = qAtan2(m(1, 2), m(2, 2));
|
||||||
|
pitch = qAsin(-m(0, 2));
|
||||||
|
yaw = qAtan2(m(0, 1), m(0, 0));
|
||||||
|
}
|
||||||
|
|
||||||
|
rpy.setX(roll * RAD2DEG);
|
||||||
|
rpy.setY(pitch * RAD2DEG);
|
||||||
|
rpy.setZ(yaw * RAD2DEG);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* // not used
|
||||||
|
|
||||||
|
void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q)
|
||||||
|
{
|
||||||
|
float phi, theta, psi;
|
||||||
|
float cphi, sphi, ctheta, stheta, cpsi, spsi;
|
||||||
|
|
||||||
|
phi = rpy.x() / 2;
|
||||||
|
theta = rpy.y() / 2;
|
||||||
|
psi = rpy.z() / 2;
|
||||||
|
cphi = cosf(phi);
|
||||||
|
sphi = sinf(phi);
|
||||||
|
ctheta = cosf(theta);
|
||||||
|
stheta = sinf(theta);
|
||||||
|
cpsi = cosf(psi);
|
||||||
|
spsi = sinf(psi);
|
||||||
|
|
||||||
|
q.setScalar(cphi * ctheta * cpsi + sphi * stheta * spsi);
|
||||||
|
q.setX(sphi * ctheta * cpsi - cphi * stheta * spsi);
|
||||||
|
q.setY(cphi * stheta * cpsi + sphi * ctheta * spsi);
|
||||||
|
q.setZ(cphi * ctheta * spsi - sphi * stheta * cpsi);
|
||||||
|
|
||||||
|
if (q.scalar() < 0) { // q0 always positive for uniqueness
|
||||||
|
q.setScalar(-q.scalar());
|
||||||
|
q.setX(-q.x());
|
||||||
|
q.setY(-q.y());
|
||||||
|
q.setZ(-q.z());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m)
|
||||||
|
{
|
||||||
|
float q0s = q.scalar() * q.scalar();
|
||||||
|
float q1s = q.x() * q.x();
|
||||||
|
float q2s = q.y() * q.y();
|
||||||
|
float q3s = q.z() * q.z();
|
||||||
|
|
||||||
|
float m00 = q0s + q1s - q2s - q3s;
|
||||||
|
float m01 = 2 * (q.x() * q.y() + q.scalar() * q.z());
|
||||||
|
float m02 = 2 * (q.x() * q.z() - q.scalar() * q.y());
|
||||||
|
float m10 = 2 * (q.x() * q.y() - q.scalar() * q.z());
|
||||||
|
float m11 = q0s - q1s + q2s - q3s;
|
||||||
|
float m12 = 2 * (q.y() * q.z() + q.scalar() * q.x());
|
||||||
|
float m20 = 2 * (q.x() * q.z() + q.scalar() * q.y());
|
||||||
|
float m21 = 2 * (q.y() * q.z() - q.scalar() * q.x());
|
||||||
|
float m22 = q0s - q1s - q2s + q3s;
|
||||||
|
|
||||||
|
m = QMatrix4x4(m00, m01, m02, 0,
|
||||||
|
m10, m11, m12, 0,
|
||||||
|
m20, m21, m22, 0,
|
||||||
|
0, 0, 0, 1);
|
||||||
|
}
|
||||||
|
*/
|
||||||
@ -0,0 +1,94 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file udptestwidget.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup 3rdParty Third-party integration
|
||||||
|
* @{
|
||||||
|
* @addtogroup AeroSimRC AeroSimRC proxy plugin
|
||||||
|
* @{
|
||||||
|
* @brief AeroSimRC simulator to HITL proxy plugin test utility
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef UDPTESTWIDGET_H
|
||||||
|
#define UDPTESTWIDGET_H
|
||||||
|
|
||||||
|
#include <QWidget>
|
||||||
|
#include <QUdpSocket>
|
||||||
|
#include <QTime>
|
||||||
|
#if defined(Q_CC_MSVC)
|
||||||
|
#define _USE_MATH_DEFINES
|
||||||
|
#endif
|
||||||
|
#include <qmath.h>
|
||||||
|
#include <QVector3D>
|
||||||
|
#include <QMatrix4x4>
|
||||||
|
#include <QDebug>
|
||||||
|
#include <QTimer>
|
||||||
|
|
||||||
|
namespace Ui {
|
||||||
|
class Widget;
|
||||||
|
}
|
||||||
|
|
||||||
|
const float RAD2DEG = (float)(180.0/M_PI);
|
||||||
|
const float DEG2RAD = (float)(M_PI/180.0);
|
||||||
|
|
||||||
|
class Widget : public QWidget
|
||||||
|
{
|
||||||
|
Q_OBJECT
|
||||||
|
|
||||||
|
public:
|
||||||
|
explicit Widget(QWidget *parent = 0);
|
||||||
|
~Widget();
|
||||||
|
|
||||||
|
private slots:
|
||||||
|
void on_btReciveStart_clicked();
|
||||||
|
void on_btReciveStop_clicked();
|
||||||
|
void on_btTransmitStart_clicked();
|
||||||
|
void on_btTransmitStop_clicked();
|
||||||
|
|
||||||
|
void readDatagram();
|
||||||
|
void sendDatagram();
|
||||||
|
|
||||||
|
void on_autoSend_clicked();
|
||||||
|
|
||||||
|
void on_autoAnswer_clicked();
|
||||||
|
|
||||||
|
private:
|
||||||
|
Ui::Widget *ui;
|
||||||
|
|
||||||
|
QTime screenTimeout;
|
||||||
|
QUdpSocket *inSocket;
|
||||||
|
QUdpSocket *outSocket;
|
||||||
|
QHostAddress outHost;
|
||||||
|
quint16 outPort;
|
||||||
|
quint32 packetCounter;
|
||||||
|
|
||||||
|
void processDatagram(const QByteArray &data);
|
||||||
|
QTimer *autoSendTimer;
|
||||||
|
|
||||||
|
void asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q);
|
||||||
|
void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy);
|
||||||
|
void asQuat2RPY(const QQuaternion &q, QVector3D &rpy);
|
||||||
|
|
||||||
|
/* // not used
|
||||||
|
* void ccRPY2Quat(const QVector3D &rpy, QQuaternion &q);
|
||||||
|
* void ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m);
|
||||||
|
*/
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // UDPTESTWIDGET_H
|
||||||
@ -0,0 +1,940 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<ui version="4.0">
|
||||||
|
<class>Widget</class>
|
||||||
|
<widget class="QWidget" name="Widget">
|
||||||
|
<property name="geometry">
|
||||||
|
<rect>
|
||||||
|
<x>0</x>
|
||||||
|
<y>0</y>
|
||||||
|
<width>440</width>
|
||||||
|
<height>525</height>
|
||||||
|
</rect>
|
||||||
|
</property>
|
||||||
|
<property name="windowTitle">
|
||||||
|
<string notr="true">udp_test</string>
|
||||||
|
</property>
|
||||||
|
<property name="windowFilePath">
|
||||||
|
<string notr="true"/>
|
||||||
|
</property>
|
||||||
|
<layout class="QVBoxLayout" name="verticalLayout">
|
||||||
|
<item>
|
||||||
|
<layout class="QGridLayout" name="gridLayout">
|
||||||
|
<item row="0" column="4">
|
||||||
|
<widget class="QPushButton" name="btReciveStart">
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>60</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Connect</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="3">
|
||||||
|
<widget class="QLineEdit" name="localPort">
|
||||||
|
<property name="enabled">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>50</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">40100</string>
|
||||||
|
</property>
|
||||||
|
<property name="maxLength">
|
||||||
|
<number>5</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="1">
|
||||||
|
<widget class="QLineEdit" name="localHost">
|
||||||
|
<property name="enabled">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">127.0.0.1</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="0">
|
||||||
|
<widget class="QLabel" name="lbLocalHost">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">sim host</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="0">
|
||||||
|
<widget class="QLabel" name="lbSimHost">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">local host:</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="2">
|
||||||
|
<widget class="QLabel" name="lbSimPort">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">local port:</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="5">
|
||||||
|
<widget class="QPushButton" name="btReciveStop">
|
||||||
|
<property name="enabled">
|
||||||
|
<bool>false</bool>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>60</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Disconnect</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="1">
|
||||||
|
<widget class="QLineEdit" name="simHost">
|
||||||
|
<property name="enabled">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">127.0.0.1</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="2">
|
||||||
|
<widget class="QLabel" name="lbLocalPort">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">sim port</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="3">
|
||||||
|
<widget class="QLineEdit" name="simPort">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>50</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">40200</string>
|
||||||
|
</property>
|
||||||
|
<property name="maxLength">
|
||||||
|
<number>5</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="4">
|
||||||
|
<widget class="QPushButton" name="btTransmitStart">
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>60</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Transmit</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="5">
|
||||||
|
<widget class="QPushButton" name="btTransmitStop">
|
||||||
|
<property name="enabled">
|
||||||
|
<bool>false</bool>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>60</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Stop</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QTabWidget" name="tabWidget">
|
||||||
|
<property name="currentIndex">
|
||||||
|
<number>0</number>
|
||||||
|
</property>
|
||||||
|
<widget class="QWidget" name="tab_1">
|
||||||
|
<attribute name="title">
|
||||||
|
<string notr="true">raw data</string>
|
||||||
|
</attribute>
|
||||||
|
<layout class="QVBoxLayout" name="verticalLayout_2">
|
||||||
|
<item>
|
||||||
|
<widget class="QListWidget" name="listWidget">
|
||||||
|
<property name="font">
|
||||||
|
<font>
|
||||||
|
<family>Terminal</family>
|
||||||
|
<pointsize>10</pointsize>
|
||||||
|
</font>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
<widget class="QWidget" name="tab_2">
|
||||||
|
<attribute name="title">
|
||||||
|
<string notr="true">channels</string>
|
||||||
|
</attribute>
|
||||||
|
<layout class="QVBoxLayout" name="verticalLayout_3">
|
||||||
|
<property name="spacing">
|
||||||
|
<number>0</number>
|
||||||
|
</property>
|
||||||
|
<property name="bottomMargin">
|
||||||
|
<number>9</number>
|
||||||
|
</property>
|
||||||
|
<item>
|
||||||
|
<widget class="QGroupBox" name="groupBox">
|
||||||
|
<property name="title">
|
||||||
|
<string>send data</string>
|
||||||
|
</property>
|
||||||
|
<layout class="QHBoxLayout" name="horizontalLayout_2">
|
||||||
|
<property name="spacing">
|
||||||
|
<number>6</number>
|
||||||
|
</property>
|
||||||
|
<property name="leftMargin">
|
||||||
|
<number>9</number>
|
||||||
|
</property>
|
||||||
|
<property name="topMargin">
|
||||||
|
<number>0</number>
|
||||||
|
</property>
|
||||||
|
<property name="bottomMargin">
|
||||||
|
<number>3</number>
|
||||||
|
</property>
|
||||||
|
<item>
|
||||||
|
<widget class="QRadioButton" name="autoAnswer">
|
||||||
|
<property name="enabled">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">answer on recieve</string>
|
||||||
|
</property>
|
||||||
|
<property name="checked">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QRadioButton" name="autoSend">
|
||||||
|
<property name="enabled">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">auto send</string>
|
||||||
|
</property>
|
||||||
|
<property name="checked">
|
||||||
|
<bool>false</bool>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QGroupBox" name="groupBox_2">
|
||||||
|
<property name="title">
|
||||||
|
<string>Flight mode</string>
|
||||||
|
</property>
|
||||||
|
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||||
|
<property name="spacing">
|
||||||
|
<number>6</number>
|
||||||
|
</property>
|
||||||
|
<property name="leftMargin">
|
||||||
|
<number>9</number>
|
||||||
|
</property>
|
||||||
|
<property name="topMargin">
|
||||||
|
<number>0</number>
|
||||||
|
</property>
|
||||||
|
<property name="bottomMargin">
|
||||||
|
<number>3</number>
|
||||||
|
</property>
|
||||||
|
<item>
|
||||||
|
<widget class="QSlider" name="flightMode">
|
||||||
|
<property name="maximum">
|
||||||
|
<number>6</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>1</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>1</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QGroupBox" name="groupBox_3">
|
||||||
|
<property name="title">
|
||||||
|
<string>Armed state</string>
|
||||||
|
</property>
|
||||||
|
<layout class="QHBoxLayout" name="horizontalLayout_3">
|
||||||
|
<property name="spacing">
|
||||||
|
<number>6</number>
|
||||||
|
</property>
|
||||||
|
<property name="leftMargin">
|
||||||
|
<number>9</number>
|
||||||
|
</property>
|
||||||
|
<property name="topMargin">
|
||||||
|
<number>0</number>
|
||||||
|
</property>
|
||||||
|
<property name="bottomMargin">
|
||||||
|
<number>3</number>
|
||||||
|
</property>
|
||||||
|
<item>
|
||||||
|
<widget class="QRadioButton" name="disarmed">
|
||||||
|
<property name="text">
|
||||||
|
<string>Disarmed</string>
|
||||||
|
</property>
|
||||||
|
<property name="checked">
|
||||||
|
<bool>true</bool>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QRadioButton" name="arming">
|
||||||
|
<property name="text">
|
||||||
|
<string>Arming</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QRadioButton" name="armed">
|
||||||
|
<property name="text">
|
||||||
|
<string>Armed</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QGroupBox" name="groupBox_4">
|
||||||
|
<property name="title">
|
||||||
|
<string>Channels</string>
|
||||||
|
</property>
|
||||||
|
<layout class="QGridLayout" name="gridLayout_3">
|
||||||
|
<property name="leftMargin">
|
||||||
|
<number>9</number>
|
||||||
|
</property>
|
||||||
|
<property name="topMargin">
|
||||||
|
<number>0</number>
|
||||||
|
</property>
|
||||||
|
<property name="rightMargin">
|
||||||
|
<number>9</number>
|
||||||
|
</property>
|
||||||
|
<property name="bottomMargin">
|
||||||
|
<number>3</number>
|
||||||
|
</property>
|
||||||
|
<property name="spacing">
|
||||||
|
<number>3</number>
|
||||||
|
</property>
|
||||||
|
<item row="0" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH1n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">CH1</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="1">
|
||||||
|
<widget class="QSlider" name="ch1">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="0" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH1">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH2n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">CH2</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="1">
|
||||||
|
<widget class="QSlider" name="ch2">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="1" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH2">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="2" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH3n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">CH3</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="2" column="1">
|
||||||
|
<widget class="QSlider" name="ch3">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="2" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH3">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="3" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH4n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">CH4</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="3" column="1">
|
||||||
|
<widget class="QSlider" name="ch4">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="3" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH4">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="4" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH5n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">CH5</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="4" column="1">
|
||||||
|
<widget class="QSlider" name="ch5">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="4" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH5">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="5" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH6n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">CH6</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="5" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH6">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="6" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH7n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Ch7</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="6" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH7">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="7" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH8n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Ch8</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="7" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH8">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="8" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH9n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Ch9</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="8" column="1">
|
||||||
|
<widget class="QSlider" name="ch9">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="8" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH9">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="9" column="0">
|
||||||
|
<widget class="QLabel" name="lbCH10n">
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">Ch10</string>
|
||||||
|
</property>
|
||||||
|
<property name="alignment">
|
||||||
|
<set>Qt::AlignCenter</set>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="9" column="1">
|
||||||
|
<widget class="QSlider" name="ch10">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="9" column="2">
|
||||||
|
<widget class="QLabel" name="lbCH10">
|
||||||
|
<property name="sizePolicy">
|
||||||
|
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||||
|
<horstretch>0</horstretch>
|
||||||
|
<verstretch>0</verstretch>
|
||||||
|
</sizepolicy>
|
||||||
|
</property>
|
||||||
|
<property name="minimumSize">
|
||||||
|
<size>
|
||||||
|
<width>30</width>
|
||||||
|
<height>0</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
<property name="text">
|
||||||
|
<string notr="true">0</string>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="5" column="1">
|
||||||
|
<widget class="QSlider" name="ch6">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="6" column="1">
|
||||||
|
<widget class="QSlider" name="ch7">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item row="7" column="1">
|
||||||
|
<widget class="QSlider" name="ch8">
|
||||||
|
<property name="minimum">
|
||||||
|
<number>-511</number>
|
||||||
|
</property>
|
||||||
|
<property name="maximum">
|
||||||
|
<number>512</number>
|
||||||
|
</property>
|
||||||
|
<property name="singleStep">
|
||||||
|
<number>16</number>
|
||||||
|
</property>
|
||||||
|
<property name="pageStep">
|
||||||
|
<number>32</number>
|
||||||
|
</property>
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickPosition">
|
||||||
|
<enum>QSlider::TicksAbove</enum>
|
||||||
|
</property>
|
||||||
|
<property name="tickInterval">
|
||||||
|
<number>128</number>
|
||||||
|
</property>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<spacer name="verticalSpacer">
|
||||||
|
<property name="orientation">
|
||||||
|
<enum>Qt::Vertical</enum>
|
||||||
|
</property>
|
||||||
|
<property name="sizeHint" stdset="0">
|
||||||
|
<size>
|
||||||
|
<width>20</width>
|
||||||
|
<height>40</height>
|
||||||
|
</size>
|
||||||
|
</property>
|
||||||
|
</spacer>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
</widget>
|
||||||
|
</item>
|
||||||
|
</layout>
|
||||||
|
</widget>
|
||||||
|
<layoutdefault spacing="6" margin="11"/>
|
||||||
|
<resources/>
|
||||||
|
<connections/>
|
||||||
|
</ui>
|
||||||
507
ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.cpp
Normal file
507
ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.cpp
Normal file
@ -0,0 +1,507 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file aerosimrc.cpp
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup GCSPlugins GCS Plugins
|
||||||
|
* @{
|
||||||
|
* @addtogroup HITLPlugin HITLv2 Plugin
|
||||||
|
* @{
|
||||||
|
* @brief The Hardware In The Loop plugin version 2
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "aerosimrcsimulator.h"
|
||||||
|
#include <extensionsystem/pluginmanager.h>
|
||||||
|
#include <coreplugin/icore.h>
|
||||||
|
#include <coreplugin/threadmanager.h>
|
||||||
|
|
||||||
|
AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms)
|
||||||
|
: Simulator(params)
|
||||||
|
{
|
||||||
|
udpCounterASrecv = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
AeroSimRCSimulator::~AeroSimRCSimulator()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AeroSimRCSimulator::setupProcess()
|
||||||
|
{
|
||||||
|
QMutexLocker locker(&lock);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort)
|
||||||
|
{
|
||||||
|
Q_UNUSED(outPort)
|
||||||
|
if (inSocket->bind(QHostAddress(host), inPort))
|
||||||
|
emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n");
|
||||||
|
else
|
||||||
|
emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
void AeroSimRCSimulator::transmitUpdate()
|
||||||
|
{
|
||||||
|
// read actuator output
|
||||||
|
ActuatorCommand::DataFields actCmdData;
|
||||||
|
actCmdData = actCommand->getData();
|
||||||
|
float channels[10];
|
||||||
|
for (int i = 0; i < 10; ++i) {
|
||||||
|
qint16 ch = actCmdData.Channel[i];
|
||||||
|
float out = -1.0;
|
||||||
|
if (ch >= 1000 && ch <= 2000) {
|
||||||
|
ch -= 1000;
|
||||||
|
out = ((float)ch / 500.0) - 1.0;
|
||||||
|
}
|
||||||
|
channels[i] = out;
|
||||||
|
}
|
||||||
|
|
||||||
|
// read flight status
|
||||||
|
FlightStatus::DataFields flightData;
|
||||||
|
flightData = flightStatus->getData();
|
||||||
|
quint8 armed;
|
||||||
|
quint8 mode;
|
||||||
|
armed = flightData.Armed;
|
||||||
|
mode = flightData.FlightMode;
|
||||||
|
|
||||||
|
QByteArray data;
|
||||||
|
// 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32)
|
||||||
|
data.resize(50);
|
||||||
|
QDataStream stream(&data, QIODevice::WriteOnly);
|
||||||
|
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
||||||
|
stream << quint32(0x52434D44); // magic header, "RCMD"
|
||||||
|
for (int i = 0; i < 10; ++i)
|
||||||
|
stream << channels[i]; // channels
|
||||||
|
stream << armed << mode; // flight status
|
||||||
|
stream << udpCounterASrecv; // packet counter
|
||||||
|
|
||||||
|
if (outSocket->writeDatagram(data, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1) {
|
||||||
|
qDebug() << "write failed: " << outSocket->errorString();
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef DBG_TIMERS
|
||||||
|
static int cntTX = 0;
|
||||||
|
if (cntTX >= 100) {
|
||||||
|
qDebug() << "TX=" << 1000.0 * 100 / timeTX.restart();
|
||||||
|
cntTX = 0;
|
||||||
|
} else {
|
||||||
|
++cntTX;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
||||||
|
{
|
||||||
|
// check size
|
||||||
|
if (data.size() > 188) {
|
||||||
|
qDebug() << "!!! big datagram: " << data.size();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
QByteArray buf = data;
|
||||||
|
QDataStream stream(&buf, QIODevice::ReadOnly);
|
||||||
|
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
||||||
|
|
||||||
|
// check magic header
|
||||||
|
quint32 magic;
|
||||||
|
stream >> magic;
|
||||||
|
if (magic != 0x4153494D) { // "AERO"
|
||||||
|
qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
float timeStep,
|
||||||
|
homeX, homeY, homeZ,
|
||||||
|
WpHX, WpHY, WpLat, WpLon,
|
||||||
|
posX, posY, posZ, // world
|
||||||
|
velX, velY, velZ, // world
|
||||||
|
angX, angY, angZ, // model
|
||||||
|
accX, accY, accZ, // model
|
||||||
|
lat, lon, agl, // world
|
||||||
|
yaw, pitch, roll, // model
|
||||||
|
volt, curr,
|
||||||
|
rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix
|
||||||
|
ch[8];
|
||||||
|
|
||||||
|
stream >> timeStep;
|
||||||
|
stream >> homeX >> homeY >> homeZ;
|
||||||
|
stream >> WpHX >> WpHY >> WpLat >> WpLon;
|
||||||
|
stream >> posX >> posY >> posZ;
|
||||||
|
stream >> velX >> velY >> velZ;
|
||||||
|
stream >> angX >> angY >> angZ;
|
||||||
|
stream >> accX >> accY >> accZ;
|
||||||
|
stream >> lat >> lon >> agl;
|
||||||
|
stream >> yaw >> pitch >> roll;
|
||||||
|
stream >> volt >> curr;
|
||||||
|
stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
|
||||||
|
stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7];
|
||||||
|
stream >> udpCounterASrecv;
|
||||||
|
|
||||||
|
Output2OP out;
|
||||||
|
memset(&out, 0, sizeof(Output2OP));
|
||||||
|
|
||||||
|
|
||||||
|
// struct Output2OP{
|
||||||
|
// float latitude;
|
||||||
|
// float longitude;
|
||||||
|
// float altitude;
|
||||||
|
// float heading;
|
||||||
|
// float groundspeed; //[m/s]
|
||||||
|
// float calibratedAirspeed; //[m/s]
|
||||||
|
// float pitch;
|
||||||
|
// float roll;
|
||||||
|
// float pressure;
|
||||||
|
// float temperature;
|
||||||
|
// float velNorth; //[m/s]
|
||||||
|
// float velEast; //[m/s]
|
||||||
|
// float velDown; //[m/s]
|
||||||
|
// float dstN; //[m]
|
||||||
|
// float dstE; //[m]
|
||||||
|
// float dstD; //[m]
|
||||||
|
// float accX; //[m/s^2]
|
||||||
|
// float accY; //[m/s^2]
|
||||||
|
// float accZ; //[m/s^2]
|
||||||
|
// float rollRate;
|
||||||
|
// float pitchRate;
|
||||||
|
// float yawRate;
|
||||||
|
// };
|
||||||
|
|
||||||
|
|
||||||
|
/**********************************************************************************************/
|
||||||
|
QTime currentTime = QTime::currentTime();
|
||||||
|
/**********************************************************************************************/
|
||||||
|
// static bool firstRun = true;
|
||||||
|
// if (settings.homeLocation) {
|
||||||
|
// if (firstRun) {
|
||||||
|
// HomeLocation::DataFields homeData;
|
||||||
|
// homeData = posHome->getData();
|
||||||
|
|
||||||
|
// homeData.Latitude = WpLat * 10e6;
|
||||||
|
// homeData.Longitude = WpLon * 10e6;
|
||||||
|
// homeData.Altitude = homeZ;
|
||||||
|
// homeData.Set = HomeLocation::SET_TRUE;
|
||||||
|
|
||||||
|
// posHome->setData(homeData);
|
||||||
|
|
||||||
|
// firstRun = false;
|
||||||
|
// }
|
||||||
|
// if (settings.homeLocRate > 0) {
|
||||||
|
// static QTime homeLocTime = currentTime;
|
||||||
|
// if (homeLocTime.secsTo(currentTime) >= settings.homeLocRate) {
|
||||||
|
// firstRun = true;
|
||||||
|
// homeLocTime = currentTime;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
/**********************************************************************************************/
|
||||||
|
// if (settings.attRaw || settings.attActual) {
|
||||||
|
QMatrix4x4 mat;
|
||||||
|
mat = QMatrix4x4( fy, fx, -fz, 0.0, // model matrix
|
||||||
|
ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z)
|
||||||
|
-uy, -ux, uz, 0.0,
|
||||||
|
0.0, 0.0, 0.0, 1.0);
|
||||||
|
mat.optimize();
|
||||||
|
|
||||||
|
QQuaternion quat; // model quat
|
||||||
|
asMatrix2Quat(mat, quat);
|
||||||
|
|
||||||
|
// rotate gravity
|
||||||
|
QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z)
|
||||||
|
QVector3D gee = QVector3D(0.0, 0.0, -GEE);
|
||||||
|
QQuaternion qWorld = quat.conjugate();
|
||||||
|
gee = qWorld.rotatedVector(gee);
|
||||||
|
acc += gee;
|
||||||
|
|
||||||
|
/*************************************************************************************/
|
||||||
|
// if (settings.attRaw) {
|
||||||
|
out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z)
|
||||||
|
out.pitchRate = angX * RAD2DEG;
|
||||||
|
out.yawRate = angZ * -RAD2DEG;
|
||||||
|
|
||||||
|
out.accX = acc.x();
|
||||||
|
out.accY = acc.y();
|
||||||
|
out.accZ = acc.z();
|
||||||
|
// }
|
||||||
|
/*************************************************************************************/
|
||||||
|
QVector3D rpy; // model roll, pitch, yaw
|
||||||
|
asMatrix2RPY(mat, rpy);
|
||||||
|
|
||||||
|
out.roll = rpy.x();
|
||||||
|
out.pitch = rpy.y();
|
||||||
|
out.heading = rpy.z();
|
||||||
|
|
||||||
|
// if (settings.attActHW) {
|
||||||
|
// // do nothing
|
||||||
|
// /*****************************************/
|
||||||
|
// } else if (settings.attActSim) {
|
||||||
|
// // take all data from simulator
|
||||||
|
// AttitudeActual::DataFields attActData;
|
||||||
|
// attActData = attActual->getData();
|
||||||
|
|
||||||
|
|
||||||
|
// attActData.Roll = rpy.x();
|
||||||
|
// attActData.Pitch = rpy.y();
|
||||||
|
// attActData.Yaw = rpy.z();
|
||||||
|
// attActData.q1 = quat.scalar();
|
||||||
|
// attActData.q2 = quat.x();
|
||||||
|
// attActData.q3 = quat.y();
|
||||||
|
// attActData.q4 = quat.z();
|
||||||
|
|
||||||
|
// attActual->setData(attActData);
|
||||||
|
// /*****************************************/
|
||||||
|
// } else if (settings.attActCalc) {
|
||||||
|
// // calculate RPY with code from Attitude module
|
||||||
|
// AttitudeActual::DataFields attActData;
|
||||||
|
// attActData = attActual->getData();
|
||||||
|
|
||||||
|
// static float q[4] = {1, 0, 0, 0};
|
||||||
|
// static float gyro_correct_int2 = 0;
|
||||||
|
|
||||||
|
// float dT = timeStep;
|
||||||
|
|
||||||
|
// AttitudeSettings::DataFields attSettData = attSettings->getData();
|
||||||
|
// float accelKp = attSettData.AccelKp * 0.1666666666666667;
|
||||||
|
// float accelKi = attSettData.AccelKp * 0.1666666666666667;
|
||||||
|
// float yawBiasRate = attSettData.YawBiasRate;
|
||||||
|
|
||||||
|
// // calibrate sensors on arming
|
||||||
|
// if (flightStatus->getData().Armed == FlightStatus::ARMED_ARMING) {
|
||||||
|
// accelKp = 2.0;
|
||||||
|
// accelKi = 0.9;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// float gyro[3] = {angY * RAD2DEG, angX * RAD2DEG, angZ * -RAD2DEG};
|
||||||
|
// float attRawAcc[3] = {acc.x(), acc.y(), acc.z()};
|
||||||
|
|
||||||
|
// // code from Attitude module begin ///////////////////////////////
|
||||||
|
// float *accels = attRawAcc;
|
||||||
|
// float grot[3];
|
||||||
|
// float accel_err[3];
|
||||||
|
|
||||||
|
// // Rotate gravity to body frame and cross with accels
|
||||||
|
// grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
|
||||||
|
// grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
|
||||||
|
// grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
|
||||||
|
|
||||||
|
// // CrossProduct
|
||||||
|
// {
|
||||||
|
// accel_err[0] = accels[1]*grot[2] - grot[1]*accels[2];
|
||||||
|
// accel_err[1] = grot[0]*accels[2] - accels[0]*grot[2];
|
||||||
|
// accel_err[2] = accels[0]*grot[1] - grot[0]*accels[1];
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // Account for accel magnitude
|
||||||
|
// float accel_mag = sqrt(accels[0] * accels[0] + accels[1] * accels[1] + accels[2] * accels[2]);
|
||||||
|
// accel_err[0] /= accel_mag;
|
||||||
|
// accel_err[1] /= accel_mag;
|
||||||
|
// accel_err[2] /= accel_mag;
|
||||||
|
|
||||||
|
// // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
||||||
|
// gyro_correct_int2 += -gyro[2] * yawBiasRate;
|
||||||
|
|
||||||
|
// // Correct rates based on error, integral component dealt with in updateSensors
|
||||||
|
// gyro[0] += accel_err[0] * accelKp / dT;
|
||||||
|
// gyro[1] += accel_err[1] * accelKp / dT;
|
||||||
|
// gyro[2] += accel_err[2] * accelKp / dT + gyro_correct_int2;
|
||||||
|
|
||||||
|
// // Work out time derivative from INSAlgo writeup
|
||||||
|
// // Also accounts for the fact that gyros are in deg/s
|
||||||
|
// float qdot[4];
|
||||||
|
// qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||||
|
// qdot[1] = (+q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||||
|
// qdot[2] = (+q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||||
|
// qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
|
||||||
|
|
||||||
|
// // Take a time step
|
||||||
|
// q[0] += qdot[0];
|
||||||
|
// q[1] += qdot[1];
|
||||||
|
// q[2] += qdot[2];
|
||||||
|
// q[3] += qdot[3];
|
||||||
|
|
||||||
|
// if(q[0] < 0) {
|
||||||
|
// q[0] = -q[0];
|
||||||
|
// q[1] = -q[1];
|
||||||
|
// q[2] = -q[2];
|
||||||
|
// q[3] = -q[3];
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // Renomalize
|
||||||
|
// float qmag = sqrt((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
|
||||||
|
// q[0] /= qmag;
|
||||||
|
// q[1] /= qmag;
|
||||||
|
// q[2] /= qmag;
|
||||||
|
// q[3] /= qmag;
|
||||||
|
|
||||||
|
// // If quaternion has become inappropriately short or is nan reinit.
|
||||||
|
// // THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||||
|
// if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
|
||||||
|
// q[0] = 1;
|
||||||
|
// q[1] = 0;
|
||||||
|
// q[2] = 0;
|
||||||
|
// q[3] = 0;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// float rpy2[3];
|
||||||
|
// // Quaternion2RPY
|
||||||
|
// {
|
||||||
|
// float q0s, q1s, q2s, q3s;
|
||||||
|
// q0s = q[0] * q[0];
|
||||||
|
// q1s = q[1] * q[1];
|
||||||
|
// q2s = q[2] * q[2];
|
||||||
|
// q3s = q[3] * q[3];
|
||||||
|
|
||||||
|
// float R13, R11, R12, R23, R33;
|
||||||
|
// R13 = 2 * (q[1] * q[3] - q[0] * q[2]);
|
||||||
|
// R11 = q0s + q1s - q2s - q3s;
|
||||||
|
// R12 = 2 * (q[1] * q[2] + q[0] * q[3]);
|
||||||
|
// R23 = 2 * (q[2] * q[3] + q[0] * q[1]);
|
||||||
|
// R33 = q0s - q1s - q2s + q3s;
|
||||||
|
|
||||||
|
// rpy2[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
|
||||||
|
// rpy2[2] = RAD2DEG * atan2f(R12, R11);
|
||||||
|
// rpy2[0] = RAD2DEG * atan2f(R23, R33);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// attActData.Roll = rpy2[0];
|
||||||
|
// attActData.Pitch = rpy2[1];
|
||||||
|
// attActData.Yaw = rpy2[2];
|
||||||
|
// attActData.q1 = q[0];
|
||||||
|
// attActData.q2 = q[1];
|
||||||
|
// attActData.q3 = q[2];
|
||||||
|
// attActData.q4 = q[3];
|
||||||
|
// attActual->setData(attActData);
|
||||||
|
// /*****************************************/
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
/**********************************************************************************************/
|
||||||
|
// if (settings.gcsReciever) {
|
||||||
|
// static QTime gcsRcvrTime = currentTime;
|
||||||
|
// if (!settings.manualOutput || gcsRcvrTime.msecsTo(currentTime) >= settings.outputRate) {
|
||||||
|
// GCSReceiver::DataFields gcsRcvrData;
|
||||||
|
// gcsRcvrData = gcsReceiver->getData();
|
||||||
|
|
||||||
|
// for (int i = 0; i < 8; ++i)
|
||||||
|
// gcsRcvrData.Channel[i] = 1500 + (ch[i] * 500);
|
||||||
|
|
||||||
|
// gcsReceiver->setData(gcsRcvrData);
|
||||||
|
// if (settings.manualOutput)
|
||||||
|
// gcsRcvrTime = currentTime;
|
||||||
|
// }
|
||||||
|
// } else if (settings.manualControl) {
|
||||||
|
// // not implemented yet
|
||||||
|
// }
|
||||||
|
/**********************************************************************************************/
|
||||||
|
// if (settings.gpsPosition) {
|
||||||
|
static QTime gpsPosTime = currentTime;
|
||||||
|
// if (gpsPosTime.msecsTo(currentTime) >= settings.gpsPosRate) {
|
||||||
|
out.altitude = posZ;
|
||||||
|
out.heading = yaw * RAD2DEG;
|
||||||
|
out.latitude = lat * 10e6;
|
||||||
|
out.longitude = lon * 10e6;
|
||||||
|
out.groundspeed = qSqrt(velX * velX + velY * velY);
|
||||||
|
// gpsPosData.GeoidSeparation = 0.0;
|
||||||
|
// gpsPosData.Satellites = 8;
|
||||||
|
// gpsPosData.PDOP = 3.0;
|
||||||
|
// gpsPosData.Status = GPSPosition::STATUS_FIX3D;
|
||||||
|
|
||||||
|
// gpsPosition->setData(gpsPosData);
|
||||||
|
// gpsPosTime = currentTime;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
/**********************************************************************************************/
|
||||||
|
// if (settings.sonarAltitude) {
|
||||||
|
// static QTime sonarAltTime = currentTime;
|
||||||
|
// if (sonarAltTime.msecsTo(currentTime) >= settings.sonarAltRate) {
|
||||||
|
// SonarAltitude::DataFields sonarAltData;
|
||||||
|
// sonarAltData = sonarAlt->getData();
|
||||||
|
|
||||||
|
// float sAlt = settings.sonarMaxAlt;
|
||||||
|
// // 0.35 rad ~= 20 degree
|
||||||
|
// if ((agl < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) {
|
||||||
|
// float x = agl * qTan(roll);
|
||||||
|
// float y = agl * qTan(pitch);
|
||||||
|
// float h = qSqrt(x*x + y*y + agl*agl);
|
||||||
|
// sAlt = qMin(h, sAlt);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// sonarAltData.Altitude = sAlt;
|
||||||
|
// sonarAlt->setData(sonarAltData);
|
||||||
|
// sonarAltTime = currentTime;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
/**********************************************************************************************/
|
||||||
|
out.dstN = posY * 100;
|
||||||
|
out.dstE = posX * 100;
|
||||||
|
out.dstD = posZ * -100;
|
||||||
|
|
||||||
|
out.velDown = velY * 100;
|
||||||
|
out.velEast = velX * 100;
|
||||||
|
out.velDown = velZ * 100; //WHY ISN'T THIS `-velZ`???
|
||||||
|
|
||||||
|
#ifdef DBG_TIMERS
|
||||||
|
static int cntRX = 0;
|
||||||
|
if (cntRX >= 100) {
|
||||||
|
qDebug() << "RX=" << 1000.0 * 100 / timeRX.restart();
|
||||||
|
cntRX = 0;
|
||||||
|
} else {
|
||||||
|
++cntRX;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
// transfomations
|
||||||
|
|
||||||
|
void AeroSimRCSimulator::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q)
|
||||||
|
{
|
||||||
|
qreal w, x, y, z;
|
||||||
|
|
||||||
|
// w always >= 0
|
||||||
|
w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0;
|
||||||
|
x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0;
|
||||||
|
y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0;
|
||||||
|
z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0;
|
||||||
|
|
||||||
|
x = copysign(x, (m(1, 2) - m(2, 1)));
|
||||||
|
y = copysign(y, (m(2, 0) - m(0, 2)));
|
||||||
|
z = copysign(z, (m(0, 1) - m(1, 0)));
|
||||||
|
|
||||||
|
q.setScalar(w);
|
||||||
|
q.setX(x);
|
||||||
|
q.setY(y);
|
||||||
|
q.setZ(z);
|
||||||
|
}
|
||||||
|
|
||||||
|
void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy)
|
||||||
|
{
|
||||||
|
qreal roll, pitch, yaw;
|
||||||
|
|
||||||
|
if (qFabs(m(0, 2)) > 0.998) {
|
||||||
|
// ~86.3°, gimbal lock
|
||||||
|
roll = 0.0;
|
||||||
|
pitch = copysign(M_PI_2, -m(0, 2));
|
||||||
|
yaw = qAtan2(-m(1, 0), m(1, 1));
|
||||||
|
} else {
|
||||||
|
roll = qAtan2(m(1, 2), m(2, 2));
|
||||||
|
pitch = qAsin(-m(0, 2));
|
||||||
|
yaw = qAtan2(m(0, 1), m(0, 0));
|
||||||
|
}
|
||||||
|
|
||||||
|
rpy.setX(roll * RAD2DEG);
|
||||||
|
rpy.setY(pitch * RAD2DEG);
|
||||||
|
rpy.setZ(yaw * RAD2DEG);
|
||||||
|
}
|
||||||
73
ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.h
Normal file
73
ground/openpilotgcs/src/plugins/hitlnew/aerosimrcsimulator.h
Normal file
@ -0,0 +1,73 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file aerosimrc.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
||||||
|
* @addtogroup GCSPlugins GCS Plugins
|
||||||
|
* @{
|
||||||
|
* @addtogroup HITLPlugin HITLv2 Plugin
|
||||||
|
* @{
|
||||||
|
* @brief The Hardware In The Loop plugin version 2
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef AEROSIMRC_H
|
||||||
|
#define AEROSIMRC_H
|
||||||
|
|
||||||
|
#include <QObject>
|
||||||
|
#include <QVector3D>
|
||||||
|
#include <QQuaternion>
|
||||||
|
#include <QMatrix4x4>
|
||||||
|
#include "simulator.h"
|
||||||
|
|
||||||
|
class AeroSimRCSimulator: public Simulator
|
||||||
|
{
|
||||||
|
Q_OBJECT
|
||||||
|
|
||||||
|
public:
|
||||||
|
AeroSimRCSimulator(const SimulatorSettings ¶ms);
|
||||||
|
~AeroSimRCSimulator();
|
||||||
|
|
||||||
|
bool setupProcess();
|
||||||
|
void setupUdpPorts(const QString& host, int inPort, int outPort);
|
||||||
|
|
||||||
|
private slots:
|
||||||
|
void transmitUpdate();
|
||||||
|
|
||||||
|
private:
|
||||||
|
quint32 udpCounterASrecv; //keeps track of udp packets received by ASim
|
||||||
|
|
||||||
|
void processUpdate(const QByteArray &data);
|
||||||
|
|
||||||
|
void asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q);
|
||||||
|
void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy);
|
||||||
|
};
|
||||||
|
|
||||||
|
class AeroSimRCSimulatorCreator : public SimulatorCreator
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
AeroSimRCSimulatorCreator(const QString &classId, const QString &description)
|
||||||
|
: SimulatorCreator (classId, description)
|
||||||
|
{}
|
||||||
|
|
||||||
|
Simulator* createSimulator(const SimulatorSettings ¶ms)
|
||||||
|
{
|
||||||
|
return new AeroSimRCSimulator(params);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // AEROSIMRC_H
|
||||||
@ -56,6 +56,129 @@ HITLConfiguration::HITLConfiguration(QString classId, QSettings* qSettings, QObj
|
|||||||
settings.latitude = qSettings->value("latitude").toString();
|
settings.latitude = qSettings->value("latitude").toString();
|
||||||
settings.longitude = qSettings->value("longitude").toString();
|
settings.longitude = qSettings->value("longitude").toString();
|
||||||
}
|
}
|
||||||
|
bool homeLocation = true;
|
||||||
|
quint16 homeLocRate = 0;
|
||||||
|
|
||||||
|
bool attRaw = true;
|
||||||
|
quint8 attRawRate = 20;
|
||||||
|
|
||||||
|
bool attActual = true;
|
||||||
|
bool attActHW = false;
|
||||||
|
bool attActSim = true;
|
||||||
|
bool attActCalc = false;
|
||||||
|
|
||||||
|
bool sonarAltitude = false;
|
||||||
|
float sonarMaxAlt = 5.0;
|
||||||
|
quint16 sonarAltRate = 50;
|
||||||
|
|
||||||
|
bool gpsPosition = true;
|
||||||
|
quint16 gpsPosRate = 200;
|
||||||
|
|
||||||
|
bool inputCommand = true;
|
||||||
|
bool gcsReciever = true;
|
||||||
|
bool manualControl = false;
|
||||||
|
|
||||||
|
bool manualOutput = false;
|
||||||
|
quint8 outputRate = 20;
|
||||||
|
|
||||||
|
|
||||||
|
settings.simulatorId = "";
|
||||||
|
settings.binPath = "";
|
||||||
|
settings.dataPath = "";
|
||||||
|
settings.manual = false;
|
||||||
|
settings.startSim = false;
|
||||||
|
settings.addNoise = false;
|
||||||
|
settings.hostAddress = "127.0.0.1";
|
||||||
|
settings.remoteHostAddress = "127.0.0.1";
|
||||||
|
settings.outPort = 0;
|
||||||
|
settings.inPort = 0;
|
||||||
|
settings.latitude = "";
|
||||||
|
settings.longitude = "";
|
||||||
|
|
||||||
|
//if a saved configuration exists load it
|
||||||
|
if(qSettings != 0) {
|
||||||
|
settings.simulatorId = qSettings->value("simulatorId").toString();
|
||||||
|
settings.binPath = qSettings->value("binPath").toString();
|
||||||
|
settings.dataPath = qSettings->value("dataPath").toString();
|
||||||
|
settings.manual = qSettings->value("manual").toBool();
|
||||||
|
settings.addNoise = qSettings->value("noiseCheckBox").toBool();
|
||||||
|
settings.startSim = qSettings->value("startSim").toBool();
|
||||||
|
settings.hostAddress = qSettings->value("hostAddress").toString();
|
||||||
|
settings.remoteHostAddress = qSettings->value("remoteHostAddress").toString();
|
||||||
|
settings.outPort = qSettings->value("outPort").toInt();
|
||||||
|
settings.inPort = qSettings->value("inPort").toInt();
|
||||||
|
settings.latitude = qSettings->value("latitude").toString();
|
||||||
|
settings.longitude = qSettings->value("longitude").toString();
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef HRRRRRRRRRRR
|
||||||
|
// if a saved configuration exists load it
|
||||||
|
if (qSettings != 0) {
|
||||||
|
settings.simulatorId = qSettings->value("simulatorId", simulatorId).toString();
|
||||||
|
settings.hostAddress = qSettings->value("hostAddress", hostAddress).toString();
|
||||||
|
settings.inPort = qSettings->value("inPort", inPort).toInt();
|
||||||
|
settings.remoteAddress = qSettings->value("remoteAddress", remoteAddress).toString();
|
||||||
|
settings.outPort = qSettings->value("outPort", outPort).toInt();
|
||||||
|
settings.binPath = qSettings->value("binPath", binPath).toString();
|
||||||
|
settings.dataPath = qSettings->value("dataPath", dataPath).toString();
|
||||||
|
|
||||||
|
settings.homeLocation = qSettings->value("homeLocation", homeLocation).toBool();
|
||||||
|
settings.homeLocRate = qSettings->value("homeLocRate", homeLocRate).toInt();
|
||||||
|
|
||||||
|
settings.attRaw = qSettings->value("attRaw", attRaw).toBool();
|
||||||
|
settings.attRawRate = qSettings->value("attRawRate", attRawRate).toInt();
|
||||||
|
|
||||||
|
settings.attActual = qSettings->value("attActual", attActual).toBool();
|
||||||
|
settings.attActHW = qSettings->value("attActHW", attActHW).toBool();
|
||||||
|
settings.attActSim = qSettings->value("attActSim", attActSim).toBool();
|
||||||
|
settings.attActCalc = qSettings->value("attActCalc", attActCalc).toBool();
|
||||||
|
|
||||||
|
settings.sonarAltitude = qSettings->value("sonarAltitude", sonarAltitude).toBool();
|
||||||
|
settings.sonarMaxAlt = qSettings->value("sonarMaxAlt", sonarMaxAlt).toFloat();
|
||||||
|
settings.sonarAltRate = qSettings->value("sonarAltRate", sonarAltRate).toInt();
|
||||||
|
|
||||||
|
settings.gpsPosition = qSettings->value("gpsPosition", gpsPosition).toBool();
|
||||||
|
settings.gpsPosRate = qSettings->value("gpsPosRate", gpsPosRate).toInt();
|
||||||
|
|
||||||
|
settings.inputCommand = qSettings->value("inputCommand", inputCommand).toBool();
|
||||||
|
settings.gcsReciever = qSettings->value("gcsReciever", gcsReciever).toBool();
|
||||||
|
settings.manualControl = qSettings->value("manualControl", manualControl).toBool();
|
||||||
|
settings.manualOutput = qSettings->value("manualOutput", manualOutput).toBool();
|
||||||
|
settings.outputRate = qSettings->value("outputRate", outputRate).toInt();
|
||||||
|
} else {
|
||||||
|
settings.simulatorId = simulatorId;
|
||||||
|
settings.hostAddress = hostAddress;
|
||||||
|
settings.inPort = inPort;
|
||||||
|
settings.remoteAddress = remoteAddress;
|
||||||
|
settings.outPort = outPort;
|
||||||
|
settings.binPath = binPath;
|
||||||
|
settings.dataPath = dataPath;
|
||||||
|
|
||||||
|
settings.homeLocation = homeLocation;
|
||||||
|
settings.homeLocRate = homeLocRate;
|
||||||
|
|
||||||
|
settings.attRaw = attRaw;
|
||||||
|
settings.attRawRate = attRawRate;
|
||||||
|
|
||||||
|
settings.attActual = attActual;
|
||||||
|
settings.attActHW = attActHW;
|
||||||
|
settings.attActSim = attActSim;
|
||||||
|
settings.attActCalc = attActCalc;
|
||||||
|
|
||||||
|
settings.sonarAltitude = sonarAltitude;
|
||||||
|
settings.sonarMaxAlt = sonarMaxAlt;
|
||||||
|
settings.sonarAltRate = sonarAltRate;
|
||||||
|
|
||||||
|
settings.gpsPosition = gpsPosition;
|
||||||
|
settings.gpsPosRate = gpsPosRate;
|
||||||
|
|
||||||
|
settings.inputCommand = inputCommand;
|
||||||
|
settings.gcsReciever = gcsReciever;
|
||||||
|
settings.manualControl = manualControl;
|
||||||
|
settings.manualOutput = manualOutput;
|
||||||
|
settings.outputRate = outputRate;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
IUAVGadgetConfiguration *HITLConfiguration::clone()
|
IUAVGadgetConfiguration *HITLConfiguration::clone()
|
||||||
@ -72,6 +195,12 @@ IUAVGadgetConfiguration *HITLConfiguration::clone()
|
|||||||
*/
|
*/
|
||||||
void HITLConfiguration::saveConfig(QSettings* qSettings) const {
|
void HITLConfiguration::saveConfig(QSettings* qSettings) const {
|
||||||
qSettings->setValue("simulatorId", settings.simulatorId);
|
qSettings->setValue("simulatorId", settings.simulatorId);
|
||||||
|
#ifdef HRRRRRRRRRRR
|
||||||
|
qSettings->setValue("hostAddress", settings.hostAddress);
|
||||||
|
qSettings->setValue("inPort", settings.inPort);
|
||||||
|
qSettings->setValue("remoteAddress", settings.remoteAddress);
|
||||||
|
qSettings->setValue("outPort", settings.outPort);
|
||||||
|
#endif
|
||||||
qSettings->setValue("binPath", settings.binPath);
|
qSettings->setValue("binPath", settings.binPath);
|
||||||
qSettings->setValue("dataPath", settings.dataPath);
|
qSettings->setValue("dataPath", settings.dataPath);
|
||||||
qSettings->setValue("manual", settings.manual);
|
qSettings->setValue("manual", settings.manual);
|
||||||
@ -82,5 +211,27 @@ void HITLConfiguration::saveConfig(QSettings* qSettings) const {
|
|||||||
qSettings->setValue("inPort", settings.inPort);
|
qSettings->setValue("inPort", settings.inPort);
|
||||||
qSettings->setValue("latitude", settings.latitude);
|
qSettings->setValue("latitude", settings.latitude);
|
||||||
qSettings->setValue("longitude", settings.longitude);
|
qSettings->setValue("longitude", settings.longitude);
|
||||||
|
|
||||||
|
#ifdef HRRRRRRRRRRR
|
||||||
|
//Added by hrrrr. Is it all used?
|
||||||
|
qSettings->setValue("homeLocation", settings.homeLocation);
|
||||||
|
qSettings->setValue("homeLocRate", settings.homeLocRate);
|
||||||
|
qSettings->setValue("attRaw", settings.attRaw);
|
||||||
|
qSettings->setValue("attRawRate", settings.attRawRate);
|
||||||
|
qSettings->setValue("attActual", settings.attActual);
|
||||||
|
qSettings->setValue("attActHW", settings.attActHW);
|
||||||
|
qSettings->setValue("attActSim", settings.attActSim);
|
||||||
|
qSettings->setValue("attActCalc", settings.attActCalc);
|
||||||
|
qSettings->setValue("sonarAltitude", settings.sonarAltitude);
|
||||||
|
qSettings->setValue("sonarMaxAlt", settings.sonarMaxAlt);
|
||||||
|
qSettings->setValue("sonarAltRate", settings.sonarAltRate);
|
||||||
|
qSettings->setValue("gpsPosition", settings.gpsPosition);
|
||||||
|
qSettings->setValue("gpsPosRate", settings.gpsPosRate);
|
||||||
|
qSettings->setValue("inputCommand", settings.inputCommand);
|
||||||
|
qSettings->setValue("gcsReciever", settings.gcsReciever);
|
||||||
|
qSettings->setValue("manualControl", settings.manualControl);
|
||||||
|
qSettings->setValue("manualOutput", settings.manualOutput);
|
||||||
|
qSettings->setValue("outputRate", settings.outputRate);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -44,24 +44,8 @@ HITLGadget::~HITLGadget()
|
|||||||
void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config)
|
void HITLGadget::loadConfiguration(IUAVGadgetConfiguration* config)
|
||||||
{
|
{
|
||||||
HITLConfiguration *m = qobject_cast<HITLConfiguration*>(config);
|
HITLConfiguration *m = qobject_cast<HITLConfiguration*>(config);
|
||||||
// FG
|
// IL2 <-- Is this still necessary? [KDS]
|
||||||
// simulator->setFGPathBin( m->fgPathBin() );
|
|
||||||
// simulator->setFGPathData( m->fgPathData() );
|
|
||||||
// simulator->setFGManualControl( m->fgManualControl() );
|
|
||||||
// // IL2
|
|
||||||
emit changeConfiguration();
|
emit changeConfiguration();
|
||||||
m_widget->setSettingParameters(m->Settings());
|
m_widget->setSettingParameters(m->Settings());
|
||||||
|
|
||||||
// m_widget->setSimulatorId(m->SimulatorId());
|
|
||||||
// m_widget->setPathBin(m->PathBin());
|
|
||||||
// m_widget->setPathData(m->PathData());
|
|
||||||
// m_widget->setHostName(m->HostName());
|
|
||||||
// m_widget->setLatitude(m->Latitude());
|
|
||||||
// m_widget->setLongitude(m->Longitude());
|
|
||||||
// m_widget->setOutPort(m->OutPort());
|
|
||||||
// m_widget->setInPort(m->InPort());
|
|
||||||
// m_widget->setManualControl(m->ManualControl());
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -9,7 +9,9 @@ HEADERS += hitlplugin.h \
|
|||||||
hitlfactory.h \
|
hitlfactory.h \
|
||||||
hitlconfiguration.h \
|
hitlconfiguration.h \
|
||||||
hitlgadget.h \
|
hitlgadget.h \
|
||||||
|
hitlnoisegeneration.h \
|
||||||
simulator.h \
|
simulator.h \
|
||||||
|
aerosimrcsimulator.h \
|
||||||
fgsimulator.h \
|
fgsimulator.h \
|
||||||
il2simulator.h \
|
il2simulator.h \
|
||||||
xplanesimulator.h
|
xplanesimulator.h
|
||||||
@ -19,9 +21,11 @@ SOURCES += hitlplugin.cpp \
|
|||||||
hitlfactory.cpp \
|
hitlfactory.cpp \
|
||||||
hitlconfiguration.cpp \
|
hitlconfiguration.cpp \
|
||||||
hitlgadget.cpp \
|
hitlgadget.cpp \
|
||||||
|
hitlnoisegeneration.cpp \
|
||||||
simulator.cpp \
|
simulator.cpp \
|
||||||
il2simulator.cpp \
|
aerosimrcsimulator.cpp \
|
||||||
fgsimulator.cpp \
|
fgsimulator.cpp \
|
||||||
|
il2simulator.cpp \
|
||||||
xplanesimulator.cpp
|
xplanesimulator.cpp
|
||||||
OTHER_FILES += hitlnew.pluginspec
|
OTHER_FILES += hitlnew.pluginspec
|
||||||
FORMS += hitloptionspage.ui \
|
FORMS += hitloptionspage.ui \
|
||||||
|
|||||||
@ -29,6 +29,7 @@
|
|||||||
#include <QtPlugin>
|
#include <QtPlugin>
|
||||||
#include <QStringList>
|
#include <QStringList>
|
||||||
#include <extensionsystem/pluginmanager.h>
|
#include <extensionsystem/pluginmanager.h>
|
||||||
|
#include "aerosimrcsimulator.h"
|
||||||
#include "fgsimulator.h"
|
#include "fgsimulator.h"
|
||||||
#include "il2simulator.h"
|
#include "il2simulator.h"
|
||||||
#include "xplanesimulator.h"
|
#include "xplanesimulator.h"
|
||||||
@ -53,6 +54,7 @@ bool HITLPlugin::initialize(const QStringList& args, QString *errMsg)
|
|||||||
|
|
||||||
addAutoReleasedObject(mf);
|
addAutoReleasedObject(mf);
|
||||||
|
|
||||||
|
addSimulator(new AeroSimRCSimulatorCreator("ASimRC", "AeroSimRC"));
|
||||||
addSimulator(new FGSimulatorCreator("FG","FlightGear"));
|
addSimulator(new FGSimulatorCreator("FG","FlightGear"));
|
||||||
addSimulator(new IL2SimulatorCreator("IL2","IL2"));
|
addSimulator(new IL2SimulatorCreator("IL2","IL2"));
|
||||||
addSimulator(new XplaneSimulatorCreator("X-Plane","X-Plane"));
|
addSimulator(new XplaneSimulatorCreator("X-Plane","X-Plane"));
|
||||||
|
|||||||
@ -29,7 +29,7 @@
|
|||||||
#define HITLPLUGIN_H
|
#define HITLPLUGIN_H
|
||||||
|
|
||||||
#include <extensionsystem/iplugin.h>
|
#include <extensionsystem/iplugin.h>
|
||||||
#include <qstringlist.h>
|
#include <QStringList>
|
||||||
|
|
||||||
#include <simulator.h>
|
#include <simulator.h>
|
||||||
|
|
||||||
|
|||||||
@ -2,7 +2,7 @@
|
|||||||
******************************************************************************
|
******************************************************************************
|
||||||
*
|
*
|
||||||
* @file hitlwidget.cpp
|
* @file hitlwidget.cpp
|
||||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||||
* @addtogroup GCSPlugins GCS Plugins
|
* @addtogroup GCSPlugins GCS Plugins
|
||||||
* @{
|
* @{
|
||||||
* @addtogroup HITLPlugin HITL Plugin
|
* @addtogroup HITLPlugin HITL Plugin
|
||||||
@ -51,12 +51,13 @@ HITLWidget::HITLWidget(QWidget *parent)
|
|||||||
widget->startButton->setEnabled(true);
|
widget->startButton->setEnabled(true);
|
||||||
widget->stopButton->setEnabled(false);
|
widget->stopButton->setEnabled(false);
|
||||||
|
|
||||||
greenColor = "rgb(35, 221, 35)";
|
greenColor = "rgb(35, 221, 35)"; //Change the green color in order to make it a bit more vibrant
|
||||||
|
strStyleEnable = QString("QFrame{background-color: %1; color: white}").arg(greenColor);
|
||||||
|
strStyleDisable = "QFrame{background-color: red; color: white}";
|
||||||
|
|
||||||
strAutopilotDisconnected = " Autopilot OFF ";
|
strAutopilotDisconnected = " Autopilot OFF ";
|
||||||
strSimulatorDisconnected = " Simulator OFF ";
|
strSimulatorDisconnected = " Simulator OFF ";
|
||||||
strAutopilotConnected = " Autopilot ON ";
|
strAutopilotConnected = " Autopilot ON ";
|
||||||
strSimulatorConnected = " Simulator ON ";
|
|
||||||
|
|
||||||
widget->apLabel->setText(strAutopilotDisconnected);
|
widget->apLabel->setText(strAutopilotDisconnected);
|
||||||
widget->simLabel->setText(strSimulatorDisconnected);
|
widget->simLabel->setText(strSimulatorDisconnected);
|
||||||
@ -64,7 +65,6 @@ HITLWidget::HITLWidget(QWidget *parent)
|
|||||||
connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked()));
|
connect(widget->startButton, SIGNAL(clicked()), this, SLOT(startButtonClicked()));
|
||||||
connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked()));
|
connect(widget->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonClicked()));
|
||||||
connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked()));
|
connect(widget->buttonClearLog, SIGNAL(clicked()), this, SLOT(buttonClearLogClicked()));
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
HITLWidget::~HITLWidget()
|
HITLWidget::~HITLWidget()
|
||||||
@ -77,15 +77,7 @@ void HITLWidget::startButtonClicked()
|
|||||||
QThread* mainThread = QThread::currentThread();
|
QThread* mainThread = QThread::currentThread();
|
||||||
qDebug() << "Main Thread: "<< mainThread;
|
qDebug() << "Main Thread: "<< mainThread;
|
||||||
|
|
||||||
// [1]
|
//Allow only one instance per simulator
|
||||||
// if(Simulator::IsStarted())
|
|
||||||
// {
|
|
||||||
// widget->textBrowser->append("HITL alreary started!");
|
|
||||||
// return;
|
|
||||||
// }
|
|
||||||
// Simulator::setStarted(true);
|
|
||||||
|
|
||||||
// [2] allow only one instance per simulator
|
|
||||||
if(Simulator::Instances().indexOf(settings.simulatorId) != -1)
|
if(Simulator::Instances().indexOf(settings.simulatorId) != -1)
|
||||||
{
|
{
|
||||||
widget->textBrowser->append(settings.simulatorId + " alreary started!");
|
widget->textBrowser->append(settings.simulatorId + " alreary started!");
|
||||||
@ -104,8 +96,7 @@ void HITLWidget::startButtonClicked()
|
|||||||
QMetaObject::invokeMethod(simulator, "onDeleteSimulator",Qt::QueuedConnection);
|
QMetaObject::invokeMethod(simulator, "onDeleteSimulator",Qt::QueuedConnection);
|
||||||
simulator = NULL;
|
simulator = NULL;
|
||||||
}
|
}
|
||||||
//fgBridge = new FlightGearBridge();
|
|
||||||
//simulator = new Simulator();
|
|
||||||
if(settings.hostAddress == "" || settings.inPort == 0)
|
if(settings.hostAddress == "" || settings.inPort == 0)
|
||||||
{
|
{
|
||||||
widget->textBrowser->append("Before start, set UDP parameters in options page!");
|
widget->textBrowser->append("Before start, set UDP parameters in options page!");
|
||||||
@ -113,16 +104,19 @@ void HITLWidget::startButtonClicked()
|
|||||||
}
|
}
|
||||||
|
|
||||||
SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId);
|
SimulatorCreator* creator = HITLPlugin::getSimulatorCreator(settings.simulatorId);
|
||||||
simulator = creator->createSimulator(settings);//hostName, outPort, inPort, manualControl, pathBin, pathData);
|
simulator = creator->createSimulator(settings);
|
||||||
// move to thread
|
|
||||||
|
// move to thread <--[BCH]
|
||||||
simulator->setName(creator->Description());
|
simulator->setName(creator->Description());
|
||||||
simulator->setSimulatorId(creator->ClassId());
|
simulator->setSimulatorId(creator->ClassId());
|
||||||
|
|
||||||
|
connect(simulator, SIGNAL(processOutput(QString)), this, SLOT(onProcessOutput(QString)));
|
||||||
|
|
||||||
// Setup process
|
// Setup process
|
||||||
widget->textBrowser->append(QString("[%1] Starting %2... ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(creator->Description()));
|
widget->textBrowser->append(QString("[%1] Starting %2... ").arg(QTime::currentTime().toString("hh:mm:ss")).arg(creator->Description()));
|
||||||
qxtLog->info("HITL: Starting " + creator->Description());
|
qxtLog->info("HITL: Starting " + creator->Description());
|
||||||
|
|
||||||
// Start bridge
|
// Start bridge
|
||||||
//bool ret = simulator->setupProcess();
|
|
||||||
bool ret = QMetaObject::invokeMethod(simulator, "setupProcess",Qt::QueuedConnection);
|
bool ret = QMetaObject::invokeMethod(simulator, "setupProcess",Qt::QueuedConnection);
|
||||||
if(ret)
|
if(ret)
|
||||||
{
|
{
|
||||||
@ -134,7 +128,6 @@ void HITLWidget::startButtonClicked()
|
|||||||
widget->stopButton->setEnabled(true);
|
widget->stopButton->setEnabled(true);
|
||||||
qxtLog->info("HITL: Starting bridge, initializing flight simulator and Autopilot connections");
|
qxtLog->info("HITL: Starting bridge, initializing flight simulator and Autopilot connections");
|
||||||
|
|
||||||
connect(simulator, SIGNAL(processOutput(QString)), widget->textBrowser, SLOT(append(QString)));
|
|
||||||
connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()),Qt::QueuedConnection);
|
connect(simulator, SIGNAL(autopilotConnected()), this, SLOT(onAutopilotConnect()),Qt::QueuedConnection);
|
||||||
connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()),Qt::QueuedConnection);
|
connect(simulator, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect()),Qt::QueuedConnection);
|
||||||
connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()),Qt::QueuedConnection);
|
connect(simulator, SIGNAL(simulatorConnected()), this, SLOT(onSimulatorConnect()),Qt::QueuedConnection);
|
||||||
@ -169,8 +162,8 @@ void HITLWidget::stopButtonClicked()
|
|||||||
|
|
||||||
widget->startButton->setEnabled(true);
|
widget->startButton->setEnabled(true);
|
||||||
widget->stopButton->setEnabled(false);
|
widget->stopButton->setEnabled(false);
|
||||||
widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: transparent; color: white}"));
|
widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}"));
|
||||||
widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: transparent; color: white}"));
|
widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{background-color: transparent; color: white}"));
|
||||||
widget->apLabel->setText(strAutopilotDisconnected);
|
widget->apLabel->setText(strAutopilotDisconnected);
|
||||||
widget->simLabel->setText(strSimulatorDisconnected);
|
widget->simLabel->setText(strSimulatorDisconnected);
|
||||||
if(simulator)
|
if(simulator)
|
||||||
@ -185,31 +178,35 @@ void HITLWidget::buttonClearLogClicked()
|
|||||||
widget->textBrowser->clear();
|
widget->textBrowser->clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void HITLWidget::onProcessOutput(QString text)
|
||||||
|
{
|
||||||
|
widget->textBrowser->append(text);
|
||||||
|
}
|
||||||
|
|
||||||
void HITLWidget::onAutopilotConnect()
|
void HITLWidget::onAutopilotConnect()
|
||||||
{
|
{
|
||||||
widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: %1; color: white}").arg(greenColor));
|
widget->apLabel->setStyleSheet(strStyleEnable);
|
||||||
widget->apLabel->setText(strAutopilotConnected);
|
widget->apLabel->setText(strAutopilotConnected);
|
||||||
qxtLog->info("HITL: Autopilot connected, initializing for HITL simulation");
|
qxtLog->info("HITL: Autopilot connected, initializing for HITL simulation");
|
||||||
}
|
}
|
||||||
|
|
||||||
void HITLWidget::onAutopilotDisconnect()
|
void HITLWidget::onAutopilotDisconnect()
|
||||||
{
|
{
|
||||||
widget->apLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: red; color: white}"));
|
widget->apLabel->setStyleSheet(strStyleDisable);
|
||||||
widget->apLabel->setText(strAutopilotDisconnected);
|
widget->apLabel->setText(strAutopilotDisconnected);
|
||||||
qxtLog->info(strAutopilotDisconnected);
|
qxtLog->info(strAutopilotDisconnected);
|
||||||
}
|
}
|
||||||
|
|
||||||
void HITLWidget::onSimulatorConnect()
|
void HITLWidget::onSimulatorConnect()
|
||||||
{
|
{
|
||||||
widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: %1; color: white}").arg(greenColor));
|
widget->simLabel->setStyleSheet(strStyleEnable);
|
||||||
widget->simLabel->setText(" " + simulator->Name() +" connected ");
|
widget->simLabel->setText(" " + simulator->Name() +" connected ");
|
||||||
qxtLog->info(QString("HITL: %1 connected").arg(simulator->Name()));
|
qxtLog->info(QString("HITL: %1 connected").arg(simulator->Name()));
|
||||||
}
|
}
|
||||||
|
|
||||||
void HITLWidget::onSimulatorDisconnect()
|
void HITLWidget::onSimulatorDisconnect()
|
||||||
{
|
{
|
||||||
widget->simLabel->setStyleSheet(QString::fromUtf8("QFrame{\n""background-color: red; color: white}"));
|
widget->simLabel->setStyleSheet(strStyleDisable);
|
||||||
widget->simLabel->setText(" " + simulator->Name() +" disconnected ");
|
widget->simLabel->setText(" " + simulator->Name() +" disconnected ");
|
||||||
qxtLog->info(QString("HITL: %1 disconnected").arg(simulator->Name()));
|
qxtLog->info(QString("HITL: %1 disconnected").arg(simulator->Name()));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -50,6 +50,7 @@ private slots:
|
|||||||
void startButtonClicked();
|
void startButtonClicked();
|
||||||
void stopButtonClicked();
|
void stopButtonClicked();
|
||||||
void buttonClearLogClicked();
|
void buttonClearLogClicked();
|
||||||
|
void onProcessOutput(QString text);
|
||||||
void onAutopilotConnect();
|
void onAutopilotConnect();
|
||||||
void onAutopilotDisconnect();
|
void onAutopilotDisconnect();
|
||||||
void onSimulatorConnect();
|
void onSimulatorConnect();
|
||||||
@ -64,8 +65,8 @@ private:
|
|||||||
QString strAutopilotDisconnected;
|
QString strAutopilotDisconnected;
|
||||||
QString strSimulatorDisconnected;
|
QString strSimulatorDisconnected;
|
||||||
QString strAutopilotConnected;
|
QString strAutopilotConnected;
|
||||||
QString strSimulatorConnected;
|
QString strStyleEnable;
|
||||||
|
QString strStyleDisable;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* HITLWIDGET_H */
|
#endif /* HITLWIDGET_H */
|
||||||
|
|||||||
@ -40,6 +40,7 @@ const float Simulator::KT2MPS = 0.514444444;
|
|||||||
const float Simulator::INHG2KPA = 3.386;
|
const float Simulator::INHG2KPA = 3.386;
|
||||||
const float Simulator::FPS2CMPS = 30.48;
|
const float Simulator::FPS2CMPS = 30.48;
|
||||||
const float Simulator::DEG2RAD = (M_PI/180.0);
|
const float Simulator::DEG2RAD = (M_PI/180.0);
|
||||||
|
const float Simulator::RAD2DEG = (180.0/M_PI);
|
||||||
|
|
||||||
|
|
||||||
Simulator::Simulator(const SimulatorSettings& params) :
|
Simulator::Simulator(const SimulatorSettings& params) :
|
||||||
|
|||||||
@ -32,22 +32,30 @@
|
|||||||
#include <QUdpSocket>
|
#include <QUdpSocket>
|
||||||
#include <QTimer>
|
#include <QTimer>
|
||||||
#include <QProcess>
|
#include <QProcess>
|
||||||
|
#include <qmath.h>
|
||||||
|
|
||||||
#include "qscopedpointer.h"
|
#include "qscopedpointer.h"
|
||||||
#include "uavtalk/telemetrymanager.h"
|
#include "uavtalk/telemetrymanager.h"
|
||||||
#include "uavobjectmanager.h"
|
#include "uavobjectmanager.h"
|
||||||
#include "actuatordesired.h"
|
|
||||||
#include "manualcontrolcommand.h"
|
|
||||||
// #include "altitudeactual.h"
|
|
||||||
#include "positionactual.h"
|
|
||||||
#include "velocityactual.h"
|
|
||||||
#include "baroaltitude.h"
|
|
||||||
#include "attitudeactual.h"
|
|
||||||
#include "gpsposition.h"
|
|
||||||
#include "homelocation.h"
|
|
||||||
#include "accels.h"
|
#include "accels.h"
|
||||||
#include "gyros.h"
|
#include "actuatordesired.h"
|
||||||
|
#include "actuatorcommand.h"
|
||||||
|
#include "attitudeactual.h"
|
||||||
|
#include "attitudesettings.h"
|
||||||
|
#include "baroaltitude.h"
|
||||||
|
#include "baroairspeed.h"
|
||||||
|
#include "gcsreceiver.h"
|
||||||
#include "gcstelemetrystats.h"
|
#include "gcstelemetrystats.h"
|
||||||
|
#include "gpsposition.h"
|
||||||
|
#include "gpsvelocity.h"
|
||||||
|
#include "gyros.h"
|
||||||
#include "flightstatus.h"
|
#include "flightstatus.h"
|
||||||
|
#include "homelocation.h"
|
||||||
|
#include "manualcontrolcommand.h"
|
||||||
|
#include "positionactual.h"
|
||||||
|
#include "sonaraltitude.h"
|
||||||
|
#include "velocityactual.h"
|
||||||
|
|
||||||
#include "utils/coordinateconversions.h"
|
#include "utils/coordinateconversions.h"
|
||||||
|
|
||||||
@ -108,6 +116,33 @@ typedef struct _CONNECTION
|
|||||||
QString longitude;
|
QString longitude;
|
||||||
} SimulatorSettings;
|
} SimulatorSettings;
|
||||||
|
|
||||||
|
|
||||||
|
struct Output2OP{
|
||||||
|
float latitude;
|
||||||
|
float longitude;
|
||||||
|
float altitude;
|
||||||
|
float heading;
|
||||||
|
float groundspeed; //[m/s]
|
||||||
|
float calibratedAirspeed; //[m/s]
|
||||||
|
float pitch;
|
||||||
|
float roll;
|
||||||
|
float pressure;
|
||||||
|
float temperature;
|
||||||
|
float velNorth; //[m/s]
|
||||||
|
float velEast; //[m/s]
|
||||||
|
float velDown; //[m/s]
|
||||||
|
float dstN; //[m]
|
||||||
|
float dstE; //[m]
|
||||||
|
float dstD; //[m]
|
||||||
|
float accX; //[m/s^2]
|
||||||
|
float accY; //[m/s^2]
|
||||||
|
float accZ; //[m/s^2]
|
||||||
|
float rollRate; //[deg/s]
|
||||||
|
float pitchRate; //[deg/s]
|
||||||
|
float yawRate; //[deg/s]
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
class Simulator : public QObject
|
class Simulator : public QObject
|
||||||
{
|
{
|
||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
@ -158,29 +193,34 @@ private slots:
|
|||||||
virtual void processUpdate(const QByteArray& data) = 0;
|
virtual void processUpdate(const QByteArray& data) = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
static const float GEE;
|
static const float GEE;
|
||||||
static const float FT2M;
|
static const float FT2M;
|
||||||
static const float KT2MPS;
|
static const float KT2MPS;
|
||||||
static const float INHG2KPA;
|
static const float INHG2KPA;
|
||||||
static const float FPS2CMPS;
|
static const float FPS2CMPS;
|
||||||
static const float DEG2RAD;
|
static const float DEG2RAD;
|
||||||
|
static const float RAD2DEG;
|
||||||
|
|
||||||
QProcess* simProcess;
|
QProcess* simProcess;
|
||||||
QTime* time;
|
QTime* time;
|
||||||
QUdpSocket* inSocket;//(new QUdpSocket());
|
QUdpSocket* inSocket;//(new QUdpSocket());
|
||||||
QUdpSocket* outSocket;
|
QUdpSocket* outSocket;
|
||||||
|
|
||||||
|
ActuatorCommand* actCommand;
|
||||||
ActuatorDesired* actDesired;
|
ActuatorDesired* actDesired;
|
||||||
ManualControlCommand* manCtrlCommand;
|
ManualControlCommand* manCtrlCommand;
|
||||||
FlightStatus* flightStatus;
|
FlightStatus* flightStatus;
|
||||||
BaroAltitude* altActual;
|
BaroAltitude* baroAlt;
|
||||||
|
BaroAirspeed* baroAirspeed;
|
||||||
AttitudeActual* attActual;
|
AttitudeActual* attActual;
|
||||||
VelocityActual* velActual;
|
VelocityActual* velActual;
|
||||||
|
GPSPosition* gpsPos;
|
||||||
|
GPSVelocity* gpsVel;
|
||||||
PositionActual* posActual;
|
PositionActual* posActual;
|
||||||
HomeLocation* posHome;
|
HomeLocation* posHome;
|
||||||
Accels* accels;
|
Accels* accels;
|
||||||
Gyros* gyros;
|
Gyros* gyros;
|
||||||
GPSPosition* gpsPos;
|
|
||||||
GCSTelemetryStats* telStats;
|
GCSTelemetryStats* telStats;
|
||||||
|
|
||||||
SimulatorSettings settings;
|
SimulatorSettings settings;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user