Commit 1f3bfba3 authored by Philippe Gaussier's avatar Philippe Gaussier
Browse files

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

Look like the problem is the mutex_lock for _write_port

PG.
parent 48507084
Pipeline #1276 failed with stage
in 1 minute and 16 seconds
...@@ -25,6 +25,8 @@ ...@@ -25,6 +25,8 @@
#include "../include/Com_Mavlink.h" #include "../include/Com_Mavlink.h"
#include "../include/Data_Bus.h" #include "../include/Data_Bus.h"
#include "Data_Drone.h"
class Thread_SerialPort : public Abstract_ThreadClass class Thread_SerialPort : public Abstract_ThreadClass
{ {
......
...@@ -10,6 +10,7 @@ ...@@ -10,6 +10,7 @@
using namespace std; using namespace std;
#include "../include/Thread_SerialPort.h" #include "../include/Thread_SerialPort.h"
#include "../include/global_variables.h"
//#define DISPLAY //#define DISPLAY
...@@ -163,6 +164,7 @@ void Thread_SerialPort::write_messages() ...@@ -163,6 +164,7 @@ void Thread_SerialPort::write_messages()
void Thread_SerialPort::read_messages() void Thread_SerialPort::read_messages()
{ {
string buffer1="";
Bus::getInstanceOf().event_newMessage(message); Bus::getInstanceOf().event_newMessage(message);
// cout<< "LOG "<<message.msgid<<"\n"; // cout<< "LOG "<<message.msgid<<"\n";
...@@ -179,31 +181,29 @@ void Thread_SerialPort::read_messages() ...@@ -179,31 +181,29 @@ void Thread_SerialPort::read_messages()
{ {
mavlink_msg_sys_status_decode(&message, &sys_status); mavlink_msg_sys_status_decode(&message, &sys_status);
Bus::getInstanceOf().update(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); //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);*/ //Display_IHM::getInstanceOf().printData(buffer1, 10, 1);*/
break; break;
} }
//ACCELEROMETERS //ACCELEROMETERS
/*case MAVLINK_MSG_ID_HIGHRES_IMU: case MAVLINK_MSG_ID_HIGHRES_IMU:
{ {
mavlink_msg_highres_imu_decode(&message, &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); //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); 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); //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); 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); //Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
break; break;
}*/ }
//ACK COMMAND //ACK COMMAND
/*case MAVLINK_MSG_ID_COMMAND_ACK: /*case MAVLINK_MSG_ID_COMMAND_ACK:
...@@ -225,12 +225,12 @@ void Thread_SerialPort::read_messages() ...@@ -225,12 +225,12 @@ void Thread_SerialPort::read_messages()
{ {
mavlink_msg_radio_status_decode(&message, &drone1.radio_status); mavlink_msg_radio_status_decode(&message, &drone1.radio_status);
break; break;
} }*/
//GPS //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); mavlink_msg_local_position_ned_decode(&message, &drone1.local_position_ned);
...@@ -244,9 +244,9 @@ void Thread_SerialPort::read_messages() ...@@ -244,9 +244,9 @@ void Thread_SerialPort::read_messages()
{ {
mavlink_msg_position_target_local_ned_decode(&message, &drone1.position_target_local_ned); 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); // 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); // Display_IHM::getInstanceOf().printData(buffer1, 11, 1);
break; break;
}*/ }*/
...@@ -273,12 +273,13 @@ void Thread_SerialPort::read_messages() ...@@ -273,12 +273,13 @@ void Thread_SerialPort::read_messages()
break; break;
}*/ }*/
/*
case MAVLINK_MSG_ID_GPS_RAW_INT: case MAVLINK_MSG_ID_GPS_RAW_INT:
{ {
mavlink_msg_gps_raw_int_decode(&message, &gps_raw_int); mavlink_msg_gps_raw_int_decode(&message, &gps_raw_int);
Bus::getInstanceOf().update(gps_raw_int); Bus::getInstanceOf().update(gps_raw_int);
break; break;
} }*/
//Attitude function //Attitude function
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
...@@ -287,16 +288,16 @@ void Thread_SerialPort::read_messages() ...@@ -287,16 +288,16 @@ void Thread_SerialPort::read_messages()
mavlink_msg_attitude_decode(&message, &attitude); mavlink_msg_attitude_decode(&message, &attitude);
Bus::getInstanceOf().update(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); // 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); // 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); // Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
break; break;
} }
/*
case MAVLINK_MSG_ID_VFR_HUD:{ case MAVLINK_MSG_ID_VFR_HUD:{
mavlink_msg_vfr_hud_decode(&message, &vfr_hud); mavlink_msg_vfr_hud_decode(&message, &vfr_hud);
Bus::getInstanceOf().update(vfr_hud); Bus::getInstanceOf().update(vfr_hud);
...@@ -314,28 +315,29 @@ void Thread_SerialPort::read_messages() ...@@ -314,28 +315,29 @@ void Thread_SerialPort::read_messages()
Bus::getInstanceOf().update(manual_control); Bus::getInstanceOf().update(manual_control);
break; break;
} }
*/
/*case MAVLINK_MSG_ID_ATTITUDE_TARGET: /*case MAVLINK_MSG_ID_ATTITUDE_TARGET:
{ {
mavlink_msg_attitude_target_decode(&message, &drone1.attitude_target); mavlink_msg_attitude_target_decode(&message, &drone1.attitude_target);
// buffer1 = "Attitude_target: "+std::to_string(drone1.attitude_target.time_boot_ms); buffer1 = "Attitude_target: "+std::to_string(drone1.attitude_target.time_boot_ms);
// Display_IHM::getInstanceOf().printData(buffer1, 16, 1); //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_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); // 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); // 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); // Display_IHM::getInstanceOf().printData(buffer1, 19, 1);
break; break;
}*/ }*/
default: default:
{ {
//printf("Warning, did not handle message id %i\n",message.msgid); printf("Warning, did not handle message id %i\n",message.msgid);
break; break;
} }
} }
cout<<"Thread_SerialPort "<<buffer1<<endl;
} }
......
...@@ -7,6 +7,12 @@ ...@@ -7,6 +7,12 @@
* @version 1.1 * @version 1.1
*/ */
#include <iostream>
#include <string>
using namespace std;
#include "../include/Com_Serial.h" #include "../include/Com_Serial.h"
Serial_Port::Serial_Port(std::string uart_name, int baudrate) Serial_Port::Serial_Port(std::string uart_name, int baudrate)
...@@ -131,9 +137,11 @@ int Serial_Port::write_message(const mavlink_message_t &message) ...@@ -131,9 +137,11 @@ int Serial_Port::write_message(const mavlink_message_t &message)
// Translate message to buffer // Translate message to buffer
unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message); 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 // Write buffer to serial port, locks port while writing
int bytesWritten = _write_port(buf,len); int bytesWritten = _write_port(buf,len);
cout<<__FUNCTION__<<" writting done"<<endl;
return bytesWritten; return bytesWritten;
} }
...@@ -378,9 +386,10 @@ bool Serial_Port::_setup_port(int baud) ...@@ -378,9 +386,10 @@ bool Serial_Port::_setup_port(int baud)
int Serial_Port::_read_port(uint8_t &cp){ int Serial_Port::_read_port(uint8_t &cp){
// Lock // Lock
// cout<<__FUNCTION__<<" : try read \n";
pthread_mutex_lock(&lock); pthread_mutex_lock(&lock);
int result = read(fd, &cp, 1); int result = read(fd, &cp, 1);
// cout<<__FUNCTION__<<" : reading nb = "<<result<<"\n";
// Unlock // Unlock
pthread_mutex_unlock(&lock); pthread_mutex_unlock(&lock);
...@@ -394,7 +403,9 @@ int Serial_Port::_read_port(uint8_t &cp){ ...@@ -394,7 +403,9 @@ int Serial_Port::_read_port(uint8_t &cp){
int Serial_Port::_write_port(char *buf, unsigned len) int Serial_Port::_write_port(char *buf, unsigned len)
{ {
// Lock // Lock
cout<<__FUNCTION__<<" : try write = "<< len<<" \n";
pthread_mutex_lock(&lock); pthread_mutex_lock(&lock);
cout<<__FUNCTION__<<" : writing \n";
// Write packet via serial link // Write packet via serial link
const int bytesWritten = static_cast<int>(write(fd, buf, len)); const int bytesWritten = static_cast<int>(write(fd, buf, len));
......
...@@ -72,9 +72,11 @@ void Serial_Port_ReadingThread::run() ...@@ -72,9 +72,11 @@ void Serial_Port_ReadingThread::run()
gettimeofday(&end_checkpoint, 0); gettimeofday(&end_checkpoint, 0);
currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec); 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); gettimeofday(&front_checkpoint, 0);
if (currentThreadDelay > task_period + task_deadline) if (currentThreadDelay > task_period + task_deadline)
...@@ -91,10 +93,24 @@ void Serial_Port_ReadingThread::run() ...@@ -91,10 +93,24 @@ void Serial_Port_ReadingThread::run()
read_messages(message); 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) void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
{ {
std::string buffer1=""; std::string buffer1="";
...@@ -106,7 +122,18 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -106,7 +122,18 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
//Display_IHM::getInstanceOf().printLog(std::to_string(message.msgid)); //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) switch (message.msgid)
{ {
case MAVLINK_MSG_ID_HEARTBEAT: case MAVLINK_MSG_ID_HEARTBEAT:
...@@ -167,11 +194,11 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -167,11 +194,11 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
{ {
mavlink_msg_highres_imu_decode(&message, &drone1.highres_imu); 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); //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); //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); //Display_IHM::getInstanceOf().printData(buffer1, 13, 1);
blc_highres_imu.floats[0] = drone1.highres_imu.xacc; blc_highres_imu.floats[0] = drone1.highres_imu.xacc;
...@@ -183,6 +210,8 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -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[6] = drone1.highres_imu.xmag;
blc_highres_imu.floats[7] = drone1.highres_imu.ymag; blc_highres_imu.floats[7] = drone1.highres_imu.ymag;
blc_highres_imu.floats[8] = drone1.highres_imu.zmag; blc_highres_imu.floats[8] = drone1.highres_imu.zmag;
cout<<"Com_SerialReadingThread "<<buffer1<<endl;
break; break;
} }
...@@ -191,9 +220,9 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -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); 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); //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); //Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
break; break;
} }
...@@ -221,12 +250,13 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -221,12 +250,13 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
{ {
mavlink_msg_attitude_decode(&message, &drone1.attitude); mavlink_msg_attitude_decode(&message, &drone1.attitude);
/*buffer1 = "Attitude_t: "+std::to_string(drone1.attitude.time_boot_ms); buffer1 = buffer1 + "\n Attitude_time: "+to_string(drone1.attitude.time_boot_ms)+"\n";
Display_IHM::getInstanceOf().printData(buffer1, 13, 1); // 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 = 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); // 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 = 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);*/ // Display_IHM::getInstanceOf().printData(buffer1, 15, 1);*/
cout<<"MAVLINK_MSG_ID_ATTITUDE : \n"<<buffer1<<endl;
break; break;
} }
...@@ -253,18 +283,20 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -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; static struct timeval _time_stamp;
gettimeofday(&_time_stamp, NULL); gettimeofday(&_time_stamp, NULL);
return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec; return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec;
} }
int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string message){ int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string message)
std::string buffer1="-> Ack-"; {
string buffer1="-> Ack- ";
buffer1+=message; buffer1+=message;
if (ack->result == MAV_RESULT_ACCEPTED) { if (ack->result == MAV_RESULT_ACCEPTED) {
...@@ -276,11 +308,13 @@ int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string m ...@@ -276,11 +308,13 @@ int Serial_Port_ReadingThread::testAck(mavlink_command_ack_t *ack, std::string m
//Display_IHM::getInstanceOf().printLog(buffer1); //Display_IHM::getInstanceOf().printLog(buffer1);
return 1; return 1;
} }
cout<<__FUNCTION__<<buffer1<<endl;
} }
void Serial_Port_ReadingThread::handle_command_ack(mavlink_command_ack_t *ack) void Serial_Port_ReadingThread::handle_command_ack(mavlink_command_ack_t *ack)
{ {
switch (ack->command) { switch (ack->command)
{
case MAV_CMD_COMPONENT_ARM_DISARM: case MAV_CMD_COMPONENT_ARM_DISARM:
if(testAck(ack, "Arm_desarm") == 1) if(testAck(ack, "Arm_desarm") == 1)
{ //if command is succeed { //if command is succeed
......
...@@ -25,47 +25,64 @@ Serial_Port_WritingThread::~Serial_Port_WritingThread() ...@@ -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; long long currentThreadDelay;
gettimeofday(&begin, 0); gettimeofday(&begin, 0);
gettimeofday(&front_checkpoint, 0); gettimeofday(&front_checkpoint, 0);
currentState = LifeCoreState::RUN; currentState = LifeCoreState::RUN;
cout<<__FUNCTION__<<endl;
while(isRunFlag()){ while(isRunFlag())
{
usleep(task_period); usleep(task_period);
gettimeofday(&end_checkpoint, 0); gettimeofday(&end_checkpoint, 0);
currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec); 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); gettimeofday(&front_checkpoint, 0);
if (currentThreadDelay > task_period + task_deadline){ if (currentThreadDelay > task_period + task_deadline)
{
currentState = LifeCoreState::DEADLINE_EXCEEDED; currentState = LifeCoreState::DEADLINE_EXCEEDED;
} }
else { else
{
currentState = LifeCoreState::RUN; currentState = LifeCoreState::RUN;
main_loop(); main_loop();
} }
} }
cout<<__FUNCTION__<<" end currentState = "<<currentState<<" "<<string_currentState[currentState]<<endl;
} }
} }
int Serial_Port_WritingThread::main_loop() int Serial_Port_WritingThread::main_loop()
{ {
std::string buffer1=""; 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);