Mindmap for a Quadcopter's Autopilot

Introduction

Drones and quadcopters are one of the coolest consumer electronic products. For an engineering student, an open source quadcopter can be an awesome educational kit to observe how the control system theories and aerodynamics theories can be applied in practice. This blog and attachments will focus on structure of Autopilot program based on Minifly open source quadcopter. Minifly open source quadcopter uses FreeRTos as real-time OS to schedule tasks. Following the code of a task function called “stabilizerTask”, we can understand the whole Autopilot’s decision process. This blog will also present a brief concept of Quaternions as rotations, Mahony filter, cascade PID controller. Readers are highly recommended to read corresponding material as prerequisites.

Since the code comment of Minifly open source quadcopter only has Chinese version, I will translate the majority of code comment of the “stabilizerTask” task, which is essential to understand the decision process. For the steps inside stabilizerTask whose structure is intricate, I have drawn Mind maps to support.

Explanation of Modes

Firstly, I will introduce corresponding behaviors of quadcopter in different modes:

ALT-Hold mode: the quadcopter will calibrate IMU first. Once the “taking off” command is received, quadcopter will hold at a specific altitude until “landing” command is received. User can adjust altitude through thrust button on RC transmitter.

Arco mode: thrust depends on RC transmitter merely. Altitude will no longer be held. Unlike ALT-Hold mode, if the thrust button is not pulled, then the motor speed is 0.

Loiter mode: quadcopter can use this mode only if an optical flow module is connected. In this mode the quadcopter will hover at a specific point and altitude. Since I haven’t purchased the optical flow module, I cannot comment on corresponding code.

Simple mode: quadcopter’s forward direction is fixed at earth coordination system.

X mode: quadcopter’s forward direction is fixed at body coordination system.

Flip mode: the quadcopter can turn upside down if this mode is enabled.

Main.c

The function of this file is initializing hardwares on quadcopter, initializing FreeRTOS, and start task scheduler.

int main()
{
   /*initializing hardware*/
   systemInit();
  
   /*create start task*/
   xTaskCreate(startTask, "START_TASK", 300, NULL, 2, &startTaskHandle);
  
   /*start task scheduler*/
   vTaskStartScheduler();
  
   while(1) {};
}
  
void startTask(void *arg)
{
   /*enter critical zone*/
   taskENTER_CRITICAL();
  
   /*wireless connection*/
   xTaskCreate(radiolinkTask, "RADIOLINK", 150, NULL, 5, NULL);
  
   /*USB transmit and receive*/
   xTaskCreate(usblinkRxTask, "USBLINK_RX", 150, NULL, 4, NULL);
   xTaskCreate(usblinkTxTask, "USBLINK_TX", 150, NULL, 3, NULL);
  
   /*information transmission with RC transmitter*/
   xTaskCreate(atkpTxTask, "ATKP_TX", 150, NULL, 3, NULL);
   xTaskCreate(atkpRxAnlTask, "ATKP_RX_ANL", 300, NULL, 6, NULL);
  
   /*parameter configuration*/
   xTaskCreate(configParamTask, "CONFIG_TASK", 150, NULL, 1, NULL);
  
   /*power management*/
   xTaskCreate(pmTask, "PWRMGNT", 150, NULL, 2, NULL);
  
   /*sensor management*/
   xTaskCreate(sensorsTask, "SENSORS", 450, NULL, 4, NULL);
  
   /*Autopliot task*/
   xTaskCreate(stabilizerTask, "STABILIZER", 450, NULL, 5, NULL);
  
   /*expand module management*/
   xTaskCreate(expModuleMgtTask, "EXP_MODULE", 150, NULL, 1, NULL);
  
   /*print left heap*/
   printf("Free heap: %d bytes\n", xPortGetFreeHeapSize());
  
   /*delete start task*/
   vTaskDelete(startTaskHandle);									
  
   /*exit critical zone*/
   taskEXIT_CRITICAL();
}

Autopliot

void stabilizerTask(void* param)
{
	u32 tick = 0;
	u32 lastWakeTime = getSysTickCnt();
	ledseqRun(SYS_LED, seq_alive);
	while(!sensorsAreCalibrated())
	{
		vTaskDelayUntil(&lastWakeTime, MAIN_LOOP_DT);
	}
	while(1) 
	{
		/*delay 1ms*/
                vTaskDelayUntil(&lastWakeTime, MAIN_LOOP_DT);
  
		/*request IMU sensor data, 500HZ*/
		if (RATE_DO_EXECUTE(RATE_500_HZ, tick))
		{
			sensorsAcquire(&sensorData, tick);
		}
  
		/*calculate quaternion from euler angule and using Mahony filter 
                  to do sensor fusion, 250 HZ */
		if (RATE_DO_EXECUTE(ATTITUDE_ESTIMAT_RATE, tick))
		{
			imuUpdate(sensorData.acc, sensorData.gyro, &state, ATTITUDE_ES
                        TIMAT_DT);
		}
  
		/*estimate position and altitude, mainly used for optical flow module,
                 250HZ*/
		if (RATE_DO_EXECUTE(POSITION_ESTIMAT_RATE, tick))
		{
			positionEstimate(&sensorData, &state, POSITION_ESTIMAT_DT);
		}
  
		/*set expected attitude and choose flight mode, 100HZ*/	
		if (RATE_DO_EXECUTE(RATE_100_HZ, tick) && getIsCalibrated()==true)
		{
			commanderGetSetpoint(&setpoint, &state);	
		}
  
                /*adjust altitude rapidly,250HZ*/
		if (RATE_DO_EXECUTE(RATE_250_HZ, tick))
		{
			fastAdjustPosZ();
		}
		
		/*acquire optical flow data, 100HZ*/
		if (RATE_DO_EXECUTE(RATE_100_HZ, tick))
		{
			getOpFlowData(&state, 0.01f);	
		}
  
		/*check if the flip mode is enabled, 500HZ*/
		if (RATE_DO_EXECUTE(RATE_500_HZ, tick) && (getCommanderCtrlMode() != 0                
                x03))
		{
			flyerFlipCheck(&setpoint, &control, &state);	
		}
  
		/*anomaly detection*/
		anomalDetec(&sensorData, &state, &control);
  
		/*Cascade PID controller*/	
		stateControl(&control, &sensorData, &state, &setpoint, tick);
  
		/*motor speed control, 500HZ*/
		if (RATE_DO_EXECUTE(RATE_500_HZ, tick))
		{
			powerControl(&control);	
		}
		tick++;
	}
}
  

sensorsAcquire

Although Minifly quadcopter is embeddedwith an MPU9250 module, only three-axis accelerometer, three-axis gyroscope andbarometer are operating.

imuUpdate

• Quaternion to Euler angle

• Runge-Kutta Method

• Mahony filter

The diagram shown below is known as the Mahony filter. The theoretical acceleration of gravity points downward. If there is no external force exerts on quadcopter and the quadcopterstay hover in the air, the acceleration reaction force can be expressed as the following matrix: [0 0 g(upward)]. After being multiplied by rotation matrix, the [0 0 g] becomes the theoretical acceleration with respect to airframe body coordination system. However, both accelerometer and gyroscope have measurement error, thus both sensor data of accelerometer and rotation matrix derived from gyroscope are inaccurate. The cross product of theoretical acceleration matrix and sensor data of accelerometer is sin(theta), where theta represents the angle between theoretical acceleration matrix and accelerometer matrix. PI controller is used to make instantaneous gyroscope data approach accelerometer. Then, Runge–Kutta method is used to find next step's quaternion based on fused quaternion on this step.

void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt)
{
	float normalise;
	float ex, ey, ez;
	float halfT = 0.5f * dt;
	float accBuf[3] = {0.f};
	Axis3f tempacc = acc;
	gyro.x = gyro.x * DEG2RAD;
	gyro.y = gyro.y * DEG2RAD;
	gyro.z = gyro.z * DEG2RAD;
  
	/* if the three-axis accelerometer is operating, then use Mahony filter*/
	if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
	{
		normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
		acc.x *= normalise;
		acc.y *= normalise;
		acc.z *= normalise;
  
		/*through vector cross product to show measured attitude error of 
                gyrpscope */
		ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
		ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
		ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
  
		/*intergration of error*/
		exInt += Ki * ex * dt ;  
		eyInt += Ki * ey * dt ;
		ezInt += Ki * ez * dt ;
  
		/*PI controller to fuse data*/
		gyro.x += Kp * ex + exInt;
		gyro.y += Kp * ey + eyInt;
		gyro.z += Kp * ez + ezInt;
  
	}
  
	/*update quaternion through Runge–Kutta methods in discrete system*/
	float q0Last = q0;
	float q1Last = q1;
	float q2Last = q2;
	float q3Last = q3;
	q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
	q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
	q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
	q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
	normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= normalise;
	q1 *= normalise;
	q2 *= normalise;
	q3 *= normalise;
  
	/*calculate rotation matrix*/
	imuComputeRotationMatrix();	
  
	/*calculate euler angle from quaternion*/
	state->attitude.pitch = -asinf(rMat[2][0]) * RAD2DEG; 
	state->attitude.roll = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
	state->attitude.yaw = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;
}
  

positionEstimate

See attached Mindmap.

commanderGetSetpoint

See attached Mindmap.

stateControl

See attached Mindmap.

Although the cascade PID controller contains up to four loops, the position loop and velocity are only used for optical flow module.