Quadrotor Autonomous Control
Quadrotor autonomously holding its x and y position, thrust is being controlled
The video above is a demonstration of the quadrotor attempting to maintain its x and y position (the x-y plane is the ground). The amount of thrust being generated by the motors (prior to adjustments made during the feedback loop) is being controlled by an Xbox controller for safety reasons. I was flying the quadrotor indoors and needed a way to kill all the motors when it became unstable. Being able to control the thrust allowed the quadrotor to lift off the ground slowly and for me to observe whether the feedback control was working properly before increasing the thrust to bring it to greater heights. As shown, I was not great at controlling the thrust precisely, causing the quadrotor to rise and drop rather abruptly. The video above does not show my best result. This was taken prior to additional fine-tuning I had performed. Unfortunately, I didn’t take a video of the best run, but the quadrotor was able to hold its position autonomously in, roughly, a 30 cm cube.
Introduction
The goal of this project is to construct a quadrotor and develop a control algorithm for maintaining its x-y position while it is in the air. The quadrotor should also be able to receive teleoperation commands from an Xbox controller, so its flight can be controlled by an operator. However, when teleoperation control ceases, the quadrotor should return to holding its position in the air, with only the thrust needed to be controlled.
Components
Raspberry Pi Zero
WIFI module
PWM module
IMU (Inertial Measurement Unit) sensor
4 DC motors
4 propellers
Lithium-ion battery
3D printed chassis
Xbox controller
Quadrotor Close-up
Closed-Loop Feedback Control
Roll, Pitch, and Yaw Rate
I used the accelerometer readings from the IMU in the x-y-z axes to calculate the roll and pitch of the quadrotor. The yaw rate is equal to the gyroscope reading in the z-axis, also taken from the IMU. In the code below, imu_data is a global array of 6 floats storing the x-y-z acceleration and x-y-z angular velocity. read_raw takes in the address of the measured value and returns an analog value. The addition of the calibration terms comes from a calibration process that is performed when the quadrotor is stationary and on a level surface (i.e. the ground or a tabletop). 1000 samples are taken during the calibration process and then averaged to surmise and adjust for the IMU bias.
void read_imu()
{
int vw;
float ax, ay, az;
vw = read_raw(59); // accel x
imu_data[3]=vw/32767.0*2;
vw = read_raw(61); // accel y
imu_data[4]=vw/32767.0*2;
vw = read_raw(63); // accel z
imu_data[5]=accel_z_calibration+vw/32767.0*2;
ax = imu_data[3];
ay = imu_data[4];
az = imu_data[5];
roll_accel = roll_calibration+atan2(ax, -az)/M_PI*180;
pitch_accel = pitch_calibration+atan2(ay, -az)/M_PI*180;
vw = read_raw(71); // gyro z
imu_data[2]=z_gyro_calibration+vw/32767.0*500;
}
Controller Input
The controller inputs are stored in an object called Keyboard, which has attributes for the pitch and roll value read from the joystick. This value is ranges from 0 to 224, in which the default value (when the joystick is centered) is equal to 112. I defined a linear function that converts this value to a pitch and roll angle. The variables in all caps are macros defined at the top of the file, since they are parameters that are constantly being tuned. For instance, I_PITCH represents the pitch integral gain. Pitch is integrated by adding pitch error multiplied by the gain to the current sum. Similarly, the same is done for roll. The desired thrust and yaw are not part of the closed feedback loop, and consequently, are mapped from controller inputs to the appropriate unit.
Proportional-Integral-Derivative (PID) Controller
The desired values for the pitch and roll are then calculated from the standard PID control function.
PID Control Function
void get_joystick()
{
Keyboard keyboard=*shared_memory;
float joy_input_pitch = keyboard.pitch/224.0*JOY_PITCH-0.57143*JOY_PITCH;
float joy_input_roll = -keyboard.roll/224.0*JOY_ROLL+0.57143*JOY_ROLL;
float pitch_error = pitch_angle-joy_input_pitch;
float roll_error = roll_angle-joy_input_roll;
intg_pitch += pitch_error*I_PITCH;
intg_roll += roll_error*I_ROLL;
// apply upper and lower limits on integrated pitch and roll
// omitted for brevity
desired_pitch = pitch_error*P_PITCH+intg_pitch*I_PITCH+imu_data[0]*D_PITCH;
desired_roll = roll_error*P_ROLL+intg_roll*I_ROLL+imu_data[1]*D_ROLL;
desired_thrust = NTRL_THRUST + (keyboard.thrust/128.0-1.0)*(NTRL_THRUST-JOY_THRUST_MIN);
desired_yaw = keyboard.yaw/224.0*JOY_YAW-0.57143*JOY_YAW;
}
Motor PWM
Labelled Motors
Each of the four motors starts off with the baseline desired_thrust. Then, depending on the location of the motor on the quadrotor, the desired_pitch and the desired_roll is either added or subtracted from the baseline. For instance, if the desired pitch is positive, then motors 0 and 3 should have their thrusts increased while motors 1 and 2, decreased. If the desired roll is positive, then motors 2 and 3 should have their thrusts increased while motors 0 and 1, decreased. A proportional controller is also added for the yaw rate to prevent the quadrotor from rotating about the z-axis. The thrust values are then sent to a function that converts them to the proper duty cycles, which, at last, are relayed to the motors.
void pid_update()
{
float yaw_error = imu_data[2]-desired_yaw;
motor_cntrl0 = desired_thrust+desired_pitch-desired_roll_joy-yaw_error*P_YAW;
motor_cntrl1 = desired_thrust-desired_pitch-desired_roll_joyL+yaw_error*P_YAW;
motor_cntrl2 = desired_thrust-desired_pitch+desired_roll_joy-yaw_error*P_YAW;
motor_cntrl3 = desired_thrust+desired_pitch+desired_roll_joy+yaw_error*P_YAW
set_PWM(0, motor_cntrl0);
set_PWM(1, motor_cntrl1);
set_PWM(2, motor_cntrl2);
set_PWM(3, motor_cntrl3);
}
