Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
f9318458
Commit
f9318458
authored
Jul 01, 2013
by
Lorenz Meier
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Updated MAVLink, fixed up HIL
parent
478b476a
Changes
12
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
250 additions
and
89 deletions
+250
-89
MAVLinkSimulationMAV.cc
src/comm/MAVLinkSimulationMAV.cc
+41
-3
QGCFlightGearLink.cc
src/comm/QGCFlightGearLink.cc
+7
-3
QGCHilLink.h
src/comm/QGCHilLink.h
+7
-3
QGCJSBSimLink.cc
src/comm/QGCJSBSimLink.cc
+2
-2
QGCXPlaneLink.cc
src/comm/QGCXPlaneLink.cc
+28
-19
QGCXPlaneLink.h
src/comm/QGCXPlaneLink.h
+2
-1
UAS.cc
src/uas/UAS.cc
+148
-46
UAS.h
src/uas/UAS.h
+8
-4
UASInterface.h
src/uas/UASInterface.h
+3
-3
QGCHilXPlaneConfiguration.cc
src/ui/QGCHilXPlaneConfiguration.cc
+3
-3
WaypointViewOnlyView.cc
src/ui/WaypointViewOnlyView.cc
+0
-1
QGCMapWidget.cc
src/ui/map/QGCMapWidget.cc
+1
-1
No files found.
src/comm/MAVLinkSimulationMAV.cc
View file @
f9318458
...
...
@@ -411,9 +411,47 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
mavlink_hil_state_t
state
;
mavlink_msg_hil_state_decode
(
&
msg
,
&
state
);
roll
=
state
.
roll
;
pitch
=
state
.
pitch
;
yaw
=
state
.
yaw
;
double
a
=
state
.
attitude_quaternion
[
0
];
double
b
=
state
.
attitude_quaternion
[
1
];
double
c
=
state
.
attitude_quaternion
[
2
];
double
d
=
state
.
attitude_quaternion
[
3
];
double
aSq
=
a
*
a
;
double
bSq
=
b
*
b
;
double
cSq
=
c
*
c
;
double
dSq
=
d
*
d
;
float
dcm
[
3
][
3
];
dcm
[
0
][
0
]
=
aSq
+
bSq
-
cSq
-
dSq
;
dcm
[
0
][
1
]
=
2.0
*
(
b
*
c
-
a
*
d
);
dcm
[
0
][
2
]
=
2.0
*
(
a
*
c
+
b
*
d
);
dcm
[
1
][
0
]
=
2.0
*
(
b
*
c
+
a
*
d
);
dcm
[
1
][
1
]
=
aSq
-
bSq
+
cSq
-
dSq
;
dcm
[
1
][
2
]
=
2.0
*
(
c
*
d
-
a
*
b
);
dcm
[
2
][
0
]
=
2.0
*
(
b
*
d
-
a
*
c
);
dcm
[
2
][
1
]
=
2.0
*
(
a
*
b
+
c
*
d
);
dcm
[
2
][
2
]
=
aSq
-
bSq
-
cSq
+
dSq
;
float
phi
,
theta
,
psi
;
theta
=
asin
(
-
dcm
[
2
][
0
]);
if
(
fabs
(
theta
-
M_PI_2
)
<
1.0e-3
f
)
{
phi
=
0.0
f
;
psi
=
(
atan2
(
dcm
[
1
][
2
]
-
dcm
[
0
][
1
],
dcm
[
0
][
2
]
+
dcm
[
1
][
1
])
+
phi
);
}
else
if
(
fabs
(
theta
+
M_PI_2
)
<
1.0e-3
f
)
{
phi
=
0.0
f
;
psi
=
atan2f
(
dcm
[
1
][
2
]
-
dcm
[
0
][
1
],
dcm
[
0
][
2
]
+
dcm
[
1
][
1
]
-
phi
);
}
else
{
phi
=
atan2f
(
dcm
[
2
][
1
],
dcm
[
2
][
2
]);
psi
=
atan2f
(
dcm
[
1
][
0
],
dcm
[
0
][
0
]);
}
roll
=
phi
;
pitch
=
theta
;
yaw
=
psi
;
rollspeed
=
state
.
rollspeed
;
pitchspeed
=
state
.
pitchspeed
;
yawspeed
=
state
.
yawspeed
;
...
...
src/comm/QGCFlightGearLink.cc
View file @
f9318458
...
...
@@ -242,6 +242,10 @@ void QGCFlightGearLink::readBytes()
// Parse string
float
roll
,
pitch
,
yaw
,
rollspeed
,
pitchspeed
,
yawspeed
;
double
lat
,
lon
,
alt
;
// XXX add
float
ind_airspeed
=
0.0
f
;
float
true_airspeed
=
0.0
f
;
double
vx
,
vy
,
vz
,
xacc
,
yacc
,
zacc
;
lat
=
values
.
at
(
1
).
toDouble
();
...
...
@@ -265,7 +269,7 @@ void QGCFlightGearLink::readBytes()
// Send updated state
emit
hilStateChanged
(
QGC
::
groundTimeUsecs
(),
roll
,
pitch
,
yaw
,
rollspeed
,
pitchspeed
,
yawspeed
,
lat
,
lon
,
alt
,
vx
,
vy
,
vz
,
xacc
,
yacc
,
zacc
);
vx
,
vy
,
vz
,
ind_airspeed
,
true_airspeed
,
xacc
,
yacc
,
zacc
);
// // Echo data for debugging purposes
// std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
...
...
@@ -299,7 +303,7 @@ bool QGCFlightGearLink::disconnectSimulation()
disconnect
(
process
,
SIGNAL
(
error
(
QProcess
::
ProcessError
)),
this
,
SLOT
(
processError
(
QProcess
::
ProcessError
)));
disconnect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
disconnect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)));
disconnect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
if
(
process
)
{
...
...
@@ -346,7 +350,7 @@ bool QGCFlightGearLink::connectSimulation()
terraSync
=
new
QProcess
(
this
);
connect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
...
...
src/comm/QGCHilLink.h
View file @
f9318458
...
...
@@ -91,16 +91,20 @@ signals:
/** @brief State update from simulation */
void
hilStateChanged
(
quint64
time_us
,
float
roll
,
float
pitch
,
float
yaw
,
float
rollspeed
,
float
pitchspeed
,
float
yawspeed
,
double
lat
,
double
lon
,
double
alt
,
float
vx
,
float
vy
,
float
vz
,
float
xacc
,
float
yacc
,
float
zacc
);
float
vx
,
float
vy
,
float
vz
,
float
ind_airspeed
,
float
true_airspeed
,
float
xacc
,
float
yacc
,
float
zacc
);
void
sensorHilGpsChanged
(
quint64
time_us
,
double
lat
,
double
lon
,
double
alt
,
int
fix_type
,
float
eph
,
float
epv
,
float
vel
,
float
cog
,
int
satellites
);
void
hilGroundTruthChanged
(
quint64
time_us
,
float
roll
,
float
pitch
,
float
yaw
,
float
rollspeed
,
float
pitchspeed
,
float
yawspeed
,
double
lat
,
double
lon
,
double
alt
,
float
vx
,
float
vy
,
float
vz
,
float
ind_airspeed
,
float
true_airspeed
,
float
xacc
,
float
yacc
,
float
zacc
);
void
sensorHilGpsChanged
(
quint64
time_us
,
double
lat
,
double
lon
,
double
alt
,
int
fix_type
,
float
eph
,
float
epv
,
float
vel
,
float
vn
,
float
ve
,
float
vd
,
float
cog
,
int
satellites
);
void
sensorHilRawImuChanged
(
quint64
time_us
,
float
xacc
,
float
yacc
,
float
zacc
,
float
xgyro
,
float
ygyro
,
float
zgyro
,
float
xmag
,
float
ymag
,
float
zmag
,
float
abs_pressure
,
float
diff_pressure
,
float
pressure_alt
,
float
temperature
,
quint
16
fields_updated
);
quint
32
fields_updated
);
/** @brief Remote host and port changed */
void
remoteChanged
(
const
QString
&
hostPort
);
...
...
src/comm/QGCJSBSimLink.cc
View file @
f9318458
...
...
@@ -271,7 +271,7 @@ bool QGCJSBSimLink::disconnectSimulation()
disconnect
(
process
,
SIGNAL
(
error
(
QProcess
::
ProcessError
)),
this
,
SLOT
(
processError
(
QProcess
::
ProcessError
)));
disconnect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
disconnect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)));
disconnect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
if
(
process
)
{
...
...
@@ -311,7 +311,7 @@ bool QGCJSBSimLink::connectSimulation()
process
=
new
QProcess
(
this
);
connect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
...
...
src/comm/QGCXPlaneLink.cc
View file @
f9318458
...
...
@@ -54,7 +54,7 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
simUpdateLast
(
QGC
::
groundTimeMilliseconds
()),
simUpdateLastText
(
QGC
::
groundTimeMilliseconds
()),
simUpdateHz
(
0
),
_sensorHilEnabled
(
fals
e
)
_sensorHilEnabled
(
tru
e
)
{
this
->
localHost
=
localHost
;
this
->
localPort
=
localPort
/*+mav->getUASID()*/
;
...
...
@@ -468,7 +468,8 @@ void QGCXPlaneLink::readBytes()
if
(
p
.
index
==
3
)
{
airspeed
=
p
.
f
[
6
]
*
0.44704
f
;
ind_airspeed
=
p
.
f
[
5
]
*
0.44704
f
;
true_airspeed
=
p
.
f
[
6
]
*
0.44704
f
;
groundspeed
=
p
.
f
[
7
]
*
0.44704
;
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
...
...
@@ -634,19 +635,25 @@ void QGCXPlaneLink::readBytes()
emit
sensorHilRawImuChanged
(
QGC
::
groundTimeUsecs
(),
xacc
,
yacc
,
zacc
,
rollspeed
,
pitchspeed
,
yawspeed
,
xmag
,
ymag
,
zmag
,
abs_pressure
,
diff_pressure
,
pressure_alt
,
temperature
,
fields_changed
);
}
int
gps_fix_type
=
3
;
float
eph
=
0.3
;
float
epv
=
0.6
;
float
vel
=
sqrt
(
vx
*
vx
+
vy
*
vy
+
vz
*
vz
);
float
cog
=
atan2
(
vy
,
vx
);
int
satellites
=
8
;
emit
sensorHilGpsChanged
(
QGC
::
groundTimeUsecs
(),
lat
,
lon
,
alt
,
gps_fix_type
,
eph
,
epv
,
vel
,
cog
,
satellites
);
emit
hilStateChanged
(
QGC
::
groundTimeUsecs
(),
roll
,
pitch
,
yaw
,
rollspeed
,
// XXX make these GUI-configurable and add randomness
int
gps_fix_type
=
3
;
float
eph
=
0.3
;
float
epv
=
0.6
;
float
vel
=
sqrt
(
vx
*
vx
+
vy
*
vy
+
vz
*
vz
);
float
cog
=
atan2
(
vy
,
vx
);
int
satellites
=
8
;
emit
sensorHilGpsChanged
(
QGC
::
groundTimeUsecs
(),
lat
,
lon
,
alt
,
gps_fix_type
,
eph
,
epv
,
vel
,
vx
,
vy
,
vz
,
cog
,
satellites
);
}
else
{
emit
hilStateChanged
(
QGC
::
groundTimeUsecs
(),
roll
,
pitch
,
yaw
,
rollspeed
,
pitchspeed
,
yawspeed
,
lat
,
lon
,
alt
,
vx
,
vy
,
vz
,
xacc
,
yacc
,
zacc
);
vx
,
vy
,
vz
,
ind_airspeed
,
true_airspeed
,
xacc
,
yacc
,
zacc
);
}
emit
hilGroundTruthChanged
(
QGC
::
groundTimeUsecs
(),
roll
,
pitch
,
yaw
,
rollspeed
,
pitchspeed
,
yawspeed
,
lat
,
lon
,
alt
,
vx
,
vy
,
vz
,
ind_airspeed
,
true_airspeed
,
xacc
,
yacc
,
zacc
);
}
if
(
!
oldConnectionState
&&
xPlaneConnected
)
...
...
@@ -694,9 +701,10 @@ bool QGCXPlaneLink::disconnectSimulation()
disconnect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
disconnect
(
mav
,
SIGNAL
(
hilActuatorsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
this
,
SLOT
(
updateActuators
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
disconnect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)));
disconnect
(
this
,
SIGNAL
(
sensorHilGpsChanged
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
int
)),
mav
,
SLOT
(
sendHilGps
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
int
)));
disconnect
(
this
,
SIGNAL
(
sensorHilRawImuChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint16
)),
mav
,
SLOT
(
sendHilSensors
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint16
)));
disconnect
(
this
,
SIGNAL
(
hilGroundTruthChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilGroundTruth
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
disconnect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
disconnect
(
this
,
SIGNAL
(
sensorHilGpsChanged
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)),
mav
,
SLOT
(
sendHilGps
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)));
disconnect
(
this
,
SIGNAL
(
sensorHilRawImuChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)),
mav
,
SLOT
(
sendHilSensors
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
if
(
uas
)
...
...
@@ -878,9 +886,10 @@ bool QGCXPlaneLink::connectSimulation()
connect
(
mav
,
SIGNAL
(
hilControlsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)),
this
,
SLOT
(
updateControls
(
uint64_t
,
float
,
float
,
float
,
float
,
uint8_t
,
uint8_t
)));
connect
(
mav
,
SIGNAL
(
hilActuatorsChanged
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
this
,
SLOT
(
updateActuators
(
uint64_t
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
sensorHilGpsChanged
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
int
)),
mav
,
SLOT
(
sendHilGps
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
int
)));
connect
(
this
,
SIGNAL
(
sensorHilRawImuChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint16
)),
mav
,
SLOT
(
sendHilSensors
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint16
)));
connect
(
this
,
SIGNAL
(
hilGroundTruthChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilGroundTruth
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
hilStateChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)),
mav
,
SLOT
(
sendHilState
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
double
,
double
,
double
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
)));
connect
(
this
,
SIGNAL
(
sensorHilGpsChanged
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)),
mav
,
SLOT
(
sendHilGps
(
quint64
,
double
,
double
,
double
,
int
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
int
)));
connect
(
this
,
SIGNAL
(
sensorHilRawImuChanged
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)),
mav
,
SLOT
(
sendHilSensors
(
quint64
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
float
,
quint32
)));
UAS
*
uas
=
dynamic_cast
<
UAS
*>
(
mav
);
if
(
uas
)
...
...
src/comm/QGCXPlaneLink.h
View file @
f9318458
...
...
@@ -193,7 +193,8 @@ protected:
float
roll
,
pitch
,
yaw
,
rollspeed
,
pitchspeed
,
yawspeed
;
double
lat
,
lon
,
alt
;
float
vx
,
vy
,
vz
,
xacc
,
yacc
,
zacc
;
float
airspeed
;
float
ind_airspeed
;
float
true_airspeed
;
float
groundspeed
;
float
xmag
,
ymag
,
zmag
,
abs_pressure
,
diff_pressure
,
pressure_alt
,
temperature
;
...
...
src/uas/UAS.cc
View file @
f9318458
This diff is collapsed.
Click to expand it.
src/uas/UAS.h
View file @
f9318458
...
...
@@ -696,11 +696,15 @@ public slots:
/** @brief Send the full HIL state to the MAV */
void
sendHilState
(
quint64
time_us
,
float
roll
,
float
pitch
,
float
yaw
,
float
rollRotationRate
,
float
pitchRotationRate
,
float
yawRotationRate
,
double
lat
,
double
lon
,
double
alt
,
float
vx
,
float
vy
,
float
vz
,
float
xacc
,
float
yacc
,
float
zacc
);
float
vx
,
float
vy
,
float
vz
,
float
ind_airspeed
,
float
true_airspeed
,
float
xacc
,
float
yacc
,
float
zacc
);
void
sendHilGroundTruth
(
quint64
time_us
,
float
roll
,
float
pitch
,
float
yaw
,
float
rollRotationRate
,
float
pitchRotationRate
,
float
yawRotationRate
,
double
lat
,
double
lon
,
double
alt
,
float
vx
,
float
vy
,
float
vz
,
float
ind_airspeed
,
float
true_airspeed
,
float
xacc
,
float
yacc
,
float
zacc
);
/** @brief RAW sensors for sensor HIL */
void
sendHilSensors
(
quint64
time_us
,
float
xacc
,
float
yacc
,
float
zacc
,
float
roll
RotationRate
,
float
pitchRotationRate
,
float
yawRotationRate
,
float
xmag
,
float
ymag
,
float
zmag
,
float
abs_pressure
,
float
diff_pressure
,
float
pressure_alt
,
float
temperature
,
quint16
fields_changed
);
void
sendHilSensors
(
quint64
time_us
,
float
xacc
,
float
yacc
,
float
zacc
,
float
roll
speed
,
float
pitchspeed
,
float
yawspeed
,
float
xmag
,
float
ymag
,
float
zmag
,
float
abs_pressure
,
float
diff_pressure
,
float
pressure_alt
,
float
temperature
,
quint32
fields_changed
);
/**
* @param time_us
...
...
@@ -714,7 +718,7 @@ public slots:
* @param cog course over ground, in radians, -pi..pi
* @param satellites
*/
void
sendHilGps
(
quint64
time_us
,
double
lat
,
double
lon
,
double
alt
,
int
fix_type
,
float
eph
,
float
epv
,
float
vel
,
float
cog
,
int
satellites
);
void
sendHilGps
(
quint64
time_us
,
double
lat
,
double
lon
,
double
alt
,
int
fix_type
,
float
eph
,
float
epv
,
float
vel
,
float
vn
,
float
ve
,
float
vd
,
float
cog
,
int
satellites
);
/** @brief Places the UAV in Hardware-in-the-Loop simulation status **/
...
...
src/uas/UASInterface.h
View file @
f9318458
...
...
@@ -369,14 +369,14 @@ public slots:
/** @brief Send the full HIL state to the MAV */
virtual
void
sendHilState
(
quint64
time_us
,
float
roll
,
float
pitch
,
float
yaw
,
float
rollspeed
,
float
pitchspeed
,
float
yawspeed
,
double
lat
,
double
lon
,
double
alt
,
float
vx
,
float
vy
,
float
vz
,
float
xacc
,
float
yacc
,
float
zacc
)
=
0
;
float
vx
,
float
vy
,
float
vz
,
float
ind_airspeed
,
float
true_airspeed
,
float
xacc
,
float
yacc
,
float
zacc
)
=
0
;
/** @brief RAW sensors for sensor HIL */
virtual
void
sendHilSensors
(
quint64
time_us
,
float
xacc
,
float
yacc
,
float
zacc
,
float
rollspeed
,
float
pitchspeed
,
float
yawspeed
,
float
xmag
,
float
ymag
,
float
zmag
,
float
abs_pressure
,
float
diff_pressure
,
float
pressure_alt
,
float
temperature
,
quint16
fields_changed
)
=
0
;
float
xmag
,
float
ymag
,
float
zmag
,
float
abs_pressure
,
float
diff_pressure
,
float
pressure_alt
,
float
temperature
,
quint32
fields_changed
)
=
0
;
/** @brief Send raw GPS for sensor HIL */
virtual
void
sendHilGps
(
quint64
time_us
,
double
lat
,
double
lon
,
double
alt
,
int
fix_type
,
float
eph
,
float
epv
,
float
vel
,
float
cog
,
int
satellites
)
=
0
;
virtual
void
sendHilGps
(
quint64
time_us
,
double
lat
,
double
lon
,
double
alt
,
int
fix_type
,
float
eph
,
float
epv
,
float
vel
,
float
vn
,
float
ve
,
float
vd
,
float
cog
,
int
satellites
)
=
0
;
protected:
...
...
src/ui/QGCHilXPlaneConfiguration.cc
View file @
f9318458
...
...
@@ -29,9 +29,9 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QWidget *
ui
->
airframeComboBox
->
setCurrentIndex
(
link
->
getAirFrameIndex
());
// XXX not implemented yet
ui
->
airframeComboBox
->
hide
();
ui
->
sensorHilCheckBox
->
setChecked
(
link
->
sensorHilEnabled
());
connect
(
link
,
SIGNAL
(
sensorHilChanged
(
bool
)),
ui
->
sensorHilCheckBox
,
SLOT
(
setChecked
(
bool
)));
connect
(
ui
->
sensorHilCheckBox
,
SIGNAL
(
clicked
(
bool
)),
link
,
SLOT
(
enableSensorHIL
(
bool
)));
ui
->
sensorHilCheckBox
->
setChecked
(
xplane
->
sensorHilEnabled
());
connect
(
xplane
,
SIGNAL
(
sensorHilChanged
(
bool
)),
ui
->
sensorHilCheckBox
,
SLOT
(
setChecked
(
bool
)));
connect
(
ui
->
sensorHilCheckBox
,
SIGNAL
(
clicked
(
bool
)),
xplane
,
SLOT
(
enableSensorHIL
(
bool
)));
connect
(
link
,
SIGNAL
(
versionChanged
(
int
)),
this
,
SLOT
(
setVersion
(
int
)));
}
...
...
src/ui/WaypointViewOnlyView.cc
View file @
f9318458
...
...
@@ -77,7 +77,6 @@ void WaypointViewOnlyView::setCurrent(bool state)
void
WaypointViewOnlyView
::
updateValues
()
{
qDebug
()
<<
"Trof: WaypointViewOnlyView::updateValues() ID:"
<<
wp
->
getId
();
// Check if we just lost the wp, delete the widget
// accordingly
if
(
!
wp
)
...
...
src/ui/map/QGCMapWidget.cc
View file @
f9318458
...
...
@@ -595,7 +595,7 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
*/
void
QGCMapWidget
::
updateWaypoint
(
int
uas
,
Waypoint
*
wp
)
{
qDebug
()
<<
__FILE__
<<
__LINE__
<<
"UPDATING WP FUNCTION CALLED"
;
//
qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED";
// Source of the event was in this widget, do nothing
if
(
firingWaypointChange
==
wp
)
{
return
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment