Commit da75dbc6 authored by Léo Coquet's avatar Léo Coquet
Browse files

Ajout du message ESTIMATOR_STATUS

parent dc396d86
Pipeline #1599 failed with stage
in 1 minute and 37 seconds
......@@ -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:
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment