Add debug info to find where is the bottleneck...

Look like the problem is the mutex_lock for _write_port

PG.

Com_server/include/Thread_SerialPort.h

@@ -25,6 +25,8 @@
 #include "../include/Com_Mavlink.h"
 
 #include "../include/Data_Bus.h"
+#include "Data_Drone.h"
+
 
 class Thread_SerialPort : public Abstract_ThreadClass 
 {

Com_server/src/Thread_SerialPort.cpp

@@ -10,6 +10,7 @@
 using namespace std;
 
 #include "../include/Thread_SerialPort.h"
+#include "../include/global_variables.h"
 
 //#define DISPLAY
 
@@ -163,6 +164,7 @@ void Thread_SerialPort::write_messages()
 
 void Thread_SerialPort::read_messages()
 {
+    string buffer1="";
     Bus::getInstanceOf().event_newMessage(message);
  //   cout<< "LOG "<<message.msgid<<"\n";
 
@@ -179,31 +181,29 @@ void Thread_SerialPort::read_messages()
         {
             mavlink_msg_sys_status_decode(&message, &sys_status);
             Bus::getInstanceOf().update(sys_status);
-            /*
-            buffer1 = "Error: "+std::to_string(drone1.sys_status.errors_count1)+" "+std::to_string(drone1.sys_status.errors_count2)+" "+std::to_string(drone1.sys_status.errors_count3)+" "+std::to_string(drone1.sys_status.errors_count4);
+            
+            buffer1 = "Error: "+std::to_string(sys_status.errors_count1)+" "+std::to_string(sys_status.errors_count2)+" "+std::to_string(sys_status.errors_count3)+" "+std::to_string(sys_status.errors_count4);
             //Display_IHM::getInstanceOf().printData(buffer1, 9, 1);
-            buffer1 = "Voltage: "+std::to_string(drone1.sys_status.voltage_battery);
+            buffer1 = "Voltage: "+std::to_string(sys_status.voltage_battery);
             //Display_IHM::getInstanceOf().printData(buffer1, 10, 1);*/
-            
             break;
         }
 
         //ACCELEROMETERS
-        /*case MAVLINK_MSG_ID_HIGHRES_IMU:
+        case MAVLINK_MSG_ID_HIGHRES_IMU:
         {
             mavlink_msg_highres_imu_decode(&message, &highres_imu);
-            Bus::getInstanceOf().update(highres_imu);
+            //mavlink_msg_highres_imu_decode(&message, &drone1.highres_imu);
+         //   Bus::getInstanceOf().update(highres_imu);
 
-            /*buffer1 = "Acc: "+std::to_string(highres_imu.xacc)+" "+std::to_string(highres_imu.yacc)+" "+std::to_string(highres_imu.zacc);
+            buffer1 = "Acc: "+std::to_string(highres_imu.xacc)+" "+std::to_string(highres_imu.yacc)+" "+std::to_string(highres_imu.zacc);
             //Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
             buffer1 = "Gyro: "+std::to_string(highres_imu.xgyro)+" "+std::to_string(highres_imu.ygyro)+" "+std::to_string(highres_imu.zgyro);
             //Display_IHM::getInstanceOf().printData(buffer1, 12, 1);
             buffer1 = "Mag: "+std::to_string(highres_imu.xmag)+" "+std::to_string(highres_imu.ymag)+" "+std::to_string(highres_imu.zmag);
             //Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
-
-            
             break;
-        }*/
+        }
 
         //ACK COMMAND
         /*case MAVLINK_MSG_ID_COMMAND_ACK:
@@ -225,12 +225,12 @@ void Thread_SerialPort::read_messages()
         {
             mavlink_msg_radio_status_decode(&message, &drone1.radio_status);
             break;
-        }
+        }*/
 
 
 
         //GPS
-        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
+     /*   case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
         {
             mavlink_msg_local_position_ned_decode(&message, &drone1.local_position_ned);
 
@@ -244,9 +244,9 @@ void Thread_SerialPort::read_messages()
         {
             mavlink_msg_position_target_local_ned_decode(&message, &drone1.position_target_local_ned);
 
-            // buffer1 = "Pos_coord_t: "+std::to_string(drone1.position_target_local_ned.x)+" "+std::to_string(drone1.position_target_local_ned.y)+" "+std::to_string(drone1.position_target_local_ned.z);
+            buffer1 = "Pos_coord_t: "+std::to_string(drone1.position_target_local_ned.x)+" "+std::to_string(drone1.position_target_local_ned.y)+" "+std::to_string(drone1.position_target_local_ned.z);
             // Display_IHM::getInstanceOf().printData(buffer1, 10, 1);
-            // buffer1 = "Pos_speed_t: "+std::to_string(drone1.position_target_local_ned.vx)+" "+std::to_string(drone1.position_target_local_ned.vy)+" "+std::to_string(drone1.position_target_local_ned.vz);
+            buffer1 = "Pos_speed_t: "+std::to_string(drone1.position_target_local_ned.vx)+" "+std::to_string(drone1.position_target_local_ned.vy)+" "+std::to_string(drone1.position_target_local_ned.vz);
             // Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
             break;
         }*/
@@ -273,12 +273,13 @@ void Thread_SerialPort::read_messages()
             break;
         }*/
 
