VM: some minor changes, trajectory not yet working

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui

@@ -391,7 +391,7 @@
            </font>
           </property>
           <property name="currentIndex">
-           <number>0</number>
+           <number>1</number>
           </property>
           <property name="usesScrollButtons">
            <bool>true</bool>

pps_ws/src/d_fall_pps/param/DroneXController.yaml

@@ -15,7 +15,7 @@ vicon_frequency : 200
 # controller_mode : 0: Basic Controller
 # controller_mode : 1: Angle Controller
 # controller_mode : 2: Trajectory Controller
-controller_mode : 1
+controller_mode : 2
 
 
 
@@ -109,5 +109,5 @@ gainMatrixPitchRatefromAngle          :  [ 0.00, 4.00, 0.00]
 gainMatrixYawRatefromAngle            :  [ 0.00, 0.00, 2.30]
 
 
-# LQR: get baseline Angle from Velocity for feedforward (roll, pitch)
-gainFeedforwardAnglefromVelocity: [-(20.0 * DEG2RAD), (20.0 * DEG2RAD)]
\ No newline at end of file
+# LQR: get baseline Angle from Velocity for feedforward (roll, pitch) [-20°,20°]=[-0.34906585, 0.34906585]
+gainFeedforwardAnglefromVelocity: [-0.34906585, 0.34906585]
\ No newline at end of file

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

@@ -237,7 +237,7 @@ void buttonPressed_follow_trajectory(){
 
 	// Initialize trajectory timers
 	flightSequence = SEQUENCE_NONE;
-	flying_state = DRONEX_FOLLOW_TRAJECTORY;
+	flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
 	total_time_since_start = 0;
 	trajectory_x_radius = 0;
 	trajectory_y_radius = 0;
@@ -536,8 +536,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 		}
 		break;
 
-		case DRONEX_FOLLOW_TRAJECTORY:
+		case DRONEX_STATE_FOLLOWING_TRAJECTORY:
+			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-request.otherCrazyflies[0].z + 0.2) < tol_land[2] ){
 
+				flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
+			}
 		break;
 
 
@@ -891,7 +896,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
 		// x dot, y dot, z dot
 		float m_velocity_for_controller_2[3];
 
-		if(flying_state == DRONEX_FOLLOW_TRAJECTORY){
+		if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){
 
 			calculateTrajectory(request);
 
@@ -1106,8 +1111,6 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
 // Gives back computes trajectory_setpoint and trajectory_velocity
 void calculateTrajectory(Controller::Request &request){
 
-	trajectory_x_radius = 1.0;
-	trajectory_y_radius =	0.4;
 	total_time_since_start = ros::Time::now().toSec() - trajectory_t0;
 
 	trajectory_setpoint[0] = trajectory_to_ms(request, total_time_since_start)[0];
@@ -1116,23 +1119,23 @@ void calculateTrajectory(Controller::Request &request){
 	trajectory_setpoint[3] = 0;
 
 	// just some deltaT to look ahead
-	float deltaT = 0.01;
-	trajectory_velocity[0] = (trajectory_to_ms(request,total_time_since_start+deltaT)[0]-request.ownCrazyflie.x)/deltaT;
-	trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start+deltaT)[1]-request.ownCrazyflie.y)/deltaT;
+	float deltaT = 1.0/m_vicon_frequency;
+	trajectory_velocity[0] = (trajectory_to_ms(request, total_time_since_start + deltaT)[0]-request.ownCrazyflie.x)/deltaT;
+	trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start + deltaT)[1]-request.ownCrazyflie.y)/deltaT;
 	trajectory_velocity[2] = 0;
 
-	/*
-	trajectory_setpoint[0] = originX + cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius);
-	trajectory_setpoint[1] = originY + sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
-	trajectory_setpoint[2] = 0.5;
-	trajectory_setpoint[3] = 0;
-	*/
-	/*
-	trajectory_velocity[0] = -2*PI/trajectory_period * sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius);
-	trajectory_velocity[1] = 2*PI/trajectory_period * cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius);
-	trajectory_velocity[2] = 0;
-	*/
-
+	// clip if velocity is too big
+	float tot_velocity_xy = sqrt(abs(pow(trajectory_velocity[0],2))+abs(pow(trajectory_velocity[1],2)));
+	if(tot_velocity_xy > 1.0){
+		trajectory_velocity[0] = 1.0 * trajectory_velocity[0]/tot_velocity_xy;
+		trajectory_velocity[1] = 1.0 * trajectory_velocity[1]/tot_velocity_xy;
+	}
+	if(trajectory_velocity[3]<-0.8){
+		trajectory_velocity[3] = -0.8;
+	}else if(trajectory_velocity[3]>0.8){
+		trajectory_velocity[3] = 0.8;
+	}
+	
 }
 
 
@@ -1211,7 +1214,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
 
 	// Calculate the x,y,z position depending on where the CF should be at a certain time t
 
-	if(trajectory_t0 < t && t <= trajectory_t1){ // Calculate position on line between xcf0 and xm1
+	if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position on line between xcf0 and xm1
 		for(int i = 0; i < 3; i++){
 			trajectory_return[i] = xcf0[i] + t*trajectory_velocity_of_CF*(xm1[i] - xcf0[i]);
 		}
@@ -1224,22 +1227,21 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
 			trajectory_return[i] = xm2[i] + (t - trajectory_t2)*trajectory_velocity_of_CF*(xms[i] - xm2[i]);
 		}
 	}else{
-		trajectory_return[0] = -1;
-		trajectory_return[1] = -1;
-		trajectory_return[2] = -1;
+		trajectory_return[0] = 0;
+		trajectory_return[1] = 0;
+		trajectory_return[2] = 0.3;
 	}
 
 
-
-	/*trajectory_x_radius = 1.0;
+	/*
+	trajectory_x_radius = 1.0;
 	trajectory_y_radius =	0.4;
-	//total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
 
 	trajectory_return[0] = originX + cos(2*PI/trajectory_period * t)*(trajectory_x_radius);
 	trajectory_return[1] = originY + sin(2*PI/trajectory_period * t)*(trajectory_y_radius);
-	trajectory_return[2] = 0.5;*/
-
+	trajectory_return[2] = 0.5;
 
+	*/
 
 	return trajectory_return;
 }
@@ -1612,9 +1614,9 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
 	debugMsg.value_2 = mothership_vel[1];
 	debugMsg.value_3 = mothership_vel[2];
 	debugMsg.value_4 = thrust_factor;
-	debugMsg.value_5 = dronexSetpoint.x;
-	debugMsg.value_6 = dronexSetpoint.y;
-	debugMsg.value_7 = dronexSetpoint.z;
+	debugMsg.value_5 = trajectory_setpoint[0];
+	debugMsg.value_6 = trajectory_setpoint[1];
+	debugMsg.value_7 = trajectory_setpoint[2];
 	// ......................
 	// debugMsg.value_10 = your_variable_name;