DroneXControllerService.cpp

//    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.
//
//    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.
//
//    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.
//
//    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/>.
//
//
//    ----------------------------------------------------------------------------------
//    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:
//    Place for students to implement their controller
//
//    ----------------------------------------------------------------------------------





// 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
//    ----------------------------------------------------------------------------------




// 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)
	{
		for(int i = 0; i < 4; ++i)
		{
			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];

			// anti windup for yaw
			if (i==3){
				if (setpoint_difference > PI)
					setpoint_difference -= 2*PI;
				if (setpoint_difference < -PI)
					setpoint_difference += 2*PI;
			}
			// 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;
		}
		//m_setpoint_for_controller[3] = m_setpoint[3];

	}
	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];
	}

}








void buttonPressed_take_off(){

	//if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
		ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Taking off...");
		previous_flying_state = flying_state;
		flying_state = DRONEX_STATE_TAKING_OFF;
	//}else{
	//	ROS_ERROR("Cannot change to DRONEX_STATE_TAKING_OFF");
	//}
}

void buttonPressed_land(){
	//if(flying_state == DRONEX_STATE_HOVER){
		ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Start flight-sequence LA...");
		// OLD:flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
		// NEW:
		flightSequence = SEQUENCE_LAND_ON_MOTHERSHIP;
}

void buttonPressed_abort(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Abort Mission!");
	previous_flying_state = flying_state;
	flying_state = DRONEX_STATE_LAND_ON_GROUND;

	// reset start position
	savedStartCoordinates = false;
	savedStartCoordinatesFromMS = false;
}


void buttonPressed_integrator_on(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn ON integrator");
	integratorFlag = DRONEX_INTEGRATOR_ON;
}

void buttonPressed_integrator_off(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn OFF integrator");
	integratorFlag = DRONEX_INTEGRATOR_OFF;
}


void buttonPressed_integrator_reset(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET integrator to zero");
	integratorFlag = DRONEX_INTEGRATOR_RESET;

	//New:
	m_time_ticks = 0;
	setarrayzero3(integrator_sum_XYZ, 3);
	//integrator_sum_XYZ = {0,0,0};
	tookOffFlag = false;
	approachedFlag = false;
	landedFlag = false;
	savedStartCoordinates = false;
	savedStartCoordinatesFromMS = false;



	std::fill(tol_takeoff.begin(),tol_takeoff.end(), 0);

	std::fill(tol_approach.begin(),tol_approach.end(), 0);

	std::fill(tol_land.begin(),tol_land.end(), 0);


	/*std::vector<float> tol_takeoff (3, 0.0);
	std::vector<float> tol_approach (3, 0.0);
	std::vector<float> tol_land (3, 0.0);
	*/

	trajectory_start_time = 0;
	total_time_since_start = 0;
	first_trajectory_calculation = true;

	trajectory_deltaT_position = 0;
	trajectory_deltaT_velocity = 0;

	/*xcf0 = */std::fill(xcf0.begin(),xcf0.end(), 0);
	std::fill(xm1.begin(),xm1.end(), 0);
	std::fill(xm2.begin(),xm2.end(), 0);
	std::fill(xms.begin(),xms.end(), 0);
	/*std::vector<float> xm1(&3, 0.0);
	std::vector<float> xm2(&3, 0.0);
	std::vector<float> xms(&3, 0.0);

	*/
	trajectory_duration_1 = 0; // Duration of flight between xcf0 and xm1
	trajectory_duration_2 = 0; // Duration of flight between xm1 and xm2
	trajectory_duration_3 = 0; // Duration of flight between xm2 and xms

	trajectory_t0 = 0; // Time when Button pressed to land on mothership
	trajectory_t1 = 0; // Time at xm1
	trajectory_t2 = 0; // Time at xm2

	xm1_normalizing_factor = 3;   // normalize the difference between xcf0 and xms
	xm1_scaling_factor = 0.6;

	xm2_distance_to_ms_at_zero_velocity = 0.6;
	xm2_distance_to_ms_scaling_factor = 1;

	trajectory_velocity_of_CF; // fetched from yaml
	trajectory_total_distance = 0;
	trajectory_duration = 0;

	// returned trajectory setpoint & velocity
	//trajectory_setpoint = {0,0,0,0}; // x,y,z,yaw
	setarrayzero4(trajectory_setpoint, 4);

	//trajectory_velocity = {0,0,0}; // x,y,z
	setarrayzero3(trajectory_velocity, 3);

	// Origin of the Flying zone
	//float originX = 0;
	//float originY = 0;


	// > The setpoints for (&x,y,z) position and yaw angle, in that order
	//m_setpoint = {0.0,0.0,0.0,0.0};
	setarrayzero4(m_setpoint, 4);
	//m_setpoint_for_controller = {0.0,0.0,0.0,0.0};
	setarrayzero4(m_setpoint_for_controller, 4);
	// > The velocities for (&x,y,z) position, in that order
	//m_velocity = {0.0,0.0,0.0};
	setarrayzero3(m_velocity, 3);
	//m_velocity_for_controller = {0.0,0.0,0.0};
	setarrayzero3(m_velocity_for_controller, 3);
	// > Small adjustments to the x-y setpoint
	m_xAdjustment = 0.0f;
	m_yAdjustment = 0.0f;

	// Boolean for whether to limit rate of change of the setpoint
	m_shouldSmoothSetpointChanges = false;

	//prev_MS_pos = {0,0,0};
	setarrayzero3(prev_MS_pos, 3);
	//current_MS_pos  = {0,0,0};
	setarrayzero3(current_MS_pos, 3);
	//mothership_vel = {0,0,0};
	setarrayzero3(mothership_vel, 3);

	// thrust factor to do a smooth landing
	thrust_factor = 1;
	reset_rviz = false;

	flightSequence = 0;

}

