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 @@
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> drone1;
//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));
}
......@@ -52,8 +51,8 @@ Serial_Port_ReadingThread::~Serial_Port_ReadingThread()