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
1f3bfba3
Commit
1f3bfba3
authored
Mar 29, 2021
by
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
Changes
7
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Com_server/include/Thread_SerialPort.h
View file @
1f3bfba3
...
...
@@ -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
View file @
1f3bfba3
...
...
@@ -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
View file @
1f3bfba3
...
...
@@ -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
View file @
1f3bfba3
...
...
@@ -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
View file @
1f3bfba3
...
...
@@ -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
);
}
if
(
blc_control_commands
.
floats
[
1
]
>
0.5
/*&& drone1.motors == ARM*/
){
if
(
blc_control_commands
.
floats
[
1
]
>
0.5
/*&& drone1.motors == ARM*/
)
{
cout
<<
" command_arm 0
\n
"
;
drone1
.
command_arm
(
0
);
}
if
(
1
/*drone1.motors == ARM*/
){
if
(
1
/*drone1.motors == ARM*/
)
{
cout
<<
" drone1.command_directControl
\n
"
;
drone1
.
command_directControl
(
blc_control_motors
.
floats
[
1
],
blc_control_motors
.
floats
[
0
],
blc_control_motors
.
floats
[
3
],
blc_control_motors
.
floats
[
2
]);
}
/*
...
...
Com_server/test/src/Data_Drone.cpp
View file @
1f3bfba3
...
...
@@ -6,6 +6,13 @@
* @version 1.0
*/
#include
<iostream>
#include
<string>
using
namespace
std
;
#include
"../include/Data_Drone.h"
...
...
@@ -19,13 +26,13 @@ Drone::Drone()
}
Drone
::
Drone
(
std
::
string
serialPort_path
,
int
serialPort_baudrate
){
Drone
::
Drone
(
std
::
string
serialPort_path
,
int
serialPort_baudrate
)
{
communication
=
DRONE_SERIAL
;
serial1
=
std
::
shared_ptr
<
Serial_Port
>
(
new
Serial_Port
(
serialPort_path
,
serialPort_baudrate
));
}
Drone
::~
Drone
()
{
serial1
.
get
()
->
close_serial
();
...
...
@@ -176,15 +183,18 @@ int Drone::command_setModeGuided(float param1){
return
write_message
(
message
);
}
int
Drone
::
command_directControl
(
float
x
,
float
y
,
float
z
,
float
r
){
int
Drone
::
command_directControl
(
float
x
,
float
y
,
float
z
,
float
r
)
{
mavlink_message_t
message
;
mavlink_msg_manual_control_pack
(
255
,
1
,
&
message
,
1
,
0
,
x
,
y
,
z
,
r
);
return
write_message
(
message
);
mavlink_msg_manual_control_pack
(
255
,
1
,
&
message
,
1
,
0
,
x
,
y
,
z
,
r
);
cout
<<
"__FUNCTION__"
<<
" message read to be sent
\n
"
;
return
write_message
(
message
);
}
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ? %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mavlink_command_long_t
Drone
::
mavlink_newCommand
(){
mavlink_command_long_t
Drone
::
mavlink_newCommand
()
{
mavlink_command_long_t
com
;
com
.
target_system
=
0
;
com
.
target_component
=
0.
;
...
...
@@ -200,9 +210,10 @@ mavlink_command_long_t Drone::mavlink_newCommand(){
return
com
;
}
int
Drone
::
command_setMode
(
Drone_mode
mode
){
switch
(
mode
){
int
Drone
::
command_setMode
(
Drone_mode
mode
)
{
switch
(
mode
)
{
case
DRONE_OFF
:
//We must reset all value to 0
Drone
::
mode
=
mode
;
...
...
@@ -219,9 +230,8 @@ int Drone::command_setMode(Drone_mode mode){
break
;
default:
break
;
}
return
0
;
}
\ No newline at end of file
}
Promethe_scripts/drone_test.symb
View file @
1f3bfba3
...
...
@@ -248,15 +248,15 @@ ech_temps = 1
%Les commentaires doivent etre mis ici.
groupe = imu , type = 14 , nbre neurones = 1 , seuil = 0.000000
taillex = 1 , tailley =
1
taillex = 1 , tailley =
9
learning rate = 1.000000
simulation speed = 1.000000
type2 = 0
groupe = f_in
posx = 4
35
, posy = 4
3
1
posx = 4
82
, posy = 41
7
reverse = -1
p_posx =
0
, p_posy =
0
debug =
1
p_posx =
824
, p_posy =
364
debug =
-3
ech_temps = 0
%Les commentaires doivent etre mis ici.
...
...
@@ -268,8 +268,8 @@ type2 = 0
groupe = f_debut
posx = 306 , posy = 510
reverse = -1
p_posx =
0
, p_posy =
0
debug =
1
p_posx =
984
, p_posy =
364
debug =
-3
ech_temps = 1
nombre de liaisons = 30
...
...
@@ -503,5 +503,5 @@ liaison entre 24 et imu , type = 5 , nbre = 1 , norme = 0.100000
temps de memorisation sortie= 0.000000
mode de calcul = 0
secondaire = 0
nom = -bpixhawk.
imu
nom = -bpixhawk.
sensors-s1-v
proba = 1.000000
Write
Preview
Supports
Markdown
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