+        /*
         case MAVLINK_MSG_ID_GPS_RAW_INT:
         {
             mavlink_msg_gps_raw_int_decode(&message, &gps_raw_int);
             Bus::getInstanceOf().update(gps_raw_int);
             break;
-        }
+        }*/
 
         //Attitude function
         case MAVLINK_MSG_ID_ATTITUDE:
@@ -287,16 +288,16 @@ void Thread_SerialPort::read_messages()
             mavlink_msg_attitude_decode(&message, &attitude);
             Bus::getInstanceOf().update(attitude);
             
-            // buffer1 = "Attitude_t: "+std::to_string(drone1.attitude.time_boot_ms);
+            buffer1 = "Attitude_t: "+std::to_string(attitude.time_boot_ms);
             // Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
-            // buffer1 = "Attitude_p: "+std::to_string(drone1.attitude.roll)+" "+std::to_string(drone1.attitude.pitch)+" "+std::to_string(drone1.attitude.yaw);
+             buffer1 = "Attitude_p: "+std::to_string(attitude.roll)+" "+std::to_string(attitude.pitch)+" "+std::to_string(attitude.yaw);
             // Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
-            // buffer1 = "Attitude_s: "+std::to_string(drone1.attitude.rollspeed)+" "+std::to_string(drone1.attitude.pitchspeed)+" "+std::to_string(drone1.attitude.yawspeed);
+             buffer1 = "Attitude_s: "+std::to_string(attitude.rollspeed)+" "+std::to_string(attitude.pitchspeed)+" "+std::to_string(attitude.yawspeed);
             // Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
             
             break;
         }
-
+        /*
         case MAVLINK_MSG_ID_VFR_HUD:{
             mavlink_msg_vfr_hud_decode(&message, &vfr_hud);
             Bus::getInstanceOf().update(vfr_hud);
@@ -314,28 +315,29 @@ void Thread_SerialPort::read_messages()
             Bus::getInstanceOf().update(manual_control);
             break;
         }
-        
+        */
         /*case MAVLINK_MSG_ID_ATTITUDE_TARGET:
         {
             mavlink_msg_attitude_target_decode(&message, &drone1.attitude_target);
 
-            // buffer1 = "Attitude_target: "+std::to_string(drone1.attitude_target.time_boot_ms);
-            // Display_IHM::getInstanceOf().printData(buffer1, 16, 1);
-            // buffer1 = "Attitude_target_q: "+std::to_string(drone1.attitude_target.q[0])+" "+std::to_string(drone1.attitude_target.q[1])+" "+std::to_string(drone1.attitude_target.q[2])+" "+std::to_string(drone1.attitude_target.q[3]);
+             buffer1 = "Attitude_target: "+std::to_string(drone1.attitude_target.time_boot_ms);
+            //Display_IHM::getInstanceOf().printData(buffer1, 16, 1);
+             buffer1 = "Attitude_target_q: "+std::to_string(drone1.attitude_target.q[0])+" "+std::to_string(drone1.attitude_target.q[1])+" "+std::to_string(drone1.attitude_target.q[2])+" "+std::to_string(drone1.attitude_target.q[3]);
             // Display_IHM::getInstanceOf().printData(buffer1, 17, 1);
-            // buffer1 = "Attitude_tar_r: "+std::to_string(drone1.attitude_target.body_roll_rate)+" "+std::to_string(drone1.attitude_target.body_pitch_rate)+" "+std::to_string(drone1.attitude_target.body_yaw_rate);
+             buffer1 = "Attitude_tar_r: "+std::to_string(drone1.attitude_target.body_roll_rate)+" "+std::to_string(drone1.attitude_target.body_pitch_rate)+" "+std::to_string(drone1.attitude_target.body_yaw_rate);
             // Display_IHM::getInstanceOf().printData(buffer1, 18, 1);
-            // buffer1 = "Attitude_tar_m: "+std::to_string(drone1.attitude_target.type_mask);
+             buffer1 = "Attitude_tar_m: "+std::to_string(drone1.attitude_target.type_mask);
             // Display_IHM::getInstanceOf().printData(buffer1, 19, 1);
             break;
         }*/
 
         default:
         {
-            //printf("Warning, did not handle message id %i\n",message.msgid);
+            printf("Warning, did not handle message id %i\n",message.msgid);
             break;
         }
     }
