DroneXControllerService.cpp 97 KB
Newer Older
1
2
3
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
//    This file is part of D-FaLL-System.
pragash1's avatar
pragash1 committed
4
//
5
6
7
8
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
pragash1's avatar
pragash1 committed
9
//
10
11
12
13
//    D-FaLL-System is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
pragash1's avatar
pragash1 committed
14
//
15
16
//    You should have received a copy of the GNU General Public License
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
pragash1's avatar
pragash1 committed
17
//
18
19
20
21
22
23
24
25
26
27
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
pragash1's avatar
pragash1 committed
28
//    Place for students to implement their controller
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
//
//    ----------------------------------------------------------------------------------





// INCLUDE THE HEADER
#include "nodes/DroneXControllerService.h"





//    ----------------------------------------------------------------------------------
//    FFFFF  U   U  N   N   CCCC  TTTTT  III   OOO   N   N
//    F      U   U  NN  N  C        T     I   O   O  NN  N
//    FFF    U   U  N N N  C        T     I   O   O  N N N
//    F      U   U  N  NN  C        T     I   O   O  N  NN
//    F       UUU   N   N   CCCC    T    III   OOO   N   N
//
//    III M   M PPPP  L     EEEEE M   M EEEEE N   N TTTTT   A   TTTTT III  OOO  N   N
//     I  MM MM P   P L     E     MM MM E     NN  N   T    A A    T    I  O   O NN  N
//     I  M M M PPPP  L     EEE   M M M EEE   N N N   T   A   A   T    I  O   O N N N
//     I  M   M P     L     E     M   M E     N  NN   T   AAAAA   T    I  O   O N  NN
//    III M   M P     LLLLL EEEEE M   M EEEEE N   N   T   A   A   T   III  OOO  N   N
//    ----------------------------------------------------------------------------------
pragash1's avatar
pragash1 committed
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134




// REMINDER OF THE NAME OF USEFUL CLASS VARIABLE
// // > Mass of the Crazyflie quad-rotor, in [grams]
// float m_mass_CF_grams;
// // > Mass of the letters to be lifted, in [grams]
// float m_mass_E_grams;
// float m_mass_T_grams;
// float m_mass_H_grams;
// // > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
// float m_mass_total_grams;
// // Thickness of the object at pick-up and put-down, in [meters]
// // > This should also account for extra height due to
// //   the surface where the object is
// float m_thickness_of_object_at_pickup;
// float m_thickness_of_object_at_putdown;
// // (x,y) coordinates of the pickup location
// std::vector<float> m_pickup_coordinates_xy(2);
// // (x,y) coordinates of the drop off location
// std::vector<float> m_dropoff_coordinates_xy_for_E(2);
// std::vector<float> m_dropoff_coordinates_xy_for_T(2);
// std::vector<float> m_dropoff_coordinates_xy_for_H(2);
// // Length of the string from the Crazyflie
// // to the end of the DroneX, in [meters]
// float m_dronex_string_length;
// // > The setpoints for (x,y,z) position and yaw angle, in that order
// float m_setpoint[4] = {0.0,0.0,0.4,0.0};
// float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
// // > Small adjustments to the x-y setpoint
// float m_xAdjustment = 0.0f;
// float m_yAdjustment = 0.0f;
// // Boolean for whether to limit rate of change of the setpoint
// bool m_shouldSmoothSetpointChanges = true;
// // Max setpoint change per second
// float m_max_setpoint_change_per_second_horizontal;
// float m_max_setpoint_change_per_second_vertical;
// float m_max_setpoint_change_per_second_yaw_degrees;
// float m_max_setpoint_change_per_second_yaw_radians;
// // Frequency at which the controller is running
// float m_vicon_frequency;


// A FEW EXTRA COMMENTS ABOUT THE MOST IMPORTANT VARIABLES

// Variable name:    m_setpoint
// Description:
// This is a float array of length 4. It specifies a location
// in space where you want the drone to be. The 4 element are:
// >> m_setpoint[0]   The x-poistion in [meters]
// >> m_setpoint[1]   The y-poistion in [meters]
// >> m_setpoint[2]   The z-poistion in [meters]
// >> m_setpoint[3]   The yaw heading angle in [radians]


// Variable name:    m_setpoint_for_controller
// Description:
// Similar to the variable "m_setpoint" this is also float array
// of length 4 that specifies an (x,y,z,yaw) location. The
// difference it that this variable specifies the location where
// the low-level controller is guiding the drone to be.
// HINT: to make changes the "m_setpoint" variable, you can edit
// the function named "perControlCycleOperations" so that the
// "m_setpoint_for_controller" changes by a maximum amount at
// each cycle of the contoller



// THIS FUNCTION IS CALLED AT "m_vicon_frequency" HERTZ.
// IT CAN BE USED TO ADJUST THINGS IN "REAL TIME".
// For example, the equation:
// >> m_max_setpoint_change_per_second_horizontal / m_vicon_frequency
// will convert the "change per second" to a "change per cycle".

void perControlCycleOperations()
{
	if (m_shouldSmoothSetpointChanges)
	{
135
		for(int i = 0; i < 4; ++i)
pragash1's avatar
pragash1 committed
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
		{
			float max_for_this_coordinate;
			// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
			switch (i)
			{
				case 0:
					max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / m_vicon_frequency;
					break;
				case 1:
					max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / m_vicon_frequency;
					break;
				case 2:
					max_for_this_coordinate = m_max_setpoint_change_per_second_vertical / m_vicon_frequency;
					break;
				case 3:
					max_for_this_coordinate = m_max_setpoint_change_per_second_yaw_radians / m_vicon_frequency;
					break;
				// Handle the exception
				default:
					max_for_this_coordinate = 0.0f;
					break;
			}

			// Compute the difference in setpoint
			float setpoint_difference = m_setpoint[i] - m_setpoint_for_controller[i];

162
163
164
165
166
167
168
			// anti windup for yaw
			if (i==3){
				if (setpoint_difference > PI)
					setpoint_difference -= 2*PI;
				if (setpoint_difference < -PI)
					setpoint_difference += 2*PI;
			}
pragash1's avatar
pragash1 committed
169
170
171
172
173
174
175
176
177
178
179
180
181
			// Clip the difference to the maximum
			if (setpoint_difference > max_for_this_coordinate)
			{
				setpoint_difference = max_for_this_coordinate;
			}
			else if (setpoint_difference < -max_for_this_coordinate)
			{
				setpoint_difference = -max_for_this_coordinate;
			}

			// Update the setpoint of the controller
			m_setpoint_for_controller[i] += setpoint_difference;
		}
182
		//m_setpoint_for_controller[3] = m_setpoint[3];
pragash1's avatar
pragash1 committed
183
184
185
186
187
188
189
190
191

	}
	else
	{
		m_setpoint_for_controller[0] = m_setpoint[0];
		m_setpoint_for_controller[1] = m_setpoint[1];
		m_setpoint_for_controller[2] = m_setpoint[2];
		m_setpoint_for_controller[3] = m_setpoint[3];
	}
192

pragash1's avatar
pragash1 committed
193
194
195
196
197
198
199
200
201
202
}








void buttonPressed_take_off(){
pragash1's avatar
pragash1 committed
203

204
	//if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
pragash1's avatar
pragash1 committed
205
206
		ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Taking off...");
		flying_state = DRONEX_STATE_TAKING_OFF;
207
208
209
	//}else{
	//	ROS_ERROR("Cannot change to DRONEX_STATE_TAKING_OFF");
	//}
pragash1's avatar
pragash1 committed
210
211
212
}

void buttonPressed_land(){
213
	//if(flying_state == DRONEX_STATE_HOVER){
mastefan's avatar
mastefan committed
214
215
		ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Start flight-sequence LA...");
		// OLD:flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
maruggv's avatar
maruggv committed
216
		// NEW:
mastefan's avatar
mastefan committed
217
		flightSequence = SEQUENCE_LAND_ON_MOTHERSHIP;
pragash1's avatar
pragash1 committed
218
219
}

pragash1's avatar
pragash1 committed
220
221
222
void buttonPressed_abort(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Abort Mission!");
	flying_state = DRONEX_STATE_LAND_ON_GROUND;
mastefan's avatar
mastefan committed
223
224
225

	// reset start position
	savedStartCoordinates = false;
pragash1's avatar
pragash1 committed
226
227
}

228
229
void buttonPressed_integrator_on(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn ON integrator");
maruggv's avatar
maruggv committed
230
	integratorFlag = DRONEX_INTEGRATOR_ON;
231
232
233
234
}

void buttonPressed_integrator_off(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn OFF integrator");
maruggv's avatar
maruggv committed
235
	integratorFlag = DRONEX_INTEGRATOR_OFF;
236
237
238
239
}

void buttonPressed_integrator_reset(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET integrator to zero");
maruggv's avatar
maruggv committed
240
	integratorFlag = DRONEX_INTEGRATOR_RESET;
241
242
}

maruggv's avatar
maruggv committed
243
244
245
246
247
248
249
250

void buttonPressed_follow_trajectory(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] FOLLOW trajectory");

	// Initialize trajectory timers
	flightSequence = SEQUENCE_NONE;
	flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
	total_time_since_start = 0;
251
252
	//trajectory_x_radius = 0;
	//trajectory_y_radius = 0;
maruggv's avatar
maruggv committed
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
	first_trajectory_calculation = true;
	trajectory_start_time = ros::Time::now().toSec();
	trajectory_t0 = 0.0;
	//trajectory_t0 = ros::Time::now().toSec();


	ROS_INFO_STREAM("trajectory start time: " << trajectory_start_time);
}


void buttonPressed_reset(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET");

}







mastefan's avatar
mastefan committed
274
275
276
277
278
279
280
281
void integratorCallback (const Setpoint& integrParams ) {
    integrator_sum_XYZ[0] = integrParams.x;
    integrator_sum_XYZ[1] = integrParams.y;
    integrator_sum_XYZ[2] = integrParams.z;
}

void WeightParamCallback (const Setpoint& weightParam ) {
    // TODO for changing yaml: set weight in yaml OR just set m_mass_CF_grams?
maruggv's avatar
maruggv committed
282
283
    m_mass_total_grams = weightParam.x;

mastefan's avatar
mastefan committed
284
285
286
287
288
289

}




pragash1's avatar
pragash1 committed
290
291


292
293
294
295
296
297





//    ------------------------------------------------------------------------------
pragash1's avatar
pragash1 committed
298
//     OOO   U   U  TTTTT  EEEEE  RRRR
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
//    O   O  U   U    T    E      R   R
//    O   O  U   U    T    EEE    RRRR
//    O   O  U   U    T    E      R  R
//     OOO    UUU     T    EEEEE  R   R
//
//     CCCC   OOO   N   N  TTTTT  RRRR    OOO   L           L       OOO    OOO   PPPP
//    C      O   O  NN  N    T    R   R  O   O  L           L      O   O  O   O  P   P
//    C      O   O  N N N    T    RRRR   O   O  L           L      O   O  O   O  PPPP
//    C      O   O  N  NN    T    R  R   O   O  L           L      O   O  O   O  P
//     CCCC   OOO   N   N    T    R   R   OOO   LLLLL       LLLLL   OOO    OOO   P
//    ----------------------------------------------------------------------------------

