Skip to content

GitLab

Commit 4956c927 authored by Amine AIT-GANI's avatar 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
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=="a1")
if(buffer_command=="a")
{
cout<<"arm \n";
drone1.command_arm(1);
}
else if(buffer_command=="!a1")
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=="!k1")
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,1000,0);
}
else if(buffer_command=="m3")
else if(buffer_command=="q")
{
drone1.command_directControl(0,0,1000,0);
drone1.command_directControl(-1000,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 server1"<<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
*/