Commit 61b4456d authored by KHELIFI Mustapha's avatar KHELIFI Mustapha
Browse files

Add functions du drone fy monitor

parent 539de33d
Pipeline #1400 failed with stage
in 1 minute and 22 seconds
......@@ -28,10 +28,15 @@ blc_channel blc_control_motors("/pixhawk.control.motors", BLC_CHANNEL_READ, 'FL3
//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.imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9);
//Channel use to know altitude of drone
blc_channel blc_attitude("/pixhawk.sensors.attitude", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 7);
//Channel use to know GPS values of drone
blc_channel blc_local_position_ned("/pixhawk.sensors.local_position_ned", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 6);
blc_channel blc_global_position("/pixhawk.sensors.global_position", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 3);
//Different blc channel use to publish data
blc_channel blc_heartbeat; //Hearbeat message PG: PB ????? pas d'init pour l'instant ????
blc_channel blc_test_detection("/pixhawk.test.detection", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 7);
void command_loop()
{
......@@ -149,8 +154,8 @@ int main(int argc, char *argv[])
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<<"Welcome to pixhawk server1"<<endl;
cout<<"Init communicationn"<<endl;
cout<<"-> Open "+device_path<<endl;
if(drone1.init_communication()==0)
{
......@@ -184,13 +189,13 @@ int main(int argc, char *argv[])
writingThread.get()->start();
sleep(1);
cout<<"Arm"<<endl;
drone1.command_arm(1);
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<<"test motors"<<endl;
//drone1.command_directControl(0,1000,0,0);
//sleep(1);
// cout<<"Unarm"<<endl;
// drone1.get()->command_arm(0);
......
......@@ -14,3 +14,10 @@ extern blc_channel blc_highres_imu;
//Different blc channel use to publish data
extern blc_channel blc_heartbeat; //Hearbeat message
extern blc_channel blc_attitude;
extern blc_channel blc_local_position_ned;
extern blc_channel blc_global_position;
//only for test
extern blc_channel blc_test_detection;
......@@ -12,10 +12,10 @@ Serial_Port_WritingThread::Serial_Port_WritingThread(int task_period, int task_d
// 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()
......
......@@ -12,6 +12,8 @@ using namespace std;
#include "../include/Com_SerialReadingThread.h"
#include "../include/global_variables.h"
//#define DISPLAY
......@@ -224,6 +226,15 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
//Display_IHM::getInstanceOf().printData(buffer1, 14, 1);
buffer1 = "Pos_speed: "+std::to_string(drone1.local_position_ned.vx)+" "+std::to_string(drone1.local_position_ned.vy)+" "+std::to_string(drone1.local_position_ned.vz)+"\n";
//Display_IHM::getInstanceOf().printData(buffer1, 15, 1);
blc_local_position_ned.floats[0] = drone1.local_position_ned.x;
blc_local_position_ned.floats[1] = drone1.local_position_ned.y;
blc_local_position_ned.floats[2] = drone1.local_position_ned.z;
blc_local_position_ned.floats[3] = drone1.local_position_ned.vx;
blc_local_position_ned.floats[4] = drone1.local_position_ned.vy;
blc_local_position_ned.floats[5] = drone1.local_position_ned.vz;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
......@@ -239,9 +250,14 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
/*mavlink_msg_global_position_int_decode(&message, &drone1.global_position_int);
mavlink_msg_global_position_int_decode(&message, &drone1.global_position_int);
buffer1 = "GPS: "+std::to_string(drone1.global_position_int.lat)+" "+std::to_string(drone1.global_position_int.lon)+" "+std::to_string(drone1.global_position_int.alt);
Display_IHM::getInstanceOf().printData(buffer1, 12, 1);*/
//Display_IHM::getInstanceOf().printData(buffer1, 12, 1);*/
blc_global_position.floats[0] = drone1.global_position_int.lat;
blc_global_position.floats[1] = drone1.global_position_int.lon;
blc_global_position.floats[2] = drone1.global_position_int.alt;
break;
}
......@@ -257,6 +273,22 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
buffer1 = buffer1 + " Attitude_s: "+to_string(drone1.attitude.rollspeed)+" "+to_string(drone1.attitude.pitchspeed)+" "+to_string(drone1.attitude.yawspeed)+"\n";
// Display_IHM::getInstanceOf().printData(buffer1, 15, 1);*/
cout<<"MAVLINK_MSG_ID_ATTITUDE : \n"<<buffer1<<endl;
blc_attitude.floats[0] = drone1.attitude.time_boot_ms;
blc_attitude.floats[1] = drone1.attitude.roll;
blc_attitude.floats[2] = drone1.attitude.pitch;
blc_attitude.floats[3] = drone1.attitude.yaw;
blc_attitude.floats[4] = drone1.attitude.rollspeed;
blc_attitude.floats[5] = drone1.attitude.pitchspeed;
blc_attitude.floats[6] = drone1.attitude.yawspeed;
/*blc_attitude.floats[0] = 5.24;
blc_attitude.floats[1] = 4.65;
blc_attitude.floats[2] = 0.45;
blc_attitude.floats[3] = 8.15;
blc_attitude.floats[4] = 2.59;
blc_attitude.floats[5] = 1.57;
blc_attitude.floats[6] = 1.20;*/
break;
}
......@@ -272,6 +304,9 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
Display_IHM::getInstanceOf().printData(buffer1, 18, 1);
buffer1 = "Attitude_tar_m: "+std::to_string(drone1.attitude_target.type_mask);
Display_IHM::getInstanceOf().printData(buffer1, 19, 1);*/
break;
}
......@@ -318,11 +353,11 @@ void Serial_Port_ReadingThread::handle_command_ack(mavlink_command_ack_t *ack)
case MAV_CMD_COMPONENT_ARM_DISARM:
if(testAck(ack, "Arm_desarm") == 1)
{ //if command is succeed
/*if(drone1.motors == ARM){
if(drone1.motors == ARM){
drone1.motors = UNARM;
}else if(drone1.motors == UNARM){
drone1.motors = ARM;
}*/
}
}
//motors
break;
......
......@@ -70,17 +70,17 @@ int Serial_Port_WritingThread::main_loop()
cout<<__FUNCTION__<<endl;
if(blc_control_commands.floats[0]>0.5 /*&& drone1.motors == UNARM*/)
if(blc_control_commands.floats[0]>0.5 && drone1.motors == UNARM)
{
cout<<" command_arm 1\n";
drone1.command_arm(1);
}
if(blc_control_commands.floats[1]>0.5 /*&& drone1.motors == ARM*/)
if(blc_control_commands.floats[1]>0.5 && drone1.motors == ARM)
{
cout<<" command_arm 0 \n";
drone1.command_arm(0);
}
if(1/*drone1.motors == ARM*/)
if(blc_control_motors.floats[0]>0.5 || blc_control_motors.floats[1]>0.5 || blc_control_motors.floats[2]>0.5 || blc_control_motors.floats[3]>0.5)
{
cout<<" drone1.command_directControl \n";
drone1.command_directControl(blc_control_motors.floats[1],blc_control_motors.floats[0],blc_control_motors.floats[3],blc_control_motors.floats[2]);
......@@ -99,8 +99,8 @@ int Serial_Port_WritingThread::main_loop()
break;
}*/
//*blc_control_arm
//blc_control_remote_vector
blc_control_arm;
blc_control_remote_vector;
return 0;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment