Commit 99e0cd04 authored by Amine AIT-GANI's avatar Amine AIT-GANI
Browse files

Modification des fichiers de test

parent e1b2198d
...@@ -42,10 +42,10 @@ ofstream compassy; ...@@ -42,10 +42,10 @@ ofstream compassy;
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);
//Channels use to control manually the drone //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 blc_channel blc_control_motors("/pixhawk.control.motors", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 6); //x, y, z, r
//Channels use to command the drone //Channels use to command the drone
blc_channel blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 9); // 1=on, 0=off blc_channel blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 7); // 1=on, 0=off
blc_channel blc_highres_imu("/pixhawk.sensors.imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9); blc_channel blc_highres_imu("/pixhawk.sensors.imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9);
//Channel use to know altitude of drone //Channel use to know altitude of drone
blc_channel blc_attitude("/pixhawk.sensors.attitude", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 7); blc_channel blc_attitude("/pixhawk.sensors.attitude", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 7);
...@@ -56,6 +56,7 @@ blc_channel blc_global_position("/pixhawk.sensors.global_position", BLC_CHANNEL_ ...@@ -56,6 +56,7 @@ blc_channel blc_global_position("/pixhawk.sensors.global_position", BLC_CHANNEL_
//Different blc channel use to publish data //Different blc channel use to publish data
blc_channel blc_heartbeat; //Hearbeat message PG: PB ????? pas d'init pour l'instant ???? 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); blc_channel blc_test_detection("/pixhawk.test.detection", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 7);
int j = 0;
void command_loop() void command_loop()
{ {
...@@ -72,13 +73,10 @@ void command_loop() ...@@ -72,13 +73,10 @@ void command_loop()
//drone1.make_command_msg_rate(1); //drone1.make_command_msg_rate(1);
drone1.command_setMode(DRONE_OFF);
//drone1.make_mode(1); //drone1.make_mode(1);
/*___________________________________*/ /*___________________________________*/
if(buffer_command=="a") if(buffer_command=="a")
{ {
cout<<"arm \n"; cout<<"arm \n";
...@@ -91,7 +89,37 @@ drone1.command_setMode(DRONE_OFF); ...@@ -91,7 +89,37 @@ drone1.command_setMode(DRONE_OFF);
drone1.command_arm(0); drone1.command_arm(0);
} }
else if(buffer_command=="h"){
cout<<"HOME_POSITION \n";
drone1.request_home_position(1,1,1);
}
else if(buffer_command=="e"){
cout<<"X_POSITION \n";
drone1.get_home_x(1);
}
else if(buffer_command=="o")
{
cout<<"current_consumed :"<<drone1.battery_status.current_consumed<<endl;
cout<<"=========================================="<<endl;
cout<<"energy_consumed :"<<drone1.battery_status.energy_consumed<<endl;
cout<<"=========================================="<<endl;
cout<<"temperature :"<<drone1.battery_status.temperature<<endl;
cout<<"=========================================="<<endl;
cout<<"voltages :"<<drone1.battery_status.voltages[10]<<endl;
cout<<"=========================================="<<endl;
cout<<"current_battery :"<<drone1.battery_status.current_battery<<endl;
cout<<"=========================================="<<endl;
cout<<"battery_function :"<<drone1.battery_status.battery_function<<endl;
cout<<"=========================================="<<endl;
cout<<"type :"<<drone1.battery_status.type<<endl;
cout<<"=========================================="<<endl;
cout<<"battery_remaining :"<<drone1.battery_status.battery_remaining<<endl;
cout<<"=========================================="<<endl;
cout<<"time_remaining :"<<drone1.battery_status.time_remaining<<endl;
cout<<"=========================================="<<endl;
cout<<"charge_state :"<<drone1.battery_status.charge_state<<endl;
}
else if(buffer_command=="m") else if(buffer_command=="m")
{ {
...@@ -100,6 +128,7 @@ drone1.command_setMode(DRONE_OFF); ...@@ -100,6 +128,7 @@ drone1.command_setMode(DRONE_OFF);
else if(buffer_command=="p") else if(buffer_command=="p")
{ {
drone1.go_to(); drone1.go_to();
} }
else if(buffer_command=="f") else if(buffer_command=="f")
{ {
...@@ -107,17 +136,32 @@ drone1.command_setMode(DRONE_OFF); ...@@ -107,17 +136,32 @@ drone1.command_setMode(DRONE_OFF);
cout<<"fly \n"; cout<<"fly \n";
ret = drone1.take_off(); ret = drone1.take_off();
cout<<"fly ret = "<<ret<<endl; cout<<"fly ret = "<<ret<<endl;
sleep_for(seconds(10)); cout<<"test"<<drone1.altitude.altitude_local<<endl;
//cout<<"pos"<<altitude.altitude_local<<endl;
} }
else if(buffer_command=="g")
{
cout<<"===================CORRDONNEES===================="<<endl;
cout<<"HOME_LATITUDE_DEG"<<drone1.home_position.latitude<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_LONGITUDE_DEG"<<drone1.home_position.longitude<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_ALTITUDE_DEG"<<drone1.home_position.altitude<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_X_metres"<<drone1.home_position.x<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_Y_metres"<<drone1.home_position.y<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_Z_metres"<<drone1.home_position.z<<endl;
cout<<"_______________________"<<endl;
cout<<"BOUSSOLE"<<drone1.highres_imu.xmag<<endl;
}
else if(buffer_command=="!f") else if(buffer_command=="!f")
{ {
//drone1.take(0); //drone1.take(0);
} }
else if(buffer_command=="b")
{
drone1.take_off_vtol();
}
else if(buffer_command=="i") else if(buffer_command=="i")
{ {
cout<<"return \n"; cout<<"return \n";
...@@ -146,12 +190,7 @@ drone1.command_setMode(DRONE_OFF); ...@@ -146,12 +190,7 @@ drone1.command_setMode(DRONE_OFF);
} }
*/ */
else if(buffer_command=="o")
{
cout<<"right \n";
drone1.move_to(1,0,0);
}
else if(buffer_command=="r") else if(buffer_command=="r")
{ {
cout<<"right \n"; cout<<"right \n";
...@@ -176,35 +215,36 @@ drone1.command_setMode(DRONE_OFF); ...@@ -176,35 +215,36 @@ drone1.command_setMode(DRONE_OFF);
{ {
drone1.move_drone_to(1,0,0); drone1.move_drone_to(1,0,0);
} }
/* else if(buffer_command=="n") else if(buffer_command=="y"){
{ drone1.move_to(1,1,1);
drone1.waypoint(0.1,0.1,0.1); }
} */
else if(buffer_command=="z") else if(buffer_command=="z")
{ {
cout<<"motor 1 \n"; cout<<"motor 1 \n";
drone1.command_directControl(1000,0,0,0); drone1.move_drone_to(0,0.01,0); //move forward 0.01m/s
} }
else if(buffer_command=="s") else if(buffer_command=="s")
{ {
cout<<"motor 1 \n"; cout<<"motor 1 \n";
drone1.command_directControl(-1000,0,0,0); drone1.move_drone_to(0,-0.01,0); //move backward 0.01m/s
} }
else if(buffer_command=="d") else if(buffer_command=="d")
{ {
drone1.command_directControl(0,1000,0,0); drone1.move_drone_to(0.01,0,0); //move right 0.01m/s
} }
else if(buffer_command=="q") else if(buffer_command=="q")
{ {
drone1.command_directControl(0,-1000,0,0); drone1.move_drone_to(-0.01,0,0); //move left 0.01m/s
} }
else if(buffer_command=="w") else if(buffer_command=="w")
{ {
drone1.command_directControl(0,0,1000,0); drone1.move_drone_to(0,0,1); //UP for 0.01m/s
} }
else if(buffer_command=="x") else if(buffer_command=="x")
{ {
drone1.command_directControl(0,0,-1000,0); drone1.move_drone_to(0,0,-0.01); //DOWN for 0.01m/s
} }
else if(buffer_command=="c") else if(buffer_command=="c")
{ {
...@@ -271,7 +311,39 @@ drone1.command_setMode(DRONE_OFF); ...@@ -271,7 +311,39 @@ drone1.command_setMode(DRONE_OFF);
// drone1.get()->write_message(message); // drone1.get()->write_message(message);
// } // }
/**
* @fn void signalHandler(int number)
* @brief Handles received signals. Triggered when a signal is received.
*
* @param number The signal code.
**/
void signalHandler(int number)
{
switch(number)
{
case SIGINT :
printf("SIGINT caught\n"); //CTRL+C
exit(0);
//TODO handle SIGINT reception situation
break;
case SIGTERM :
printf("SIGTERM caught\n"); //kill (term command)
//TODO handle SIGTERM reception situation
exit(0);
break;
case SIGFPE :
printf("SIGFPE caught\n"); //Floating-Point arithmetic Exception
//TODO handle SIGFPE reception situation
exit(0); //to prevent infinite loop
break;
default :
printf("Signal number : %d caught\n", number);
}
}
/** /**
* Only the main exe of the programm * Only the main exe of the programm
...@@ -282,8 +354,18 @@ int main(int argc, char *argv[]) ...@@ -282,8 +354,18 @@ 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
struct sigaction action;
if(argc>1) {strcpy(device_path ,argv[1]); printf("port = %s \n",device_path);} if(argc>1) {strcpy(device_path ,argv[1]); printf("port = %s \n",device_path);}
//signal handler initialisation
sigfillset(&action.sa_mask);
action.sa_handler = signalHandler;
action.sa_flags = 0;
//Make the list of all SIG to catch (will not be caught if not listed here, the list is not definitive and all are not necessarily usefull)
if(sigaction(SIGINT, &action, NULL) != 0) printf("SIGINT couldn't be attached\n"); //ctrl+C
if(sigaction(SIGTERM, &action, NULL) != 0) printf("SIGTERM couldn't be attached\n"); //kill [PID]
if(sigaction(SIGFPE, &action, NULL) != 0) printf("SIGFPE couldn't be attached\n"); //integer divided by 0
drone1.open(device_path, device_baudrate); drone1.open(device_path, device_baudrate);
string buffer1=""; string buffer1="";
......
...@@ -161,6 +161,7 @@ public: ...@@ -161,6 +161,7 @@ public:
mavlink_highres_imu_t highres_imu; //Accelerometers of the system mavlink_highres_imu_t highres_imu; //Accelerometers of the system
mavlink_altitude_t altitude; //Altitude data mavlink_altitude_t altitude; //Altitude data
mavlink_home_position_t home_position;
mavlink_attitude_t attitude; //Attitude of the drone use to manual control and target attitude mavlink_attitude_t attitude; //Attitude of the drone use to manual control and target attitude
mavlink_attitude_target_t attitude_target; //Attitude of the drone use to manual control and target attitude mavlink_attitude_target_t attitude_target; //Attitude of the drone use to manual control and target attitude
...@@ -238,6 +239,7 @@ public: ...@@ -238,6 +239,7 @@ public:
// int command_setModeGuided(float param1); // int command_setModeGuided(float param1);
int make_command_msg_rate(int param1); int make_command_msg_rate(int param1);
int command_directControl(float x, float y, float z, float r); int command_directControl(float x, float y, float z, float r);
int move_drone_to(float x, float y, float z);
int return_to_launch(int param1); int return_to_launch(int param1);
int take(int param1) ; int take(int param1) ;
int take_off(); int take_off();
...@@ -245,14 +247,20 @@ public: ...@@ -245,14 +247,20 @@ public:
int command_right(float param1) ; int command_right(float param1) ;
int command_left(float param1) ; int command_left(float param1) ;
int go_to(); int go_to();
int waypointrl(float param5); int waypoint(float param5, float param6, float param7);
int battery_check(float x);
int waypointforback(float param6); int waypointforback(float param6);
int waypointupdown(float param7); int waypointupdown(float param7);
int take_off_vtol(); int take_off_vtol();
int followhold(); int followhold();
int make_mode(int param1); int make_mode(int param1);
int get_home_x(float x);
int request_home_position(float x, float y, float z);
void send_setpoint_velocity(float vx, float vy, float vz);
int move_to(float param1, float param2, float param3); int move_to(float param1, float param2, float param3);
......
...@@ -358,7 +358,7 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -358,7 +358,7 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
printf("Type: %d ", hb.type); printf("Type: %d ", hb.type);
printf("Autopilot type: %d ", hb.autopilot); printf("Autopilot type: %d ", hb.autopilot);
printf("System mode: %d ", hb.base_mode); printf("System mode: %d ", hb.base_mode);
printf("Custom mode: %d ", hb.custom_mode); // printf("Custom mode: %ald ", hb.custom_mode);
printf("System status: %d ", hb.system_status); printf("System status: %d ", hb.system_status);
printf("Mavlink version: %d \n", hb.mavlink_version); printf("Mavlink version: %d \n", hb.mavlink_version);
...@@ -625,7 +625,16 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message) ...@@ -625,7 +625,16 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
printf("altitude_local %f altitude_relative %f altitude_terrain %f bottom_clearance %f\n",altitude->altitude_local , altitude->altitude_relative, altitude->altitude_terrain,altitude->bottom_clearance ); printf("altitude_local %f altitude_relative %f altitude_terrain %f bottom_clearance %f\n",altitude->altitude_local , altitude->altitude_relative, altitude->altitude_terrain,altitude->bottom_clearance );
break; break;
} }
/* case MAVLINK_MSG_ID_HOME_POSITION:
{
printf("MAVLINK_MSG_ID_HOME_POSITION \n");
//mavlink_home_position_t* home_position;
mavlink_msg_home_position_decode(&message, &drone1.home_position);
//home_position= &drone1.home_position;
printf("latitude %d, longitude %d, altitude %d \n", drone1.home_position.latitude, drone1.home_position.longitude, drone1.home_position.altitude);
break;
} */
case MAVLINK_MSG_ID_VIBRATION: case MAVLINK_MSG_ID_VIBRATION:
{ {
// printf("MAVLINK_MSG_ID_VIBRATION ???\n"); // printf("MAVLINK_MSG_ID_VIBRATION ???\n");
......
...@@ -74,6 +74,7 @@ void Serial_Port_WritingThread::run() ...@@ -74,6 +74,7 @@ void Serial_Port_WritingThread::run()
} }
} }
int i = 0;
int Serial_Port_WritingThread::main_loop() int Serial_Port_WritingThread::main_loop()
{ {
...@@ -81,10 +82,9 @@ int Serial_Port_WritingThread::main_loop() ...@@ -81,10 +82,9 @@ int Serial_Port_WritingThread::main_loop()
//cout<<"__FUNCTION__"<<endl; //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)
{ {
drone1.command_setMode(DRONE_OFF); //drone1.command_setMode(DRONE_OFF);
cout<<" command_arm 1\n"; cout<<" command_arm 1\n";
drone1.command_arm(1); drone1.command_arm(1);
drone1.motors = ARM; drone1.motors = ARM;
...@@ -100,6 +100,8 @@ int Serial_Port_WritingThread::main_loop() ...@@ -100,6 +100,8 @@ int Serial_Port_WritingThread::main_loop()
{ {
cout<<" takeoff 0 \n"; cout<<" takeoff 0 \n";
drone1.take_off(); drone1.take_off();
} }
if(blc_control_commands.floats[3]>0.5 && drone1.motors == ARM) if(blc_control_commands.floats[3]>0.5 && drone1.motors == ARM)
...@@ -107,51 +109,71 @@ int Serial_Port_WritingThread::main_loop() ...@@ -107,51 +109,71 @@ int Serial_Port_WritingThread::main_loop()
cout<<" landing 0 \n"; cout<<" landing 0 \n";
drone1.landing(); drone1.landing();
} }
if(blc_control_commands.floats[4]>0.5 && drone1.motors == ARM)
if(blc_control_commands.floats[4]>0.5 && drone1.motors == ARM)
{ {
cout<<" Return to llaunch0 \n"; cout<<" Return to llaunch0 \n";
drone1.return_to_launch(1); drone1.return_to_launch(1);
} }
if(blc_control_commands.floats[5]>0.5 && drone1.motors == ARM)
{
cout <<"UPPPPP 1\n";
drone1.waypoint(0,0,5);
}
if(blc_control_commands.floats[6]>0.5 && drone1.motors == ARM)
{
cout <<"DOWNNN 1\n";
drone1.waypoint(0,0,3);
}
if(blc_control_motors.floats[0]>0.5 && drone1.motors == ARM) if(blc_control_motors.floats[0]>0.5 && drone1.motors == ARM)
{ {
cout <<"move_to _right 1\n"; cout <<"move_to _right 1\n";
drone1.command_directControl(0,1000,0,0); drone1.move_drone_to(0.5,0,0); //move right 0.5m/s
} }
if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM) if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM)
{ {
cout <<"move_to left 1\n"; cout <<"move_to left 1\n";
drone1.command_directControl(0,-1000,0,0); drone1.move_drone_to(-0.5,0,0); //move left 0.5m/s
} }
/*
if(blc_control_motors.floats[2]>0.5 && drone1.motors == ARM) if(blc_control_motors.floats[2]>0.5 && drone1.motors == ARM)
{ {
cout <<"move_forward 1\n"; cout <<"move_forward 1\n";
drone1.command_directControl(1000,0,0,0); drone1.move_drone_to(0,0.5,0);
} }
if(blc_control_motors.floats[3]>0.5 && drone1.motors == ARM) if(blc_control_motors.floats[3]>0.5 && drone1.motors == ARM)
{ {
cout <<"move_backward 1\n"; cout <<"move_backward 1\n";
drone1.command_directControl(-1000,0,0,0); drone1.move_drone_to(0,-0.5,0);
} */ }
/*
if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM) if(blc_control_motors.floats[4]>0.5 && drone1.motors == ARM)
{
cout <<"TURN RIGHT 1\n";
drone1.command_directControl(0,0,0,1000);
}
if(blc_control_motors.floats[5]<0.5 && drone1.motors == ARM)
{
cout <<"TURN LEFT 1\n";
drone1.command_directControl(0,0,0,-1000);
}
/* if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM)
{ {
cout <<"forward 1\n"; cout <<"UP 1\n";
float f =0.01; drone1.command_directControl(0,0,0,1000);
f = f + 0.01;
drone1.waypointforback(f);
} }
if(blc_control_motors.floats[3]<0.5 && drone1.motors == ARM) if(blc_control_commands.floats[8]<0.5 && drone1.motors == ARM)
{ {
cout <<"backward 1\n"; cout <<"DOWN 1\n";
float b = -0.01; drone1.command_directControl(0,0,0,-1000);
b = b - 0.01;
drone1.waypointforback(b);
} */ } */
/* /*
if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM) if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM)
......
...@@ -6,6 +6,8 @@ ...@@ -6,6 +6,8 @@
* @version 1.0 * @version 1.0
*/ */
#include <chrono>
#include <ctime>
#include <iostream> #include <iostream>
#include <string> #include <string>
...@@ -82,8 +84,24 @@ int Drone::init_communication() ...@@ -82,8 +84,24 @@ int Drone::init_communication()
return 0; return 0;
} }
/*
void Drone::send_setpoint_velocity(float vx, float vy, float vz) {
/* Documentation start from bit 1 instead 0,
* but implementation PX4 Firmware #1151 starts from 0
*/
// uint16_t ignore_all_except_v_xyz = (7<<6)|(7<<0);
// ENU->NED. Issue #49.
/* set_position_target_local_ned(std::chrono::system_clock::now(),
MAV_FRAME_LOCAL_NED,
ignore_all_except_v_xyz,
0.0, 0.0, 0.0,
vy, vx, -vz,
0.0, 0.0, 0.0);
}