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
...@@ -92,6 +92,8 @@ link_libraries( ...@@ -92,6 +92,8 @@ link_libraries(
ncurses ncurses
) )
###################################################################################################### ######################################################################################################
### APPLICATION ## ### APPLICATION ##
###################################################################################################### ######################################################################################################
......
...@@ -12,13 +12,23 @@ ...@@ -12,13 +12,23 @@
#include "include/PixhawkServer.h" #include "include/PixhawkServer.h"
#include <iostream> #include <iostream>
#include <string> #include <string>
#include <iostream>
#include <fstream>
using namespace std; using namespace std;
string device_path = "/dev/ttyUSB0"; char device_path[255] = "/dev/ttyUSB0";
int device_baudrate = 57600; 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_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); 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 ...@@ -40,46 +50,96 @@ blc_channel blc_test_detection("/pixhawk.test.detection", BLC_CHANNEL_READ, 'IN1
void command_loop() void command_loop()
{ {
int ret;
mavlink_message_t message; mavlink_message_t message;
string buffer_command = ""; string buffer_command = "";
writingThread.get()->set_task_period(100000);
cout<<"Enter command : \n"; cout<<"Enter command : \n";
cin >> buffer_command; cin >> buffer_command;
if(buffer_command=="a1") if(buffer_command=="a")
{ {
cout<<"arm \n"; cout<<"arm \n";
drone1.command_arm(1); drone1.command_arm(1);
} }
else if(buffer_command=="!a1") else if(buffer_command=="!a")
{ {
drone1.command_arm(0); 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"; cout<<"kill \n";
drone1.command_kill(1); drone1.command_kill(1);
} }
else if(buffer_command=="!k1") else if(buffer_command=="!k")
{ {
drone1.command_kill(0); drone1.command_kill(0);
} }
else if(buffer_command=="m1") else if(buffer_command=="z")
{ {
cout<<"motor 1 \n"; cout<<"motor 1 \n";
drone1.command_directControl(1000,0,1000,0);
drone1.command_directControl(1000,0,0,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); drone1.command_directControl(0,0,0,1000);
} }
...@@ -140,23 +200,32 @@ void command_loop() ...@@ -140,23 +200,32 @@ void command_loop()
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
// General var // 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=""; string buffer1="";
// General object
shared_ptr<Serial_Port_ReadingThread> readingThread;
shared_ptr<Serial_Port_WritingThread> writingThread;
// shared_ptr<Display_IHM> ihm; // shared_ptr<Display_IHM> ihm;
// drone1 = shared_ptr<Drone>(new Drone(device_path, device_baudrate)); // drone1 = shared_ptr<Drone>(new Drone(device_path, device_baudrate));
// drone1 = 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(1000000, 200)); //beug ne marche pas
writingThread = shared_ptr<Serial_Port_WritingThread>(new Serial_Port_WritingThread(500, 200));
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<<"Init communicationn"<<endl;
cout<<"-> Open "+device_path<<endl; cout<<"-> Open " <<device_path<<endl;
if(drone1.init_communication()==0) if(drone1.init_communication()==0)
{ {
cout<<"-> Succes"<<endl; cout<<"-> Succes"<<endl;
...@@ -184,8 +253,9 @@ int main(int argc, char *argv[]) ...@@ -184,8 +253,9 @@ int main(int argc, char *argv[])
//Display_IHM::getInstanceOf().displayDroneState(drone1); //Display_IHM::getInstanceOf().displayDroneState(drone1);
//Display_IHM::getInstanceOf().displayMotorsState(drone1); //Display_IHM::getInstanceOf().displayMotorsState(drone1);
//Display_IHM::getInstanceOf().printLog("Begin"); //Display_IHM::getInstanceOf().printLog("Begin");
readingThread.get()->start();
readingThread.get()->start();
writingThread.get()->start(); writingThread.get()->start();
sleep(1); sleep(1);
...@@ -203,10 +273,12 @@ int main(int argc, char *argv[]) ...@@ -203,10 +273,12 @@ int main(int argc, char *argv[])
// readingThread.get()->stop(); // readingThread.get()->stop();
// writingThread.get()->stop(); // writingThread.get()->stop();
while(true) while(true)
{ {
command_loop(); command_loop();
} }
MyFile.close();
MyFilew.close();
exit(0); exit(0);
} }
...@@ -43,8 +43,8 @@ enum LifeCoreState { ...@@ -43,8 +43,8 @@ enum LifeCoreState {
*/ */
class Abstract_ThreadClass { class Abstract_ThreadClass {
protected:
public :
// REAL TIME VAR // REAL TIME VAR
/** /**
* timeval that save the begin of the running method * timeval that save the begin of the running method
...@@ -92,7 +92,7 @@ protected: ...@@ -92,7 +92,7 @@ protected:
*/ */
LifeCoreState currentState=LifeCoreState::TO_INIT;; LifeCoreState currentState=LifeCoreState::TO_INIT;;
public :
/** /**
* Default constructor : take in parameters the task period time and the task deadline time * Default constructor : take in parameters the task period time and the task deadline time
......
...@@ -17,6 +17,12 @@ ...@@ -17,6 +17,12 @@
#include <mutex> #include <mutex>
#include <sys/time.h> #include <sys/time.h>
#include "Abstract_ThreadClass.h" #include "Abstract_ThreadClass.h"
#include "Data_Drone.h" #include "Data_Drone.h"
...@@ -47,6 +53,8 @@ public: ...@@ -47,6 +53,8 @@ public:
Serial_Port_ReadingThread(int task_period, int task_deadline); Serial_Port_ReadingThread(int task_period, int task_deadline);
~Serial_Port_ReadingThread(); ~Serial_Port_ReadingThread();
int task_period = 1000;
void run(); void run();
uint64_t get_time_usec(); uint64_t get_time_usec();
......
...@@ -35,6 +35,8 @@ protected: ...@@ -35,6 +35,8 @@ protected:
public: public:
int task_period = 100000;
/** /**
* Default constructor of the class. Need to have the * Default constructor of the class. Need to have the
*/ */
...@@ -42,6 +44,8 @@ public: ...@@ -42,6 +44,8 @@ public:
~Serial_Port_WritingThread(); ~Serial_Port_WritingThread();
void run(); void run();
void set_task_period(int period);
/** /**
* Direct writing method command * Direct writing method command
......
...@@ -109,6 +109,9 @@ public: ...@@ -109,6 +109,9 @@ public:
mavlink_local_position_ned_t local_position_ned; mavlink_local_position_ned_t local_position_ned;
mavlink_position_target_local_ned_t position_target_local_ned; mavlink_position_target_local_ned_t position_target_local_ned;
mavlink_gps_raw_int_t gps_raw_int;
int departure_alt;
public: public:
...@@ -122,6 +125,8 @@ public: ...@@ -122,6 +125,8 @@ public:
Drone(std::string serialPort_path, int serialPort_baudrate); Drone(std::string serialPort_path, int serialPort_baudrate);
~Drone(); ~Drone();
void open(char *serialPort_path, int serialPort_baudrate);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMUNICATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMUNICATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
...@@ -162,18 +167,30 @@ public: ...@@ -162,18 +167,30 @@ public:
int command_setMode(Drone_mode mode); int command_setMode(Drone_mode mode);
int command_arm(int param1); int command_arm(int param1);
//int command_float(int param1);
int command_kill(int param1); int command_kill(int param1);
int command_setModeGuided(float param1); int command_setModeGuided(float param1);
int command_directControl(float x, float y, float z, float r); 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(); mavlink_command_long_t mavlink_newCommand();
}; };
......
...@@ -3,18 +3,28 @@ ...@@ -3,18 +3,28 @@
* @date 18/04/19. * @date 18/04/19.
*/ */
#include <iostream>
#include <string>
#include <cstdlib>
using namespace std;
#include <sys/time.h> #include <sys/time.h>
#include "../include/Abstract_ThreadClass.h" #include "../include/Abstract_ThreadClass.h"
//%%%%%%%%%%%%%%%%%%%%%%%%% begin/end phase function %%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%% 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_period=task_period;
task_deadline=task_deadline; task_deadline=task_deadline;
cout << "task_period = " << task_period <<endl;
} }
Abstract_ThreadClass::~Abstract_ThreadClass(){ Abstract_ThreadClass::~Abstract_ThreadClass()
{
currentState = LifeCoreState::QUIT; currentState = LifeCoreState::QUIT;
...@@ -25,7 +35,8 @@ Abstract_ThreadClass::~Abstract_ThreadClass(){ ...@@ -25,7 +35,8 @@ Abstract_ThreadClass::~Abstract_ThreadClass(){
//%%%%%%%%%%%%%%%%%%%%%%%%% run function %%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%% run function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Abstract_ThreadClass::run(){ void Abstract_ThreadClass::run()
{
long long currentThreadDelay; long long currentThreadDelay;
...@@ -34,79 +45,94 @@ void Abstract_ThreadClass::run(){ ...@@ -34,79 +45,94 @@ void Abstract_ThreadClass::run(){
currentState = LifeCoreState::RUN; currentState = LifeCoreState::RUN;
while(isRunFlag()){ while(isRunFlag())
{
usleep(task_period); usleep(task_period);
cout<<"abstract class thread : run() "<<endl;
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 )
{
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;
} //}
} }
} }
} }
//%%%%%%%%%%%%%%%%%%%%%%%%% control function %%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%% control function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Abstract_ThreadClass::init(){ void Abstract_ThreadClass::init()
{
currentState = LifeCoreState::INIT; currentState = LifeCoreState::INIT;
currentState = LifeCoreState::READY; currentState = LifeCoreState::READY;
} }
void Abstract_ThreadClass::start(){ void Abstract_ThreadClass::start()
{
setRunFlag(true); setRunFlag(true);
currentState = LifeCoreState::RUN; currentState = LifeCoreState::RUN;
principalThread= std::thread(&Abstract_ThreadClass::run, this); principalThread= std::thread(&Abstract_ThreadClass::run, this);
} }
void Abstract_ThreadClass::stop(){ void Abstract_ThreadClass::stop()
{
currentState = LifeCoreState::STOP; currentState = LifeCoreState::STOP;
setRunFlag(false); setRunFlag(false);
principalThread.join(); principalThread.join();
} }
void Abstract_ThreadClass::lazyStop(){ void Abstract_ThreadClass::lazyStop()
{
currentState = LifeCoreState::STOP; currentState = LifeCoreState::STOP;
setRunFlag(false); setRunFlag(false);
} }
void Abstract_ThreadClass::play(){ void Abstract_ThreadClass::play()
{
currentState = LifeCoreState::RUN; currentState = LifeCoreState::RUN;
setRunFlag(true); setRunFlag(true);
} }
void Abstract_ThreadClass::pause(){ void Abstract_ThreadClass::pause()
{
} }
void Abstract_ThreadClass::join(){ void Abstract_ThreadClass::join()
{
principalThread.join(); principalThread.join();
} }
//%%%%%%%%%%%%%%%%%%%%%%%%% getters and setters %%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%% getters and setters %%%%%%%%%%%%%%%%%%%%%%%%%%%%