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

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

	}
	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...");
		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!");
	flying_state = DRONEX_STATE_LAND_ON_GROUND;

	// reset start position
	savedStartCoordinates = 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;
}


void buttonPressed_follow_trajectory(){
	ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] FOLLOW trajectory");
	flightSequence = SEQUENCE_NONE;
	flying_state = DRONEX_FOLLOW_TRAJECTORY;
	total_time_since_start = 0;
	trajectory_x_radius = 0;
	trajectory_y_radius = 0;
	trajectory_start_time = 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_CF_grams = weightParam.x;

}

void PitchBaselineParamCallback(const Setpoint& pitchAngleParam ) {
    // TODO
}











//    ------------------------------------------------------------------------------
//     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.4;//0.6;


			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");
			}
			//setpointCallback(dronexSetpoint);
		}
		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;

			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:
		{
			//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
			//to land on mothership (17.10. vm)
			dronexSetpoint.x = request.otherCrazyflies[0].x;	//set setpoint to droneX x y and z coordinates
			dronexSetpoint.y = request.otherCrazyflies[0].y;
			dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
		}
		break;

		case DRONEX_STATE_LAND_ON_GROUND:
		{
			if(tookOffFlag){
				ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
				// to land on ground (17.10. vm)
				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");

			if(!savedStartCoordinates)
			{
				startCoordinateX = request.ownCrazyflie.x; // Nöd sicher öbs das brucht. Idee: Afangscoordinate abspeichere zum in Hover state z ende.
				startCoordinateY = request.ownCrazyflie.y;
				startCoordinateZ = request.ownCrazyflie.z;

				savedStartCoordinates = true;
				//setpointCallback(dronexSetpoint);
				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;

			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");
				flying_state = DRONEX_STATE_HOVER;
			}
		}
		break;

		case DRONEX_STATE_HOVER:
		{
			//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
			// keep setpoint constant
			/*
			dronexSetpoint.x = dronexSetpoint.x;
			dronexSetpoint.y = dronexSetpoint.y;
			dronexSetpoint.z = dronexSetpoint.z;
			*/
		}
		break;

		case DRONEX_FOLLOW_TRAJECTORY:

		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");
		flying_state = DRONEX_STATE_TAKING_OFF;

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

			if(approachedFlag){
				ROS_INFO_STREAM("Entering: DRONEX_STATE_LAND_ON_MOTHERSHIP");
				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]
			}
		}

	}
