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, as well as 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.

Setting up 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