Controller

The Crazyflie platform is completely customizable when it comes to the control design. As long as the controller can be written in C or C++, we can implement it in the Platform. This applies to both the controllers run on the PC Client and the controllers run on the Firmware of the Crazyflie. Originally the only controllers in the Platform were Firmware PID's that controlled the angular position and rates of the Crazyflie for hand flight. The original controller layout can be seen in Figure 1.

Below you will find documentation on the different types of Controllers that have been applied to the platform, organized by their location in the Platform.

Firmware

Original PID Controllers (Angular Rotation)

Directly on the Crazyflie Firmware there are 5 PID controllers as seen here:

The Pitch and Roll angular position PID's receive constant setpoints from some outside source (gamepad controller or automated controller). The outputs of those angular position controllers is then fed into the angular rate controllers as shown. This final output is then fed into the Motor Command block which converts the angular rate setpoints into motor commands.

---

State Estimator

(***NOT YET IMPLEMENTED***)

Bitcraze is currently working on adding a state estimator onto the Firmware to handle control calculations instead of the PID.

The code is in place but the controller is not yet used as of 7/13/16.

PC Client

PID Controllers (Position and Yaw)

The first controller implemented on the Client were 4 PID controllers used to stabilize the Crazyflie's position in X, Y, and Z, and angular position in Yaw respectively. These PID's feed directly into the Firmware PID's as seen here:

The controllers inside the red box are the original PID controllers located on the Firmware as shown above in the Original PID's.

Initialize the PID's

1. To start we create a struct with all the necessary variables (code from pid.h):

typedef struct
{
  float desired;     //< set point
  float error;        //< error
  float prevError;    //< previous error
  float integ;        //< integral
  float deriv;        //< derivative
  float kp;           //< proportional gain
  float ki;           //< integral gain
  float kd;           //< derivative gain
  float outP;         //< proportional output (debugging)
  float outI;         //< integral output (debugging)
  float outD;         //< derivative output (debugging)
  float iLimit;       //< integral limit
  float iLimitLow;    //< integral limit
  float dt;           //< delta-time dt
  float angularLimit;  //sets limit for how far Crazyflie can tip, to prevent flipping
  float angularLimitLow;  //^
  float thrustLimit;	//Sets upper limit for Thrust Output (60000?)  **?
  float thrustLimitLow;	//Sets lower limit for thrust Output (10001)
} PidObject;

2. To simplify scaling up the code to multiple Crazyflies, we will create a ControllerObject that contains multiple PID structs as shown (code in controller.h):

typedef struct
{
	PidObject pidRoll;
	PidObject pidPitch;
	PidObject pidYaw;

	PidObject pidX;
	PidObject pidY;
	PidObject pidZ;

	bool isInit;
	
} ControllerObject;

3. Then we initialize the Objects to 0 at the start of each run (code in eris_vrpn.cpp):

ControllerObject pidCtrl1 = { 0 }; // init all fields to zero
ControllerObject pidCtrl2 = { 0 }; // init all fields to zero
ControllerObject pidCtrl3 = { 0 }; // init all fields to zero
ControllerObject pidCtrl4 = { 0 }; // init all fields to zero

4. Then populate the PidObjects with the desired parameters (code in pid.c):

void pidInit(PidObject* pid, const float desired, const float kp,
					const float ki, const float kd, const float dt)
{
	pid->error = 0;
	pid->prevError = 0;
	pid->integ = 0;
	pid->deriv = 0;
	pid->desired = desired;
	pid->kp = kp;
	pid->ki = ki;
	pid->kd = kd;
	pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT;
	pid->iLimitLow = -DEFAULT_PID_INTEGRATION_LIMIT;
	pid->angularLimit = DEFAULT_PID_ANGLE_OUTPUT;
	pid->angularLimitLow = -DEFAULT_PID_ANGLE_OUTPUT;
	pid->thrustLimit = DEFAULT_PID_HIGH_THRUST_LIMIT;
	pid->thrustLimitLow = DEFAULT_PID_LOW_THRUST_LIMIT;
	pid->dt = dt;
}

Using the above function for each of the PidObjects inside of the ControllerObject below (code in controller.c):

void controllerInit( ControllerObject * ctrl )
{
	if(ctrl->isInit)
		return;
		
	pidInit(&ctrl->pidY, 0, PID_Y_KP, PID_Y_KI, PID_Y_KD, CAMERA_UPDATE_DT);  //for Roll PID
	pidInit(&ctrl->pidX, 0, PID_X_KP, PID_X_KI, PID_X_KD, CAMERA_UPDATE_DT);  //for Pitch PID
	pidInit(&ctrl->pidYaw, 0, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, CAMERA_UPDATE_DT);	
	pidInit(&ctrl->pidZ, 0, PID_Z_KP, PID_Z_KI, PID_Z_KD, CAMERA_UPDATE_DT); 
	
	pidSetIntegralLimit(&ctrl->pidY, PID_Y_INTEGRATION_LIMIT);
	pidSetIntegralLimit(&ctrl->pidX, PID_X_INTEGRATION_LIMIT);
	pidSetIntegralLimit(&ctrl->pidYaw, PID_YAW_INTEGRATION_LIMIT);
	pidSetIntegralLimit(&ctrl->pidZ, PID_Z_INTEGRATION_LIMIT); 
	
	ctrl->isInit = true;
}

