Ajout du message ESTIMATOR_STATUS

Com_server/test/src/Com_SerialReadingThread.cpp

@@ -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:
 		{