Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
/**
* @file
* @brief 3dConnexion 3dMouse interface for QGroundControl
*
* @author Matthias Krebs <makrebs@student.ethz.ch>
*
*/
#include "Mouse6dofInput.h"
#include "UAS.h"
#include "UASManager.h"
Mouse6dofInput::Mouse6dofInput(Mouse3DInput* mouseInput) :
mouse3DMax(0.01), // TODO: check maximum value fot plugged device
uas(NULL),
done(false),
xValue(0.0),
yValue(0.0),
zValue(0.0),
aValue(0.0),
bValue(0.0),
cValue(0.0)
{
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
// Connect 3DxWare SDK MotionEvent
connect(mouseInput, SIGNAL(Move3d(std::vector<float>&)), this, SLOT(motion3DMouse(std::vector<float>&)));
// TODO: Connect button mapping
//connect(mouseInput, SIGNAL(On3dmouseKeyDown(int)), this, SLOT);
//connect(mouseInput, SIGNAL(On3dmouseKeyUp(int)), this, SLOT);
}
Mouse6dofInput::~Mouse6dofInput()
{
done = true;
}
void Mouse6dofInput::setActiveUAS(UASInterface* uas)
{
// Only connect / disconnect is the UAS is of a controllable UAS class
UAS* tmp = 0;
if (this->uas)
{
tmp = dynamic_cast<UAS*>(this->uas);
if(tmp)
{
disconnect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
// Todo: disconnect button mapping
}
}
this->uas = uas;
tmp = dynamic_cast<UAS*>(this->uas);
if(tmp) {
connect(this, SIGNAL(mouse6dofChanged(double,double,double,double,double,double)), tmp, SLOT(setManual6DOFControlCommands(double,double,double,double,double,double)));
// Todo: disconnect button mapping
}
if (!isRunning())
{
start();
}
}
void Mouse6dofInput::init()
{
// Make sure active UAS is set
setActiveUAS(UASManager::instance()->getActiveUAS());
}
void Mouse6dofInput::run()
{
init();
forever
{
if (done)
{
done = false;
exit();
}
// Bound x value
if (xValue > 1.0) xValue = 1.0;
if (xValue < -1.0) xValue = -1.0;
// Bound x value
if (yValue > 1.0) yValue = 1.0;
if (yValue < -1.0) yValue = -1.0;
// Bound x value
if (zValue > 1.0) zValue = 1.0;
if (zValue < -1.0) zValue = -1.0;
// Bound x value
if (aValue > 1.0) aValue = 1.0;
if (aValue < -1.0) aValue = -1.0;
// Bound x value
if (bValue > 1.0) bValue = 1.0;
if (bValue < -1.0) bValue = -1.0;
// Bound x value
if (cValue > 1.0) cValue = 1.0;
if (cValue < -1.0) cValue = -1.0;
emit mouse6dofChanged(xValue, yValue, zValue, aValue, bValue, cValue);
// Sleep, update rate of 3d mouse is approx. 50 Hz (1000 ms / 50 = 20 ms)
QGC::SLEEP::msleep(20);
}
}
void Mouse6dofInput::motion3DMouse(std::vector<float> &motionData)
{
if (motionData.size() < 6) return;
xValue = (double)1.0e2f*motionData[ 1 ] / mouse3DMax;
yValue = (double)1.0e2f*motionData[ 0 ] / mouse3DMax;
zValue = (double)1.0e2f*motionData[ 2 ] / mouse3DMax;
aValue = (double)1.0e2f*motionData[ 4 ] / mouse3DMax;
bValue = (double)1.0e2f*motionData[ 3 ] / mouse3DMax;
cValue = (double)1.0e2f*motionData[ 5 ] / mouse3DMax;
qDebug() << "NEW 3D MOUSE VALUES -- X" << xValue << " -- Y" << yValue << " -- Z" << zValue << " -- A" << aValue << " -- B" << bValue << " -- C" << cValue;
}