+    cout<<"Thread_SerialPort "<<buffer1<<endl;
 
 } 
 

Com_server/test/src/Com_Serial.cpp

@@ -7,6 +7,12 @@
  * @version 1.1 
  */
 
+
+#include <iostream>
+#include <string>
+
+using namespace std;
+
 #include "../include/Com_Serial.h"
 
 Serial_Port::Serial_Port(std::string uart_name, int baudrate)
@@ -131,9 +137,11 @@ int Serial_Port::write_message(const mavlink_message_t &message)
 
 	// Translate message to buffer
 	unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
+   cout<<__FUNCTION__<<" after mavlink_msg_to_send_buffer"<<endl;
 
 	// Write buffer to serial port, locks port while writing
 	int bytesWritten = _write_port(buf,len);
+   cout<<__FUNCTION__<<" writting done"<<endl;
 
 	return bytesWritten;
 }
@@ -378,9 +386,10 @@ bool Serial_Port::_setup_port(int baud)
 int Serial_Port::_read_port(uint8_t &cp){
 
 	// Lock
+//   cout<<__FUNCTION__<<" : try read \n";
 	pthread_mutex_lock(&lock);
-
 	int result = read(fd, &cp, 1);
+//   cout<<__FUNCTION__<<" : reading nb = "<<result<<"\n"; 
 
 	// Unlock
 	pthread_mutex_unlock(&lock);
@@ -394,7 +403,9 @@ int Serial_Port::_read_port(uint8_t &cp){
 int Serial_Port::_write_port(char *buf, unsigned len)
 {
 	// Lock
+   cout<<__FUNCTION__<<" : try write = "<< len<<" \n";
 	pthread_mutex_lock(&lock);
+   cout<<__FUNCTION__<<" : writing \n";
 
 	// Write packet via serial link
 	const int bytesWritten = static_cast<int>(write(fd, buf, len));

Com_server/test/src/Com_SerialReadingThread.cpp

@@ -72,9 +72,11 @@ void Serial_Port_ReadingThread::run()
 
         gettimeofday(&end_checkpoint, 0);
         currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec);
-
-        if (currentThreadDelay > task_period )
+        
+ /*       if (currentThreadDelay > task_period )
         {
+            cout<<"ReadingThread delay = "<<currentThreadDelay<<endl;
+
             gettimeofday(&front_checkpoint, 0);
 
             if (currentThreadDelay > task_period + task_deadline)
@@ -91,10 +93,24 @@ void Serial_Port_ReadingThread::run()
                     read_messages(message);
                 }
             }
-        }
+        }*/
+        
+    //    cout<<"ReadingThread delay = "<<currentThreadDelay<<endl;
+        
+               currentState = LifeCoreState::RUN;
+                mavlink_message_t message;
+                success = drone1.read_message(message);
+                if(success)
+                {
+                   cout<<__FUNCTION__<<" read_message call \n";
+                    read_messages(message);
+                }
     }
 }
 
