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
589fe3af
Commit
589fe3af
authored
Aug 18, 2010
by
Bryan Godbolt
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
getSignals() seems to work, but causes crash
parent
2402fc33
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
175 additions
and
95 deletions
+175
-95
OpalLink.cc
src/comm/OpalLink.cc
+162
-94
OpalLink.h
src/comm/OpalLink.h
+13
-0
UDPLink.cc
src/comm/UDPLink.cc
+0
-1
No files found.
src/comm/OpalLink.cc
View file @
589fe3af
...
@@ -5,6 +5,8 @@ OpalLink::OpalLink() :
...
@@ -5,6 +5,8 @@ OpalLink::OpalLink() :
heartbeatTimer
(
new
QTimer
(
this
)),
heartbeatTimer
(
new
QTimer
(
this
)),
heartbeatRate
(
MAVLINK_HEARTBEAT_DEFAULT_RATE
),
heartbeatRate
(
MAVLINK_HEARTBEAT_DEFAULT_RATE
),
m_heartbeatsEnabled
(
true
),
m_heartbeatsEnabled
(
true
),
getSignalsTimer
(
new
QTimer
(
this
)),
getSignalsPeriod
(
1000
),
receiveBuffer
(
new
QQueue
<
QByteArray
>
()),
receiveBuffer
(
new
QQueue
<
QByteArray
>
()),
systemID
(
1
),
systemID
(
1
),
componentID
(
1
)
componentID
(
1
)
...
@@ -17,85 +19,153 @@ OpalLink::OpalLink() :
...
@@ -17,85 +19,153 @@ OpalLink::OpalLink() :
LinkManager
::
instance
()
->
add
(
this
);
LinkManager
::
instance
()
->
add
(
this
);
// Start heartbeat timer, emitting a heartbeat at the configured rate
// Start heartbeat timer, emitting a heartbeat at the configured rate
qDebug
()
<<
"OpalLink::OpalLink:: Connect the timer"
;
QObject
::
connect
(
heartbeatTimer
,
SIGNAL
(
timeout
()),
this
,
SLOT
(
heartbeat
()));
QObject
::
connect
(
heartbeatTimer
,
SIGNAL
(
timeout
()),
this
,
SLOT
(
heartbeat
()));
heartbeatTimer
->
start
(
1000
/
heartbeatRate
);
QObject
::
connect
(
getSignalsTimer
,
SIGNAL
(
timeout
()),
this
,
SLOT
(
getSignals
()));
}
}
void
OpalLink
::
run
()
/*
*
Communication
*
*/
qint64
OpalLink
::
bytesAvailable
()
{
{
qDebug
()
<<
"OpalLink::run():: Starting the thread"
;
return
0
;
}
}
int
OpalLink
::
getId
(
)
void
OpalLink
::
writeBytes
(
const
char
*
bytes
,
qint64
length
)
{
{
return
id
;
}
}
QString
OpalLink
::
getName
()
void
OpalLink
::
readBytes
(
char
*
bytes
,
qint64
maxLength
)
{
{
return
name
;
receiveDataMutex
.
lock
();
qDebug
()
<<
"OpalLink::readBytes(): Reading a message. size of buffer: "
<<
receiveBuffer
->
count
();
QByteArray
message
=
receiveBuffer
->
dequeue
();
if
(
maxLength
<
message
.
size
())
{
qDebug
()
<<
"OpalLink::readBytes:: Buffer Overflow"
;
memcpy
(
bytes
,
message
.
data
(),
maxLength
);
}
else
{
memcpy
(
bytes
,
message
.
data
(),
message
.
size
());
}
emit
bytesReceived
(
this
,
message
);
receiveDataMutex
.
unlock
();
}
}
void
OpalLink
::
setName
(
QString
nam
e
)
void
OpalLink
::
receiveMessage
(
mavlink_message_t
messag
e
)
{
{
this
->
name
=
name
;
emit
nameChanged
(
this
->
name
);
}
bool
OpalLink
::
isConnected
()
{
// Create buffer
//qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
char
buffer
[
MAVLINK_MAX_PACKET_LEN
];
return
connectState
;
// Write message into buffer, prepending start sign
int
len
=
mavlink_msg_to_send_buffer
((
uint8_t
*
)(
buffer
),
&
message
);
// If link is connected
if
(
isConnected
())
{
receiveBuffer
->
enqueue
(
QByteArray
(
buffer
,
len
));
emit
bytesReady
(
this
);
}
}
}
qint64
OpalLink
::
getNominalDataRate
()
void
OpalLink
::
heartbeat
()
{
{
return
0
;
//unknown
if
(
m_heartbeatsEnabled
)
{
qDebug
()
<<
"OpalLink::heartbeat(): Generate a heartbeat"
;
mavlink_message_t
beat
;
mavlink_msg_heartbeat_pack
(
systemID
,
componentID
,
&
beat
,
MAV_HELICOPTER
,
MAV_AUTOPILOT_GENERIC
);
receiveMessage
(
beat
);
}
}
}
int
OpalLink
::
getLinkQuality
()
void
OpalLink
::
getSignals
()
{
{
return
-
1
;
//not supported
qDebug
()
<<
"OpalLink::getSignals(): Attempting to acquire signals"
;
unsigned
long
timeout
=
0
;
unsigned
short
acqGroup
=
0
;
//this is actually group 1 in the model
unsigned
short
allocatedSignals
=
NUM_OUTPUT_SIGNALS
;
unsigned
short
*
numSignals
=
new
unsigned
short
(
0
);
double
*
timestep
=
new
double
(
0
);
double
values
[
NUM_OUTPUT_SIGNALS
]
=
{};
unsigned
short
*
lastValues
=
new
unsigned
short
(
false
);
unsigned
short
*
decimation
=
new
unsigned
short
(
0
);
int
returnVal
=
OpalGetSignals
(
timeout
,
acqGroup
,
allocatedSignals
,
numSignals
,
timestep
,
values
,
lastValues
,
decimation
);
if
(
returnVal
==
EOK
)
{
qDebug
()
<<
"OpalLink::getSignals: Timestep="
<<
*
timestep
;
// << ", Last? " << (bool)(*lastValues);
}
else
if
(
returnVal
==
EAGAIN
)
{
qDebug
()
<<
"OpalLink::getSignals: Data was not ready"
;
}
// if returnVal == EAGAIN => data just wasn't ready
else
if
(
returnVal
!=
EAGAIN
)
{
getSignalsTimer
->
stop
();
displayErrorMsg
();
}
/* deallocate used memory */
delete
timestep
;
delete
lastValues
;
delete
lastValues
;
delete
decimation
;
}
}
qint64
OpalLink
::
getTotalUpstream
()
/*
*
Administrative
*
*/
void
OpalLink
::
run
()
{
{
statisticsMutex
.
lock
();
qDebug
()
<<
"OpalLink::run():: Starting the thread"
;
qint64
totalUpstream
=
bitsSentTotal
/
((
MG
::
TIME
::
getGroundTimeNow
()
-
connectionStartTime
)
/
1000
);
statisticsMutex
.
unlock
();
return
totalUpstream
;
}
}
qint64
OpalLink
::
getTotalDownstream
()
{
int
OpalLink
::
getId
()
statisticsMutex
.
lock
();
{
qint64
totalDownstream
=
bitsReceivedTotal
/
((
MG
::
TIME
::
getGroundTimeNow
()
-
connectionStartTime
)
/
1000
);
return
id
;
statisticsMutex
.
unlock
();
return
totalDownstream
;
}
}
qint64
OpalLink
::
getCurrentUpstream
()
QString
OpalLink
::
getName
()
{
{
return
0
;
//unknown
return
name
;
}
}
qint64
OpalLink
::
getMaxUpstream
(
)
void
OpalLink
::
setName
(
QString
name
)
{
{
return
0
;
//unknown
this
->
name
=
name
;
emit
nameChanged
(
this
->
name
);
}
}
qint64
OpalLink
::
getBitsSent
()
{
bool
OpalLink
::
isConnected
()
{
return
bitsSentTotal
;
//qDebug() << "OpalLink::isConnected:: connectState: " << connectState;
return
connectState
;
}
}
qint64
OpalLink
::
getBitsReceived
()
{
return
bitsReceivedTotal
;
}
bool
OpalLink
::
isFullDuplex
()
{
return
false
;
}
bool
OpalLink
::
connect
()
bool
OpalLink
::
connect
()
{
{
...
@@ -105,22 +175,17 @@ bool OpalLink::connect()
...
@@ -105,22 +175,17 @@ bool OpalLink::connect()
if
(
OpalConnect
(
101
,
false
,
&
modelState
)
==
EOK
)
if
(
OpalConnect
(
101
,
false
,
&
modelState
)
==
EOK
)
{
{
connectState
=
true
;
connectState
=
true
;
emit
connected
();
heartbeatTimer
->
start
(
1000
/
heartbeatRate
);
getSignalsTimer
->
start
(
getSignalsPeriod
);
}
}
else
else
{
{
connectState
=
false
;
connectState
=
false
;
setLastErrorMsg
();
displayErrorMsg
();
QMessageBox
msgBox
;
msgBox
.
setIcon
(
QMessageBox
::
Critical
);
msgBox
.
setText
(
lastErrorMsg
);
msgBox
.
exec
();
}
}
emit
connected
(
connectState
);
emit
connected
(
connectState
);
if
(
connectState
)
{
emit
connected
();
}
return
connectState
;
return
connectState
;
}
}
...
@@ -129,6 +194,15 @@ bool OpalLink::disconnect()
...
@@ -129,6 +194,15 @@ bool OpalLink::disconnect()
return
false
;
return
false
;
}
}
void
OpalLink
::
displayErrorMsg
()
{
setLastErrorMsg
();
QMessageBox
msgBox
;
msgBox
.
setIcon
(
QMessageBox
::
Critical
);
msgBox
.
setText
(
lastErrorMsg
);
msgBox
.
exec
();
}
void
OpalLink
::
setLastErrorMsg
()
void
OpalLink
::
setLastErrorMsg
()
{
{
char
buf
[
512
];
char
buf
[
512
];
...
@@ -138,64 +212,58 @@ void OpalLink::setLastErrorMsg()
...
@@ -138,64 +212,58 @@ void OpalLink::setLastErrorMsg()
lastErrorMsg
.
append
(
buf
);
lastErrorMsg
.
append
(
buf
);
}
}
qint64
OpalLink
::
bytesAvailable
()
/*
*
Statisctics
*
*/
qint64
OpalLink
::
getNominalDataRate
()
{
{
return
0
;
return
0
;
//unknown
}
}
void
OpalLink
::
writeBytes
(
const
char
*
bytes
,
qint64
length
)
int
OpalLink
::
getLinkQuality
(
)
{
{
return
-
1
;
//not supported
}
}
qint64
OpalLink
::
getTotalUpstream
()
void
OpalLink
::
readBytes
(
char
*
bytes
,
qint64
maxLength
)
{
{
statisticsMutex
.
lock
();
qint64
totalUpstream
=
bitsSentTotal
/
((
MG
::
TIME
::
getGroundTimeNow
()
-
connectionStartTime
)
/
1000
);
statisticsMutex
.
unlock
();
return
totalUpstream
;
}
receiveDataMutex
.
lock
();
qint64
OpalLink
::
getTotalDownstream
()
{
qDebug
()
<<
"OpalLink::readBytes(): Reading a message. size of buffer: "
<<
receiveBuffer
->
count
();
statisticsMutex
.
lock
();
QByteArray
message
=
receiveBuffer
->
dequeue
();
qint64
totalDownstream
=
bitsReceivedTotal
/
((
MG
::
TIME
::
getGroundTimeNow
()
-
connectionStartTime
)
/
1000
);
if
(
maxLength
<
message
.
size
())
statisticsMutex
.
unlock
();
{
return
totalDownstream
;
qDebug
()
<<
"OpalLink::readBytes:: Buffer Overflow"
;
memcpy
(
bytes
,
message
.
data
(),
maxLength
);
}
else
{
memcpy
(
bytes
,
message
.
data
(),
message
.
size
());
}
emit
bytesReceived
(
this
,
message
);
receiveDataMutex
.
unlock
();
}
}
void
OpalLink
::
heartbeat
()
qint64
OpalLink
::
getCurrentUpstream
()
{
{
return
0
;
//unknown
}
if
(
m_heartbeatsEnabled
)
qint64
OpalLink
::
getMaxUpstream
()
{
{
qDebug
()
<<
"OpalLink::heartbeat(): Generate a heartbeat"
;
return
0
;
//unknown
mavlink_message_t
beat
;
}
mavlink_msg_heartbeat_pack
(
systemID
,
componentID
,
&
beat
,
MAV_HELICOPTER
,
MAV_AUTOPILOT_GENERIC
);
receiveMessage
(
beat
);
}
qint64
OpalLink
::
getBitsSent
()
{
return
bitsSentTotal
;
}
}
void
OpalLink
::
receiveMessage
(
mavlink_message_t
message
)
qint64
OpalLink
::
getBitsReceived
()
{
{
return
bitsReceivedTotal
;
}
// Create buffer
char
buffer
[
MAVLINK_MAX_PACKET_LEN
];
// Write message into buffer, prepending start sign
int
len
=
mavlink_msg_to_send_buffer
((
uint8_t
*
)(
buffer
),
&
message
);
// If link is connected
if
(
isConnected
())
{
receiveBuffer
->
enqueue
(
QByteArray
(
buffer
,
len
));
emit
bytesReady
(
this
);
}
bool
OpalLink
::
isFullDuplex
()
{
return
false
;
}
}
src/comm/OpalLink.h
View file @
589fe3af
...
@@ -26,6 +26,12 @@
...
@@ -26,6 +26,12 @@
#include "OpalApi.h"
#include "OpalApi.h"
#include "string.h"
#include "string.h"
/*
Configuration info for the model
*/
#define NUM_OUTPUT_SIGNALS 6
class
OpalLink
:
public
LinkInterface
class
OpalLink
:
public
LinkInterface
{
{
Q_OBJECT
Q_OBJECT
...
@@ -72,6 +78,8 @@ public slots:
...
@@ -72,6 +78,8 @@ public slots:
void
heartbeat
();
void
heartbeat
();
void
getSignals
();
protected
slots
:
protected
slots
:
void
receiveMessage
(
mavlink_message_t
message
);
void
receiveMessage
(
mavlink_message_t
message
);
...
@@ -95,12 +103,17 @@ protected:
...
@@ -95,12 +103,17 @@ protected:
QMutex
receiveDataMutex
;
QMutex
receiveDataMutex
;
QString
lastErrorMsg
;
QString
lastErrorMsg
;
void
setLastErrorMsg
();
void
setLastErrorMsg
();
void
displayErrorMsg
();
void
setName
(
QString
name
);
void
setName
(
QString
name
);
QTimer
*
heartbeatTimer
;
///< Timer to emit heartbeats
QTimer
*
heartbeatTimer
;
///< Timer to emit heartbeats
int
heartbeatRate
;
///< Heartbeat rate, controls the timer interval
int
heartbeatRate
;
///< Heartbeat rate, controls the timer interval
bool
m_heartbeatsEnabled
;
///< Enabled/disable heartbeat emission
bool
m_heartbeatsEnabled
;
///< Enabled/disable heartbeat emission
QTimer
*
getSignalsTimer
;
int
getSignalsPeriod
;
QQueue
<
QByteArray
>*
receiveBuffer
;
QQueue
<
QByteArray
>*
receiveBuffer
;
QByteArray
*
sendBuffer
;
QByteArray
*
sendBuffer
;
...
...
src/comm/UDPLink.cc
View file @
589fe3af
...
@@ -244,7 +244,6 @@ void UDPLink::setName(QString name)
...
@@ -244,7 +244,6 @@ void UDPLink::setName(QString name)
emit
nameChanged
(
this
->
name
);
emit
nameChanged
(
this
->
name
);
}
}
qint64
UDPLink
::
getNominalDataRate
()
{
qint64
UDPLink
::
getNominalDataRate
()
{
return
54000000
;
// 54 Mbit
return
54000000
;
// 54 Mbit
}
}
...
...
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