Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in / Register
Toggle navigation
Menu
Open sidebar
Philippe Gaussier
Drone FlyMonitor2
Commits
c923962e
Commit
c923962e
authored
Mar 26, 2021
by
Philippe Gaussier
Browse files
Bu correction and code simplification.
drone1 is now a global object. Same thing for the blaar variables. PG.
parent
b7d14923
Changes
7
Hide whitespace changes
Inline
Side-by-side
Com_server/test/TestDroneRemoteControl.cpp
View file @
c923962e
...
...
@@ -15,137 +15,83 @@
using
namespace
std
;
shared_ptr
<
Drone
>
drone1
;
string
device_path
=
"/dev/ttyUSB0"
;
int
device_baudrate
=
57600
;
Drone
drone1
(
device_path
,
device_baudrate
);
/**
* Only the main exe of the programm
* @return
*/
int
main
(
int
argc
,
char
*
argv
[])
{
// General var
bool
flag_run
=
true
;
//Flag use for the main loop of the system
string
device_path
=
"/dev/ttyUSB0"
;
int
device_baudrate
=
57600
;
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
));
readingThread
=
shared_ptr
<
Serial_Port_ReadingThread
>
(
new
Serial_Port_ReadingThread
(
500
,
200
,
drone1
));
writingThread
=
shared_ptr
<
Serial_Port_WritingThread
>
(
new
Serial_Port_WritingThread
(
500
,
200
,
drone1
));
cout
<<
"Welcome to pixhawk server"
<<
endl
;
cout
<<
"Init communication"
<<
endl
;
cout
<<
"-> Open "
+
device_path
<<
endl
;
if
(
drone1
.
get
()
->
init_communication
()
==
0
)
{
cout
<<
"-> Succes"
<<
endl
;
}
else
{
cout
<<
"-> Fail, exiting now"
<<
endl
;
sleep
(
2
);
exit
(
1
);
}
cout
<<
"Pull parameters"
<<
endl
;
if
(
drone1
.
get
()
->
init_parameters
(
10000
)
==
0
)
{
cout
<<
"-> Succes"
<<
endl
;
}
else
{
cout
<<
"-> Fail, exiting"
<<
endl
;
sleep
(
2
);
exit
(
1
);
}
// Display
//Display_IHM::getInstanceOf().displayDroneState(drone1);
//Display_IHM::getInstanceOf().displayMotorsState(drone1);
//Display_IHM::getInstanceOf().printLog("Begin");
readingThread
.
get
()
->
start
();
writingThread
.
get
()
->
start
();
sleep
(
1
);
cout
<<
"Arm"
<<
endl
;
drone1
.
get
()
->
command_arm
(
1
);
sleep
(
1
);
cout
<<
"test motors"
<<
endl
;
drone1
.
get
()
->
command_directControl
(
0
,
1000
,
0
,
0
);
sleep
(
1
);
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
);
//Channels use to control manually the drone
blc_channel
blc_control_motors
(
"/pixhawk.control.motors"
,
BLC_CHANNEL_READ
,
'
FL32
'
,
'
NDEF
'
,
1
,
4
);
//x, y, z, r
// cout<<"Unarm"<<endl;
// drone1.get()->command_arm(0);
// sleep(1
);
//Channels use to command the drone
blc_channel
blc_control_commands
(
"/pixhawk.control.commands"
,
BLC_CHANNEL_READ
,
'
FL32
'
,
'
NDEF
'
,
1
,
4
);
// 1=on, 0=off
blc_channel
blc_highres_imu
(
"/pixhawk.sensors.highres_imu"
,
BLC_CHANNEL_WRITE
,
'
FL32
'
,
'
NDEF
'
,
1
,
9
);
// readingThread.get()->stop();
// writingThread.get()->stop();
command_loop
();
//Different blc channel use to publish data
blc_channel
blc_heartbeat
;
//Hearbeat message PG: PB ????? pas d'init pour l'instant ????
return
0
;
}
void
command_loop
()
{
mavlink_message_t
message
;
string
buffer_command
=
""
;
cout
<<
"Enter command :
\n
"
;
cin
>>
buffer_command
;
if
(
buffer_command
==
"a1"
)
{
cout
<<
"arm
\n
"
;
drone1
.
get
()
->
command_arm
(
1
);
drone1
.
command_arm
(
1
);
}
else
if
(
buffer_command
==
"!a1"
)
{
drone1
.
get
()
->
command_arm
(
0
);
drone1
.
command_arm
(
0
);
}
else
if
(
buffer_command
==
"k1"
)
{
drone1
.
get
()
->
command_kill
(
1
);
cout
<<
"kill
\n
"
;
drone1
.
command_kill
(
1
);
}
else
if
(
buffer_command
==
"!k1"
)
{
drone1
.
get
()
->
command_kill
(
0
);
drone1
.
command_kill
(
0
);
}
else
if
(
buffer_command
==
"m1"
)
{
drone1
.
get
()
->
command_directControl
(
1000
,
0
,
0
,
0
);
cout
<<
"motor 1
\n
"
;
drone1
.
command_directControl
(
1000
,
0
,
0
,
0
);
}
else
if
(
buffer_command
==
"m2"
)
{
drone1
.
get
()
->
command_directControl
(
0
,
1000
,
0
,
0
);
drone1
.
command_directControl
(
0
,
1000
,
0
,
0
);
}
else
if
(
buffer_command
==
"m3"
)
{
drone1
.
get
()
->
command_directControl
(
0
,
0
,
1000
,
0
);
drone1
.
command_directControl
(
0
,
0
,
1000
,
0
);
}
else
if
(
buffer_command
==
"m4"
)
{
drone1
.
get
()
->
command_directControl
(
0
,
0
,
0
,
1000
);
drone1
.
command_directControl
(
0
,
0
,
0
,
1000
);
}
else
if
(
buffer_command
==
"!m"
)
{
drone1
.
get
()
->
command_directControl
(
0
,
0
,
0
,
0
);
drone1
.
command_directControl
(
0
,
0
,
0
,
0
);
}
else
if
(
buffer_command
==
"tone"
)
{
cout
<<
"Tone
\n
"
;
//Display_IHM::getInstanceOf().printLog("PLAY TONE");
mavlink_msg_play_tune_pack
(
255
,
drone1
.
get
()
->
component_id
,
&
message
,
1
,
1
,
"AAAA"
,
""
);
drone1
.
get
()
->
write_message
(
message
);
mavlink_msg_play_tune_pack
(
255
,
drone1
.
component_id
,
&
message
,
1
,
1
,
"AAAA"
,
""
);
drone1
.
write_message
(
message
);
}
else
{
cout
<<
"command not recognized
\n
"
;
//Display_IHM::getInstanceOf().printLog("Error: what ?");
}
...
...
@@ -179,3 +125,83 @@ void command_loop()
// mavlink_setAttitudeTarget(&message, *drone1.get(), 0, 0, 0, 800);
// drone1.get()->write_message(message);
// }
/**
* Only the main exe of the programm
* @return
*/
int
main
(
int
argc
,
char
*
argv
[])
{
// General var
bool
flag_run
=
true
;
//Flag use for the main loop of the system
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);
readingThread
=
shared_ptr
<
Serial_Port_ReadingThread
>
(
new
Serial_Port_ReadingThread
(
500
,
200
));
writingThread
=
shared_ptr
<
Serial_Port_WritingThread
>
(
new
Serial_Port_WritingThread
(
500
,
200
));
cout
<<
"Welcome to pixhawk server"
<<
endl
;
cout
<<
"Init communication"
<<
endl
;
cout
<<
"-> Open "
+
device_path
<<
endl
;
if
(
drone1
.
init_communication
()
==
0
)
{
cout
<<
"-> Succes"
<<
endl
;
}
else
{
cout
<<
"-> Fail, exiting now"
<<
endl
;
sleep
(
2
);
exit
(
1
);
}
cout
<<
"Pull parameters"
<<
endl
;
if
(
drone1
.
init_parameters
(
10000
)
==
0
)
{
cout
<<
"-> Succes"
<<
endl
;
}
else
{
cout
<<
"-> Fail, exiting"
<<
endl
;
sleep
(
2
);
exit
(
1
);
}
// Display
//Display_IHM::getInstanceOf().displayDroneState(drone1);
//Display_IHM::getInstanceOf().displayMotorsState(drone1);
//Display_IHM::getInstanceOf().printLog("Begin");
readingThread
.
get
()
->
start
();
writingThread
.
get
()
->
start
();
sleep
(
1
);
cout
<<
"Arm"
<<
endl
;
drone1
.
command_arm
(
1
);
sleep
(
1
);
cout
<<
"test motors"
<<
endl
;
drone1
.
command_directControl
(
0
,
1000
,
0
,
0
);
sleep
(
1
);
// cout<<"Unarm"<<endl;
// drone1.get()->command_arm(0);
// sleep(1);
// readingThread.get()->stop();
// writingThread.get()->stop();
while
(
true
)
{
command_loop
();
}
exit
(
0
);
}
Com_server/test/include/Com_SerialReadingThread.h
View file @
c923962e
...
...
@@ -23,8 +23,8 @@
#include
"blc_channel.h"
#include
"blc_program.h"
class
Serial_Port_ReadingThread
:
public
Abstract_ThreadClass
{
class
Serial_Port_ReadingThread
:
public
Abstract_ThreadClass
{
protected:
/**
* Serial port use with the drone
...
...
@@ -34,21 +34,17 @@ protected:
/**
* Drone buffer
*/
std
::
shared_ptr
<
Drone
>
drone
1
;
//
std::shared_ptr<Drone>
pt_
drone;
/**
* Mavlink message dictionnary
*/
std
::
map
<
int
,
std
::
string
>
mavlink_dic
;
//Different blc channel use to publish data
std
::
shared_ptr
<
blc_channel
>
blc_heartbeat
;
//Hearbeat message
std
::
shared_ptr
<
blc_channel
>
blc_highres_imu
;
public:
Serial_Port_ReadingThread
(
int
task_period
,
int
task_deadline
,
std
::
shared_ptr
<
Drone
>
drone1
);
Serial_Port_ReadingThread
(
int
task_period
,
int
task_deadline
);
~
Serial_Port_ReadingThread
();
void
run
();
...
...
@@ -61,7 +57,6 @@ public:
void
read_messages
(
mavlink_message_t
message
);
int
testAck
(
mavlink_command_ack_t
*
ack
,
std
::
string
message
);
void
handle_command_ack
(
mavlink_command_ack_t
*
ack
);
};
#endif //VEHICLEWATCHER_Abstract_ThreadClass_H
Com_server/test/include/Com_SerialWritingThread.h
View file @
c923962e
...
...
@@ -21,35 +21,24 @@
#include
"blc_channel.h"
#include
"blc_program.h"
class
Serial_Port_WritingThread
:
public
Abstract_ThreadClass
{
class
Serial_Port_WritingThread
:
public
Abstract_ThreadClass
{
protected:
/**
* Drone buffer
*/
std
::
shared_ptr
<
Drone
>
drone1
;
//
std::shared_ptr<Drone> drone1;
// Different buffer use for get the data
int
time_stamps
;
//Channels use to control manually the drone
std
::
shared_ptr
<
blc_channel
>
blc_control_motors
;
//x, y, z, r
std
::
shared_ptr
<
blc_channel
>
blc_control_commands
;
// 1=on, 0=off
public:
/**
* Default constructor of the class. Need to have the
*/
Serial_Port_WritingThread
(
int
task_period
,
int
task_deadline
,
std
::
shared_ptr
<
Drone
>
drone1
);
Serial_Port_WritingThread
(
int
task_period
,
int
task_deadline
);
~
Serial_Port_WritingThread
();
void
run
();
...
...
@@ -58,7 +47,6 @@ public:
* Direct writing method command
*/
int
main_loop
();
};
#endif //VEHICLEWATCHER_Abstract_ThreadClass_H
Com_server/test/src/Cmd_ControlThread.cpp
View file @
c923962e
...
...
@@ -4,18 +4,18 @@
*/
#include
"../include/Cmd_ControlThread.h"
#include
"../include/global_variables.h"
Serial_Port_WritingThread
::
Serial_Port_WritingThread
(
int
task_period
,
int
task_deadline
,
std
::
shared_ptr
<
Drone
>
drone1
)
:
Serial_Port_WritingThread
::
Serial_Port_WritingThread
(
int
task_period
,
int
task_deadline
)
:
Abstract_ThreadClass
(
task_period
,
task_deadline
)
{
Serial_Port_WritingThread
::
drone1
=
drone1
;
//
Serial_Port_WritingThread::drone1 = drone1;
//Channels use to arm the drone
blc_control_arm
=
std
::
shared_ptr
<
blc_channel
>
(
new
blc_channel
(
"/pixhawk.control.arm"
,
BLC_CHANNEL_READ
,
'
IN16
'
,
'
NDEF
'
,
1
,
1
));
;
// 1=on, 0=off
//
blc_control_arm= std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.control.arm", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 1)); // 1=on, 0=off
//Channels use to control manually the drone
blc_control_remote_vector
=
std
::
shared_ptr
<
blc_channel
>
(
new
blc_channel
(
"/pixhawk.control.remoteVectors"
,
BLC_CHANNEL_WRITE
,
'
FL32
'
,
'
NDEF
'
,
1
,
4
));
//
blc_control_remote_vector = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.control.remoteVectors", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 4));
}
Serial_Port_WritingThread
::~
Serial_Port_WritingThread
()
...
...
@@ -23,8 +23,26 @@ Serial_Port_WritingThread::~Serial_Port_WritingThread()
}
void
Serial_Port_WritingThread
::
run
(){
int
Serial_Port_WritingThread
::
main_loop
()
{
//Main loop of the system
switch
(
drone1
.
mode
==
DRONE_OFF
)
{
}
// We take care only for difference in mode control
/*if(blc_message_arm.ints16[0] ==1){
}*/
return
0
;
}
void
Serial_Port_WritingThread
::
run
()
{
long
long
currentThreadDelay
;
gettimeofday
(
&
begin
,
0
);
...
...
@@ -32,38 +50,27 @@ void Serial_Port_WritingThread::run(){
currentState
=
LifeCoreState
::
RUN
;
while
(
isRunFlag
()){
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
);
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
{
//PG: strange code (check older version)
currentState
=
LifeCoreState
::
RUN
;
main_loop
();
main_loop
();
// PG: strange name?
}
}
}
}
int
Serial_Port_WritingThread
::
main_loop
(){
//Main loop of the system
switch
(
drone1
.
get
()
->
mode
==
DRONE_OFF
){
}
// We take care only for difference in mode control
/*if(blc_message_arm.get()->ints16[0] ==1){
}*/
return
0
;
}
Com_server/test/src/Com_Serial.cpp
View file @
c923962e
...
...
@@ -9,21 +9,25 @@
#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
)
{
initialize_defaults
();
Serial_Port
::
uart_name
=
uart_name
;
Serial_Port
::
baudrate
=
baudrate
;
}
Serial_Port
::
Serial_Port
(){
Serial_Port
::
Serial_Port
()
{
initialize_defaults
();
}
Serial_Port
::~
Serial_Port
(){
Serial_Port
::~
Serial_Port
()
{
pthread_mutex_destroy
(
&
lock
);
}
void
Serial_Port
::
initialize_defaults
(){
void
Serial_Port
::
initialize_defaults
()
{
// Initialize attributes
debug
=
false
;
fd
=
-
1
;
...
...
@@ -45,8 +49,8 @@ void Serial_Port::initialize_defaults(){
/**
* Read from Serial
*/
int
Serial_Port
::
read_message
(
mavlink_message_t
&
message
)
{
int
Serial_Port
::
read_message
(
mavlink_message_t
&
message
)
{
uint8_t
cp
;
mavlink_status_t
status
;
uint8_t
msgReceived
=
false
;
...
...
@@ -121,7 +125,8 @@ int Serial_Port::read_message(mavlink_message_t &message){
/**
* Write to Serial
*/
int
Serial_Port
::
write_message
(
const
mavlink_message_t
&
message
){
int
Serial_Port
::
write_message
(
const
mavlink_message_t
&
message
)
{
char
buf
[
300
];
// Translate message to buffer
...
...
@@ -133,12 +138,11 @@ int Serial_Port::write_message(const mavlink_message_t &message){
return
bytesWritten
;
}
/**
* Open Serial Port
*/
int
Serial_Port
::
open_serial
()
{
int
Serial_Port
::
open_serial
()
{
fd
=
_open_port
(
uart_name
.
c_str
());
// Check success
...
...
@@ -164,17 +168,15 @@ int Serial_Port::open_serial(){
lastStatus
.
packet_rx_drop_count
=
0
;
status
=
true
;
return
0
;
}
/**
* Close Serial Port
*/
void
Serial_Port
::
close_serial
()
{
void
Serial_Port
::
close_serial
()
{
//Display_IHM::getInstanceOf().printLog("CLOSE PORT\n");
int
result
=
close
(
fd
);
...
...
@@ -190,12 +192,14 @@ void Serial_Port::close_serial(){
/**
* Quit Handler
*/
void
Serial_Port
::
handle_quit
(){
try
{
void
Serial_Port
::
handle_quit
()
{
try
{
close_serial
();
}
catch
(
int
error
)
{
catch
(
int
error
)
{
fprintf
(
stderr
,
"Warning, could not stop serial port
\n
"
);
}
}
...
...
@@ -252,16 +256,14 @@ bool Serial_Port::_setup_port(int baud)
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
config
.
c_iflag
&=
~
(
IGNBRK
|
BRKINT
|
ICRNL
|
INLCR
|
PARMRK
|
INPCK
|
ISTRIP
|
IXON
);
config
.
c_iflag
&=
~
(
IGNBRK
|
BRKINT
|
ICRNL
|
INLCR
|
PARMRK
|
INPCK
|
ISTRIP
|
IXON
);
// Output flags - Turn off output processing
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
config
.
c_oflag
&=
~
(
OCRNL
|
ONLCR
|
ONLRET
|
ONOCR
|
OFILL
|
OPOST
);
config
.
c_oflag
&=
~
(
OCRNL
|
ONLCR
|
ONLRET
|
ONOCR
|
OFILL
|
OPOST
);
#ifdef OLCUC
config
.
c_oflag
&=
~
OLCUC
;
...
...
@@ -370,7 +372,6 @@ bool Serial_Port::_setup_port(int baud)
}
/**
* Read Port with Lock
*/
...
...
@@ -390,8 +391,8 @@ int Serial_Port::_read_port(uint8_t &cp){
/**
* Write Port with Lock
*/
int
Serial_Port
::
_write_port
(
char
*
buf
,
unsigned
len
)
{
int
Serial_Port
::
_write_port
(
char
*
buf
,
unsigned
len
)
{
// Lock
pthread_mutex_lock
(
&
lock
);
...
...
@@ -404,7 +405,6 @@ int Serial_Port::_write_port(char *buf, unsigned len){
// Unlock
pthread_mutex_unlock
(
&
lock
);
return
bytesWritten
;
}
...
...
Com_server/test/src/Com_SerialReadingThread.cpp
View file @
c923962e
...
...
@@ -5,17 +5,16 @@
*/
#include
"../include/Com_SerialReadingThread.h"
#include
"../include/global_variables.h"
//#define DISPLAY
Serial_Port_ReadingThread
::
Serial_Port_ReadingThread
(
int
task_period
,
int
task_deadline
,
std
::
shared_ptr
<
Drone
>
drone1
)
:
Serial_Port_ReadingThread
::
Serial_Port_ReadingThread
(
int
task_period
,
int
task_deadline
)
:
Abstract_ThreadClass
(
task_period
,
task_deadline
)
{
//Init object
Serial_Port_ReadingThread
::
drone1
=
drone1
;
//
Serial_Port_ReadingThread::drone1 = drone1;
//Init dictionnary
//mavlink_dic
...
...
@@ -42,7 +41,7 @@ Serial_Port_ReadingThread::Serial_Port_ReadingThread(int task_period, int task_d
//blc_global_position_int = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.global_position_int", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));
// blc_position_target_local_ned = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.position_target_local_ned", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));
// blc_position_target_global_int = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.position_target_global_int", BLC_CHANNEL_WRITE, 'IN16', 'NDEF', 1, 6));
blc_highres_imu
=
std
::
shared_ptr
<
blc_channel
>
(
new
blc_channel
(
"/pixhawk.sensors.highres_imu"
,
BLC_CHANNEL_WRITE
,
'
FL32
'
,
'
NDEF
'
,
1
,
9
));
//
blc_highres_imu = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.highres_imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9));
// blc_attitude = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.sensors.attitude", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9));