*/

	calculateDroneXVelocity(request);

	setpointCallback(dronexSetpoint);

	/*dronexSetpoint.x = request.otherCrazyflies[0].x;
	dronexSetpoint.y = request.otherCrazyflies[0].y;
	dronexSetpoint.z = request.otherCrazyflies[0].z + 0.3*/

	//setpointCallback(dronexSetpoint);
	// CALL THE FUNCTION FOR PER CYLCE OPERATIONS

	perControlCycleOperations();

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


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






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

	if(flying_state == DRONEX_STATE_GROUND || flying_state == DRONEX_STATE_ON_MOTHERSHIP){
		motorsOFF(response);
	}
	else{
		calculateControlOutputDroneX(request, response);

		// for debugging:
		/*ROS_INFO_STREAM("deltaX to Mothership: " << request.ownCrazyflie.x - request.otherCrazyflies[0].x);
		ROS_INFO_STREAM("deltaY to Mothership: " << request.ownCrazyflie.y - request.otherCrazyflies[0].y);
		ROS_INFO_STREAM("deltaZ to Mothership: " << request.ownCrazyflie.z - request.otherCrazyflies[0].z);*/
	}


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




	if(controller_mode == 0){
		// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
		// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
		// > Define a local array to fill in with the body frame error
		float current_bodyFrameError[12];
		// > Call the function to perform the conversion



		convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller, current_bodyFrameError);

		integrator_XYZ(current_bodyFrameError);
		calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
	}else if(controller_mode == 1){




		// LQR for Angles

		// Read Mothership coordinates
		// x,y,z,yaw
		float ms_coordinates[4];
		/*ms_coordinates[0] = request.otherCrazyflies[0].x;
		ms_coordinates[1] = request.otherCrazyflies[0].y;
		ms_coordinates[2] = request.otherCrazyflies[0].z;
		ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/

		/*ms_coordinates[0] = dronexSetpoint.x;
		ms_coordinates[1] = dronexSetpoint.y;
		ms_coordinates[2] = dronexSetpoint.z;
		ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/

		ms_coordinates[0] = m_setpoint_for_controller[0];
		ms_coordinates[1] = m_setpoint_for_controller[1];
		ms_coordinates[2] = m_setpoint_for_controller[2];
		ms_coordinates[3] = request.otherCrazyflies[0].yaw;

		// Load Mothership velocity
		// x dot, y dot, z dot
		float ms_velocity[3];
		ms_velocity[0] = drone_X_vel[0];
		ms_velocity[1] = drone_X_vel[1];
		ms_velocity[2] = drone_X_vel[2];



		// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
		// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
		// > Define a local array to fill in with the body frame error
		float stateErrorBody[12];
		// > Call the function to perform the conversion
		convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody);


		float rollAngle_forResponse = 0;
		float pitchAngle_forResponse = 0;
		float thrustAdjustment = 0;

		// TODO: Do not forget to implement the yawController
		float yawAngle_forResponse = 0;


		integrator_XYZ(stateErrorBody);

		// integrator

		// BODY FRAME Y CONTROLLER
		rollAngle_forResponse  -= gainIntegratorAngle[0] * integrator_sum_XYZ[1];
		// BODY FRAME X CONTROLLER
		pitchAngle_forResponse -= gainIntegratorAngle[1] * integrator_sum_XYZ[0];
		// > ALITUDE CONTROLLER (i.e., z-controller):
		thrustAdjustment       -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];


		// Perform the "-Kx" LQR computation for the rates and thrust:
		for(int i = 0; i < 6; ++i){
			// BODY FRAME Y CONTROLLER
			rollAngle_forResponse -= 1.1*gainMatrixRollAngle[i] * stateErrorBody[i];
			// BODY FRAME X CONTROLLER
			pitchAngle_forResponse -= 1.1*gainMatrixPitchAngle[i] * stateErrorBody[i];
			// BODY FRAME X CONTROLLER
			//thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];

		}

		// Calculate the angle in x-y plane of DroneX for transformation to local coordinates
		float droneXAngle;
		float ms_velocity_norm = sqrt(pow(ms_velocity[0], 2.0) + pow(ms_velocity[1], 2.0)); //(ms_velocity[0])^2 * (ms_velocity[1])^2);

		if(ms_velocity_norm != 0){
			if (ms_velocity[1] >= 0)
			{
				droneXAngle = acos(ms_velocity[0] / ms_velocity_norm);
			}else{
				droneXAngle = -acos(ms_velocity[0] / ms_velocity_norm);
			}
		}

		// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
		float rollAngle_baseline = 1;
		float pitchAngle_baseline = 1;

		// TODO
		rollAngle_forResponse += rollAngle_baseline * (cos(droneXAngle) * ms_velocity[0]- sin(droneXAngle) * ms_velocity[1]);
		pitchAngle_forResponse += pitchAngle_forResponse * (sin(droneXAngle) * ms_velocity[0] + cos(droneXAngle) * ms_velocity[1]);


		// Calculate the Force Feedforward
		float F_in_newtons = (gravity * m_mass_total_grams)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse)*1000.0);




		// LQR for Rates
		// Calculate the Roll and Pitch Angle error

		float AngleError[3] = {
			stateErrorBody[6] - rollAngle_forResponse,
			stateErrorBody[7] - pitchAngle_forResponse,
			stateErrorBody[8]
		};

		float rollRate_forResponse = 0;
		float pitchRate_forResponse = 0;
		float yawRate_forResponse = 0;
		for(int i = 0; i < 4; i++){
			rollRate_forResponse -= gainMatrixRollRatefromAngle[i] * AngleError[i];
			pitchRate_forResponse -= gainMatrixPitchRatefromAngle[i] * AngleError[i];
			yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i];
		}

		/*for(int i = 0; i < 9; ++i){
			thrustAdjustment      -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
		}*/

		for(int i = 0; i < 6; ++i){
			thrustAdjustment      -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
		}


		//thrustAdjustment -= F;


		// UPDATE THE "RETURN" THE VARIABLE NAMED "response"

		// Put the computed rates and thrust into the "response" variable
		// > For roll, pitch, and yaw:
		response.controlOutput.roll  = rollRate_forResponse;
		response.controlOutput.pitch = pitchRate_forResponse;
		response.controlOutput.yaw   = yawRate_forResponse;
		// > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
		// > NOTE: remember that the thrust is commanded per motor, so you sohuld
		//         consider whether the "thrustAdjustment" computed by your
		//         controller needed to be divided by 4 or not.
		thrustAdjustment = thrustAdjustment / 4.0;
		// > Compute the feed-forward force
		float feed_forward_thrust_per_motor = F_in_newtons / 4.0; //m_mass_total_grams * 9.81/(1000*4);
		// > Put in the per motor commands
		response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
		response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
		response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
		response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);


		// Specify that this controller is a rate controller
		// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
		response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
		// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;


		// An alternate debugging technique is to print out data directly to the
		// command line from which this node was launched.
		/*if (shouldDisplayDebugInfo)
		{
			// An example of "printing out" the data from the "request" argument to the
			// command line. This might be useful for debugging.
			ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
			ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
			ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
			ROS_INFO_STREAM("roll       [deg]: " << request.ownCrazyflie.roll);
			ROS_INFO_STREAM("pitch      [deg]: " << request.ownCrazyflie.pitch);
			ROS_INFO_STREAM("yaw        [deg]: " << request.ownCrazyflie.yaw);
			ROS_INFO_STREAM("Delta t      [s]: " << request.ownCrazyflie.acquiringTime);

			// An example of "printing out" the control actions computed.
			ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
			ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
			ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
			ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);

			// An example of "printing out" the "thrust-to-command" conversion parameters.
			ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
			ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
			ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);

			// An example of "printing out" the per motor 16-bit command computed.
			ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
			ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
			ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
			ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
		}*/


	}else if(controller_mode == 2){// Trajectory Follower


		//TODO
		// Pragash: I've added this controller mode 2 to implement the trajectory tracking in the future
		// At the moment it's the same as in controller mode 1







		// LQR for Angles

		// Read Mothership coordinates
		// x,y,z,yaw
		float ms_coordinates[4];
		/*ms_coordinates[0] = request.otherCrazyflies[0].x;
		ms_coordinates[1] = request.otherCrazyflies[0].y;
		ms_coordinates[2] = request.otherCrazyflies[0].z;
		ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/

		/*ms_coordinates[0] = dronexSetpoint.x;
		ms_coordinates[1] = dronexSetpoint.y;
		ms_coordinates[2] = dronexSetpoint.z;
		ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/


		// Load Mothership velocity
		// x dot, y dot, z dot
		float ms_velocity[3];

		if(flying_state == DRONEX_FOLLOW_TRAJECTORY){

			calculateTrajectory();

			ms_coordinates[0] = trajectory_setpoint[0];
			ms_coordinates[1] = trajectory_setpoint[1];
			ms_coordinates[2] = trajectory_setpoint[2];
			ms_coordinates[3] = trajectory_setpoint[3];


			ms_velocity[0] = trajectory_velocity[0];
			ms_velocity[1] = trajectory_velocity[1];
			ms_velocity[2] = trajectory_velocity[2];

			ms_velocity[0] = 0;
			ms_velocity[1] = 0;
			ms_velocity[2] = 0;

		}else{
			ms_coordinates[0] = m_setpoint_for_controller[0];
			ms_coordinates[1] = m_setpoint_for_controller[1];
			ms_coordinates[2] = m_setpoint_for_controller[2];
			ms_coordinates[3] = request.otherCrazyflies[0].yaw;


			ms_velocity[0] = drone_X_vel[0];
			ms_velocity[1] = drone_X_vel[1];
			ms_velocity[2] = drone_X_vel[2];
		}



		// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
		// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
		// > Define a local array to fill in with the body frame error
		float stateErrorBody[12];
		// > Call the function to perform the conversion
		convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody);


		float rollAngle_forResponse = 0;
		float pitchAngle_forResponse = 0;
		float thrustAdjustment = 0;

		// TODO: Do not forget to implement the yawController
		float yawAngle_forResponse = 0;


		integrator_XYZ(stateErrorBody);

		// integrator

		// BODY FRAME Y CONTROLLER
		rollAngle_forResponse  -= gainIntegratorAngle[0] * integrator_sum_XYZ[1];
		// BODY FRAME X CONTROLLER
		pitchAngle_forResponse -= gainIntegratorAngle[1] * integrator_sum_XYZ[0];
		// > ALITUDE CONTROLLER (i.e., z-controller):
		thrustAdjustment       -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];


		// Perform the "-Kx" LQR computation for the rates and thrust:
		for(int i = 0; i < 6; ++i){
			// BODY FRAME Y CONTROLLER
			rollAngle_forResponse -= 1.1*gainMatrixRollAngle[i] * stateErrorBody[i];
			// BODY FRAME X CONTROLLER
			pitchAngle_forResponse -= 1.1*gainMatrixPitchAngle[i] * stateErrorBody[i];
			// BODY FRAME X CONTROLLER
			//thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];

		}

		// Calculate the angle in x-y plane of DroneX for transformation to local coordinates
		float droneXAngle;