Skip to content
GitLab
Commit c923962e authored by Philippe Gaussier's avatar Philippe Gaussier
Browse files

Bu correction and code simplification. 

drone1 is now a global object.
Same thing for the blaar variables.

PG.
parent b7d14923
Hide whitespace changes
Inline Side-by-side
Com_server/test/TestDroneRemoteControl.cpp
View file @ c923962e
...@@ -15,137 +15,83 @@ ...@@ -15,137 +15,83 @@
using namespace std; using namespace std;
shared_ptr<Drone> drone1; string device_path = "/dev/ttyUSB0";
int device_baudrate = 57600;
Drone drone1(device_path, device_baudrate);
/** blc_channel blc_control_arm("/pixhawk.control.arm", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 1); // 1=on, 0=off;
* Only the main exe of the programm blc_channel blc_control_remote_vector("/pixhawk.control.remoteVectors", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 4);
* @return //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
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);
// cout<<"Unarm"<<endl; //Channels use to command the drone
// drone1.get()->command_arm(0); blc_channel blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 4); // 1=on, 0=off
// sleep(1); blc_channel blc_highres_imu("/pixhawk.sensors.highres_imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9);
// readingThread.get()->stop(); //Different blc channel use to publish data
// writingThread.get()->stop(); blc_channel blc_heartbeat; //Hearbeat message PG: PB ????? pas d'init pour l'instant ????
command_loop();
return 0;
}
void command_loop() void command_loop()
{ {
mavlink_message_t message; mavlink_message_t message;
string buffer_command = ""; string buffer_command = "";
cout<<"Enter command : \n";
cin >> buffer_command; cin >> buffer_command;
if(buffer_command=="a1") if(buffer_command=="a1")
{ {
cout<<"arm \n"; cout<<"arm \n";
drone1.get()->command_arm(1); drone1.command_arm(1);
} }
else if(buffer_command=="!a1") else if(buffer_command=="!a1")
{ {
drone1.get()->command_arm(0); drone1.command_arm(0);
} }
else if(buffer_command=="k1") else if(buffer_command=="k1")
{ {
drone1.get()->command_kill(1); cout<<"kill \n";
drone1.command_kill(1);
} }
else if(buffer_command=="!k1") else if(buffer_command=="!k1")
{ {
drone1.get()->command_kill(0); drone1.command_kill(0);
} }
else if(buffer_command=="m1") 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") 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") 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") 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") 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") else if(buffer_command=="tone")
{ {
cout<<"Tone \n";
//Display_IHM::getInstanceOf().printLog("PLAY TONE"); //Display_IHM::getInstanceOf().printLog("PLAY TONE");
mavlink_msg_play_tune_pack(255, drone1.get()->component_id, &message, 1, 1 , "AAAA", ""); mavlink_msg_play_tune_pack(255, drone1.component_id, &message, 1, 1 , "AAAA", "");
drone1.get()->write_message(message); drone1.write_message(message);
} }
else else
{ {
cout<<"command not recognized\n";
//Display_IHM::getInstanceOf().printLog("Error: what ?"); //Display_IHM::getInstanceOf().printLog("Error: what ?");
} }
...@@ -179,3 +125,83 @@ void command_loop() ...@@ -179,3 +125,83 @@ void command_loop()
// mavlink_setAttitudeTarget(&message, *drone1.get(), 0, 0, 0, 800); // mavlink_setAttitudeTarget(&message, *drone1.get(), 0, 0, 0, 800);
// drone1.get()->write_message(message); // 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 @@ ...@@ -23,8 +23,8 @@
#include "blc_channel.h" #include "blc_channel.h"
#include "blc_program.h" #include "blc_program.h"
class Serial_Port_ReadingThread : public Abstract_ThreadClass { class Serial_Port_ReadingThread : public Abstract_ThreadClass
{
protected: protected:
/** /**
* Serial port use with the drone * Serial port use with the drone
...@@ -34,21 +34,17 @@ protected: ...@@ -34,21 +34,17 @@ protected:
/** /**
* Drone buffer * Drone buffer
*/ */
std::shared_ptr<Drone> drone1; //std::shared_ptr<Drone> pt_drone;
/** /**
* Mavlink message dictionnary * Mavlink message dictionnary
*/ */
std::map<int, std::string> mavlink_dic; 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: 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(); ~Serial_Port_ReadingThread();
void run(); void run();
...@@ -61,7 +57,6 @@ public: ...@@ -61,7 +57,6 @@ public:
void read_messages(mavlink_message_t message); void read_messages(mavlink_message_t message);
int testAck(mavlink_command_ack_t *ack, std::string message); int testAck(mavlink_command_ack_t *ack, std::string message);
void handle_command_ack(mavlink_command_ack_t *ack); void handle_command_ack(mavlink_command_ack_t *ack);
}; };
#endif //VEHICLEWATCHER_Abstract_ThreadClass_H #endif //VEHICLEWATCHER_Abstract_ThreadClass_H
Com_server/test/include/Com_SerialWritingThread.h
View file @ c923962e
...@@ -21,35 +21,24 @@ ...@@ -21,35 +21,24 @@
#include "blc_channel.h" #include "blc_channel.h"
#include "blc_program.h" #include "blc_program.h"
class Serial_Port_WritingThread : public Abstract_ThreadClass { class Serial_Port_WritingThread : public Abstract_ThreadClass
{
protected: protected:
/** /**
* Drone buffer * Drone buffer
*/ */
std::shared_ptr<Drone> drone1; //std::shared_ptr<Drone> drone1;
// Different buffer use for get the data // Different buffer use for get the data
int time_stamps; 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: public:
/** /**
* Default constructor of the class. Need to have the * 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(); ~Serial_Port_WritingThread();
void run(); void run();
...@@ -58,7 +47,6 @@ public: ...@@ -58,7 +47,6 @@ public:
* Direct writing method command * Direct writing method command
*/ */
int main_loop(); int main_loop();
}; };
#endif //VEHICLEWATCHER_Abstract_ThreadClass_H #endif //VEHICLEWATCHER_Abstract_ThreadClass_H
Com_server/test/src/Cmd_ControlThread.cpp
View file @ c923962e
...@@ -4,18 +4,18 @@ ...@@ -4,18 +4,18 @@
*/ */
#include "../include/Cmd_ControlThread.h" #include "../include/Cmd_ControlThread.h"
#include "../include/global_variables.h"
Serial_Port_WritingThread::Serial_Port_WritingThread(int task_period, int task_deadline):
Serial_Port_WritingThread::Serial_Port_WritingThread(int task_period, int task_deadline, std::shared_ptr<Drone> drone1):
Abstract_ThreadClass(task_period, task_deadline) Abstract_ThreadClass(task_period, task_deadline)
{ {
Serial_Port_WritingThread::drone1 = drone1; // Serial_Port_WritingThread::drone1 = drone1;
//Channels use to arm the drone //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 //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() Serial_Port_WritingThread::~Serial_Port_WritingThread()
...@@ -23,8 +23,26 @@ 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; long long currentThreadDelay;
gettimeofday(&begin, 0); gettimeofday(&begin, 0);
...@@ -32,38 +50,27 @@ void Serial_Port_WritingThread::run(){ ...@@ -32,38 +50,27 @@ void Serial_Port_WritingThread::run(){
currentState = LifeCoreState::RUN; currentState = LifeCoreState::RUN;
while(isRunFlag()){ while(isRunFlag())
{
usleep(task_period); usleep(task_period);
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
{ //PG: strange code (check older version)
currentState = LifeCoreState::RUN; 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 @@ ...@@ -9,21 +9,25 @@
#include "../include/Com_Serial.h" #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(); initialize_defaults();
Serial_Port::uart_name = uart_name; Serial_Port::uart_name = uart_name;
Serial_Port::baudrate = baudrate; Serial_Port::baudrate = baudrate;
} }
Serial_Port::Serial_Port(){ Serial_Port::Serial_Port()
{
initialize_defaults(); initialize_defaults();
} }
Serial_Port::~Serial_Port(){ Serial_Port::~Serial_Port()
{
pthread_mutex_destroy(&lock); pthread_mutex_destroy(&lock);
} }
void Serial_Port::initialize_defaults(){ void Serial_Port::initialize_defaults()
{
// Initialize attributes // Initialize attributes
debug = false; debug = false;
fd = -1; fd = -1;
...@@ -45,8 +49,8 @@ void Serial_Port::initialize_defaults(){ ...@@ -45,8 +49,8 @@ void Serial_Port::initialize_defaults(){
/** /**
* Read from Serial * Read from Serial
*/ */
int Serial_Port::read_message(mavlink_message_t &message){ int Serial_Port::read_message(mavlink_message_t &message)
{
uint8_t cp; uint8_t cp;
mavlink_status_t status; mavlink_status_t status;
uint8_t msgReceived = false; uint8_t msgReceived = false;
...@@ -121,7 +125,8 @@ int Serial_Port::read_message(mavlink_message_t &message){ ...@@ -121,7 +125,8 @@ int Serial_Port::read_message(mavlink_message_t &message){
/** /**
* Write to Serial * Write to Serial
*/