void setarrayzero3(float arr[3], int n){
	for(int i = 0; i<n; i++){
		arr[i] = 0;
	}
}

void setarrayzero4(float arr[4], int n){
	for(int i = 0; i<n; i++){
		arr[i] = 0;
	}
}


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

	// Initialize trajectory timers
	flightSequence = SEQUENCE_NONE;
	previous_flying_state = flying_state;
	flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
	total_time_since_start = 0;
	//trajectory_x_radius = 0;
	//trajectory_y_radius = 0;
	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");

}







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?
    m_mass_total_grams = weightParam.x;


}











//    ------------------------------------------------------------------------------
//     OOO   U   U  TTTTT  EEEEE  RRRR
//    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
//
// IMPORTANT NOTES FOR "onboardControllerType"  AND AXIS CONVENTIONS
// > The allowed values for "onboardControllerType" are in the "Defines" section at the
//   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"
//   specify the angular rate in [radians/second] that will be requested from the
//   PID controllers running in the Crazyflie 2.0 firmware.
// > With CF_COMMAND_TYPE_RATE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
//   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).
// > With CF_COMMAND_TYPE_RATE the axis convention for the roll, pitch, and yaw body rates returned
//   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)
{

	// Keep track of time
	m_time_ticks++;
	m_time_seconds = float(m_time_ticks) / m_vicon_frequency;

	switch(flying_state){
		case DRONEX_STATE_APPROACH:
		{
			//ROS_INFO("DRONEX_STATE_APPROACH");
			dronexSetpoint.x = request.otherCrazyflies[0].x;
			dronexSetpoint.y = request.otherCrazyflies[0].y;
			dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;

			/*
			ROS_INFO_STREAM("APPROACH: (x,y,z) Difference: ("
				<< request.ownCrazyflie.x-dronexSetpoint.x << ", "
				<< request.ownCrazyflie.y-dronexSetpoint.y << ", "
				<< request.ownCrazyflie.z-dronexSetpoint.z << ")");
			*/
			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] ){
				approachedFlag = true;
				ROS_INFO("approached");
			}
		}
		break;

		case DRONEX_STATE_GROUND:
		{
			//ROS_INFO("DRONEX_STATE_GROUND");
			// Variable for choosing flight sequence off
			flightSequence = SEQUENCE_NONE;

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

			integratorFlag == DRONEX_INTEGRATOR_OFF;

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

		}
		break;

		case DRONEX_STATE_ON_MOTHERSHIP:
		{
			//ROS_INFO("DRONEX_STATE_ON_MOTHERSHIP");
			// Variable for choosing flight sequence off
			flightSequence = SEQUENCE_NONE;

			// 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:
		{
			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] ){

				//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;

			//}else {
			//
			//	flying_state = DRONEX_STATE_APPROACH;
			//	ROS_INFO_STREAM("Entering from DRONEX_STATE_LAND_ON_MOTHERSHIP: DRONEX_STATE_APPROACH");
			//}
		}
		break;

		case DRONEX_STATE_LAND_ON_GROUND:
		{
			if(tookOffFlag){
				ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
				dronexSetpoint.x = request.ownCrazyflie.x;
				dronexSetpoint.y = request.ownCrazyflie.y;
				dronexSetpoint.z = 0.0;

				tookOffFlag = false;
			}

		}
		break;

		case DRONEX_STATE_TAKING_OFF:
		{
			//ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
			thrust_factor = 1;

			//Check where we start from, if from mothership first go straight up to a certain height
			// before aiming the startCoordinates.
			if(previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP  && taking_off_from_ms_height_gain){
				if(!savedStartCoordinatesFromMS)
				{
					startCoordinateFromMS[0] = request.otherCrazyflies[0].x;
					startCoordinateFromMS[1] = request.otherCrazyflies[0].y;
					startCoordinateFromMS[2] = request.otherCrazyflies[0].z;

					savedStartCoordinatesFromMS = true;

					ROS_INFO_STREAM("DRONEX: saved start Coordinates");
					ROS_INFO_STREAM("x = " << startCoordinateX);
					ROS_INFO_STREAM("y = " << startCoordinateY);
					ROS_INFO_STREAM("z = " << startCoordinateZ);

				}





					dronexSetpoint.x = startCoordinateFromMS[0];
					dronexSetpoint.y = startCoordinateFromMS[1];
					dronexSetpoint.z = startCoordinateFromMS[1] + 0.4;





				// 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 << ")");

				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]) {

					//tookOffFlag = true;

					//ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
					//previous_flying_state = flying_state;
					//flying_state = DRONEX_STATE_HOVER;

					//reset_rviz = true;
					taking_off_from_ms_height_gain = false;
				}
			}else{ // previous_flying_state == DRONEX_STATE_ON_MOTHERSHIP








			if(!savedStartCoordinates)
			{
				startCoordinateX = request.ownCrazyflie.x;
				startCoordinateY = request.ownCrazyflie.y;
				startCoordinateZ = request.ownCrazyflie.z;

				savedStartCoordinates = true;

				ROS_INFO_STREAM("DRONEX: saved start Coordinates");
				ROS_INFO_STREAM("x = " << startCoordinateX);
				ROS_INFO_STREAM("y = " << startCoordinateY);
				ROS_INFO_STREAM("z = " << startCoordinateZ);

			}





				dronexSetpoint.x = startCoordinateX;
				dronexSetpoint.y = startCoordinateY;
				dronexSetpoint.z = startCoordinateZ + 0.4;





			// 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 << ")");

			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]) {
				ROS_INFO("took off");
				tookOffFlag = true;

				ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
				previous_flying_state = flying_state;
				flying_state = DRONEX_STATE_HOVER;
				taking_off_from_ms_height_gain = false;
				reset_rviz = true;
			}
		}
	}
		break;

		case DRONEX_STATE_HOVER:
		{
			//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
			// keep setpoint constant

			// for testing hover over mothership
			/*
			dronexSetpoint.x = request.otherCrazyflies[0].x;
			dronexSetpoint.y = request.otherCrazyflies[0].y;
			dronexSetpoint.z = request.otherCrazyflies[0].z+0.3;
			*/

		}
		break;

		case DRONEX_STATE_FOLLOWING_TRAJECTORY:
			/*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] )*/
			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]) ){

				previous_flying_state = flying_state;
				flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
				ROS_INFO_STREAM("Entering from Trajectory: DRONEX_STATE_LAND_ON_MOTHERSHIP");
			} else{
				ROS_INFO_STREAM("distance to xm2: "<<request.ownCrazyflie.x-xm2[0]<<", "<<request.ownCrazyflie.y-xm2[1]<<", "<<request.ownCrazyflie.z-xm2[2]);
			}
		break;


	} // END switch case




	// flightSeqeunce 1: simple approaching and landing on static mothership
	if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
		//ROS_INFO_STREAM("Entering: DRONEX_STATE_TAKING_OFF");
		previous_flying_state = flying_state;
		flying_state = DRONEX_STATE_TAKING_OFF;

		//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
		if(tookOffFlag){
			//ROS_INFO_STREAM("Entering: DRONEX_STATE_APPROACH");
			previous_flying_state = flying_state;
			flying_state = DRONEX_STATE_APPROACH;

			if(approachedFlag){
				ROS_INFO_STREAM("Entering: DRONEX_STATE_LAND_ON_MOTHERSHIP");
				previous_flying_state = flying_state;
				flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
			}

		}

	}

