Minor changes to the FSM, new int pickupmode (yaml) to test chocolate pickup.

pps_ws/src/d_fall_pps/include/nodes/DroneXControllerService.h

@@ -517,6 +517,9 @@ float green_2 = 1.0f;
 float blue_2 = 0.0f;
 
 
+int pickupmode = 0; // 0=land; 1=hover;
+
+
 // RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
 // The "CrazyflieData" type used for the "request" variable is a
 // structure as defined in the file "CrazyflieData.msg" which has the following

pps_ws/src/d_fall_pps/param/DroneXController.yaml

 # Mass of the crazyflie
-mass_CF : 28 #32
+mass_CF : 31 #28 #32
 
 
 # Max setpoint change per second
@@ -28,8 +28,8 @@ trajectory_deltaT_velocity: 0.35
 # distance to first point on trajectory
 xm1_x_distance: 0.5
 # distance from MS to second point on trajectory
-xm2_distance_to_ms: 0.6
-xm2_height_over_ms: 0.3
+xm2_distance_to_ms: 0.2 #0.6
+xm2_height_over_ms: 0.4 #0.3
 
 xm1_height: 0.6
 
@@ -43,12 +43,13 @@ xm2_distance_to_ms_at_zero_velocity : 0.6   # xm2 distance behind the mothership
 xm2_distance_to_ms_scaling_factor : 1       # Scale in direction of the mothership velocity
 
 # tolerances
-tol_takeoff:  [0.07, 0.07, 0.07]
+tol_takeoff:  [0.07, 0.07, 0.07] #[0.07, 0.07, 0.07]
 tol_approach: [0.3, 0.3, 0.1]
-tol_land:     [0.01, 0.01, 0.05]
-land_height: 0.3
+tol_land:     [0.05, 0.05, 0.05] #[0.01, 0.01, 0.05]
+land_height:  0.03 #0.3
 start_height: 0.9
 
+pickupmode: 0 # 0=land directly; 1=hover to pickup chocolate
 
 
 

pps_ws/src/d_fall_pps/src/nodes/DroneXControllerService.cpp

@@ -562,6 +562,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 			dronexSetpoint.y = request.ownCrazyflie.y;
 			dronexSetpoint.z = request.ownCrazyflie.z;
 
+			// Uncomment this if this part of the FSM should not be below together with the modes.
+			//motorsOFF(response);
+
 		}
 		break;
 
@@ -583,6 +586,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 			dronexSetpoint.y = request.ownCrazyflie.y;
 			dronexSetpoint.z = request.ownCrazyflie.z;
 
+
+
+			// Uncomment this if this part of the FSM should not be below together with the modes.
+			// if(pickupmode == 0){						// land directly/normally
+			// 	motorsOFF(response);
+			// }else{										// hover over mothership to pickup chocolate
+			// 	flying_state = DRONEX_STATE_HOVER;
+			// }
+
+
 		}
 		break;
 
@@ -603,20 +616,43 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 			//	flying_state = DRONEX_STATE_APPROACH;
 			//	ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
 			//}
+
+
+
+			// Uncomment this if this part of the FSM should not be below together with the modes.
+			// if((abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) &&
+			// 	(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
+			// 	(abs(request.ownCrazyflie.z - land_height - request.otherCrazyflies[0].z) < tol_land[2]) ){
+
+			// 	ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
+			// 	flying_state = DRONEX_STATE_ON_MOTHERSHIP;
+			// 	previous_flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
+			// }
+
 		}
 		break;
 
 		case DRONEX_STATE_LAND_ON_GROUND:
 		{
 			if(tookOffFlag){
-				ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
+				//ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
 				dronexSetpoint.x = request.ownCrazyflie.x;
 				dronexSetpoint.y = request.ownCrazyflie.y;
 				dronexSetpoint.z = 0.0;
 
 				tookOffFlag = false;
+
+				
 			}
 
+
+			// Uncomment this if this part of the FSM should not be below together with the modes.
+			// if(request.ownCrazyflie.z < 0.06 ){
+			// 		ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
+			// 		flying_state = DRONEX_STATE_GROUND;
+			// 		previous_flying_state = DRONEX_STATE_LAND_ON_GROUND;
+			// }
+
 		}
 		break;
 
@@ -904,7 +940,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 	}
 
 	if(flying_state == DRONEX_STATE_ON_MOTHERSHIP){
-		motorsOFF(response);
+		if(pickupmode == 0){						// land directly/normally
+			motorsOFF(response);
+		}else{										// hover over mothership to pickup chocolate
+			flying_state = DRONEX_STATE_HOVER;
+		}
 		//ROS_INFO("On Mothership");
 	}else if(flying_state == DRONEX_STATE_GROUND){
 		motorsOFF(response);
@@ -913,6 +953,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 		calculateControlOutputDroneX(request, response, stateErrorBody);	// chooses controller mode
 	}
 
+	// Uncomment this to have a separated FSM
+	// if(!(flying_state == DRONEX_STATE_GROUND) && !(flying_state == DRONEX_STATE_ON_MOTHERSHIP)){
+	// 	calculateControlOutputDroneX(request, response, stateErrorBody);	// chooses controller mode
+		
+	// }
+
 
 	// PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
 	if (shouldPublishCurrent_xyz_yaw)
@@ -2472,6 +2518,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
 	xm2_distance_to_ms_at_zero_velocity = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms_at_zero_velocity");
 	xm2_distance_to_ms_scaling_factor = getParameterFloat(nodeHandle_for_dronexController, "xm2_distance_to_ms_scaling_factor");
 
+	pickupmode = getParameterInt(nodeHandle_for_dronexController, "pickupmode");
+
 
 	// DEBUGGING: Print out one of the parameters that was loaded
 	ROS_INFO_STREAM("[DRONEX CONTROLLER] DEBUGGING: the fetched DroneXController/mass_CF = " << m_mass_CF_grams);