+
+	static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES;
+
 void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
 {
     std::string buffer1="";
@@ -106,7 +122,18 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
 
     //Display_IHM::getInstanceOf().printLog(std::to_string(message.msgid));
     
-    cout<<buffer1<<endl;
+    cout<<__FUNCTION__<<buffer1<<endl;
+    cout<<"  msgid = "<<message.msgid<<endl;
+    for(unsigned int i=0;i<sizeof(mavlink_message_names);i++)
+    {
+      // cout<<"i = "<<i<<" "<<mavlink_message_names[i].msgid<<" "<<mavlink_message_names[i].name<<endl;
+       if(mavlink_message_names[i].msgid==message.msgid)
+       {
+          cout<<" message name = "<<mavlink_message_names[i].name<<endl;
+          break;
+       }
+    }
+    
     switch (message.msgid)
     {
         case MAVLINK_MSG_ID_HEARTBEAT:
@@ -167,11 +194,11 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
         {
             mavlink_msg_highres_imu_decode(&message, &drone1.highres_imu);
 
-            buffer1 = "Acc: "+std::to_string(drone1.highres_imu.xacc)+" "+std::to_string(drone1.highres_imu.yacc)+" "+std::to_string(drone1.highres_imu.zacc);
+            buffer1 = buffer1+" Acc: "+std::to_string(drone1.highres_imu.xacc)+" "+std::to_string(drone1.highres_imu.yacc)+" "+std::to_string(drone1.highres_imu.zacc)+"\n";
             //Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
-            buffer1 = "Gyro: "+std::to_string(drone1.highres_imu.xgyro)+" "+std::to_string(drone1.highres_imu.ygyro)+" "+std::to_string(drone1.highres_imu.zgyro);
+            buffer1 = buffer1+" Gyro: "+std::to_string(drone1.highres_imu.xgyro)+" "+std::to_string(drone1.highres_imu.ygyro)+" "+std::to_string(drone1.highres_imu.zgyro)+"\n";
             //Display_IHM::getInstanceOf().printData(buffer1, 12, 1);
-            buffer1 = "Mag: "+std::to_string(drone1.highres_imu.xmag)+" "+std::to_string(drone1.highres_imu.ymag)+" "+std::to_string(drone1.highres_imu.zmag);
+            buffer1 = buffer1+" Mag: "+std::to_string(drone1.highres_imu.xmag)+" "+std::to_string(drone1.highres_imu.ymag)+" "+std::to_string(drone1.highres_imu.zmag)+"\n";
             //Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
 
             blc_highres_imu.floats[0] = drone1.highres_imu.xacc;
@@ -183,6 +210,8 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
             blc_highres_imu.floats[6] = drone1.highres_imu.xmag;
             blc_highres_imu.floats[7] = drone1.highres_imu.ymag;
             blc_highres_imu.floats[8] = drone1.highres_imu.zmag;
+            
+            cout<<"Com_SerialReadingThread "<<buffer1<<endl;
             break;
         }
 
@@ -191,9 +220,9 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
         {
             mavlink_msg_local_position_ned_decode(&message, &drone1.local_position_ned);
 
-            buffer1 = "Pos_coord: "+std::to_string(drone1.local_position_ned.x)+" "+std::to_string(drone1.local_position_ned.y)+" "+std::to_string(drone1.local_position_ned.z);
+            buffer1 =  buffer1 + "\n Pos_coord: "+std::to_string(drone1.local_position_ned.x)+" "+std::to_string(drone1.local_position_ned.y)+" "+std::to_string(drone1.local_position_ned.z)+"\n";
             //Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
-            buffer1 = "Pos_speed: "+std::to_string(drone1.local_position_ned.vx)+" "+std::to_string(drone1.local_position_ned.vy)+" "+std::to_string(drone1.local_position_ned.vz);
+            buffer1 = "Pos_speed: "+std::to_string(drone1.local_position_ned.vx)+" "+std::to_string(drone1.local_position_ned.vy)+" "+std::to_string(drone1.local_position_ned.vz)+"\n";
             //Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
             break;
         }
@@ -221,12 +250,13 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
         {
             mavlink_msg_attitude_decode(&message, &drone1.attitude);
             
-            /*buffer1 = "Attitude_t: "+std::to_string(drone1.attitude.time_boot_ms);
-            Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
-            buffer1 = "Attitude_p: "+std::to_string(drone1.attitude.roll)+" "+std::to_string(drone1.attitude.pitch)+" "+std::to_string(drone1.attitude.yaw);
-            Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
-            buffer1 = "Attitude_s: "+std::to_string(drone1.attitude.rollspeed)+" "+std::to_string(drone1.attitude.pitchspeed)+" "+std::to_string(drone1.attitude.yawspeed);
-            Display_IHM::getInstanceOf().printData(buffer1, 15, 1);*/
+            buffer1 = buffer1 + "\n Attitude_time: "+to_string(drone1.attitude.time_boot_ms)+"\n";
+           // Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
+            buffer1 = buffer1 + " Attitude_p (roll, pitch, yaw): "+to_string(drone1.attitude.roll)+" "+to_string(drone1.attitude.pitch)+" "+std::to_string(drone1.attitude.yaw)+"\n";
+           // Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
+            buffer1 = buffer1 + " Attitude_s: "+to_string(drone1.attitude.rollspeed)+" "+to_string(drone1.attitude.pitchspeed)+" "+to_string(drone1.attitude.yawspeed)+"\n";
+          //  Display_IHM::getInstanceOf().printData(buffer1, 15, 1);*/
+           cout<<"MAVLINK_MSG_ID_ATTITUDE : \n"<<buffer1<<endl;
             
             break;
         }
@@ -253,18 +283,20 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
 
 
     }
-   cout<<buffer1<<endl;
+   cout<<"End function : "<<buffer1<<endl;
 } 
 
 
