Mise a jour des fonctions de test

Com_server/test/src/Com_SerialReadingThread.cpp

@@ -10,7 +10,7 @@
 #include <chrono>
 #include <iostream>
 #include <fstream>	
-
+#include <cmath>
 
 #include <sys/time.h>
 
@@ -22,6 +22,9 @@ using namespace std;
 #include "../include/global_variables.h"
 
 extern ofstream MyFile;
+extern ofstream compassx;
+extern ofstream compassy;
+
 
 
 //#define DISPLAY
@@ -433,10 +436,44 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
             blc_highres_imu.floats[7] = drone1.highres_imu.ymag;
             blc_highres_imu.floats[8] = drone1.highres_imu.zmag;
             
-       //     cout<<"Com_SerialReadingThread "<<buffer1<<endl;
+            double xmag = drone1.highres_imu.xmag;
+            double ymag = drone1.highres_imu.ymag;
+            double angle;
+            if ( xmag>=-0.7 && xmag<=0.7 ) {
+				angle = acos(xmag);
+				cout << "acos(xmag) = " << angle << " radians" << endl;
+				cout << "acos(x) = " << angle*180/3.1415 << " degrees" << endl;
+			}
+			else if ( ymag>=-0.7 && ymag<=0.7 )
+			{	
+				angle = acos(ymag);
+				cout << "acos(ymag) = " << angle << " radians" << endl;
+				cout << "acos(x) = " << angle*180/3.1415 << " degrees" << endl;
+			}
+				
+			
+            cout << "xmag : " << drone1.highres_imu.xmag << "ymag :" << drone1.highres_imu.ymag << "zmag :" << drone1.highres_imu.zmag << endl;
+            compassx << drone1.highres_imu.xmag << endl;
+			compassy << drone1.highres_imu.ymag << endl;
+			cout << "buffer 1  mag" << buffer1 << endl;
+
+            cout<<"Com_SerialReadingThread "<<buffer1<<endl;
             break;
         }
