Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
e784c17e
Commit
e784c17e
authored
Apr 16, 2013
by
Lorenz Meier
Browse files
Ask mavlink app to start on PX4 boards if connected via NSH
parent
16558d70
Changes
4
Hide whitespace changes
Inline
Side-by-side
src/comm/LinkManager.cc
View file @
e784c17e
...
...
@@ -94,6 +94,8 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
{
// Protocol is new, add
connect
(
link
,
SIGNAL
(
bytesReceived
(
LinkInterface
*
,
QByteArray
)),
protocol
,
SLOT
(
receiveBytes
(
LinkInterface
*
,
QByteArray
)));
// Add status
connect
(
link
,
SIGNAL
(
connected
(
bool
)),
protocol
,
SLOT
(
linkStatusChanged
(
bool
)));
// Store the connection information in the protocol links map
protocolLinks
.
insertMulti
(
protocol
,
link
);
}
...
...
src/comm/MAVLinkProtocol.cc
View file @
e784c17e
...
...
@@ -174,6 +174,33 @@ QString MAVLinkProtocol::getLogfileName()
}
}
void
MAVLinkProtocol
::
linkStatusChanged
(
bool
connected
)
{
LinkInterface
*
link
=
qobject_cast
<
LinkInterface
*>
(
QObject
::
sender
());
if
(
link
)
{
if
(
connected
)
{
// Send command to start MAVLink
// XXX hacky but safe
// Start NSH
const
char
init
[]
=
{
0x0d
,
0x0d
,
0x0d
};
link
->
writeBytes
(
init
,
1
);
QGC
::
SLEEP
::
msleep
(
500
);
// Stop any running mavlink instance
char
*
cmd
=
"mavlink stop
\n
"
;
link
->
writeBytes
(
cmd
,
strlen
(
cmd
));
link
->
writeBytes
(
init
,
2
);
cmd
=
"uorb start"
;
link
->
writeBytes
(
cmd
,
strlen
(
cmd
));
link
->
writeBytes
(
init
,
2
);
cmd
=
"sh /etc/init.d/rc.usb
\n
"
;
link
->
writeBytes
(
cmd
,
strlen
(
cmd
));
link
->
writeBytes
(
init
,
4
);
}
}
}
/**
* The bytes are copied by calling the LinkInterface::readBytes() method.
* This method parses all incoming bytes and constructs a MAVLink packet.
...
...
src/comm/MAVLinkProtocol.h
View file @
e784c17e
...
...
@@ -128,6 +128,7 @@ public:
public
slots
:
/** @brief Receive bytes from a communication interface */
void
receiveBytes
(
LinkInterface
*
link
,
QByteArray
b
);
void
linkStatusChanged
(
bool
connected
);
/** @brief Send MAVLink message through serial interface */
void
sendMessage
(
mavlink_message_t
message
);
/** @brief Send MAVLink message */
...
...
src/comm/ProtocolInterface.h
View file @
e784c17e
...
...
@@ -55,6 +55,7 @@ public:
public
slots
:
virtual
void
receiveBytes
(
LinkInterface
*
link
,
QByteArray
b
)
=
0
;
virtual
void
linkStatusChanged
(
bool
connected
)
=
0
;
signals:
/** @brief Update the packet loss from one system */
...
...
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