// This function is the callback that is linked to the "DroneXController" service that
// is advertised in the main function. This must have arguments that match the
// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
// folder)
//
// The arument "request" is a structure provided to this service with the following two
// properties:
// request.ownCrazyflie
// This property is itself a structure of type "CrazyflieData",  which is defined in the
// file "CrazyflieData.msg", and has the following properties
// string crazyflieName
//     float64 x                         The x position of the Crazyflie [metres]
//     float64 y                         The y position of the Crazyflie [metres]
//     float64 z                         The z position of the Crazyflie [metres]
//     float64 roll                      The roll component of the intrinsic Euler angles [radians]
//     float64 pitch                     The pitch component of the intrinsic Euler angles [radians]
//     float64 yaw                       The yaw component of the intrinsic Euler angles [radians]
//     float64 acquiringTime #delta t    The time elapsed since the previous "CrazyflieData" was received [seconds]
//     bool occluded                     A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
// request.otherCrazyflies
// This property is an array of "CrazyflieData" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
// service by this function, it has only the following property
// response.ControlCommand
// This property is iteself a structure of type "ControlCommand", which is defined in
// the file "ControlCommand.msg", and has the following properties:
//     float32 roll                      The command sent to the Crazyflie for the body frame x-axis
//     float32 pitch                     The command sent to the Crazyflie for the body frame y-axis
//     float32 yaw                       The command sent to the Crazyflie for the body frame z-axis
//     uint16 motorCmd1                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd2                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd3                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd4                  The command sent to the Crazyflie for motor 1
//     uint8 onboardControllerType       The flag sent to the Crazyflie for indicating how to implement the command
pragash1's avatar
pragash1 committed
350
//
351
352
// IMPORTANT NOTES FOR "onboardControllerType"  AND AXIS CONVENTIONS
// > The allowed values for "onboardControllerType" are in the "Defines" section at the
pragash1's avatar
pragash1 committed
353
354
355
356
357
//   top of this file, they are:
//   CF_COMMAND_TYPE_MOTOR
//   CF_COMMAND_TYPE_RATE
//   CF_COMMAND_TYPE_ANGLE.
// > With CF_COMMAND_TYPE_RATE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
358
359
//   specify the angular rate in [radians/second] that will be requested from the
//   PID controllers running in the Crazyflie 2.0 firmware.
pragash1's avatar
pragash1 committed
360
// > With CF_COMMAND_TYPE_RATE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
361
362
363
//   are the baseline motor commands requested from the Crazyflie, with the adjustment
//   for body rates being added on top of this in the firmware (i.e., as per the code
//   of the "distribute_power" function provided in exercise sheet 2).
pragash1's avatar
pragash1 committed
364
// > With CF_COMMAND_TYPE_RATE the axis convention for the roll, pitch, and yaw body rates returned
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
//   in "response.ControlCommand" should use right-hand coordinate axes with x-forward
//   and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
//   thrust). To assist, teh following is an ASCII art of this convention:
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
//  > This is a top view,
//  > M1 to M4 stand for Motor 1 to Motor 4,
//  > "CW"  indicates that the motor rotates Clockwise,
//  > "CCW" indicates that the motor rotates Counter-Clockwise,
//  > By right-hand axis convention, the positive z-direction points our of the screen,
//  > This being a "top view" means tha the direction of positive thrust produced
//    by the propellers is also out of the screen.
//
//        ____                         ____
//       /    \                       /    \
//  (CW) | M4 |           x           | M1 | (CCW)
//       \____/\          ^          /\____/
//            \ \         |         / /
//             \ \        |        / /
//              \ \______ | ______/ /
//               \        |        /
//                |       |       |
//        y <-------------o       |
//                |               |
//               / _______________ \
//              / /               \ \
//             / /                 \ \
//        ____/ /                   \ \____
//       /    \/                     \/    \
// (CCW) | M3 |                       | M2 | (CW)
//       \____/                       \____/
//
//
//
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{

pragash1's avatar
pragash1 committed
404
405
406
	// Keep track of time
	m_time_ticks++;
	m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
407

408
409
	switch(flying_state){
		case DRONEX_STATE_APPROACH:
410
		{
mastefan's avatar
mastefan committed
411
			//ROS_INFO("DRONEX_STATE_APPROACH");
412
413
			dronexSetpoint.x = request.otherCrazyflies[0].x;
			dronexSetpoint.y = request.otherCrazyflies[0].y;
maruggv's avatar
maruggv committed
414
			dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
mastefan's avatar
mastefan committed
415

maruggv's avatar
maruggv committed
416
			/*
maruggv's avatar
maruggv committed
417
418
419
			ROS_INFO_STREAM("APPROACH: (x,y,z) Difference: ("
				<< request.ownCrazyflie.x-dronexSetpoint.x << ", "
				<< request.ownCrazyflie.y-dronexSetpoint.y << ", "
mastefan's avatar
mastefan committed
420
				<< request.ownCrazyflie.z-dronexSetpoint.z << ")");
maruggv's avatar
maruggv committed
421
			*/
mastefan's avatar
mastefan committed
422
423
			if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_approach[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_approach[1] &&
				abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_approach[2] ){
mastefan's avatar
mastefan committed
424
425
426
				approachedFlag = true;
				ROS_INFO("approached");
			}
427
		}
428
429
		break;

430
		case DRONEX_STATE_GROUND:
431
		{
mastefan's avatar
mastefan committed
432
433
			//ROS_INFO("DRONEX_STATE_GROUND");
			// Variable for choosing flight sequence off
mastefan's avatar
mastefan committed
434
			flightSequence = SEQUENCE_NONE;
mastefan's avatar
mastefan committed
435
436
437
438
439
440

			// Flags of landing sequence reset
			tookOffFlag = false;
			approachedFlag = false;
			//bool landedFlag = true;

441
442
			integratorFlag == DRONEX_INTEGRATOR_OFF;

mastefan's avatar
mastefan committed
443
444
445
446
			dronexSetpoint.x = request.ownCrazyflie.x;
			dronexSetpoint.y = request.ownCrazyflie.y;
			dronexSetpoint.z = request.ownCrazyflie.z;

447
448
		}
		break;
449

mastefan's avatar
mastefan committed
450
451
452
453
		case DRONEX_STATE_ON_MOTHERSHIP:
		{
			//ROS_INFO("DRONEX_STATE_ON_MOTHERSHIP");
			// Variable for choosing flight sequence off
mastefan's avatar
mastefan committed
454
			flightSequence = SEQUENCE_NONE;
mastefan's avatar
mastefan committed
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469

			// Flags of landing sequence reset
			tookOffFlag = false;
			approachedFlag = false;
			//bool landedFlag = true;

			dronexSetpoint.x = request.ownCrazyflie.x;
			dronexSetpoint.y = request.ownCrazyflie.y;
			dronexSetpoint.z = request.ownCrazyflie.z;

		}
		break;

		case DRONEX_STATE_LAND_ON_MOTHERSHIP:
		{
470
471
472
473
			integratorFlag == DRONEX_INTEGRATOR_ON;
			//if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
			//	abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
			//	abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] ){
maruggv's avatar
maruggv committed
474
475
476
477
478
479

				//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
				dronexSetpoint.x = request.otherCrazyflies[0].x;
				dronexSetpoint.y = request.otherCrazyflies[0].y;
				dronexSetpoint.z = request.otherCrazyflies[0].z + 0.03;

480
481
482
483
484
			//}else {
			//	
			//	flying_state = DRONEX_STATE_APPROACH;
			//	ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
			//}
mastefan's avatar
mastefan committed
485
486
487
		}
		break;

pragash1's avatar
pragash1 committed
488
		case DRONEX_STATE_LAND_ON_GROUND:
489
		{
mastefan's avatar
mastefan committed
490
491
			if(tookOffFlag){
				ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
mastefan's avatar
mastefan committed
492
493
				dronexSetpoint.x = request.ownCrazyflie.x;
				dronexSetpoint.y = request.ownCrazyflie.y;
mastefan's avatar
mastefan committed
494
				dronexSetpoint.z = 0.0;
pragash1's avatar
pragash1 committed
495

mastefan's avatar
mastefan committed
496
497
				tookOffFlag = false;
			}
pragash1's avatar
pragash1 committed
498

499
500
		}
		break;
mastefan's avatar
mastefan committed
501

502
		case DRONEX_STATE_TAKING_OFF:
503
		{
mastefan's avatar
mastefan committed
504
			//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
pragash1's avatar
pragash1 committed
505

mastefan's avatar
mastefan committed
506
507
			if(!savedStartCoordinates)
			{
maruggv's avatar
maruggv committed
508
				startCoordinateX = request.ownCrazyflie.x;
mastefan's avatar
mastefan committed
509
				startCoordinateY = request.ownCrazyflie.y;
mastefan's avatar
mastefan committed
510
				startCoordinateZ = request.ownCrazyflie.z;
511

mastefan's avatar
mastefan committed
512
				savedStartCoordinates = true;
maruggv's avatar
maruggv committed
513

mastefan's avatar
mastefan committed
514
515
516
517
				ROS_INFO_STREAM("DRONEX: saved start Coordinates");
				ROS_INFO_STREAM("x = " << startCoordinateX);
				ROS_INFO_STREAM("y = " << startCoordinateY);
				ROS_INFO_STREAM("z = " << startCoordinateZ);
maruggv's avatar
maruggv committed
518

mastefan's avatar
mastefan committed
519
520
521
			}

			dronexSetpoint.x = startCoordinateX;
522
			dronexSetpoint.y = startCoordinateY;
mastefan's avatar
mastefan committed
523
			dronexSetpoint.z = startCoordinateZ + 0.4;
maruggv's avatar
maruggv committed
524
525
526
527
528
529

			// For debugging the integrator
			//	ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
			//		<< request.ownCrazyflie.x-dronexSetpoint.x << ", "
			//		<< request.ownCrazyflie.y-dronexSetpoint.y << ", "
			//		<< request.ownCrazyflie.z-dronexSetpoint.z << ")");
mastefan's avatar
mastefan committed
530

mastefan's avatar
mastefan committed
531
532
			if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
				abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
mastefan's avatar
mastefan committed
533
534
				ROS_INFO("took off");
				tookOffFlag = true;
mastefan's avatar
mastefan committed
535
536

				ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
mastefan's avatar
mastefan committed
537
				flying_state = DRONEX_STATE_HOVER;
538
539

				reset_rviz = true;
mastefan's avatar
mastefan committed
540
			}
541
542
543
		}
		break;

mastefan's avatar
mastefan committed
544
		case DRONEX_STATE_HOVER:
545
		{
maruggv's avatar
maruggv committed
546
			//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
mastefan's avatar
mastefan committed
547
			// keep setpoint constant
maruggv's avatar
maruggv committed
548
549

			// for testing hover over mothership
mastefan's avatar
mastefan committed
550
			/*
maruggv's avatar
maruggv committed
551
552
553
			dronexSetpoint.x = request.otherCrazyflies[0].x;
			dronexSetpoint.y = request.otherCrazyflies[0].y;
			dronexSetpoint.z = request.otherCrazyflies[0].z+0.3;
mastefan's avatar
mastefan committed
554
			*/
maruggv's avatar
maruggv committed
555

556
557
		}
		break;
mastefan's avatar
mastefan committed
558

maruggv's avatar
maruggv committed
559
		case DRONEX_STATE_FOLLOWING_TRAJECTORY:
560
			/*if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_approach[0] &&
mastefan's avatar
mastefan committed
561
				abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_approach[1] &&
562
				abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_approach[2] )*/
563
564
565
566
567
568
			if( (abs(request.ownCrazyflie.x-xm2[0]) < tol_approach[0] &&
				abs(request.ownCrazyflie.y-xm2[1]) < tol_approach[1] &&
				abs(request.ownCrazyflie.z-xm2[2]) < tol_approach[2] ) || 
				(abs(request.ownCrazyflie.x-xms[0]) < tol_approach[0] &&
				abs(request.ownCrazyflie.y-xms[1]) < tol_approach[1] &&
				abs(request.ownCrazyflie.z-xms[2]) < tol_approach[2]) ){
maruggv's avatar
maruggv committed
569

570
				flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
mastefan's avatar
mastefan committed
571
				ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP");
572
573
			} else{
				ROS_INFO_STREAM("distance to xm2: "<<request.ownCrazyflie.x-xm2[0]<<", "<<request.ownCrazyflie.y-xm2[1]<<", "<<request.ownCrazyflie.z-xm2[2]);
maruggv's avatar
maruggv committed
574
575
576
577
			}
		break;


578
	} // END switch case
