Modification des fichiers de test

Com_server/test/TestDroneRemoteControl.cpp

@@ -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_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
+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
-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);
 	//Channel use to know altitude of drone
 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_
     //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);
+int j = 0;
 
 void command_loop()
 {
@@ -72,13 +73,10 @@ void command_loop()
 
 //drone1.make_command_msg_rate(1);
 
-drone1.command_setMode(DRONE_OFF);
-
 //drone1.make_mode(1);
 /*___________________________________*/
 
-
-
+	
     if(buffer_command=="a")
     {
 		cout<<"arm \n";
@@ -91,7 +89,37 @@ drone1.command_setMode(DRONE_OFF);
         
         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")
     {
         
@@ -100,6 +128,7 @@ drone1.command_setMode(DRONE_OFF);
     else if(buffer_command=="p")
     {
 		drone1.go_to();
+		
 	}
     else if(buffer_command=="f")
     {
@@ -107,17 +136,32 @@ drone1.command_setMode(DRONE_OFF);
        cout<<"fly \n";
    ret = drone1.take_off();
       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")
     {
         //drone1.take(0);
     }
-    else if(buffer_command=="b")
-    {
-        drone1.take_off_vtol();
-    }
+
     else  if(buffer_command=="i")
     { 
        cout<<"return \n";
@@ -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")
     {
        cout<<"right \n";
@@ -176,35 +215,36 @@ drone1.command_setMode(DRONE_OFF);
     {
         drone1.move_drone_to(1,0,0);
     }
-/*	else if(buffer_command=="n")
-	{
-		drone1.waypoint(0.1,0.1,0.1);
-	} */
+    else if(buffer_command=="y"){
+		drone1.move_to(1,1,1);
+	}
+	
+	
     else if(buffer_command=="z")
     {
         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")
     {
         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")
     {
-        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")
     {
-        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")
     {
-        drone1.command_directControl(0,0,1000,0);
+        drone1.move_drone_to(0,0,1);  //UP for 0.01m/s
     }
     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")
     {
@@ -271,7 +311,39 @@ drone1.command_setMode(DRONE_OFF);
 //     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
@@ -282,8 +354,18 @@ int main(int argc, char *argv[])
     // General var
   //  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);}
-   
+  
+   //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);
 
     string buffer1="";

Com_server/test/include/Data_Drone.h

@@ -161,6 +161,7 @@ public:
 
     mavlink_highres_imu_t highres_imu; //Accelerometers of the system
 	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_target_t attitude_target; //Attitude of the drone use to manual control and target attitude
@@ -238,6 +239,7 @@ public:
 //	int command_setModeGuided(float param1);
 	int make_command_msg_rate(int param1);
 	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 take(int param1) ;
 	int take_off();
@@ -245,14 +247,20 @@ public:
 	int command_right(float param1) ;
 	int command_left(float param1) ;
 	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 waypointupdown(float param7);
 	int take_off_vtol();
 
 	int followhold();
 	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);
 

Com_server/test/src/Com_SerialReadingThread.cpp

@@ -358,7 +358,7 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
             printf("Type:  %d  ", hb.type);
             printf("Autopilot type: %d ", hb.autopilot);
             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("Mavlink version: %d \n", hb.mavlink_version);
                 
@@ -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 );    
 			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:
         {
       //     printf("MAVLINK_MSG_ID_VIBRATION ???\n");

Com_server/test/src/Com_SerialWritingThread.cpp

@@ -74,6 +74,7 @@ void Serial_Port_WritingThread::run()
         
     }
 }
+	int i = 0;
 
 int Serial_Port_WritingThread::main_loop()
 {
@@ -81,10 +82,9 @@ int Serial_Port_WritingThread::main_loop()
    
     //cout<<"__FUNCTION__"<<endl;
 
-
     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";
         drone1.command_arm(1);
         drone1.motors = ARM;
@@ -100,6 +100,8 @@ int Serial_Port_WritingThread::main_loop()
     {
         cout<<" takeoff 0 \n";
         drone1.take_off();
+        
+      
     }
     
        if(blc_control_commands.floats[3]>0.5 && drone1.motors == ARM)
@@ -107,51 +109,71 @@ int Serial_Port_WritingThread::main_loop()
         cout<<" landing 0 \n";
         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";
         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)
     {
+		
 		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)
     {	
 		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)
     {	
 		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)
     {	
 		cout <<"move_backward 1\n";
-		drone1.command_directControl(-1000,0,0,0);
-	} */
-/*	
-	if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM)
+		drone1.move_drone_to(0,-0.5,0);
+	} 
+	
+	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";
-		float f =0.01;
-		f = f + 0.01;
-		drone1.waypointforback(f);  
+		cout <<"UP 1\n";
+		drone1.command_directControl(0,0,0,1000); 
 	}
-	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";
-		float b = -0.01;
-		b = b - 0.01;
-		drone1.waypointforback(b);  
+		cout <<"DOWN 1\n";
+		drone1.command_directControl(0,0,0,-1000); 
 	} */
 /*	
 	if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM)

Com_server/test/src/Data_Drone.cpp

@@ -6,6 +6,8 @@
  * @version 1.0
  */
 
+#include <chrono>
+#include <ctime>    
 
 #include <iostream>
 #include <string>
@@ -82,8 +84,24 @@ int Drone::init_communication()
 
     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);
+	} 
+