Mixing motors

I flew the quad in the simulator this weekend trying to tune the rate PIDs.

First problem is the D factor which is very sensitive to sensor noise. For this I added a butterworth filter with a 5Hz cutoff. I’ll probably need to tune this later.

Next problem is that my motor mixing doesn’t preserve total thrust, but instead tries to preserve angular velocity. Rookie mistake I presume.

First a stolen picture of a quad in ‘plus’ configuration:

Motor 1 is towards the front of the quad, 2 to the right, 3 is in the back and 4 to the left. As you can tell from the picture, diagonally opposite motors spin in the same direction – so 1 & 3 spin clockwise while 2 & 4 spin counter-clockwise.

If all motors were spinning in the same direction, the whole quad would start to spin in the opposite direction to counter the rotation of the motors. For this reason the motors spin the way they do – to counter this effect.

Now if the quad wants to go forward, it runs motor 3 faster and motor 1 slower causing it to tilt forward. This will get it to accelerate forward as some of the downward thrust goes backwards now.

Same for going left, right and backwards.

If the quad wants to rotate clockwise, it spins motors 2 & 4 faster than motors 1 & 3. This causes the quad frame to spin in the opposite direction than motors 2&4 – so clockwise. 

Back to the mixing:

The inputs to the mixer are 4 quantities:

1. collective throttle

2. yaw rotation factor – rotation along the Z axis obtained with differential thrust of diagonal motors

3. pitch factor – rotation along the X axis

4. roll factor – along the Y axis

The mixer then proceeds to calculate the throttle of each motor based on their position in the X-Y plane. The hard constraints are 1 – never go below a min throttle to prevent the motor stopping and 2 – never go above full throttle (because physics..)

I had code in place to satisfy these hard constraints in a clever way but it turns out I wasn’t so clever. Take this for example:

//check if under minimum
    if (min < m_min_output)
    {
        for (auto& motor: m_motors)
        {
            motor.output += m_min_output - min;
            max = math::max(max, motor.output);
        }
        min = m_min_output;
    }
    
    //check if over maximum
    if (max > m_max_output)
    {
        for (auto& motor: m_motors)
        {
            motor.output -= max - m_max_output;
            min = math::min(min, motor.output);
        }
        max = m_max_output;
    }

If a motor goes above max (100%), distribute the spill to all other motors. Same when going below min (5%).

Turns out this doesn’t preserve total throttle, but it does preserve angular velocity.

As a consequence sometimes my quad would fly away even at min throttle.

Time to fix this.

Integrating gyros and complimentary filters

I’m integrating the gyro angular rate into a quaternion at 500Hz with this code:


auto av = gyroscope.value.angular_velocity*dt;
auto const& a = m_local_to_world_quat;
auto hd = av*0.5f;
float w = /*(hd.w * a.w)*/ - (hd.x * a.x) - (hd.y * a.y) - (hd.z * a.z);
float x = (hd.x * a.w) /*+ (hd.w * a.x)*/ + (hd.z * a.y) - (hd.y * a.z);
float y = (hd.y * a.w) /*+ (hd.w * a.y)*/ + (hd.x * a.z) - (hd.z * a.x);
float z = (hd.z * a.w) /*+ (hd.w * a.z)*/ + (hd.y * a.x) - (hd.x * a.y);
m_local_to_world_quat.x += x;
m_local_to_world_quat.y += y;
m_local_to_world_quat.z += z;
m_local_to_world_quat.w += w;
m_local_to_world_quat.normalize();

 

It gives me a very accurate rotation with very little drift – if the sensor is properly calibrated.

To eliminate the drift I use a complimentary filter between the gyro quaternion and another (noisy) quaternion built from the compass and accelerometer. The gyro quat captures high frequency rotation and the accelerometer/compass quat captures low frequency rotations but is very noisy on short term. To combine them I do this:


m_local_to_world_quat = math::lerp(m_local_to_world_quat, noisy_quat, mu);


Where mu is the interpolation factor. Initially I used 0.01, big enough to compensate for gyro drift and gyro cross-axis winding and saturation. But it’s too big and the m_local_to_world quat catches a bit of accelerometer noise.
I switched to a variable mu and so far it seems much better:

float mu = 0.001f + math::length(av);


The 0.001 is big enough to compensate for idle drift – but not too big. The av is the gyro rate sample – so the bigger the rotation rate, the bigger the mu.