579

mastefan's avatar
mastefan committed
580

maruggv's avatar
maruggv committed
581
582


mastefan's avatar
mastefan committed
583
	// flightSeqeunce 1: simple approaching and landing on static mothership
584
	if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
mastefan's avatar
mastefan committed
585
		//ROS_INFO_STREAM("Entering: DRONEX_STATE_TAKING_OFF");
mastefan's avatar
mastefan committed
586
		flying_state = DRONEX_STATE_TAKING_OFF;
mastefan's avatar
mastefan committed
587

588
589
		//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
		if(tookOffFlag){
mastefan's avatar
mastefan committed
590
			//ROS_INFO_STREAM("Entering: DRONEX_STATE_APPROACH");
591
592
593
			flying_state = DRONEX_STATE_APPROACH;

			if(approachedFlag){
mastefan's avatar
mastefan committed
594
595
				ROS_INFO_STREAM("Entering: DRONEX_STATE_LAND_ON_MOTHERSHIP");
				flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
mastefan's avatar
mastefan committed
596
597
			}

598
599
		}

600
	}
601

mastefan's avatar
mastefan committed
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
/*
	// flightSequence 2: approach and land with velocity optimized controller
	// TODO: define SEQUENCE names in .h (maybe rename sequences)
	if (flightSequence == SEQUENCE_2){
		flying_state = DRONEX_STATE_TAKING_OFF;

		if (tookOffFlag){
			// TODO:
			// approach landing zone: maybe a point "behind" the mothership in some angle
			// maybe turn to yaw so that CF points to mothership
			// -> in DRONEX_STATE_APPROACH or own function

			if (approachedFlag){
				// TODO:
				// turn on velocity optimized controller
				// land and turn off motors, when velocity and position requirements met
				// -> define i.e. tol_velocity, tol_land[3]
			}
		}

	}
*/