+        
+  /*    case MAVLINK_MSG_ID_COMPASSMOT_STATUS:
+        
+			mavlink_compassmot_status_decode(&message, &drone1.compassmot_status);
 
+            buffer1 = buffer1+" CompensationX: "+std::to_string(drone1.compassmot_status.CompensationX)+" CompensationY:"+std::to_string(drone1.compassmot_status.CompensationY)+" CompensationZ :"+std::to_string(drone1.compassmot_status.CompensationZ)+"\n";
+          
+            blc_compassmot_status.floats[0] = drone1.compassmot_status.CompensationX;
+            blc_compassmot_status.floats[1] = drone1.compassmot_status.CompensationY;
+            blc_compassmot_status.floats[2] = drone1.compassmot_status.CompensationZ;
+      
+       //     cout<<"Com_SerialReadingThread "<<buffer1<<endl;
+            break;
+		*/
 		case MAVLINK_MSG_ID_GPS_RAW_INT:
 		{
 		//	cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << endl;

Com_server/test/src/Com_SerialWritingThread.cpp

@@ -81,9 +81,11 @@ int Serial_Port_WritingThread::main_loop()
    
     //cout<<"__FUNCTION__"<<endl;
 
+
     if(blc_control_commands.floats[0]>0.5 && drone1.motors == UNARM)
-    {		 
-       cout<<" command_arm 1\n";
+    {
+		drone1.command_setMode(DRONE_OFF);		 
+		cout<<" command_arm 1\n";
         drone1.command_arm(1);
         drone1.motors = ARM;
     }
@@ -100,8 +102,76 @@ int Serial_Port_WritingThread::main_loop()
         drone1.take_off();
     }
     
+       if(blc_control_commands.floats[3]>0.5 && drone1.motors == ARM)
+    {
+        cout<<" landing 0 \n";
+        drone1.landing();
+    }
     
+        if(blc_control_commands.floats[4]>0.5 && drone1.motors == ARM)
+    {
+        cout<<" Return to llaunch0 \n";
+        drone1.return_to_launch(1);
+    }
     
+	if(blc_control_motors.floats[0]>0.5 && drone1.motors == ARM)
+    {
+		cout <<"move_to	_right 1\n";
+		drone1.command_directControl(0,1000,0,0);
+	}
+	
+	if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM)
+    {	
+		cout <<"move_to	left 1\n";
+		drone1.command_directControl(0,-1000,0,0);
+	}
+/*	
+		if(blc_control_motors.floats[2]>0.5 && drone1.motors == ARM)
+    {	
+		cout <<"move_forward 1\n";
+		drone1.command_directControl(1000,0,0,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)
+    {
+		cout <<"forward 1\n";
+		float f =0.01;
+		f = f + 0.01;
+		drone1.waypointforback(f);  
+	}
+	if(blc_control_motors.floats[3]<0.5 && drone1.motors == ARM)
+    {
+		cout <<"backward 1\n";
+		float b = -0.01;
+		b = b - 0.01;
+		drone1.waypointforback(b);  
+	} */
+/*	
+	if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM)
+    {
+		cout <<"up 1\n";
+		float u = 0.01;
+		u = u + 0.01;
+		drone1.waypointupdown(u);  
+	}
+	
+	if(blc_control_commands.floats[8]>0.5 && drone1.motors == ARM)
+    {
+		cout <<"down 1\n";
+		float d =0.01;
+		d = d -0.01;
+		drone1.waypointupdown(d);  
+	}
+	
+	*/
+	
 	//drone1.return_to_launch(1);  // retour au position initiale 
 	
  /*   float offset=0.05;

Com_server/test/src/Data_Drone.cpp

@@ -136,7 +136,7 @@ int Drone::command_arm(int param1)
     command.command = MAV_CMD_COMPONENT_ARM_DISARM;
     command.confirmation = false;
     command.param1           = param1;
-	command.param2           = 21196;
+	command.param2           = 21196;     //21196;  Force Arm/disarm  0 // ????
 
     mavlink_msg_command_long_encode(255, 1, &message, &command);
 
@@ -152,6 +152,155 @@ int Drone::command_arm(int param1)
 }
 
 
+int Drone::make_mode(int param1)
+{
+    //Message buffer
+	mavlink_message_t message;
+    mavlink_command_long_t command;
+    command=mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_DO_SET_MODE;
+ //   command.confirmation = false;
+    command.param1           = 193;
+	command.param2           = 1;     //21196;   // ????
+	command.param3 = 0;
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+
+    if(param1 >0)
+    {
+        //Display_IHM::getInstanceOf().printLog("ARMING");
+    }else
+    {
+        //Display_IHM::getInstanceOf().printLog("STOP ARMING");
+    }
+
+    return write_message(message);
+}
+
+
+
+int Drone::make_command_msg_rate(int param1)
+{	
+	
+	float interval_us = 0.0f;
+
+
+        
+  
+    //Message buffer
+	mavlink_message_t message;
+    mavlink_command_long_t command;
+    command=mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
+    command.param1           = 1; //rate
+	command.param2           = 1; //intervall en us
+
+
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+
+    if(param1 >0)
+    {
+        //Display_IHM::getInstanceOf().printLog("ARMING");
+    }else
+    {
+        //Display_IHM::getInstanceOf().printLog("STOP ARMING");
+    }
+
+    return write_message(message);
+}
+
+
+
+int Drone::waypointrl(float param5)
+{
+    //Message buffer
+	mavlink_message_t message;
+    mavlink_command_long_t command;
+    command=mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_NAV_WAYPOINT;
+    command.confirmation = false;
+    command.param1           = 0;  //Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) min : 0 (seconds)
+	command.param2           = 0;	//Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)  min 0 (m)
+	command.param3           = 0;	//0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. (m)
+	command.param4           = 0;	//Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). (deg)
+	command.param5           = 0;	//Latitude (m)
+//	command.param6           = 0;	//Longitude
+//	command.param7           = 0;	//Altitude
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+    return write_message(message);
+}
+
+int Drone::waypointforback(float param6)
+{
+    //Message buffer
+	mavlink_message_t message;
+    mavlink_command_long_t command;
+    command=mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_NAV_WAYPOINT;
+    command.confirmation = false;
+    command.param1           = 0;  //Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) min : 0 (seconds)
+	command.param2           = 0;	//Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)  min 0 (m)
+	command.param3           = 0;	//0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. (m)
+	command.param4           = 0;	//Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). (deg)
+//	command.param5           = 0;	//Latitude (m)
+	command.param6           = 0;	//Longitude
+//	command.param7           = 0;	//Altitude
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+    return write_message(message);
+}
+
+int Drone::waypointupdown(float param7)
+{
+    //Message buffer
+	mavlink_message_t message;
+    mavlink_command_long_t command;
+    command=mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_NAV_WAYPOINT;
+    command.confirmation = false;
+    command.param1           = 0;  //Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) min : 0 (seconds)
+	command.param2           = 0;	//Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)  min 0 (m)
+	command.param3           = 0;	//0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. (m)
+	command.param4           = 0;	//Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). (deg)
+//	command.param5           = 0;	//Latitude (m)
+//	command.param6           = 0;	//Longitude
+	command.param7           = 0;	//Altitude
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+    return write_message(message);
+}
+
+int Drone::go_to()
+{
+    //Message buffer
+	mavlink_message_t message;
+    mavlink_command_long_t command;
+    command=mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_DO_REPOSITION;
+    command.confirmation = false;
+    command.param1           = -1;
+	command.param4           = NAN;
+	command.param5           = 0;
+	command.param6           = 0;
+	command.param7           = 1;
+
+
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+
+   
+    return write_message(message);
+}
+
+
 
 int Drone::return_to_launch(int param1)
 {
@@ -162,8 +311,7 @@ int Drone::return_to_launch(int param1)
     command.target_system = 1;
     command.target_component = 1;
     command.command = MAV_CMD_NAV_RETURN_TO_LAUNCH;
-    command.confirmation = false;
-    command.param1           = param1;
+  //  command.param1           = param1;
 
     mavlink_msg_command_long_encode(255, 1, &message, &command);
 
@@ -187,14 +335,43 @@ int Drone::take_off()
     command = mavlink_newCommand();
     command.target_system = 1;
     command.target_component = 1;
-     command.command = MAV_CMD_NAV_TAKEOFF;  
+    command.command = MAV_CMD_NAV_TAKEOFF;  
     command.confirmation = true ;
-    command.param1 = 0;  //Pitch
-	command.param2 = 0;
-	command.param3 = 0;
+    command.param1 = 245;  //Minimum pitch (if airspeed sensor present), desired pitch without sensor
+	command.param2 = 1e+06;
+	command.param3 = NAN; 
+	command.param4 = NAN; //Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).
+	command.param5 = NAN;  //global_position_int.lat; // local_position_ned.x;	//Latitude
+	command.param6 = NAN; //global_position_int.lon; //local_position_ned.y;    //longitude 
+//	printf("departure alt = %d \n",departure_alt);
+	command.param7 = NAN; // 10m altitude
+	
+	
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+	
+	cout <<"param1"<<command.param1<<"param2"<<command.param2<<"param3"<<command.param3<<"param4 "<<command.param4<<"param5 "<<command.param5<<"param6 "<<command.param6<<"param7 "<<command.param7<<endl;
+
+
+
+    return  write_message(message);
+}
+int Drone::take_off_vtol() 
+{
+    //Message buffer
+    mavlink_message_t message;
+    mavlink_command_long_t command;
+	printf("===========================  take_off Demande au drone de monter en altitude \n");
+    command = mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_NAV_VTOL_TAKEOFF;  
+    command.confirmation = true ;
+   // command.param1 = 0;  //Pitch
+	//command.param2 = VTOL_TRANSITION_HEADING;
+	//command.param3 = 0;
 	command.param4 = NAN; //YAW
 	command.param5 = gps_raw_int.lat;  //global_position_int.lat; // local_position_ned.x;	//Latitude
-	command.param6 = gps_raw_int.lon; //global_position_int.lon; //local_position_ned.y;    //longitude
+	command.param6 = gps_raw_int.lon; //global_position_int.lon; //local_position_ned.y;    //longitude 
 	printf("departure alt = %d \n",departure_alt);
 	command.param7 = departure_alt+1000; // 10m altitude
 
@@ -203,6 +380,33 @@ int Drone::take_off()
     return  write_message(message);
 }
 
+int Drone::followhold() 
+{
+    //Message buffer
+    mavlink_message_t message;
+    mavlink_command_long_t command;
+	printf("===========================  take_off Demande au drone de monter en altitude \n");
+    command = mavlink_newCommand();
+    command.target_system = 1;
+    command.target_component = 1;
+    command.command = MAV_CMD_DO_FOLLOW;  
+    command.confirmation = true ;
+    command.param1 = 0;  //Hold mode
+	//command.param2 = VTOL_TRANSITION_HEADING;
+	//command.param3 = 0;
+	command.param4 = 2; //Altitude mode: 0: Keep current altitude, 1: keep altitude difference to target, 2: go to a fixed altitude above home.
+	command.param5 = departure_alt+1000; // 10m altitudeAltitude above home. (used if mode=2)
+	//command.param6 = gps_raw_int.lon; //global_position_int.lon; //local_position_ned.y;    //longitude 
+	printf("departure alt = %d \n",departure_alt);
+	command.param7 = 1; //Time to land in which the MAV should go to the default position hold mode after a message RX timeout.
+
+    mavlink_msg_command_long_encode(255, 1, &message, &command);
+
+    return  write_message(message);
+}
+
+
+
 
 int Drone::landing() 
 {
@@ -215,13 +419,13 @@ int Drone::landing()
     command.target_component = 1;
      command.command = MAV_CMD_NAV_LAND;  
     command.confirmation = true ;
-    command.param1 = 1;
-	command.param2 = 0;
-	command.param3 = 0;
-	command.param4 = NAN;
-	command.param5 = gps_raw_int.lat;
-	command.param6 = gps_raw_int.lat;
-	command.param7 = departure_alt; 
+    command.param1 = 1;			//Minimum target altitude if landing is aborted (0 = undefined/use system default).
+	command.param2 = 0;			//Precision land mode. //PRECISION_LAND_MODE
+	command.param3 = 0;			//Empty
+	command.param4 = NAN;		//Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).
+	command.param5 = gps_raw_int.lat;	//Latitude
+	command.param6 = gps_raw_int.lat;	//Longitude
+	command.param7 = departure_alt; 	//Landing altitude (ground level in current frame).
 
     mavlink_msg_command_long_encode(255, 1, &message, &command);
 
@@ -314,12 +518,11 @@ int Drone::command_kill(int param1)
     return write_message(message);
 }
 
-int Drone::command_setModeGuided(float param1)
+/*int Drone::command_setModeGuided(float param1)
 {
     //Message buffer
 	mavlink_message_t message;
     mavlink_command_long_t command;
-
     command=mavlink_newCommand();
     command.target_system = 1;
     command.target_component = 1;
@@ -365,7 +568,10 @@ int Drone::command_directControl(float x, float y, float z, float r)
 	int16_t xx,yy,zz,rr;
 	xx=(int16_t)x; yy=(int16_t)y; zz=(int16_t)z; rr=(int16_t)r;
   //  mavlink_msg_manual_control_pack(255, 1, &message, 1, 0, xx, yy, zz, rr);
-        mavlink_msg_manual_control_pack(255, 1, &message, 1, xx, yy, zz, rr,1);
+        mavlink_msg_manual_control_pack(255, 1, &message, 1, xx, yy, zz, rr,1); //xx :  Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. 
+															//yy : Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. 
+															//zz : Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
+															//rr : Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. 
 
   cout<<"command_directControl  : "<<xx<<" "<<yy<<" "<<zz<<" "<<rr<<" "<<endl;
   return write_message(message);
@@ -534,13 +740,99 @@ master.mav.command_long_send( master.target_system, master.target_component, mav
     Follow me: base_mode:157, main_mode:4, sub_mode:8
 */
 
