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
000e0ef4
Commit
000e0ef4
authored
Apr 28, 2015
by
Don Gagne
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Remove the "Simulate" feature
parent
2f7fe9dd
Changes
19
Hide whitespace changes
Inline
Side-by-side
Showing
19 changed files
with
4 additions
and
2896 deletions
+4
-2896
QGCApplication.pro
QGCApplication.pro
+0
-8
QGCApplication.cc
src/QGCApplication.cc
+0
-1
LinkConfiguration.h
src/comm/LinkConfiguration.h
+0
-1
MAVLinkSimulationLink.cc
src/comm/MAVLinkSimulationLink.cc
+0
-860
MAVLinkSimulationLink.h
src/comm/MAVLinkSimulationLink.h
+0
-138
MAVLinkSimulationMAV.cc
src/comm/MAVLinkSimulationMAV.cc
+0
-522
MAVLinkSimulationMAV.h
src/comm/MAVLinkSimulationMAV.h
+0
-75
MAVLinkSimulationWaypointPlanner.cc
src/comm/MAVLinkSimulationWaypointPlanner.cc
+0
-1131
MAVLinkSimulationWaypointPlanner.h
src/comm/MAVLinkSimulationWaypointPlanner.h
+0
-75
MAVLinkSwarmSimulationLink.cc
src/comm/MAVLinkSwarmSimulationLink.cc
+0
-12
MAVLinkSwarmSimulationLink.h
src/comm/MAVLinkSwarmSimulationLink.h
+0
-19
MockLinkMissionItemHandler.h
src/comm/MockLinkMissionItemHandler.h
+0
-1
MainWindow.cc
src/ui/MainWindow.cc
+0
-22
MainWindow.h
src/ui/MainWindow.h
+0
-5
MainWindow.ui
src/ui/MainWindow.ui
+0
-12
QGCMAVLinkLogPlayer.cc
src/ui/QGCMAVLinkLogPlayer.cc
+2
-1
QGCMAVLinkLogPlayer.h
src/ui/QGCMAVLinkLogPlayer.h
+2
-2
QGCUnconnectedInfoWidget.cc
src/ui/uas/QGCUnconnectedInfoWidget.cc
+0
-10
UASListWidget.cc
src/ui/uas/UASListWidget.cc
+0
-1
No files found.
QGCApplication.pro
View file @
000e0ef4
...
...
@@ -235,10 +235,6 @@ HEADERS += \
src
/
comm
/
LinkInterface
.
h
\
src
/
comm
/
LinkManager
.
h
\
src
/
comm
/
MAVLinkProtocol
.
h
\
src
/
comm
/
MAVLinkSimulationLink
.
h
\
src
/
comm
/
MAVLinkSimulationMAV
.
h
\
src
/
comm
/
MAVLinkSimulationWaypointPlanner
.
h
\
src
/
comm
/
MAVLinkSwarmSimulationLink
.
h
\
src
/
comm
/
ProtocolInterface
.
h
\
src
/
comm
/
QGCFlightGearLink
.
h
\
src
/
comm
/
QGCHilLink
.
h
\
...
...
@@ -378,10 +374,6 @@ SOURCES += \
src
/
comm
/
LinkConfiguration
.
cc
\
src
/
comm
/
LinkManager
.
cc
\
src
/
comm
/
MAVLinkProtocol
.
cc
\
src
/
comm
/
MAVLinkSimulationLink
.
cc
\
src
/
comm
/
MAVLinkSimulationMAV
.
cc
\
src
/
comm
/
MAVLinkSimulationWaypointPlanner
.
cc
\
src
/
comm
/
MAVLinkSwarmSimulationLink
.
cc
\
src
/
comm
/
QGCFlightGearLink
.
cc
\
src
/
comm
/
QGCJSBSimLink
.
cc
\
src
/
comm
/
QGCXPlaneLink
.
cc
\
...
...
src/QGCApplication.cc
View file @
000e0ef4
...
...
@@ -49,7 +49,6 @@
#include "QGCMessageBox.h"
#include "MainWindow.h"
#include "UDPLink.h"
#include "MAVLinkSimulationLink.h"
#include "SerialLink.h"
#include "QGCSingleton.h"
#include "LinkManager.h"
...
...
src/comm/LinkConfiguration.h
View file @
000e0ef4
...
...
@@ -44,7 +44,6 @@ public:
TypeTcp
,
///< TCP Link
// TODO Below is not yet implemented
#if 0
TypeSimulation, ///< Simulation Link
TypeForwarding, ///< Forwarding Link
TypeXbee, ///< XBee Proprietary Link
TypeOpal, ///< Opal-RT Link
...
...
src/comm/MAVLinkSimulationLink.cc
deleted
100644 → 0
View file @
2f7fe9dd
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Implementation of simulated system link
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <cmath>
#include <QTime>
#include <QImage>
#include <QDebug>
#include <QFileInfo>
#include "LinkManager.h"
#include "MAVLinkProtocol.h"
#include "MAVLinkSimulationLink.h"
#include "QGCMAVLink.h"
#include "QGC.h"
#include "MAVLinkSimulationMAV.h"
/**
* Create a simulated link. This link is connected to an input and output file.
* The link sends one line at a time at the specified sendrate. The timing of
* the sendrate is free of drift, which means it is stable on the long run.
* However, small deviations are mixed in to vary the sendrate slightly
* in order to simulate disturbances on a real communication link.
*
* @param readFile The file with the messages to read (must be in ASCII format, line breaks can be Unix or Windows style)
* @param writeFile The received messages are written to that file
* @param rate The rate at which the messages are sent (in intervals of milliseconds)
**/
MAVLinkSimulationLink
::
MAVLinkSimulationLink
(
QString
readFile
,
QString
writeFile
,
int
rate
)
:
readyBytes
(
0
),
timeOffset
(
0
)
{
this
->
rate
=
rate
;
_isConnected
=
false
;
onboardParams
=
QMap
<
QString
,
float
>
();
onboardParams
.
insert
(
"PID_ROLL_K_P"
,
0.5
f
);
onboardParams
.
insert
(
"PID_PITCH_K_P"
,
0.5
f
);
onboardParams
.
insert
(
"PID_YAW_K_P"
,
0.5
f
);
onboardParams
.
insert
(
"PID_XY_K_P"
,
100.0
f
);
onboardParams
.
insert
(
"PID_ALT_K_P"
,
0.5
f
);
onboardParams
.
insert
(
"SYS_TYPE"
,
1
);
onboardParams
.
insert
(
"SYS_ID"
,
systemId
);
onboardParams
.
insert
(
"RC4_REV"
,
0
);
onboardParams
.
insert
(
"RC5_REV"
,
1
);
onboardParams
.
insert
(
"HDNG2RLL_P"
,
0.7
f
);
onboardParams
.
insert
(
"HDNG2RLL_I"
,
0.01
f
);
onboardParams
.
insert
(
"HDNG2RLL_D"
,
0.02
f
);
onboardParams
.
insert
(
"HDNG2RLL_IMAX"
,
500.0
f
);
onboardParams
.
insert
(
"RLL2SRV_P"
,
0.4
f
);
onboardParams
.
insert
(
"RLL2SRV_I"
,
0.0
f
);
onboardParams
.
insert
(
"RLL2SRV_D"
,
0.0
f
);
onboardParams
.
insert
(
"RLL2SRV_IMAX"
,
500.0
f
);
// Comments on the variables can be found in the header file
simulationFile
=
new
QFile
(
readFile
,
this
);
if
(
simulationFile
->
exists
()
&&
simulationFile
->
open
(
QIODevice
::
ReadOnly
|
QIODevice
::
Text
))
{
simulationHeader
=
simulationFile
->
readLine
();
}
receiveFile
=
new
QFile
(
writeFile
,
this
);
lastSent
=
QGC
::
groundTimeMilliseconds
();
if
(
simulationFile
->
exists
())
{
this
->
name
=
"Simulation: "
+
QFileInfo
(
simulationFile
->
fileName
()).
fileName
();
}
else
{
this
->
name
=
"MAVLink simulation link"
;
}
// Initialize the pseudo-random number generator
srand
(
QTime
::
currentTime
().
msec
());
maxTimeNoise
=
0
;
LinkManager
::
instance
()
->
_addLink
(
this
);
}
MAVLinkSimulationLink
::~
MAVLinkSimulationLink
()
{
//TODO Check destructor
// fileStream->flush();
// outStream->flush();
// Force termination, there is no
// need for cleanup since
// this thread is not manipulating
// any relevant data
terminate
();
delete
simulationFile
;
}
void
MAVLinkSimulationLink
::
run
()
{
status
.
voltage_battery
=
0
;
status
.
errors_comm
=
0
;
system
.
base_mode
=
MAV_MODE_PREFLIGHT
;
system
.
custom_mode
=
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
|
MAV_MODE_FLAG_SAFETY_ARMED
;
system
.
system_status
=
MAV_STATE_UNINIT
;
forever
{
static
quint64
last
=
0
;
if
(
QGC
::
groundTimeMilliseconds
()
-
last
>=
rate
)
{
if
(
_isConnected
)
{
mainloop
();
readBytes
();
}
else
{
// Sleep for substantially longer
// if not connected
QGC
::
SLEEP
::
msleep
(
500
);
}
last
=
QGC
::
groundTimeMilliseconds
();
}
QGC
::
SLEEP
::
msleep
(
3
);
}
}
void
MAVLinkSimulationLink
::
sendMAVLinkMessage
(
const
mavlink_message_t
*
msg
)
{
// Allocate buffer with packet data
uint8_t
buf
[
MAVLINK_MAX_PACKET_LEN
];
unsigned
int
bufferlength
=
mavlink_msg_to_send_buffer
(
buf
,
msg
);
// Pack to link buffer
readyBufferMutex
.
lock
();
for
(
unsigned
int
i
=
0
;
i
<
bufferlength
;
i
++
)
{
readyBuffer
.
enqueue
(
*
(
buf
+
i
));
}
readyBufferMutex
.
unlock
();
}
void
MAVLinkSimulationLink
::
enqueue
(
uint8_t
*
stream
,
uint8_t
*
index
,
mavlink_message_t
*
msg
)
{
// Allocate buffer with packet data
uint8_t
buf
[
MAVLINK_MAX_PACKET_LEN
];
unsigned
int
bufferlength
=
mavlink_msg_to_send_buffer
(
buf
,
msg
);
//add data into datastream
memcpy
(
stream
+
(
*
index
),
buf
,
bufferlength
);
(
*
index
)
+=
bufferlength
;
}
void
MAVLinkSimulationLink
::
mainloop
()
{
// Test for encoding / decoding packets
// Test data stream
streampointer
=
0
;
// Fake system values
static
float
fullVoltage
=
4.2
f
*
3.0
f
;
static
float
emptyVoltage
=
3.35
f
*
3.0
f
;
static
float
voltage
=
fullVoltage
;
static
float
drainRate
=
0.025
f
;
// x.xx% of the capacity is linearly drained per second
mavlink_attitude_t
attitude
;
memset
(
&
attitude
,
0
,
sizeof
(
mavlink_attitude_t
));
mavlink_raw_imu_t
rawImuValues
;
memset
(
&
rawImuValues
,
0
,
sizeof
(
mavlink_raw_imu_t
));
uint8_t
buffer
[
MAVLINK_MAX_PACKET_LEN
];
int
bufferlength
;
mavlink_message_t
msg
;
// Timers
static
unsigned
int
rate1hzCounter
=
1
;
static
unsigned
int
rate10hzCounter
=
1
;
static
unsigned
int
rate50hzCounter
=
1
;
static
unsigned
int
circleCounter
=
0
;
// Vary values
// VOLTAGE
// The battery is drained constantly
voltage
=
voltage
-
((
fullVoltage
-
emptyVoltage
)
*
drainRate
/
rate
);
if
(
voltage
<
3.550
f
*
3.0
f
)
voltage
=
3.550
f
*
3.0
f
;
static
int
state
=
0
;
if
(
state
==
0
)
{
state
++
;
}
// 50 HZ TASKS
if
(
rate50hzCounter
==
1000
/
rate
/
40
)
{
if
(
simulationFile
->
isOpen
())
{
if
(
simulationFile
->
atEnd
())
{
// We reached the end of the file, start from scratch
simulationFile
->
reset
();
simulationHeader
=
simulationFile
->
readLine
();
}
// Data was made available, read one line
// first entry is the timestamp
QString
values
=
QString
(
simulationFile
->
readLine
());
QStringList
parts
=
values
.
split
(
"
\t
"
);
QStringList
keys
=
simulationHeader
.
split
(
"
\t
"
);
//qDebug() << simulationHeader;
//qDebug() << values;
bool
ok
;
static
quint64
lastTime
=
0
;
static
quint64
baseTime
=
0
;
quint64
time
=
QString
(
parts
.
first
()).
toLongLong
(
&
ok
,
10
);
// FIXME Remove multiplicaton by 1000
time
*=
1000
;
if
(
ok
)
{
if
(
timeOffset
==
0
)
{
timeOffset
=
time
;
baseTime
=
time
;
}
if
(
lastTime
>
time
)
{
// We have wrapped around in the logfile
// Add the measurement time interval to the base time
baseTime
+=
lastTime
-
timeOffset
;
}
lastTime
=
time
;
time
=
time
-
timeOffset
+
baseTime
;
// Gather individual measurement values
for
(
int
i
=
1
;
i
<
(
parts
.
size
()
-
1
);
++
i
)
{
// Get one data field
bool
res
;
double
d
=
QString
(
parts
.
at
(
i
)).
toDouble
(
&
res
);
if
(
!
res
)
d
=
0
;
if
(
keys
.
value
(
i
,
""
)
==
"Accel._X"
)
{
rawImuValues
.
xacc
=
d
;
}
if
(
keys
.
value
(
i
,
""
)
==
"Accel._Y"
)
{
rawImuValues
.
yacc
=
d
;
}
if
(
keys
.
value
(
i
,
""
)
==
"Accel._Z"
)
{
rawImuValues
.
zacc
=
d
;
}
if
(
keys
.
value
(
i
,
""
)
==
"Gyro_Phi"
)
{
rawImuValues
.
xgyro
=
d
;
attitude
.
rollspeed
=
((
d
-
29.000
)
/
15000.0
)
*
2.7
-
2.7
-
2.65
;
}
if
(
keys
.
value
(
i
,
""
)
==
"Gyro_Theta"
)
{
rawImuValues
.
ygyro
=
d
;
attitude
.
pitchspeed
=
((
d
-
29.000
)
/
15000.0
)
*
2.7
-
2.7
-
2.65
;
}
if
(
keys
.
value
(
i
,
""
)
==
"Gyro_Psi"
)
{
rawImuValues
.
zgyro
=
d
;
attitude
.
yawspeed
=
((
d
-
29.000
)
/
3000.0
)
*
2.7
-
2.7
-
2.65
;
}
if
(
keys
.
value
(
i
,
""
)
==
"roll_IMU"
)
{
attitude
.
roll
=
d
;
}
if
(
keys
.
value
(
i
,
""
)
==
"pitch_IMU"
)
{
attitude
.
pitch
=
d
;
}
if
(
keys
.
value
(
i
,
""
)
==
"yaw_IMU"
)
{
attitude
.
yaw
=
d
;
}
//Accel._X Accel._Y Accel._Z Battery Bottom_Rotor CPU_Load Ground_Dist. Gyro_Phi Gyro_Psi Gyro_Theta Left_Servo Mag._X Mag._Y Mag._Z Pressure Right_Servo Temperature Top_Rotor pitch_IMU roll_IMU yaw_IMU
}
// Send out packets
// ATTITUDE
attitude
.
time_boot_ms
=
time
/
1000
;
// Pack message and get size of encoded byte string
mavlink_msg_attitude_encode
(
systemId
,
componentId
,
&
msg
,
&
attitude
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// IMU
rawImuValues
.
time_usec
=
time
;
rawImuValues
.
xmag
=
0
;
rawImuValues
.
ymag
=
0
;
rawImuValues
.
zmag
=
0
;
// Pack message and get size of encoded byte string
mavlink_msg_raw_imu_encode
(
systemId
,
componentId
,
&
msg
,
&
rawImuValues
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
//qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer;
//qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
}
}
rate50hzCounter
=
1
;
}
// 10 HZ TASKS
if
(
rate10hzCounter
==
1000
/
rate
/
9
)
{
rate10hzCounter
=
1
;
double
lastX
=
x
;
double
lastY
=
y
;
double
lastZ
=
z
;
double
hackDt
=
0.1
f
;
// 100 ms
// Move X Position
x
=
12.0
*
sin
(((
double
)
circleCounter
)
/
200.0
);
y
=
5.0
*
cos
(((
double
)
circleCounter
)
/
200.0
);
z
=
1.8
+
1.2
*
sin
(((
double
)
circleCounter
)
/
200.0
);
double
xSpeed
=
(
x
-
lastX
)
/
hackDt
;
double
ySpeed
=
(
y
-
lastY
)
/
hackDt
;
double
zSpeed
=
(
z
-
lastZ
)
/
hackDt
;
circleCounter
++
;
// x = (x > 5.0f) ? 5.0f : x;
// y = (y > 5.0f) ? 5.0f : y;
// z = (z > 3.0f) ? 3.0f : z;
// x = (x < -5.0f) ? -5.0f : x;
// y = (y < -5.0f) ? -5.0f : y;
// z = (z < -3.0f) ? -3.0f : z;
mavlink_message_t
ret
;
// Send back new position
mavlink_msg_local_position_ned_pack
(
systemId
,
componentId
,
&
ret
,
0
,
x
,
y
,
-
fabs
(
z
),
xSpeed
,
ySpeed
,
zSpeed
);
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
ret
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// // GPS RAW
// mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f);
// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
// //add data into datastream
// memcpy(stream+streampointer,buffer, bufferlength);
// streampointer += bufferlength;
// GLOBAL POSITION
mavlink_msg_global_position_int_pack
(
systemId
,
componentId
,
&
ret
,
0
,
(
473780.28137103
+
(
x
))
*
1E3
,
(
85489.9892510421
+
(
y
))
*
1E3
,
(
z
+
550.0
)
*
1000.0
,
(
z
+
550.0
)
*
1000.0
-
1
,
xSpeed
,
ySpeed
,
zSpeed
,
yaw
);
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
ret
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// GLOBAL POSITION VEHICLE 2
mavlink_msg_global_position_int_pack
(
systemId
+
1
,
componentId
+
1
,
&
ret
,
0
,
(
473780.28137103
+
(
x
+
0.00001
))
*
1E3
,
(
85489.9892510421
+
((
y
/
2
)
+
0.00001
))
*
1E3
,
(
z
+
550.0
)
*
1000.0
,
(
z
+
550.0
)
*
1000.0
-
1
,
xSpeed
,
ySpeed
,
zSpeed
,
yaw
);
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
ret
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// // ATTITUDE VEHICLE 2
// mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
// sendMAVLinkMessage(&ret);
// // GLOBAL POSITION VEHICLE 3
// mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
// //add data into datastream
// memcpy(stream+streampointer,buffer, bufferlength);
// streampointer += bufferlength;
static
int
rcCounter
=
0
;
if
(
rcCounter
==
2
)
{
mavlink_rc_channels_t
chan
;
chan
.
time_boot_ms
=
0
;
chan
.
chan1_raw
=
1000
+
((
int
)(
fabs
(
x
)
*
1000
)
%
2000
);
chan
.
chan2_raw
=
1000
+
((
int
)(
fabs
(
y
)
*
1000
)
%
2000
);
chan
.
chan3_raw
=
1000
+
((
int
)(
fabs
(
z
)
*
1000
)
%
2000
);
chan
.
chan4_raw
=
(
chan
.
chan1_raw
+
chan
.
chan2_raw
)
/
2.0
f
;
chan
.
chan5_raw
=
(
chan
.
chan3_raw
+
chan
.
chan4_raw
)
/
2.0
f
;
chan
.
chan6_raw
=
(
chan
.
chan3_raw
+
chan
.
chan2_raw
)
/
2.0
f
;
chan
.
chan7_raw
=
(
chan
.
chan4_raw
+
chan
.
chan2_raw
)
/
2.0
f
;
chan
.
chan8_raw
=
0
;
chan
.
rssi
=
100
;
mavlink_msg_rc_channels_encode
(
systemId
,
componentId
,
&
msg
,
&
chan
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
rcCounter
=
0
;
}
rcCounter
++
;
}
// 1 HZ TASKS
if
(
rate1hzCounter
==
1000
/
rate
/
1
)
{
// STATE
static
int
statusCounter
=
0
;
if
(
statusCounter
==
100
)
{
system
.
base_mode
=
(
system
.
base_mode
+
1
)
%
MAV_MODE_ENUM_END
;
statusCounter
=
0
;
}
statusCounter
++
;
static
int
detectionCounter
=
6
;
if
(
detectionCounter
%
10
==
0
)
{
}
detectionCounter
++
;
status
.
voltage_battery
=
voltage
*
1000
;
// millivolts
status
.
load
=
33
*
detectionCounter
%
1000
;
// Pack message and get size of encoded byte string
mavlink_msg_sys_status_encode
(
systemId
,
componentId
,
&
msg
,
&
status
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// Pack debug text message
mavlink_statustext_t
text
;
text
.
severity
=
0
;
strcpy
((
char
*
)(
text
.
text
),
"Text message from system 32"
);
mavlink_msg_statustext_encode
(
systemId
,
componentId
,
&
msg
,
&
text
);
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
/*
// Pack message and get size of encoded byte string
mavlink_msg_boot_pack(systemId, componentId, &msg, version);
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;*/
// HEARTBEAT
static
int
typeCounter
=
0
;
uint8_t
mavType
;
if
(
typeCounter
<
10
)
{
mavType
=
MAV_TYPE_QUADROTOR
;
}
else
{
mavType
=
typeCounter
%
(
MAV_TYPE_GCS
);
}
typeCounter
++
;
// Pack message and get size of encoded byte string
mavlink_msg_heartbeat_pack
(
systemId
,
componentId
,
&
msg
,
mavType
,
MAV_AUTOPILOT_PIXHAWK
,
system
.
base_mode
,
system
.
custom_mode
,
system
.
system_status
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// Pack message and get size of encoded byte string
mavlink_msg_heartbeat_pack
(
systemId
+
1
,
componentId
+
1
,
&
msg
,
mavType
,
MAV_AUTOPILOT_GENERIC
,
system
.
base_mode
,
system
.
custom_mode
,
system
.
system_status
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// Send controller states
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// Pack message and get size of encoded byte string
mavlink_msg_sys_status_encode
(
54
,
componentId
,
&
msg
,
&
status
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
rate1hzCounter
=
1
;
}
// FULL RATE TASKS
// Default is 50 Hz
/*
// 50 HZ TASKS
if (rate50hzCounter == 1000 / rate / 50)
{
//streampointer = 0;
// Attitude
// Pack message and get size of encoded byte string
mavlink_msg_attitude_pack(systemId, componentId, &msg, usec, roll, pitch, yaw, 0, 0, 0);
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
rate50hzCounter = 1;
}*/
readyBufferMutex
.
lock
();
for
(
unsigned
int
i
=
0
;
i
<
streampointer
;
i
++
)
{
readyBuffer
.
enqueue
(
*
(
stream
+
i
));
}
readyBufferMutex
.
unlock
();
// Increment counters after full main loop
rate1hzCounter
++
;
rate10hzCounter
++
;
rate50hzCounter
++
;
}
void
MAVLinkSimulationLink
::
writeBytes
(
const
char
*
data
,
qint64
size
)
{
// Parse bytes
mavlink_message_t
msg
;
mavlink_status_t
comm
;
uint8_t
stream
[
2048
];
int
streampointer
=
0
;
uint8_t
buffer
[
MAVLINK_MAX_PACKET_LEN
];
int
bufferlength
=
0
;
// Initialize drop count to 0 so it isn't referenced uninitialized when returned at the bottom of this function
comm
.
packet_rx_drop_count
=
0
;
// Output all bytes as hex digits
for
(
int
i
=
0
;
i
<
size
;
i
++
)
{
if
(
mavlink_parse_char
(
getMavlinkChannel
(),
data
[
i
],
&
msg
,
&
comm
))
{
// MESSAGE RECEIVED!
// qDebug() << "SIMULATION LINK RECEIVED MESSAGE!";
emit
messageReceived
(
msg
);
switch
(
msg
.
msgid
)
{
// SET THE SYSTEM MODE
case
MAVLINK_MSG_ID_SET_MODE
:
{
mavlink_set_mode_t
mode
;
mavlink_msg_set_mode_decode
(
&
msg
,
&
mode
);
// Set mode indepent of mode.target
system
.
base_mode
=
mode
.
base_mode
;
}
break
;
// EXECUTE OPERATOR ACTIONS
case
MAVLINK_MSG_ID_COMMAND_LONG
:
{
mavlink_command_long_t
action
;
mavlink_msg_command_long_decode
(
&
msg
,
&
action
);
// qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system;
// FIXME MAVLINKV10PORTINGNEEDED
// switch (action.action) {
// case MAV_ACTION_LAUNCH:
// status.status = MAV_STATE_ACTIVE;
// status.mode = MAV_MODE_AUTO;
// break;
// case MAV_ACTION_RETURN:
// status.status = MAV_STATE_ACTIVE;
// break;
// case MAV_ACTION_MOTORS_START:
// status.status = MAV_STATE_ACTIVE;
// status.mode = MAV_MODE_LOCKED;
// break;
// case MAV_ACTION_MOTORS_STOP:
// status.status = MAV_STATE_STANDBY;
// status.mode = MAV_MODE_LOCKED;
// break;
// case MAV_ACTION_EMCY_KILL:
// status.status = MAV_STATE_EMERGENCY;
// status.mode = MAV_MODE_MANUAL;
// break;
// case MAV_ACTION_SHUTDOWN:
// status.status = MAV_STATE_POWEROFF;
// status.mode = MAV_MODE_LOCKED;
// break;
// }
}
break
;
case
MAVLINK_MSG_ID_PARAM_REQUEST_LIST
:
{
// qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
mavlink_param_request_list_t
read
;
mavlink_msg_param_request_list_decode
(
&
msg
,
&
read
);
if
(
read
.
target_system
==
systemId
)
{
// Output all params
// Iterate through all components, through all parameters and emit them
QMap
<
QString
,
float
>::
iterator
i
;
// Iterate through all components / subsystems
int
j
=
0
;
for
(
i
=
onboardParams
.
begin
();
i
!=
onboardParams
.
end
();
++
i
)
{
if
(
j
!=
5
)
{
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack
(
read
.
target_system
,
componentId
,
&
msg
,
i
.
key
().
toStdString
().
c_str
(),
i
.
value
(),
MAV_PARAM_TYPE_REAL32
,
onboardParams
.
size
(),
j
);
// Allocate buffer with packet data
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
}
j
++
;
}
// qDebug() << "SIMULATION SENT PARAMETERS TO GCS";
}
}
break
;