Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Philippe Gaussier
Drone FlyMonitor2
Commits
da75dbc6
Commit
da75dbc6
authored
Oct 18, 2021
by
Léo Coquet
Browse files
Ajout du message ESTIMATOR_STATUS
parent
dc396d86
Pipeline
#1599
failed with stage
in 1 minute and 37 seconds
Changes
1
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Com_server/test/src/Com_SerialReadingThread.cpp
View file @
da75dbc6
...
...
@@ -31,6 +31,10 @@ extern ofstream compassy;
double
t_old
=
-
1.
;
long
pcktCount
=
0
;
int
pcktLostCount
=
0
;
int
lastPckt
=
0
;
Serial_Port_ReadingThread
::
Serial_Port_ReadingThread
(
int
task_period
,
int
task_deadline
)
:
Abstract_ThreadClass
(
task_period
,
task_deadline
)
{
...
...
@@ -294,6 +298,7 @@ MAVLINK_MSG_ID_GOPRO_POWER_OFF 216
MAVLINK_MSG_ID_GOPRO_COMMAND 217
MAVLINK_MSG_ID_GOPRO_RESPONSE 218
MAVLINK_MSG_ID_RPM 226
MAVLINK_MESSAGE_ID_ESTIMATOR_STATUS 230
MAVLINK_MSG_ID_VIBRATION 241
MAVLINK_MSG_ID_HOME_POSITION 242
MAVLINK_MSG_ID_SET_HOME_POSITION 243
...
...
@@ -317,7 +322,6 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
struct
timeval
t
;
double
t_current
,
dt
;
std
::
string
buffer1
=
""
;
// Handle current timestamp
drone1
.
timestamps
=
get_time_usec
();
...
...
@@ -338,6 +342,26 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
}
}
if
(
message
.
seq
!=
lastPckt
+
1
)
{
int
lost
=
0
;
if
(
message
.
seq
>
lastPckt
)
lost
=
message
.
seq
-
lastPckt
-
1
;
else
lost
=
255
-
lastPckt
+
message
.
seq
;
if
(
lost
)
{
pcktLostCount
+=
lost
;
double
percent
=
(
double
)
pcktLostCount
/
(
pcktLostCount
+
pcktCount
);
percent
*=
100.0
;
printf
(
"%d lost packet(s). %f percents packets lost.
\n
"
,
pcktLostCount
,
percent
);
}
}
pcktCount
++
;
//Test printf
//printf("seq : %u pcktLostCount : %d pcktCount : %ld lastPckt : %d\n", message.seq, pcktLostCount, pcktCount, lastPckt);
lastPckt
=
message
.
seq
;
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_HEARTBEAT
:
...
...
@@ -644,7 +668,22 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
{
printf
(
"MAVLINK_MSG_ID_PING ???
\n
"
);
break
;
}
}
case
MAVLINK_MSG_ID_ESTIMATOR_STATUS
:
{
mavlink_msg_estimator_status_decode
(
&
message
,
&
drone1
.
estimator_status
);
printf
(
"ESTIMATOR_STATUS_FLAG : %u
\n
"
,
drone1
.
estimator_status
.
flags
);
break
;
}
case
MAVLINK_MSG_ID_PARAM_VALUE
:
{
mavlink_msg_param_value_decode
(
&
message
,
&
drone1
.
param_value
);
printf
(
"ID_PARAM_VALUE ???
\n
"
);
break
;
}
case
MAVLINK_MSG_ID_COMMAND_LONG
:
{
...
...
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