mastefan's avatar
mastefan committed
625
626
627
628
629
630
631

	// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
	// > After this function is complete the class variable
	//   "current_stateInertialEstimate" is updated and ready
	//   to be used for subsequent controller copmutations
	performEstimatorUpdate_forStateInterial(request);

maruggv's avatar
maruggv committed
632
	calculateMSVelocity(request);
mastefan's avatar
mastefan committed
633

pragash1's avatar
pragash1 committed
634
635
636
637
	// THIS IS THE START OF THE "OUTER" CONTROL LOOP
	// > i.e., this is the control loop run on this laptop
	// > this function is called at the frequency specified
	// > this function performs all estimation and control
638
639


maruggv's avatar
maruggv committed
640
	if(controller_mode == 0){	// lqr controller
641

maruggv's avatar
maruggv committed
642
		m_shouldSmoothSetpointChanges = true;
643
		dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;
maruggv's avatar
maruggv committed
644

maruggv's avatar
maruggv committed
645
		// do not change, use setpoint defined in states
646

maruggv's avatar
maruggv committed
647
	}else if(controller_mode == 1){	// nested lqr controller
648

maruggv's avatar
maruggv committed
649
650
		if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){	// Trajectory Follower
			// to implement the trajectory tracking
maruggv's avatar
maruggv committed
651
			m_shouldSmoothSetpointChanges = false;
652
653
654
655
656
657
658
659
660
661
662
663
664

			calculateTrajectory(request);

			dronexSetpoint.x = trajectory_setpoint[0];
			dronexSetpoint.y = trajectory_setpoint[1];
			dronexSetpoint.z = trajectory_setpoint[2];
			//m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
			dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;	// same yaw as mothership

			dronexVelocity.x = trajectory_velocity[0];
			dronexVelocity.y = trajectory_velocity[1];
			dronexVelocity.z = trajectory_velocity[2];

mastefan's avatar
mastefan committed
665
666


667
			/*
maruggv's avatar
maruggv committed
668
669
670
			m_setpoint_for_controller_2[0] = request.owncrazyflie.x;
			m_setpoint_for_controller_2[1] = request.owncrazyflie.y;
			m_setpoint_for_controller_2[2] = request.owncrazyflie.z;
671
672
673
674
675
676

			m_velocity_for_controller_2[0] = 1;
			m_velocity_for_controller_2[1] = 0;
			m_velocity_for_controller_2[2] = 0;
			*/

maruggv's avatar
maruggv committed
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
		}else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP){	// landing (close to mothership)

			m_shouldSmoothSetpointChanges = true;

			dronexSetpoint.x = request.otherCrazyflies[0].x;	// setpoint on mothership
			dronexSetpoint.y = request.otherCrazyflies[0].y;
			dronexSetpoint.z = request.otherCrazyflies[0].z+0.03;
			dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;

			dronexVelocity.x = mothership_vel[0];	// velocity of mothership
			dronexVelocity.y = mothership_vel[1];
			dronexVelocity.z = mothership_vel[2];

		}else {	// standard: if not following trajectory

			m_shouldSmoothSetpointChanges = true;
maruggv's avatar
maruggv committed
693

694
695
			dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;	// same yaw as mothership

maruggv's avatar
maruggv committed
696
697
698
			dronexVelocity.x = 0;
			dronexVelocity.y = 0;
			dronexVelocity.z = 0;
699
700
701
		}
	}