-uint64_t Serial_Port_ReadingThread::get_time_usec(){
+uint64_t Serial_Port_ReadingThread::get_time_usec()
+{
 	static struct timeval _time_stamp;
 	gettimeofday(&_time_stamp, NULL);
 	return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec;
 }
 
-int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string message){
-    std::string buffer1="-> Ack-";
+int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string message)
+{
+    string buffer1="-> Ack- ";
     buffer1+=message;
 
     if (ack->result == MAV_RESULT_ACCEPTED) {
@@ -276,11 +308,13 @@ int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string m
         //Display_IHM::getInstanceOf().printLog(buffer1);
         return 1;
     }
+    cout<<__FUNCTION__<<buffer1<<endl;
 }
 
 void Serial_Port_ReadingThread::handle_command_ack(mavlink_command_ack_t *ack)
 {
-    switch (ack->command) {
+    switch (ack->command) 
+    {
         case MAV_CMD_COMPONENT_ARM_DISARM:
             if(testAck(ack, "Arm_desarm") == 1)
             { //if command is succeed

Com_server/test/src/Com_SerialWritingThread.cpp

@@ -25,47 +25,64 @@ Serial_Port_WritingThread::~Serial_Port_WritingThread()
 
 }
 
-void Serial_Port_WritingThread::run(){
+string string_currentState[]={"TO_INIT", "INIT", "READY", "RUN", "STOP", "QUIT", "PROBLEM", "DEADLINE_EXCEEDED"};
 
+void Serial_Port_WritingThread::run()
+{
     long long currentThreadDelay;
 
     gettimeofday(&begin, 0);
     gettimeofday(&front_checkpoint, 0);
 
     currentState = LifeCoreState::RUN;
-
-    while(isRunFlag()){
+    cout<<__FUNCTION__<<endl;
+    while(isRunFlag())
+    {
         usleep(task_period);
 
         gettimeofday(&end_checkpoint, 0);
         currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec);
+        cout<<__FUNCTION__<<" wake up "<<endl;
 
-        if (currentThreadDelay > task_period ){
+        if (currentThreadDelay > task_period )
+        {
 
             gettimeofday(&front_checkpoint, 0);
 
-            if (currentThreadDelay > task_period + task_deadline){
+            if (currentThreadDelay > task_period + task_deadline)
+            {
                 currentState = LifeCoreState::DEADLINE_EXCEEDED;
             }
-            else {
+            else 
+            {
                 currentState = LifeCoreState::RUN;
                 main_loop();
             }
         }
+        cout<<__FUNCTION__<<" end currentState = "<<currentState<<" "<<string_currentState[currentState]<<endl;
+        
     }
 }
 
 int Serial_Port_WritingThread::main_loop()
 {
     std::string buffer1="";
+    
+    cout<<__FUNCTION__<<endl;
 
-    if(blc_control_commands.floats[0]>0.5 /*&& drone1.motors == UNARM*/){
+    if(blc_control_commands.floats[0]>0.5 /*&& drone1.motors == UNARM*/)
+    {
+       cout<<" command_arm 1\n";
         drone1.command_arm(1);