Fastest Method to Determine the Elements of an Array From Look-up Table-CodePudding

Hope everyone is doing well. I am working on a Quadcopter project right now, and I have designed a Gain Scheduling based Controller.

I am using the roll, pitch, and yaw angles of the quadcopter to determine the operating interval. Let me explain the basic idea:

I divided roll, pitch, and yaw into four operating intervals. They are 0-30 degrees, 30-50 degrees, 50-70 degrees, and 70-90 degrees. Therefore, for the roll control, I have 4 PID Controller gain-sets, for pitch control, 4 PID Controller Gain-sets, and for yaw control 4 PID Controller Gain-sets as well. Each Gain Set contains 3 elements. The method has a simple Look-Up table as such (Each Gain Set is an array of 3, including the Proportional, Integral, and Derivative Gains):

This look-up table is constructed for both roll, pitch and yaw angles separately. And as I mentioned, Gain-sets are arrays of 3 elements.

To find the appropriate Gain Set for roll, pitch, and yaw angles I do not know what kind of method would be fastest to use in C, since I am going to implement this Gain Scheduling method on a microcontroller and this gain scheduling method is going to operate in an interrupt function.

I am afraid that if-else structure would be too slow for my needs and therefore wanted have your opinions on that. Thank you so much in advance.

Here is the snippet of the code I wrote (it only covers the Roll/Phi Control part):

...
float Phi_Gain_Set_0_30[3] = {5.1838, 0.3882, 0};
float Phi_Gain_Set_30_50[3] = {5.1838, 0.3882, 0};
float Phi_Gain_Set_50_70[3] = {5.1838, 0.3882, 0};
float Phi_Gain_Set_70_90[3] = {5.1838, 0.3882, 0};
...

void scheduleGains(float *rollGainSet, float *pitchGainSet, float *yawGainSet, float phi_deg, float theta_deg, float psi_deg)
{
    if (phi_deg >= 0 && phi_deg < 30)
    {
        float Kp_phi = Phi_Gain_Set_0_30[0];
        float Kd_phi = Phi_Gain_Set_0_30[1];
        float Ki_phi = Phi_Gain_Set_0_30[2];
    }
    else if (phi_deg >= 30 && phi_deg < 50)
    {
        float Kp_phi = Phi_Gain_Set_30_50[0];
        float Kd_phi = Phi_Gain_Set_30_50[1];
        float Ki_phi = Phi_Gain_Set_30_50[2];
    }
    else if (phi_deg >= 50 && phi_deg < 70)
    {
        float Kp_phi = Phi_Gain_Set_50_70[0];
        float Kd_phi = Phi_Gain_Set_50_70[1];
        float Ki_phi = Phi_Gain_Set_50_70[2];
    }
    else if (phi_deg >= 70 && phi_deg < 90)
    {
        float Kp_phi = Phi_Gain_Set_70_90[0];
        float Kd_phi = Phi_Gain_Set_70_90[1];
        float Ki_phi = Phi_Gain_Set_70_90[2];
    }
    else
    {
        float Kp_phi = Phi_Gain_Set_0_30[0];
        float Kd_phi = Phi_Gain_Set_0_30[1];
        float Ki_phi = Phi_Gain_Set_0_30[2];
    }

    rollGainSet[0] = Kp_phi;
    rollGainSet[1] = Kd_phi;
    rollGainSet[2] = Ki_phi;


}

CodePudding user response：

As others have commented, it may be fast enough already. So why bother?

Anyway, in a general case, as Nick ODell noted, your comparison is uselessly complex. Just use one fence per if is enough. Moreover you can use memcpy to make the code shorter and clearer.

#include <stdio.h>
#include <string.h>

float Phi_Gain_Set_0_30[3] = {5.1838, 0.3882, 0};
float Phi_Gain_Set_30_50[3] = {1, 2, 3};
float Phi_Gain_Set_50_70[3] = {4, 5, 6};
float Phi_Gain_Set_70_90[3] = {7, 8, 9};

void scheduleGains(float *rollGainSet, float *pitchGainSet, float *yawGainSet, float phi_deg, float theta_deg, float psi_deg)
{
    if (phi_deg < 30) {
        memcpy(rollGainSet, Phi_Gain_Set_0_30, sizeof(float[3]));
    }
    else if (phi_deg < 50) {
        memcpy(rollGainSet, Phi_Gain_Set_30_50, sizeof(float[3]));
    }
    else if (phi_deg < 70) {
        memcpy(rollGainSet, Phi_Gain_Set_50_70, sizeof(float[3]));
    }
    else if (phi_deg < 90) {
        memcpy(rollGainSet, Phi_Gain_Set_70_90, sizeof(float[3]));
    }
    else {
        memcpy(rollGainSet, Phi_Gain_Set_0_30, sizeof(float[3]));
    }
}

int main(void) 
{
    float test_angles[] = { 17.3, 22.99, 81.398, 65.2 };
    size_t ntests = sizeof test_angles / sizeof *test_angles;
    
    for (size_t i = 0; i < ntests;   i) {
        float rgs[3];
        scheduleGains(rgs, NULL, NULL, test_angles[i], 0, 0);
        printf("phi_deg=%g --> {%g, %g, %g}\n", test_angles[i], rgs[0], rgs[1], rgs[2]);
    }
    
    return 0;
}

Next time help us with a Minimal, Reproducible Example, also with a main().

You can do slightly better (if needed), on average, by using binary search on that range:

void scheduleGains(float *rollGainSet, float *pitchGainSet, float *yawGainSet, float phi_deg, float theta_deg, float psi_deg)
{
    if (phi_deg < 50) {
        if (phi_deg < 30) {
            memcpy(rollGainSet, Phi_Gain_Set_0_30, sizeof(float[3]));
        }
        else {
            memcpy(rollGainSet, Phi_Gain_Set_30_50, sizeof(float[3]));
        }
    }
    else {
        if (phi_deg < 70) {
            memcpy(rollGainSet, Phi_Gain_Set_50_70, sizeof(float[3]));
        }
        else {
            if (phi_deg < 90) {
                memcpy(rollGainSet, Phi_Gain_Set_70_90, sizeof(float[3]));
            }
            else {
                memcpy(rollGainSet, Phi_Gain_Set_0_30, sizeof(float[3]));
            }
        }
    }
}

This gives you 2 comparisons for 0-30, 30-50, 50-70, and 3 comparisons for 70-90 or more. But I don't believe this can make any difference. Moreover, if the probability of having low values of phi_deg is high, the first version is better. As always:

It is a capital mistake to theorize before one has data. Sherlock Holmes