/*
	// 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]
			}
		}

	}
*/


	// 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);

	calculateMSVelocity(request);

	// 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


	if(controller_mode == 0){	// lqr controller

		m_shouldSmoothSetpointChanges = true;
		dronexSetpoint.yaw = request.otherCrazyflies[0].yaw;

		// do not change, use setpoint defined in states

	}else if(controller_mode == 1){	// nested lqr controller

		if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){	// Trajectory Follower
			// to implement the trajectory tracking
			m_shouldSmoothSetpointChanges = false;

			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];



			/*
			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;

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

		}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;

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

			dronexVelocity.x = 0;
			dronexVelocity.y = 0;
			dronexVelocity.z = 0;
		}
	}

	setpointCallback(dronexSetpoint);
	m_velocity_for_controller[0] = dronexVelocity.x;
	m_velocity_for_controller[1] = dronexVelocity.y;
	m_velocity_for_controller[2] = dronexVelocity.z;

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

	// > Call the function to perform the conversion
	float stateErrorBody[12];
	convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller, stateErrorBody);


	// CARRY OUT THE CONTROLLER COMPUTATIONS
	// Call the function that performs the control computations for this mode

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

	if(flying_state == DRONEX_STATE_LAND_ON_GROUND && (request.ownCrazyflie.z < 0.05 )){
		ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
		flying_state = DRONEX_STATE_GROUND;
		previous_flying_state = DRONEX_STATE_LAND_ON_GROUND;
	}
	else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && 	(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 - 0.03 - 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;
	}

	if(flying_state == DRONEX_STATE_ON_MOTHERSHIP){
		motorsOFF(response);
		ROS_INFO("On Mothership");
	}else if(flying_state == DRONEX_STATE_GROUND){
		motorsOFF(response);
	}
	else{
		calculateControlOutputDroneX(request, response, stateErrorBody);	// chooses controller mode
	}


	// PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
	if (shouldPublishCurrent_xyz_yaw)
	{
		publish_current_xyz_yaw(request.ownCrazyflie.x,request.ownCrazyflie.y,request.ownCrazyflie.z,request.ownCrazyflie.yaw);
	}

	// PUBLISH THE DEBUG MESSAGE (if required)
	if (shouldPublishDebugMessage)
	{
		construct_and_publish_debug_message(request,response);
	}


	// RETURN "true" TO INDICATE THAT THE COMPUTATIONS WERE SUCCESSFUL
	return true;
}

void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context){
	area = context.localArea;
	originX = (area.xmin + area.xmax) / 2.0;
	originY = (area.ymin + area.ymax) / 2.0;

	ROS_INFO_STREAM("New OriginX: " << originX << " New OriginY: " << originY);
}


//	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
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response, float (&stateErrorBody)[12]){

	if(controller_mode == 0){

		// integrator
		integrator_XYZ(stateErrorBody);

		// Compute control output via LQR controller:
		calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);

	}else if(controller_mode == 1){

		// integrator
		integrator_XYZ(stateErrorBody);

		// Compute control output via Nested LQR controller:
		calculateControlOutput_viaLQRforRates_Nested(stateErrorBody, request, response);

	}else{ // Don't know what to do ^^
		ROS_ERROR("[DRONEXCONTROLLERSERVICE] Please change the controller mode");
	}

}