Omni-Directional Robot Part 5 – Modding CleanFlight

Skyline32 mini
Advertisements

Modding CleanFlight for an Omni-Directional Robot.

So I have previously tried to use a CleanFlight flight controller for my Omni-Directional robot without any modifications. Due to CleanFlight being made for model aircraft there are some limitations for use in a ground vehicle.

So to make it usable in a robot I have cracked open the source code and made some changes. For this I have downloaded V2.2.0 source from Github here: https://github.com/cleanflight/cleanflight/releases

Pitch and Roll PID removal

The most obvious change was to decouple the Pitch and Roll gyro from the motor output. Looking at the code in src/main/flight/mixer.c the relevant code starts at line 751.

for (int i = 0; i < motorCount; i++) {
 float mix =
 scaledAxisPidRoll * currentMixer[i].roll +
 scaledAxisPidPitch * currentMixer[i].pitch +
 scaledAxisPidYaw * currentMixer[i].yaw;

For each motor a result of the PID function for each axis is multiplied by a value for that axis that is supplied by the mixer setup. The scaled PID values for each axis are calculated starting from lines 734.

const float scaledAxisPidRoll =
constrainf(axisPIDSum[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
const float scaledAxisPidPitch =
constrainf(axisPIDSum[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;

The axisPIDSum values come from src/main/flight/pid.c line 543.

axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis] + axisPID_D[axis];

And axisPID_P[axis] is a product of a constant, error  and RC command (line 495). So this is where the RC commands enter the PID function. There is no need to change this but I can bypass the PID function altogether.

So we just need to remove scaledAxisPidRoll and scaledAxisPidPitch and switch with scaled RC commands of the relevant axis. Here is what I have:

for (int i = 0; i < motorCount; i++) {
float mix =
(rcCommand[ROLL]/1000.0f) * currentMixer[i].roll +
(rcCommand[PITCH]/1000.0f) * currentMixer[i].pitch +
scaledAxisPidYaw * currentMixer[i].yaw;

rcCommand[*] is a value from 0 to 1000 and the scaledAxisPid values are between 0 and 1 hence to divide by 1000.

Pin throttle to 50%

A model aircraft has 4 axis of movement while a Omni-Directional robot has three so we have 1 too many. The throttle input is not needed for a robot and I had set to 50% on my transmitter. So while I am editing the code I removed it from the mixing function. Line 635 is what we want.

float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle));

I didn’t use and variables here but simply substituted 0.5 for the throttle value.

float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + 0.5f * currentMixer[i].throttle));
Dead-band compensation

As explained previously my reversing ESCs have a dead-band around 50% throttle. This is to allow for noise and twitching of the throttle signal but in my case prevents fine adjustments by the gyro. So I wanted some code that will cause the output to skip over the dead-band but still allow a center value to stop the motor.

Dead-Band compensation
Dead-Band compensation

What I want is if the throttle position is bellow 50% scale the value to fit bellow 45% and if above 50% scale to fit above 55%.

Here is my code.

//remove deadband and scale remaining
    //deadband limits
    float DeadBandHigh = 0.55f;
    float DeadBandLow = 0.45f;
    float MidPoint = 0.5f;
    float temp = 0.0f;

    for (int i = 0; i < motorCount; i++){
        if (motorMix[i] < MidPoint){
            motorMix[i] *= DeadBandLow/MidPoint;
        }else{
            temp = 1 - motorMix[i];
            temp *= (1 - DeadBandHigh)/MidPoint;
            motorMix[i] = 1 - temp;
        }
    }

I will remind you that I don’t write the nearest code but it does work.

Leave a Reply