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
4956c927
Commit
4956c927
authored
Jul 20, 2021
by
Amine AIT-GANI
Browse files
Mise a jour des fonctions de test.
PG.
parent
61b4456d
Pipeline
#1448
failed with stage
in 2 minutes and 48 seconds
Changes
13
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Com_server/CMakeLists.txt
View file @
4956c927
...
...
@@ -92,6 +92,8 @@ link_libraries(
ncurses
)
######################################################################################################
### APPLICATION ##
######################################################################################################
...
...
Com_server/test/TestDroneRemoteControl.cpp
View file @
4956c927
...
...
@@ -12,13 +12,23 @@
#include "include/PixhawkServer.h"
#include <iostream>
#include <string>
#include <iostream>
#include <fstream>
using
namespace
std
;
string
device_path
=
"/dev/ttyUSB0"
;
char
device_path
[
255
]
=
"/dev/ttyUSB0"
;
int
device_baudrate
=
57600
;
Drone
drone1
(
device_path
,
device_baudrate
);
//Drone drone1(device_path, device_baudrate);
Drone
drone1
;
// General object
shared_ptr
<
Serial_Port_ReadingThread
>
readingThread
;
shared_ptr
<
Serial_Port_WritingThread
>
writingThread
;
ofstream
MyFile
;
ofstream
MyFilew
;
blc_channel
blc_control_arm
(
"/pixhawk.control.arm"
,
BLC_CHANNEL_READ
,
'
IN16
'
,
'
NDEF
'
,
1
,
1
);
// 1=on, 0=off;
blc_channel
blc_control_remote_vector
(
"/pixhawk.control.remoteVectors"
,
BLC_CHANNEL_WRITE
,
'
FL32
'
,
'
NDEF
'
,
1
,
4
);
...
...
@@ -40,46 +50,96 @@ blc_channel blc_test_detection("/pixhawk.test.detection", BLC_CHANNEL_READ, 'IN1
void
command_loop
()
{
int
ret
;
mavlink_message_t
message
;
string
buffer_command
=
""
;
writingThread
.
get
()
->
set_task_period
(
100000
);
cout
<<
"Enter command :
\n
"
;
cin
>>
buffer_command
;
if
(
buffer_command
==
"a
1
"
)
if
(
buffer_command
==
"a"
)
{
cout
<<
"arm
\n
"
;
drone1
.
command_arm
(
1
);
}
else
if
(
buffer_command
==
"!a
1
"
)
else
if
(
buffer_command
==
"!a"
)
{
drone1
.
command_arm
(
0
);
}
else
if
(
buffer_command
==
"k1"
)
else
if
(
buffer_command
==
"f"
)
{
cout
<<
"fly
\n
"
;
ret
=
drone1
.
take_off
();
cout
<<
"fly ret = "
<<
ret
<<
endl
;
}
else
if
(
buffer_command
==
"!f"
)
{
//drone1.take(0);
}
else
if
(
buffer_command
==
"i"
)
{
cout
<<
"return
\n
"
;
drone1
.
return_to_launch
(
1
);
}
else
if
(
buffer_command
==
"!i"
)
{
drone1
.
return_to_launch
(
0
);
}
else
if
(
buffer_command
==
"l"
)
{
cout
<<
"landing
\n
"
;
drone1
.
landing
();
}
/*if(buffer_command=="u")
{
cout<<"takeoff \n";
drone1.command_up(1);
}
else if(buffer_command=="!u")
{
drone1.command_up(0);
}
*/
else
if
(
buffer_command
==
"r"
)
{
cout
<<
"right
\n
"
;
drone1
.
command_right
(
1
);
}
else
if
(
buffer_command
==
"!r"
)
{
drone1
.
command_right
(
0
);
}
else
if
(
buffer_command
==
"k"
)
{
cout
<<
"kill
\n
"
;
drone1
.
command_kill
(
1
);
}
else
if
(
buffer_command
==
"!k
1
"
)
else
if
(
buffer_command
==
"!k"
)
{
drone1
.
command_kill
(
0
);
}
else
if
(
buffer_command
==
"
m1
"
)
else
if
(
buffer_command
==
"
z
"
)
{
cout
<<
"motor 1
\n
"
;
drone1
.
command_directControl
(
1000
,
0
,
0
,
0
);
drone1
.
command_directControl
(
1000
,
0
,
1000
,
0
);
}
else
if
(
buffer_command
==
"
m2
"
)
else
if
(
buffer_command
==
"
d
"
)
{
drone1
.
command_directControl
(
0
,
1000
,
0
,
0
);
drone1
.
command_directControl
(
0
,
1000
,
100
0
,
0
);
}
else
if
(
buffer_command
==
"
m3
"
)
else
if
(
buffer_command
==
"
q
"
)
{
drone1
.
command_directControl
(
0
,
0
,
1000
,
0
);
drone1
.
command_directControl
(
-
100
0
,
0
,
1000
,
0
);
}
else
if
(
buffer_command
==
"
m4
"
)
else
if
(
buffer_command
==
"
s
"
)
{
drone1
.
command_directControl
(
0
,
0
,
0
,
1000
);
}
...
...
@@ -140,23 +200,32 @@ void command_loop()
int
main
(
int
argc
,
char
*
argv
[])
{
// General var
bool
flag_run
=
true
;
//Flag use for the main loop of the system
// bool flag_run = true; //Flag use for the main loop of the system
if
(
argc
>
1
)
{
strcpy
(
device_path
,
argv
[
1
]);
printf
(
"port = %s
\n
"
,
device_path
);}
drone1
.
open
(
device_path
,
device_baudrate
);
string
buffer1
=
""
;
// General object
shared_ptr
<
Serial_Port_ReadingThread
>
readingThread
;
shared_ptr
<
Serial_Port_WritingThread
>
writingThread
;
// shared_ptr<Display_IHM> ihm;
// drone1 = shared_ptr<Drone>(new Drone(device_path, device_baudrate));
// drone1 = new Drone(device_path, device_baudrate);
// ofstream MyFile("mesures.txt");
MyFile
.
open
(
"mesures.txt"
);
MyFilew
.
open
(
"tcdrain.txt"
);
readingThread
=
shared_ptr
<
Serial_Port_ReadingThread
>
(
new
Serial_Port_ReadingThread
(
500
,
200
));
writingThread
=
shared_ptr
<
Serial_Port_WritingThread
>
(
new
Serial_Port_WritingThread
(
500
,
200
));
writingThread
=
shared_ptr
<
Serial_Port_WritingThread
>
(
new
Serial_Port_WritingThread
(
1000000
,
200
));
//beug ne marche pas
readingThread
=
shared_ptr
<
Serial_Port_ReadingThread
>
(
new
Serial_Port_ReadingThread
(
10000
,
200
));
//beug ne marche pas
cout
<<
"Welcome to pixhawk server
1
"
<<
endl
;
cout
<<
"Welcome to pixhawk server"
<<
endl
;
cout
<<
"Init communicationn"
<<
endl
;
cout
<<
"-> Open
"
+
device_path
<<
endl
;
cout
<<
"-> Open
"
<<
device_path
<<
endl
;
if
(
drone1
.
init_communication
()
==
0
)
{
cout
<<
"-> Succes"
<<
endl
;
...
...
@@ -184,8 +253,9 @@ int main(int argc, char *argv[])
//Display_IHM::getInstanceOf().displayDroneState(drone1);
//Display_IHM::getInstanceOf().displayMotorsState(drone1);
//Display_IHM::getInstanceOf().printLog("Begin");
readingThread
.
get
()
->
start
();
//Display_IHM::getInstanceOf().printLog("Begin");
readingThread
.
get
()
->
start
();
writingThread
.
get
()
->
start
();
sleep
(
1
);
...
...
@@ -203,10 +273,12 @@ int main(int argc, char *argv[])
// readingThread.get()->stop();
// writingThread.get()->stop();
while
(
true
)
{
command_loop
();
}
MyFile
.
close
();
MyFilew
.
close
();
exit
(
0
);
}
Com_server/test/include/Abstract_ThreadClass.h
View file @
4956c927
...
...
@@ -43,8 +43,8 @@ enum LifeCoreState {
*/
class
Abstract_ThreadClass
{
protected:
public
:
// REAL TIME VAR
/**
* timeval that save the begin of the running method
...
...
@@ -92,7 +92,7 @@ protected:
*/
LifeCoreState
currentState
=
LifeCoreState
::
TO_INIT
;;
public
:
/**
* Default constructor : take in parameters the task period time and the task deadline time
...
...
Com_server/test/include/Com_SerialReadingThread.h
View file @
4956c927
...
...
@@ -17,6 +17,12 @@
#include <mutex>
#include <sys/time.h>
#include "Abstract_ThreadClass.h"
#include "Data_Drone.h"
...
...
@@ -47,6 +53,8 @@ public:
Serial_Port_ReadingThread
(
int
task_period
,
int
task_deadline
);
~
Serial_Port_ReadingThread
();
int
task_period
=
1000
;
void
run
();
uint64_t
get_time_usec
();
...
...
Com_server/test/include/Com_SerialWritingThread.h
View file @
4956c927
...
...
@@ -35,6 +35,8 @@ protected:
public:
int
task_period
=
100000
;
/**
* Default constructor of the class. Need to have the
*/
...
...
@@ -42,6 +44,8 @@ public:
~
Serial_Port_WritingThread
();
void
run
();
void
set_task_period
(
int
period
);
/**
* Direct writing method command
...
...
Com_server/test/include/Data_Drone.h
View file @
4956c927
...
...
@@ -109,6 +109,9 @@ public:
mavlink_local_position_ned_t
local_position_ned
;
mavlink_position_target_local_ned_t
position_target_local_ned
;
mavlink_gps_raw_int_t
gps_raw_int
;
int
departure_alt
;
public:
...
...
@@ -122,6 +125,8 @@ public:
Drone
(
std
::
string
serialPort_path
,
int
serialPort_baudrate
);
~
Drone
();
void
open
(
char
*
serialPort_path
,
int
serialPort_baudrate
);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMUNICATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
...
...
@@ -162,18 +167,30 @@ public:
int
command_setMode
(
Drone_mode
mode
);
int
command_arm
(
int
param1
);
//int command_float(int param1);
int
command_kill
(
int
param1
);
int
command_setModeGuided
(
float
param1
);
int
command_directControl
(
float
x
,
float
y
,
float
z
,
float
r
);
int
return_to_launch
(
int
param1
);
int
take
(
int
param1
)
;
int
take_off
();
int
landing
();
int
command_right
(
float
param1
)
;
int
command_left
(
float
param1
)
;
int
command_up
(
float
param1
)
;
int
command_down
(
float
param1
)
;
int
command_pow
(
float
param1
)
;
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ? %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mavlink_command_long_t
mavlink_newCommand
();
};
...
...
Com_server/test/src/Abstract_ThreadClass.cpp
View file @
4956c927
...
...
@@ -3,18 +3,28 @@
* @date 18/04/19.
*/
#include <iostream>
#include <string>
#include <cstdlib>
using
namespace
std
;
#include <sys/time.h>
#include "../include/Abstract_ThreadClass.h"
//%%%%%%%%%%%%%%%%%%%%%%%%% begin/end phase function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
Abstract_ThreadClass
::
Abstract_ThreadClass
(
int
task_period
,
int
task_deadline
){
Abstract_ThreadClass
::
Abstract_ThreadClass
(
int
task_period
,
int
task_deadline
)
{
task_period
=
task_period
;
task_deadline
=
task_deadline
;
cout
<<
"task_period = "
<<
task_period
<<
endl
;
}
Abstract_ThreadClass
::~
Abstract_ThreadClass
(){
Abstract_ThreadClass
::~
Abstract_ThreadClass
()
{
currentState
=
LifeCoreState
::
QUIT
;
...
...
@@ -25,7 +35,8 @@ Abstract_ThreadClass::~Abstract_ThreadClass(){
//%%%%%%%%%%%%%%%%%%%%%%%%% run function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
void
Abstract_ThreadClass
::
run
(){
void
Abstract_ThreadClass
::
run
()
{
long
long
currentThreadDelay
;
...
...
@@ -34,79 +45,94 @@ void Abstract_ThreadClass::run(){
currentState
=
LifeCoreState
::
RUN
;
while
(
isRunFlag
()){
while
(
isRunFlag
())
{
usleep
(
task_period
);
cout
<<
"abstract class thread : run() "
<<
endl
;
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
)
{
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
;
}
//
}
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%% control function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
void
Abstract_ThreadClass
::
init
(){
void
Abstract_ThreadClass
::
init
()
{
currentState
=
LifeCoreState
::
INIT
;
currentState
=
LifeCoreState
::
READY
;
}
void
Abstract_ThreadClass
::
start
(){
void
Abstract_ThreadClass
::
start
()
{
setRunFlag
(
true
);
currentState
=
LifeCoreState
::
RUN
;
principalThread
=
std
::
thread
(
&
Abstract_ThreadClass
::
run
,
this
);
}
void
Abstract_ThreadClass
::
stop
(){
void
Abstract_ThreadClass
::
stop
()
{
currentState
=
LifeCoreState
::
STOP
;
setRunFlag
(
false
);
principalThread
.
join
();
}
void
Abstract_ThreadClass
::
lazyStop
(){
void
Abstract_ThreadClass
::
lazyStop
()
{
currentState
=
LifeCoreState
::
STOP
;
setRunFlag
(
false
);
}
void
Abstract_ThreadClass
::
play
(){
void
Abstract_ThreadClass
::
play
()
{
currentState
=
LifeCoreState
::
RUN
;
setRunFlag
(
true
);
}
void
Abstract_ThreadClass
::
pause
(){
void
Abstract_ThreadClass
::
pause
()
{
}
void
Abstract_ThreadClass
::
join
(){
void
Abstract_ThreadClass
::
join
()
{
principalThread
.
join
();
}
//%%%%%%%%%%%%%%%%%%%%%%%%% getters and setters %%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool
Abstract_ThreadClass
::
isRunFlag
()
const
{
bool
Abstract_ThreadClass
::
isRunFlag
()
const
{
return
runFlag
;
}
void
Abstract_ThreadClass
::
setRunFlag
(
bool
runFlag
)
{
void
Abstract_ThreadClass
::
setRunFlag
(
bool
runFlag
)
{
runFlag_mutex
.
lock
();
runFlag
=
runFlag
;
runFlag_mutex
.
unlock
();
}
LifeCoreState
Abstract_ThreadClass
::
getCurrentState
()
const
{
LifeCoreState
Abstract_ThreadClass
::
getCurrentState
()
const
{
return
currentState
;
}
...
...
Com_server/test/src/Com_Serial.cpp
View file @
4956c927
...
...
@@ -10,9 +10,19 @@
#include <iostream>
#include <string>
#include <sys/time.h>
#include <chrono>
using
namespace
std
;
#include <iostream>
#include <fstream>
extern
ofstream
MyFilew
;
double
t_oldw
=
-
1.
;
#include "../include/Com_Serial.h"
Serial_Port
::
Serial_Port
(
std
::
string
uart_name
,
int
baudrate
)
...
...
@@ -137,11 +147,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
;
//
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
;
//
cout<<__FUNCTION__<<" writting done"<<endl;
return
bytesWritten
;
}
...
...
@@ -318,7 +328,8 @@ bool Serial_Port::_setup_port(int baud)
case
9600
:
cfsetispeed
(
&
config
,
B9600
);
cfsetospeed
(
&
config
,
B9600
);
break
;
break
;
case
19200
:
cfsetispeed
(
&
config
,
B19200
);
cfsetospeed
(
&
config
,
B19200
);
...
...
@@ -377,14 +388,15 @@ bool Serial_Port::_setup_port(int baud)
// Done!
return
true
;
}
}
/**
* Read Port with Lock
*/
int
Serial_Port
::
_read_port
(
uint8_t
&
cp
)
{
int
Serial_Port
::
_read_port
(
uint8_t
&
cp
)
{
// Lock
// cout<<__FUNCTION__<<" : try read \n";
pthread_mutex_lock
(
&
lock
);
...
...
@@ -402,21 +414,74 @@ 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
"
;
struct
timeval
tw
;
double
t_currentw
,
dtw
;
gettimeofday
(
&
tw
,
NULL
);
t_currentw
=
tw
.
tv_sec
+
tw
.
tv_usec
*
1e-6
;
// 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
));
// Wait until all data has been written
tcdrain
(
fd
);
using
namespace
std
::
chrono
;
// Use auto keyword to avoid typing long
// type definitions to get the timepoint
// at this instant use function now()
auto
start
=
high_resolution_clock
::
now
();
tcdrain
(
fd
);
auto
stop
=
high_resolution_clock
::
now
();
auto
duration
=
duration_cast
<
microseconds
>
(
stop
-
start
);
cout
<<
"Time taken by function: "
<<
duration
.
count
()
<<
" microseconds"
<<
endl
;
MyFilew
<<
duration
.
count
()
<<
endl
;
/*
int iterations = 1913;
for (int i=0; i<iterations; i++) {
tcdrain(fd);
sum += add;
add /= 2.0;
}
gettimeofday(&end, 0);
long seconds = end.tv_sec - begin.tv_sec;
long microseconds = end.tv_usec - begin.tv_usec;
double elapsed = seconds + microseconds*1e-6;
// Unlock
pthread_mutex_unlock
(
&
lock
);
/*if(t_oldw>0.)
{
// Wait until all data has been written
dtw=t_currentw-t_oldw;
cout<<"\n \n >>>> latencytcdrain: "<<dtw<<"\n\n\n";
t_oldw=t_currentw;
cout<<"latencytcdrain = "<<t_oldw<<endl;
double sum = 0;
double add = 1;
// Start measuring time
struct timeval begin, end;
gettimeofday(&begin, 0);
}*/
// MyFilew << elapsed << endl;
pthread_mutex_unlock
(
&
lock
);
return
bytesWritten
;
}
Com_server/test/src/Com_SerialReadingThread.cpp
View file @
4956c927
...
...
@@ -4,18 +4,29 @@
* trash-put /dev/shm/file
*/