Quadcopter code structure
I've been meaning to post this for a while but after soldering everything together and correcting the errors in the PCB (details blogged here: Resolving UART bus conflict and Power control), THE THING FLIES! In fact it flies pretty well.
I thought I would record the code structure of the quadcopter. Below, psuedo-code for the quadcopter main loop, focusing on stabilisation:
main loop
{
(1) Get raw gyroscope, accelerometer and magnetometer data
(2) Update estimates of pitch, roll (and yaw)
(3) PID controllers:
Calculate a desired gyro velocity for each angle based on the difference between the current quadcopter pitch/roll/yaw and the desired pitch/roll/yaw (i.e. gyro_pitch_desired = (desired_pitch - actual_pitch) * Kp)
Using the calculated desired angular rotation rates and the actual angular rotation rates, use PID controllers to produce an output to mix with the throttle for the motors.
(4) Calculate motor PWM using the output from the pitch, roll and yaw PID controllers
(5) Send PWM pulses to ESCs
(6) Wait until 4 ms has elapsed since (1) to ensure solid 250 Hz update
}
Some notes on each section:
(1) Get IMU data
I've gone full circle on this: I started initially trying to use DMP to calculate the angles for drone stabilisation. Although the angles were calculated fine, the fastest update rates were 200 Hz (okay) but for some reason the gyroscope angular velocities were very noisy in the data packet. So now I'm calculating the roll and pitch using raw data from the MPU6050 - also I learn much more this way! :)(2) Calculate angles
The gyroscopes are primarily used to calculate the current angle. The gyroscope readings are turned into degrees per second and multiplied by the time elapsed (1/250 Hz = 4 ms), and then added to the current angle. I have a mode on the quadcopter where I can output data streams to the serial - on the PC side I then have a Processing sketch to display the data graphically. This was very useful for empirically determining complimentary filter coefficients for the gyroscope values (i.e. value = a * value + (1-a)*new_value). The gyroscope readings are also calibrated to subtract a small systematic error when the gyroscope is stationery, i.e.:gyros[0] -= gyros_cal[0];
gyros[1] -= gyros_cal[1];
gyros[2] -= gyros_cal[2];
new_yaw_rate = ((double)gyros[2])/65.536; // Convert to degrees per second
new_pitch_rate = ((double)gyros[1])/65.536;
new_roll_rate = ((double)gyros[0])/65.536;
pitch_gyro_rate = (pitch_gyro * 0.7) + (new_pitch_rate*0.3); // Complimentary filter, low pass
roll_gyro_rate = (roll_gyro * 0.7) + (new_roll_rate*0.3); // Contains smoothed angular velocities in
yaw_gyro_rate = (yaw_gyro * 0.7) + (new_yaw_rate*0.3); // degress per second.
angle_pitch += ((float)gyros[1]) / (CYCLE_FREQ * 65.536); // Add to current angles and account
angle_roll += ((float)gyros[0]) / (CYCLE_FREQ * 65.536); // for time elapsed since last reading (4 ms)
// CYCLE_FREQ is defined elsewhere as 250, i.e. 250 Hz.
Because the gyroscope will drift over time, the accelerometer pitch and roll angles are mixed in to correct for this. The accelerometer determination of pitch and roll are calculated thus:
// Infact I now think it is correct to use the following angle calculations to be consistent
// with the Tait-Bryan angles
acc_total_vector = sqrt(((int32_t)accs[1]*accs[1])+((int32_t)accs[2]*accs[2])); angle_roll_acc = Quickatan2(accs[1],accs[2])*180/M_PI;
angle_pitch_acc = Quickatan2(-accs[0],acc_total_vector)*180/M_PI;
Finally the angles derived from the gyroscope and the accelerometer are lightly blended:
alpha = 0.001;
angle_pitch = angle_pitch * (1-alpha) + pitch_acc*alpha;
angle_roll = angle_roll * (1-alpha) + roll_acc*alpha;
(3) PID control
Initially I had a single PID controller to attempt to stabilise based on the difference in the desired angle and the actual angle. In fact, I had some trouble getting this to work well - instead the stabilisation is performed using the gyroscope, with a P-controller wrapped around it. The difference in angles is scaled to provide a set point, a target angular velocity for the PID controller, the output of which is then used to control the motors. I.e. for pitch and roll:pitch_pid.Setpoint = K_p*pitch_diff; // set desired rate of change of gyro, based on angle difference
roll_pid.Setpoint = K_p*roll_diff;
roll_pid.Input = roll_gyro_rate;
pitch_pid.Input= pitch_gyro_rate;
Compute(&pitch_pid);
Compute(&roll_pid);
(4) Calculate Motor PWMs
Motors 'offsets' are derived from the PID controller in (3) and added to the motor PWM signal:roll_offset = (int16_t) roll_pid.Output;
pitch_offset = (int16_t) pitch_pid.Output;
yaw_offset = (int16_t) yaw_pid.Output;
I started with a '+' configuration and switched to an 'X' configuration on the basis that it can be more stable (and looks cooler and gives room for a mounted camera without obscuring the view).
#ifndef X_CONFIG
PWMouts[1].pwm = PWM_BASE + throttle - pitch_offset - yaw_offset; // NORTH
PWMouts[2].pwm = PWM_BASE + throttle + pitch_offset - yaw_offset; // SOUTH
PWMouts[3].pwm = PWM_BASE + throttle + roll_offset + yaw_offset; // WEST
PWMouts[0].pwm = PWM_BASE + throttle - roll_offset + yaw_offset; // EAST
#else
PWMouts[0].pwm = PWM_BASE + throttle - pitch_offset - roll_offset - yaw_offset; // front right -CCW
PWMouts[2].pwm = PWM_BASE + throttle + pitch_offset - roll_offset + yaw_offset; // back right- CW
PWMouts[3].pwm = PWM_BASE + throttle + pitch_offset + roll_offset - yaw_offset; // 3 back left - CCW
PWMouts[1].pwm = PWM_BASE + throttle - pitch_offset + roll_offset + yaw_offset; // 1 front left -CW
}
#endif
(5) Update motors with PWM signals
In my first pass, I thought I was being clever with an interrupt based PWM system - I wouldn't have to do this 'manually', it would be taken care of with interrupts. In hindsight it wasn't very good and actually what's been better is the simplest option, turn all the PWM signals HIGH and then have a timing loop to switch them LOW again:
// Set all pins high:
output_high(*PWMouts[0].port, PWMouts[0].pin);
output_high(*PWMouts[1].port, PWMouts[1].pin);
output_high(*PWMouts[2].port, PWMouts[2].pin);
output_high(*PWMouts[3].port, PWMouts[3].pin);
finishedPWMs = 0;
uint8_t mask=0;
microloop = TCNT1; // Between 0-64000; Holds how long steps (1)-(4) took
TCNT1 = 0; // Reset Timer1 for timing the ESC PWMs
while (finishedPWMs!=15)
{
for (uint8_t i=0;i<4;i++)
{
mask = 1<<i; // 1,2,4,8
if (!(finishedPWMs & mask)) // if hasn't gone low
{
if (TCNT1>=PWMouts[i].pwm) {output_low(*PWMouts[i].port, PWMouts[i].pin); finishedPWMs|=mask;}
}
}
if (TCNT1>32000) break; // > 2 ms : something went wrong, safety escape
}
I'm using the 16-bit Timer 1 here, with no pre-scalar, i.e. the timers for my quadcopter code are all specified in clock ticks, not micro/milli-seconds. At 16 MHz clock, this gives 32000 ticks for 2 ms. This gives me good resolution for specifying the motor speeds - probably beyond the resolution of the ESC, but hey, better too much precision than not enough...(6) Waiting... loop timing
Finally there is a piece of code that waits for the requisite amount of time before entering the top of the loop, depending on the value of CYCLE_FREQ.
#if CYCLE_FREQ>=250
if (timer1>0) USART_Pputstring(PSTR("Loop overflow!"));
while (TCNT1+microloop<=64000); // 64000 clock ticks = 4000 micro secs = 250 Hz
TCNT1 = 0;
timer1=0; // Used as an interrupt flag
#else // Else CYCLE_FREQ IS SLOW (<250 Hz)
uint32_t clockdelay=0;
// Each compare A match on timer1 is 4 ms (=64000 clock ticks)
while (clockdelay<(16000000/CYCLE_FREQ))
{
clockdelay = ((uint32_t)timer1*64000) + TCNT1+microloop;
}
timer1=0;
TCNT1 =0;
#endif
"timer1" is incremented every time the COMPA ISR is triggered when TCNT1 reaches 64000, also resetting TCNT1. This has the effect of being a 4 ms timer; therefore if we reset TCNT1 and clear timer1=0, if timer1 has a non-zero value we know that the loop took longer than 4ms.
void init_timer1(void)
{
TCCR1A = 0; // No output on OC1A or OC1B, disable hardware PWM
TCCR1B = 1; // 1= System clock, CK
TIMSK1 = (1 << OCIE1A); // OCR1A compare match
OCR1A = 64000; // 4 ms
TCNT1 = 0;
timer1=0;
}
ISR (TIMER1_COMPA_vect)
{
timer1++;
TCNT1 = 0;
}
Conclusions
So there we have it. I have written blog posts about elements of the code that I ended up not using, I wanted to record that process with some discussion and show how the working code fits together. At the moment I am finding the best way to implement a hover feature, combining accelerometer and barometer data, so stay tuned...!
Comments
Post a Comment