mastefan's avatar
mastefan committed
702
703
704
705
	setpointCallback(dronexSetpoint);
	m_velocity_for_controller[0] = dronexVelocity.x;
	m_velocity_for_controller[1] = dronexVelocity.y;
	m_velocity_for_controller[2] = dronexVelocity.z;
maruggv's avatar
maruggv committed
706
707

	// CALL THE FUNCTION FOR PER CYLCE OPERATIONS: limits setpoint changes and velocity for controller
708
709
710
	perControlCycleOperations();

	// > Call the function to perform the conversion
maruggv's avatar
maruggv committed
711
	float stateErrorBody[12];
712
713
	convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller, stateErrorBody);

maruggv's avatar
maruggv committed
714

pragash1's avatar
pragash1 committed
715
716
	// CARRY OUT THE CONTROLLER COMPUTATIONS
	// Call the function that performs the control computations for this mode
717

718
	// Turn motors off if wanted or do LQR-control
719

mastefan's avatar
mastefan committed
720
	if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.05 )){
mastefan's avatar
mastefan committed
721
722
		ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
		flying_state = DRONEX_STATE_GROUND;
723
	}
mastefan's avatar
mastefan committed
724
	else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && 	(abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) &&
maruggv's avatar
maruggv committed
725
																(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
mastefan's avatar
mastefan committed
726
																(abs(request.ownCrazyflie.z - 0.03 - request.otherCrazyflies[0].z) < tol_land[2]) ){
mastefan's avatar
mastefan committed
727
728
		ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
		flying_state = DRONEX_STATE_ON_MOTHERSHIP;
729
	}
mastefan's avatar
mastefan committed
730
731

	if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
732
733
734
		motorsOFF(response);
	}
	else{
maruggv's avatar
maruggv committed
735
		calculateControlOutputDroneX(request, response, stateErrorBody);	// chooses controller mode
pragash1's avatar
pragash1 committed
736
	}
pragash1's avatar
pragash1 committed
737
738
739
740


	// PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
	if (shouldPublishCurrent_xyz_yaw)
741
	{
pragash1's avatar
pragash1 committed
742
		publish_current_xyz_yaw(request.ownCrazyflie.x,request.ownCrazyflie.y,request.ownCrazyflie.z,request.ownCrazyflie.yaw);
743
744
	}

pragash1's avatar
pragash1 committed
745
746
747
748
749
	// PUBLISH THE DEBUG MESSAGE (if required)
	if (shouldPublishDebugMessage)
	{
		construct_and_publish_debug_message(request,response);
	}
750

maruggv's avatar
maruggv committed
751

pragash1's avatar
pragash1 committed
752
753
754
	// RETURN "true" TO INDICATE THAT THE COMPUTATIONS WERE SUCCESSFUL
	return true;
}
mastefan's avatar
mastefan committed
755

maruggv's avatar
maruggv committed
756
757
758
759
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context){
	area = context.localArea;
	originX = (area.xmin + area.xmax) / 2.0;
	originY = (area.ymin + area.ymax) / 2.0;
mastefan's avatar
mastefan committed
760

maruggv's avatar
maruggv committed
761
762
	ROS_INFO_STREAM("New OriginX: " << originX << " New OriginY: " << originY);
}
mastefan's avatar
mastefan committed
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780


//	State Error Body
//	1)	x Error
//	2)	y Error
//	3)	z Error
//	4)	x dot Error
//	5)	y dot Error
//	6)	z dot Error
//	7)	Roll
//	8)	Pitch
//	9)	yaw
//	10)	Roll dot
//	11)	Pitch dot
//	12)	Yaw dot


// DroneX Controller
mastefan's avatar
mastefan committed
781
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response, float (&stateErrorBody)[12]){
mastefan's avatar
mastefan committed
782
783
784

	if(controller_mode == 0){

maruggv's avatar
maruggv committed
785
		// integrator
786
		integrator_XYZ(stateErrorBody);
maruggv's avatar
maruggv committed
787
788

		// Compute control output via LQR controller:
789
		calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
maruggv's avatar
maruggv committed
790
791
792

	}else if(controller_mode == 1){

793
		// integrator
mastefan's avatar
mastefan committed
794
795
		integrator_XYZ(stateErrorBody);

maruggv's avatar
maruggv committed
796
		// Compute control output via Nested LQR controller:
797
		calculateControlOutput_viaLQRforRates_Nested(stateErrorBody, request, response);
mastefan's avatar
mastefan committed
798

maruggv's avatar
maruggv committed
799
800
	}else{ // Don't know what to do ^^
		ROS_ERROR("[DRONEXCONTROLLERSERVICE] Please change the controller mode");
mastefan's avatar
mastefan committed
801
802
	}

maruggv's avatar
maruggv committed
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
}

















// Calculate a trajectory