-int Drone::command_setMode(Drone_mode mode)
+/*
+std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
+SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_id)
+{
+    const uint8_t flag_safety_armed = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;
+    const uint8_t flag_hitl_enabled = _hitl_enabled ? MAV_MODE_FLAG_HIL_ENABLED : 0;
+
+    const uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed | flag_hitl_enabled;
+
+    // Note: the safety flag is not needed in future versions of the PX4 Firmware
+    //       but want to be rather safe than sorry.
+    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+    uint8_t custom_sub_mode = 0;
+
+    switch (flight_mode) {
+        case FlightMode::Hold:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+            break;
+        case FlightMode::ReturnToLaunch:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+            break;
+        case FlightMode::Takeoff:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+            break;
+        case FlightMode::Land:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+            break;
+        case FlightMode::Mission:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+            break;
+        case FlightMode::FollowMe:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
+            break;
+        case FlightMode::Offboard:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
+            break;
+        case FlightMode::Manual:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;
+            break;
+        case FlightMode::Posctl:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;
+            break;
+        case FlightMode::Altctl:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;
+            break;
+        case FlightMode::Rattitude:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_RATTITUDE;
+            break;
+        case FlightMode::Acro:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_ACRO;
+            break;
+        case FlightMode::Stabilized:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_STABILIZED;
+            break;
+        default:
+            LogErr() << "Unknown Flight mode.";
+            MavlinkCommandSender::CommandLong empty_command{};
+            return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command);
+    }
+
+    MavlinkCommandSender::CommandLong command{};
+
+    command.command = MAV_CMD_DO_SET_MODE;
+    command.params.param1 = float(mode);
+    command.params.param2 = float(custom_mode);
+    command.params.param3 = float(custom_sub_mode);
+    command.target_component_id = component_id;
+
+    return std::make_pair<>(MavlinkCommandSender::Result::Success, command);
+}
+*/
+//int Drone::command_setMode(Drone_mode my_mode)
+
+ int Drone::command_setMode(Drone_mode my_mode)
 {
-    switch(mode)
+    const uint8_t flag_safety_armed = MAV_MODE_FLAG_SAFETY_ARMED; // = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;
+    const uint8_t flag_hitl_enabled = MAV_MODE_FLAG_HIL_ENABLED; // = _hitl_enabled ? MAV_MODE_FLAG_HIL_ENABLED : 0;
+
+    const uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed | flag_hitl_enabled;
+//	Drone_mode my_mode;
+    // Note: the safety flag is not needed in future versions of the PX4 Firmware
+    //  but want to be rather safe than sorry.
+    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+    uint8_t custom_sub_mode = 0;	
+    
+   FlightMode flight_mode = FlightMode::Hold;
+   custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+   
+   /*  switch(my_mode)
     {
         case DRONE_OFF: 
-        //We must reset all value to 0
-            Drone::mode=mode;
+        //We must reset all value to 0			
+            Drone::mode=flight_mode;
         break;
 
         case DRONE_MANUAL_DIRECT: 
@@ -549,17 +841,94 @@ int Drone::command_setMode(Drone_mode mode)
                 //beep();
             }else{
                 //Display_IHM::getInstanceOf().printLog("Server: mode manual_direct");
-                Drone::mode=mode;
+                Drone::mode=flight_mode;
             }
         break;
 
         default:
         break;
     }
-
-    return 0;
+         return 0;
+*/
+	
+//	FlightMode flight_mode = FlightMode::Hold;
+
+    switch (flight_mode) {
+        case FlightMode::Hold:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+            break;
+        case FlightMode::ReturnToLaunch:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+            break;
+        case FlightMode::Takeoff:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+            break;
+        case FlightMode::Land:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+            break;
+        case FlightMode::Mission:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+            break;
+        case FlightMode::FollowMe:
+            custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
+            break;
+        case FlightMode::Offboard:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
+            break;
+        case FlightMode::Manual:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;
+            break;
+        case FlightMode::Posctl:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;
+            break;
+        case FlightMode::Altctl:
+            custom_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;
+            break;
+        case FlightMode::Rattitude:
+