Discussions

Hello all!

I did some litle tinkering with the code to change the motor commands to actually represent the produced thrust, so that if you give esc pwm command of 50%, it would actually correspond to 50% thrust and so on.

Graph might make it clearer. Blue line is the original measured data points, taken from the info provided on 880kv motors with 12x45 props. Since there is so little measurements, I had to resort to some dirty interpolation tricks to get little more points so there can be some reasonable fit.

The red line then represents 3rd degree polynome (Ax^3 + Bx^2 + Cx + D) fitted to the interpolated data. the offset is caused by scaling factor which makes the end correspond to 1, and not 0.8 (we would be missing some power if this was not the case).

And then the code to make this happen inside the APM:

First off, we need a function to calculate the polynomials, they are like this:

//Calculates powers up to x^3, and returns the results on reference parameters

static void pow3(float base, float& base2, float& base3)
{
    base2 = base*base;
    base3 = base2*base;

}

//Calculates third degree polynome Ax^3 + Bx^2 + Cx + D, where x=base

static float polynome(float base, float A, float B,  float C, float D)
{
    float base2 = 0, base3 = 0;
    pow3(base, base2, base3);
    return (  (A*base3) + (B*base2) + (C*base) + D);
}

//Returns the pwm command required to get the desired thrust from the motor

static int16_t linearize_thrust(const int& out_min, const int& out_max, const int& pwm_in)
{
    float scale =  1.1314;
    int range = out_max - out_min;
    float norm = (float)(pwm_in - out_min) / (float)range;

    float out = scale*polynome(norm, 1.94252312331125, -3.18980232442278, 2.15241011525948, -0.0212837690567283);

    return ((out*range)+out_min);
}

This is not nearly the most optimal way of doing this, since floats are somewhat heavy to calculate, but I did not optimize this any further since I felt like it would become too messy to read (and debug..) that way.

Now these functions can be implemented by just inputting the motor commands here, I did it just by changin few lines on my "static void output_motors_armed()" function (at motors_quad.pde)

I changed the lines:

motor_out[MOT_1]        = g.rc_3.radio_out - roll_out;
 // left motor
motor_out[MOT_2]        = g.rc_3.radio_out + roll_out;
 // front motor
 motor_out[MOT_3]        = g.rc_3.radio_out + pitch_out;
 // back motor
 motor_out[MOT_4]     = g.rc_3.radio_out - pitch_out;

to:

radio3_out         = g.rc_3.radio_out;

// right motor
motor_out[MOT_1] = linearize_thrust(out_min, out_max, radio3_out - roll_out);
 // left motor
motor_out[MOT_2] = linearize_thrust(out_min, out_max, radio3_out + roll_out);
 // front motor
motor_out[MOT_3] = linearize_thrust(out_min, out_max, radio3_out + pitch_out);
 // back motor
motor_out[MOT_4] = linearize_thrust(out_min, out_max, radio3_out - pitch_out);

Once again, this could be optimized further by calculating linearization for radio3_out and roll and pitch out (but roll and pitch out need different min and max values). This would propably be a good idea on hexa or octo since there are more motor commands to calculate.

Performance of this code is somewhat adequate on my opinion, I tested these commands on a loop, and looping it for 200 times (4 motors, 50hz) took around 20ms, which does not seem to hamper the stabilization.

I did a very short test flight today, so this code really is not all that tested, but it atleast did seem to make the handling of the copter more consistent, and I would imagine that this might improve the performance on loiter and althold somewhat, but its really not a magic bullet that cures all, but might improve performance on some situations.

What really would be needed to make this better, is much more measurement data on the actual thurst that the motors provide with different esc outputs, and also it might be helpfull to input the voltage as some sort of scaler, since the actual rpm changes as the voltage drops.

Also the method of scaling does not necessarily have to be polynome, it could also be just a table of values where the result could be just interpolated, this could be computationally cheaper.

Heres a link to the motors_quad.pde functions that I used: http://pastebin.com/wuEa9cQm

(note: functions are configured to work for + configuration quad, change the functions if you want to test on something else)

Hopefully this provides at least some ideas to work on.