// Gives back computes trajectory_setpoint and trajectory_velocity
void calculateTrajectory(Controller::Request &request){

	total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;

828
	std::vector<float> trajectory_temp_present(3);
maruggv's avatar
maruggv committed
829
830
831
832
	trajectory_temp_present = trajectory_to_ms(request, total_time_since_start + trajectory_deltaT_position);
	trajectory_setpoint[0] = trajectory_temp_present[0];
	trajectory_setpoint[1] = trajectory_temp_present[1];
	trajectory_setpoint[2] = trajectory_temp_present[2];
maruggv's avatar
maruggv committed
833
834
	trajectory_setpoint[3] = 0;

835
	std::vector<float> trajectory_temp_future(3);
maruggv's avatar
maruggv committed
836
837
838
839
840
841
842
843
844
845
846
847
848
	trajectory_temp_future = trajectory_to_ms(request, total_time_since_start + trajectory_deltaT_velocity);
	trajectory_velocity[0] = trajectory_temp_future[3];
	trajectory_velocity[1] = trajectory_temp_future[4];
	trajectory_velocity[2] = trajectory_temp_future[5];






	// first try to follow trajectory velocity (keep to maybe put in comparison later)

	/*
maruggv's avatar
maruggv committed
849
	// just some deltaT to look ahead
mastefan's avatar
mastefan committed
850
	float deltaT = 0.2;
851

mastefan's avatar
mastefan committed
852
853
	std::vector<float> trajectory_temp_setpoint_future(3);
	trajectory_temp_setpoint_future = trajectory_to_ms(request, total_time_since_start + deltaT);
maruggv's avatar
maruggv committed
854

855
856
	trajectory_velocity[0] = (trajectory_temp_setpoint_future[0]-request.ownCrazyflie.x)/deltaT;
	trajectory_velocity[1] = (trajectory_temp_setpoint_future[1]-request.ownCrazyflie.y)/deltaT;
maruggv's avatar
maruggv committed
857
	trajectory_velocity[2] = 0;
mastefan's avatar
mastefan committed
858

859

mastefan's avatar
mastefan committed
860
861
862
863
864
865
866
867
868
869
870
	// clip if velocity is too big
	float tot_velocity_xy = sqrt(pow(trajectory_velocity[0],2)+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[2]<-0.8){
		trajectory_velocity[2] = -0.8;
	}else if(m_velocity[2]>0.8){
		trajectory_velocity[2] = 0.8;
	}
871

maruggv's avatar
maruggv committed
872

873
	// TO BE TESTED: (different concept for clipping velocity)
maruggv's avatar
maruggv committed
874
875
876
877
878

	float trajectory_real_velocity[3];
	trajectory_real_velocity[0] = (trajectory_temp_setpoint_future[0]-trajectory_temp_setpoint[0])/deltaT;
	trajectory_real_velocity[1] = (trajectory_temp_setpoint_future[1]-trajectory_temp_setpoint[1])/deltaT;
	trajectory_real_velocity[2] = (trajectory_temp_setpoint_future[2]-trajectory_temp_setpoint[2])/deltaT;
879

880
881
882
883
	float tot_real_velocity_xy = sqrt(pow(trajectory_real_velocity[0],2)+pow(trajectory_real_velocity[1],2));
	trajectory_velocity[0] = trajectory_velocity[0]*tot_real_velocity_xy/tot_velocity_xy;
	trajectory_velocity[1] = trajectory_velocity[1]*tot_real_velocity_xy/tot_velocity_xy;
	*/
884

mastefan's avatar
mastefan committed
885
886
887
888
}



maruggv's avatar
maruggv committed
889
890
891
892
893
894
895
// returns (x,y,z) for a given time t
// Calculate the trajectory from the Crazyflie to the mothership
// Trajectory: xcf0 -> xm1 -> xm2 -> xms, All segments are straight lines. (For testing purposes) Later should be curves
// xcf0: inital(Button pressed) position of Crazyflie
// xm1: next Point after the Crazyflie: the Drone should reach the final height by xm1
// xm2: Point behind the mothership
// xms: Point on mothership where we will land
mastefan's avatar
mastefan committed
896

maruggv's avatar
maruggv committed
897
898
899
900
// Please use trajectory_t0 as starting time
// Input: t in [trajectory_t0 and trajectory_t3]
// If t > trajectory_t3 then OUTPUT: (-1,-1,-1) -> exception handling
std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
mastefan's avatar
mastefan committed
901

maruggv's avatar
maruggv committed
902
903
	// (x,y,z,vx,vy,vz) where abs(vx,vy,vz) = trajectory_velocity_of_CF
	std::vector<float> trajectory_return(6);
mastefan's avatar
mastefan committed
904
905


maruggv's avatar
maruggv committed
906
907
908
909
910
911
912
	// If this method is called the first time after follow trajectory button
	// or land on mothership button
	// then save xcf0 and calculate xm1
	if(first_trajectory_calculation){
		xcf0[0] = request.ownCrazyflie.x;
		xcf0[1] = request.ownCrazyflie.y;
		xcf0[2] = request.ownCrazyflie.z;
mastefan's avatar
mastefan committed
913

maruggv's avatar
maruggv committed
914
		// TODO possible change: choose xm1 based on relative position CF and mothership, CF velocity, mothership velocity
915
916
917
918
919
920
921
922
923
924
925
		float xm1_normalized_direction = 1.0/(xm1_normalizing_factor) * (request.otherCrazyflies[0].x - xcf0[0]);

		//normalized direction has to be between -1 and 1 such that together with the scaling factor we get reasonable values
		if(xm1_normalized_direction > 1){
			xm1_normalized_direction = 1;
		}else if(xm1_normalized_direction < -1){
			xm1_normalized_direction = -1;
		}

		xm1_x_distance = xm1_scaling_factor*xm1_normalized_direction;

maruggv's avatar
maruggv committed
926
927
		xm1[0] = xcf0[0] + xm1_x_distance; // xm1_x_distance hardcoded at the moment
		xm1[1] = xcf0[1];
mastefan's avatar
mastefan committed
928
		xm1[2] = request.otherCrazyflies[0].z + xm1_height;
mastefan's avatar
mastefan committed
929

maruggv's avatar
maruggv committed
930
931
		first_trajectory_calculation = false;
	}