Note: If we have multiple Crazyflies we need a ControllerObject for each Crazyflie (code in eris_vrpn.cpp)

	controllerResetAllPID( &pidCtrl1 );
	controllerInit( &pidCtrl1 );
#endif
#if NUM_QUADS >= 2
	controllerResetAllPID( &pidCtrl2 );
	controllerInit( &pidCtrl2 );
#endif
#if NUM_QUADS >=3
	controllerResetAllPID( &pidCtrl3 );
	controllerInit( &pidCtrl3 );
#endif
#if NUM_QUADS >=4
	controllerResetAllPID( &pidCtrl4 );
	controllerInit( &pidCtrl4 );
#endif

Now the initialization of the Controllers is complete.

Calculating the PID's

1. Each loop we want to calculate the PID output (code in pid.c):

float pidUpdateAngle(PidObject* pid, const float measured, const bool updateError) //**USED FOR ATTITUDE CONTROL
{
	float output;
	
	if(updateError)
	{
		pid->error = pid->desired - measured;  //Calculates current error
	}
	
	pid->integ += pid->error * pid->dt;            //Calculates integral term
	if(pid->integ > pid->iLimit)                   //Checks that the integral term stays below the integral limit (anti-windup)
	{
		pid->integ = pid->iLimit; 
	}
	else if(pid->integ < pid->iLimitLow)
	{
		pid->integ = pid->iLimitLow;
	}
	
	pid->deriv = (pid->error - pid->prevError) / pid->dt;  //Calculates derivative term
	
	pid->outP = pid->kp* pid->error;
	pid->outI = pid->ki * pid->integ;
	pid->outD = pid->kd * pid->deriv;
	
	output = pid->outP + pid->outI + pid->outD;  //Adds up all 3 calculated terms for output
	
	if(output > pid->angularLimit)	             //Limits amount Crazyflie can tip
	{
		output = pid->angularLimit;
	}
	
	if(output < pid->angularLimitLow)	     //Limits amount Crazyflie can tip
	{
		output = pid->angularLimitLow;
	}
	
	pid->prevError = pid->error;                 //Stores the current error for next loop
	
	return output;
}

Note: There are 3 different versions of this Function (for Angle, Thrust, and Yaw) since each has different output limits.

2. Using the function above, combine all PID calculations into one function (code in controller.c):

void controllerCorrectAttitudePID(ControllerObject * ctrl,
    float xPositionActual, float yPositionActual, float zPositionActual, float eulerYawActual, float xPositionDesired, float yPositionDesired, 
        float zPositionDesired, float eulerYawDesired, float* rollControlOutput, float* pitchControlOutput, float* yawControlOutput, float* thrustControlOutput)
{
	pidSetDesired(&ctrl->pidX, xPositionDesired);
	*pitchControlOutput = pidUpdateAngle(&ctrl->pidX, xPositionActual, false);		//Originally true
	
	pidSetDesired(&ctrl->pidY, yPositionDesired);
	*rollControlOutput = pidUpdateAngle(&ctrl->pidY, yPositionActual, false);		//Originally true
	
	pidSetDesired(&ctrl->pidZ, zPositionDesired);
	*thrustControlOutput = pidUpdateThrust(&ctrl->pidZ,zPositionActual, true);
	
	float yawError;
	pidSetDesired(&ctrl->pidYaw, eulerYawDesired);
	yawError = eulerYawDesired - eulerYawActual;
	if(yawError > 180.0)                                       //Ensures Yaw error stays between -180 and 180 degrees
		yawError -= 360.0;
	else if(yawError < -180.0)
		yawError += 360.0;
	pidSetError(&ctrl->pidYaw, yawError);
	
	*yawControlOutput = pidUpdateYaw(&ctrl->pidYaw, eulerYawActual, true);  	//originally false
}

3. In each Callback, calculate the Controller outputs, assigned to pointer values*rollControlOutput, *pitchControlOutput, etc... (code in eris_vrpn.cpp):

xError1 = xPositionDesired1 - xPosition1;
yError1 = yPositionDesired1 - yPosition1;           //Calculates error externally
controllerSetXYError(&pidCtrl1, xError1, yError1);

controllerCorrectAttitudePID(&pidCtrl1, t.pos[0], t.pos[1], zPosition1, correctedYaw1,
	                        xPositionDesired1, yPositionDesired1, zPositionDesired1,
		                   yawDesired1, &rollControlOutput1, &pitchControlOutput1,
			              &yawControlOutput1, &thrustControlOutput1);

Note: We calculate error externally for X and Y because certain swarm controllers need special setpoint modifiers (this way we can switch modes without effecting the default setpoint):

Example: Follow Hand Mode Error (Crazyflie1 will have setpoint -0.5 m lower in X and +0.5 m higher in Y than your hand trackable):

...
xError1 = (xPositionHand - 0.5) - xPosition1;
yError1 = (yPositionHand + 0.5) - yPosition1;
controllerSetXYError(&pidCtrl1, xError1, yError1);
...

4. Repeat for each set of new location data.