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
ce6fb7b7
Commit
ce6fb7b7
authored
Aug 10, 2014
by
Lorenz Meier
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Updated MAVLink to most recent revision, implemented all changed functionality equivalent
parent
01776370
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
with
33 additions
and
89 deletions
+33
-89
GLC_lib
libs/GLC_lib
+0
-1
MAVLinkSimulationLink.cc
src/comm/MAVLinkSimulationLink.cc
+0
-24
MAVLinkSimulationMAV.cc
src/comm/MAVLinkSimulationMAV.cc
+0
-23
MAVLinkSimulationWaypointPlanner.cc
src/comm/MAVLinkSimulationWaypointPlanner.cc
+1
-27
UAS.cc
src/uas/UAS.cc
+32
-14
No files found.
GLC_lib
@
ef1adb08
Subproject commit ef1adb0843a9f8073bce9e641b2a0d9bf002ea39
src/comm/MAVLinkSimulationLink.cc
View file @
ce6fb7b7
...
...
@@ -394,13 +394,7 @@ void MAVLinkSimulationLink::mainloop()
// y = (y < -5.0f) ? -5.0f : y;
// z = (z < -3.0f) ? -3.0f : z;
// Send back new setpoint
mavlink_message_t
ret
;
mavlink_msg_local_position_setpoint_pack
(
systemId
,
componentId
,
&
ret
,
MAV_FRAME_LOCAL_NED
,
spX
,
spY
,
spZ
,
spYaw
);
// spYaw/180.0*M_PI);
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
ret
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
// Send back new position
mavlink_msg_local_position_ned_pack
(
systemId
,
componentId
,
&
ret
,
0
,
x
,
y
,
-
fabs
(
z
),
xSpeed
,
ySpeed
,
zSpeed
);
...
...
@@ -702,24 +696,6 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
system
.
base_mode
=
mode
.
base_mode
;
}
break
;
case
MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT
:
{
mavlink_set_local_position_setpoint_t
set
;
mavlink_msg_set_local_position_setpoint_decode
(
&
msg
,
&
set
);
spX
=
set
.
x
;
spY
=
set
.
y
;
spZ
=
set
.
z
;
spYaw
=
set
.
yaw
;
// Send back new setpoint
mavlink_message_t
ret
;
mavlink_msg_local_position_setpoint_pack
(
systemId
,
componentId
,
&
ret
,
MAV_FRAME_LOCAL_NED
,
spX
,
spY
,
spZ
,
spYaw
);
bufferlength
=
mavlink_msg_to_send_buffer
(
buffer
,
&
msg
);
//add data into datastream
memcpy
(
stream
+
streampointer
,
buffer
,
bufferlength
);
streampointer
+=
bufferlength
;
}
break
;
// EXECUTE OPERATOR ACTIONS
case
MAVLINK_MSG_ID_COMMAND_LONG
:
{
...
...
src/comm/MAVLinkSimulationMAV.cc
View file @
ce6fb7b7
...
...
@@ -518,28 +518,5 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
// }
// }
break
;
case
MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT
:
{
mavlink_set_local_position_setpoint_t
sp
;
mavlink_msg_set_local_position_setpoint_decode
(
&
msg
,
&
sp
);
if
(
sp
.
target_system
==
this
->
systemid
)
{
nav_mode
=
0
;
previousSPX
=
nextSPX
;
previousSPY
=
nextSPY
;
previousSPZ
=
nextSPZ
;
nextSPX
=
sp
.
x
;
nextSPY
=
sp
.
y
;
nextSPZ
=
sp
.
z
;
// Rotary wing
//nextSPYaw = sp.yaw;
// Airplane
//yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
//if (!firstWP) firstWP = true;
}
//qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
}
break
;
}
}
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @
ce6fb7b7
...
...
@@ -522,33 +522,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq)
*/
void
MAVLinkSimulationWaypointPlanner
::
send_setpoint
(
uint16_t
seq
)
{
if
(
seq
<
waypoints
->
size
())
{
mavlink_mission_item_t
*
cur
=
waypoints
->
at
(
seq
);
// send new set point to local IMU
if
(
cur
->
frame
==
MAV_FRAME_LOCAL_NED
||
cur
->
frame
==
MAV_FRAME_LOCAL_ENU
)
{
mavlink_message_t
msg
;
mavlink_set_local_position_setpoint_t
PControlSetPoint
;
PControlSetPoint
.
target_system
=
systemid
;
PControlSetPoint
.
target_component
=
MAV_COMP_ID_IMU
;
PControlSetPoint
.
x
=
cur
->
x
;
PControlSetPoint
.
y
=
cur
->
y
;
PControlSetPoint
.
z
=
cur
->
z
;
PControlSetPoint
.
yaw
=
cur
->
param4
;
PControlSetPoint
.
coordinate_frame
=
cur
->
frame
;
mavlink_msg_set_local_position_setpoint_encode
(
systemid
,
compid
,
&
msg
,
&
PControlSetPoint
);
link
->
sendMAVLinkMessage
(
&
msg
);
emit
messageSent
(
msg
);
uint64_t
now
=
QGC
::
groundTimeMilliseconds
();
timestamp_last_send_setpoint
=
now
;
}
else
if
(
verbose
)
{
qDebug
(
"No new set point sent to IMU because the new waypoint %u had no local coordinates
\n
"
,
cur
->
seq
);
}
}
Q_UNUSED
(
seq
);
}
void
MAVLinkSimulationWaypointPlanner
::
send_waypoint_count
(
uint8_t
target_systemid
,
uint8_t
target_compid
,
uint16_t
count
)
...
...
src/uas/UAS.cc
View file @
ce6fb7b7
...
...
@@ -1123,12 +1123,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break
;
}
}
case
MAVLINK_MSG_ID_
ROLL_PITCH_YAW_THRUST_SETPOIN
T
:
case
MAVLINK_MSG_ID_
ATTITUDE_TARGE
T
:
{
mavlink_roll_pitch_yaw_thrust_setpoint_t
out
;
mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode
(
&
message
,
&
out
);
mavlink_attitude_target_t
out
;
mavlink_msg_attitude_target_decode
(
&
message
,
&
out
);
float
roll
,
pitch
,
yaw
;
mavlink_quaternion_to_euler
(
out
.
q
,
&
roll
,
&
pitch
,
&
yaw
);
quint64
time
=
getUnixTimeFromMs
(
out
.
time_boot_ms
);
emit
attitudeThrustSetPointChanged
(
this
,
out
.
roll
,
out
.
pitch
,
out
.
yaw
,
out
.
thrust
,
time
);
emit
attitudeThrustSetPointChanged
(
this
,
roll
,
pitch
,
yaw
,
out
.
thrust
,
time
);
}
break
;
case
MAVLINK_MSG_ID_MISSION_COUNT
:
...
...
@@ -1274,22 +1276,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
break
;
case
MAVLINK_MSG_ID_
LOCAL_POSITION_SETPOINT
:
case
MAVLINK_MSG_ID_
POSITION_TARGET_LOCAL_NED
:
{
if
(
multiComponentSourceDetected
&&
wrongComponent
)
{
break
;
}
mavlink_local_position_setpoint_t
p
;
mavlink_msg_local_position_setpoint_decode
(
&
message
,
&
p
);
emit
positionSetPointsChanged
(
uasId
,
p
.
x
,
p
.
y
,
p
.
z
,
p
.
yaw
,
QGC
::
groundTimeUsecs
());
mavlink_position_target_local_ned_t
p
;
mavlink_msg_position_target_local_ned_decode
(
&
message
,
&
p
);
quint64
time
=
getUnixTimeFromMs
(
p
.
time_boot_ms
);
emit
positionSetPointsChanged
(
uasId
,
p
.
x
,
p
.
y
,
p
.
z
,
0
/* XXX remove yaw and move it to attitude */
,
time
);
}
break
;
case
MAVLINK_MSG_ID_SET_
LOCAL_POSITION_SETPOINT
:
case
MAVLINK_MSG_ID_SET_
POSITION_TARGET_LOCAL_NED
:
{
mavlink_set_
local_position_setpoint
_t
p
;
mavlink_msg_set_
local_position_setpoint
_decode
(
&
message
,
&
p
);
emit
userPositionSetPointsChanged
(
uasId
,
p
.
x
,
p
.
y
,
p
.
z
,
p
.
yaw
);
mavlink_set_
position_target_local_ned
_t
p
;
mavlink_msg_set_
position_target_local_ned
_decode
(
&
message
,
&
p
);
emit
userPositionSetPointsChanged
(
uasId
,
p
.
x
,
p
.
y
,
p
.
z
,
0
/* XXX remove yaw and move it to attitude */
);
}
break
;
case
MAVLINK_MSG_ID_STATUSTEXT
:
...
...
@@ -2905,9 +2908,24 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
if
(((
base_mode
&
MAV_MODE_FLAG_DECODE_POSITION_MANUAL
)
&&
(
base_mode
&
MAV_MODE_FLAG_DECODE_POSITION_SAFETY
))
||
(
base_mode
&
MAV_MODE_FLAG_HIL_ENABLED
))
{
mavlink_message_t
message
;
mavlink_msg_setpoint_6dof_pack
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
message
,
this
->
uasId
,
(
float
)
x
,
(
float
)
y
,
(
float
)
z
,
(
float
)
roll
,
(
float
)
pitch
,
(
float
)
yaw
);
float
q
[
4
];
mavlink_euler_to_quaternion
(
roll
,
pitch
,
yaw
,
q
);
quint8
mask
;
// Do not control rates and throttle
mask
|=
(
1
<<
0
)
|
(
1
<<
1
)
|
(
1
<<
2
);
// ignore rates
mask
|=
(
1
<<
6
);
// ignore throttle
mavlink_msg_set_attitude_target_pack
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
message
,
QGC
::
groundTimeMilliseconds
(),
this
->
uasId
,
0
,
mask
,
q
,
0
,
0
,
0
,
0
);
sendMessage
(
message
);
quint8
position_mask
;
position_mask
|=
(
1
<<
3
)
|
(
1
<<
4
)
|
(
1
<<
5
)
|
(
1
<<
6
)
|
(
1
<<
7
)
|
(
1
<<
8
);
mavlink_msg_set_position_target_local_ned_pack
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
message
,
QGC
::
groundTimeMilliseconds
(),
this
->
uasId
,
0
,
MAV_FRAME_LOCAL_NED
,
position_mask
,
x
,
y
,
z
,
0
,
0
,
0
,
0
,
0
,
0
);
sendMessage
(
message
);
qDebug
()
<<
__FILE__
<<
__LINE__
<<
": SENT 6DOF CONTROL MESSAGE: x"
<<
x
<<
" y: "
<<
y
<<
" z: "
<<
z
<<
" roll: "
<<
roll
<<
" pitch: "
<<
pitch
<<
" yaw: "
<<
yaw
;
qDebug
()
<<
__FILE__
<<
__LINE__
<<
": SENT 6DOF CONTROL MESSAGE
S
: x"
<<
x
<<
" y: "
<<
y
<<
" z: "
<<
z
<<
" roll: "
<<
roll
<<
" pitch: "
<<
pitch
<<
" yaw: "
<<
yaw
;
//emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
}
...
...
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