mastefan's avatar
mastefan committed
932

933
934
	float sinYaw = sin(request.otherCrazyflies[0].yaw);
	float cosYaw = cos(request.otherCrazyflies[0].yaw);
mastefan's avatar
mastefan committed
935

maruggv's avatar
maruggv committed
936
937
938
	// TODO cosYaw and sinYaw maybe otherway round
	// xm2_distance_to_ms: hardcoded distance
	// xm2_distance_to_ms should be chosen based on mothership velocity
939
940
941
942
943
944
945
946
947
948
949
950


	float ms_abs_velocity = sqrt(mothership_vel[0] * mothership_vel[0] + mothership_vel[1] * mothership_vel[1] + mothership_vel[2] * mothership_vel[2]);

	float normalized_ms_velocity = ms_abs_velocity/xm2_distance_to_ms_at_zero_velocity;

	if(normalized_ms_velocity > xm2_distance_to_ms_at_zero_velocity){
		normalized_ms_velocity = xm2_distance_to_ms_at_zero_velocity;
	}

	xm2_distance_to_ms = xm2_distance_to_ms_at_zero_velocity - xm2_distance_to_ms_scaling_factor * normalized_ms_velocity;

maruggv's avatar
maruggv committed
951
952
	xm2[0] = request.otherCrazyflies[0].x - cosYaw*xm2_distance_to_ms;
	xm2[1] = request.otherCrazyflies[0].y - sinYaw*xm2_distance_to_ms;
953
	xm2[2] = request.otherCrazyflies[0].z + xm2_height_over_ms;
mastefan's avatar
mastefan committed
954

maruggv's avatar
maruggv committed
955
956
	xms[0] = request.otherCrazyflies[0].x;
	xms[1] = request.otherCrazyflies[0].y;
mastefan's avatar
mastefan committed
957
	xms[2] = request.otherCrazyflies[0].z+0.2; // TODO add a certain height to not collide with mothership
mastefan's avatar
mastefan committed
958
959


maruggv's avatar
maruggv committed
960
	// Calculate the whole trajectory distance
961
962
963
	trajectory_total_distance = 	calculate_distance_in_xyz(xcf0,xm1) +
									calculate_distance_in_xyz(xm1,xm2) +
									calculate_distance_in_xyz(xm2,xms);
mastefan's avatar
mastefan committed
964
965


maruggv's avatar
maruggv committed
966
	trajectory_duration = trajectory_total_distance/trajectory_velocity_of_CF;
mastefan's avatar
mastefan committed
967

maruggv's avatar
maruggv committed
968
969
970
971
972
973
974
	// Calculate each duration of the different segments of the trajectory
	// Time between xcf0 and xm1
	trajectory_duration_1 = calculate_distance_in_xyz(xcf0, xm1)/trajectory_velocity_of_CF;
	// Time between xm1 and xm2
	trajectory_duration_2 = calculate_distance_in_xyz(xm1,xm2)/trajectory_velocity_of_CF;
	// Time between xm2 and xms
	trajectory_duration_3 = calculate_distance_in_xyz(xm2,xms)/trajectory_velocity_of_CF;
mastefan's avatar
mastefan committed
975

maruggv's avatar
maruggv committed
976
977
978
979
980
981
	// Time when at xm1
	trajectory_t1 = trajectory_t0 + trajectory_duration_1;
	// Time when at xm2
	trajectory_t2 = trajectory_t1 + trajectory_duration_2;
	// Time when at xms
	trajectory_t3 = trajectory_t2 + trajectory_duration_3;
mastefan's avatar
mastefan committed
982
983
984



maruggv's avatar
maruggv committed
985
986
	// Calculate the x,y,z position depending on where the CF should be at a certain time t

maruggv's avatar
maruggv committed
987
	if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position & velocity on line between xcf0 and xm1
maruggv's avatar
maruggv committed
988
		for(int i = 0; i < 3; i++){
mastefan's avatar
mastefan committed
989
			trajectory_return[i] = xcf0[i] + t*(xm1[i] - xcf0[i])/trajectory_t1;
maruggv's avatar
maruggv committed
990
			trajectory_return[i+3] = (xm1[i]-xcf0[i])/calculate_distance_in_xyz(xcf0, xm1) * trajectory_velocity_of_CF;
maruggv's avatar
maruggv committed
991
992
993
		}
	}else if(trajectory_t1 < t && t <= trajectory_t2){ // Calculate position on line between xm1 and xm2
		for(int i = 0; i < 3; i++){
mastefan's avatar
mastefan committed
994
			trajectory_return[i] = xm1[i] + (t - trajectory_t1)*(xm2[i] - xm1[i])/(trajectory_t2 - trajectory_t1);
maruggv's avatar
maruggv committed
995
			trajectory_return[i+3] = (xm2[i]-xm1[i])/calculate_distance_in_xyz(xm1, xm2) * trajectory_velocity_of_CF;
maruggv's avatar
maruggv committed
996
997
998
		}
	}else if(trajectory_t2 < t && t <= trajectory_t3){ // Calculate position on line between xm2 and xms
		for(int i = 0; i < 3; i++){
mastefan's avatar
mastefan committed
999
			trajectory_return[i] = xm2[i] + (t - trajectory_t2)*(xms[i] - xm2[i])/(trajectory_t3 - trajectory_t2);
maruggv's avatar
maruggv committed
1000
			trajectory_return[i+3] = (xms[i]-xm2[i])/calculate_distance_in_xyz(xms, xm2) * trajectory_velocity_of_CF;