Discussions

Had a problem with getting ch1_max and ch2_max not getting set high enough

Posted by Dave Whittington on February 12, 2010 at 6:29pm

I am not test flying but only bench testing 2.5 and as I tested the output of the stabilize mode (and autopilot as well), I noticed that during the 2 or so second max stick movement before "remove before flight" jumper, the rudder/elevator max vales (ch1_max, ch2_max) were only 1680 or so. This seems to happen because the "filtered" ch1 values are a little slow (using defaults filter values) in movement - not sure. I tried different movement rates but until I lengthened the 2 sec to 4 sec or so, I couldn't get the 1900 or so counts that I expect. The result of this is a very reduced control limit during the "set_ch1_degrees" in the servo section. Maybe this is with my equipment only - I printed out the max values after the 2 sec timeout to debug the problem.

 

Secondly, on a quite different subject - the roll PID code in the control_attitude TAB.  The Generic PID code is as follows:

 

/* Generic PID Equations
error = desired_output - measured output;
servo_output = Kp * error + Ki* (integration_of_error) + Kd * (derivative_of_error);
*/

The code for the pitch loop seems to follow that idea:

 integrators[I_PITCH] +=  (error * PITCH_I * (float)deltaMiliSeconds)/1000.0f;

 servo_output = (error * PITCH_P) + integrators[I_PITCH];

 

But the roll loop confuses me:

 

// new_integrator = last_integrator + error*last_integrator*dt; (WHY the inclusion of the last_integrator at the last??)
 integrators[I_ROLL] +=  (error * integrators[I_ROLL] * (float)deltaMiliSeconds)/1000.0f;

 

maybe it all works out with the call to the "smooth_attitude_roll" routine but I just don't grasp the initial part??

 

Thanks

 

Dve

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –
Follow

Replies

  • Dave Whittington February 28, 2010 at 10:32am
    Mark,
    Thanks again for the advice on setting fixed maxes/mins for the channels - it does bother me some though that we can't get the startup check to work well - not sure what the problem is there (yet). After dpoing that and setting all the sensors(roll and pitch) to ZERO in temporary code, I checked STABILIZE mode and got full range in servo deflection ONLY if I move the transmitter sticks slowly - if I give a full deflection of the stick quickly, I see some limiting come into play at about half servo deflection. Maybe that is appropriate, but I would like to discover why it behaves that way - any ideas?
    Dave
  • Dave Whittington February 28, 2010 at 10:24am
    To all who might be interested in some floating point function timings for the current ArduPilot. I was impressed that the ArduIMU code could be extended by Jose for his quadcopter to include a reasonable amount of autopilot code. This led me to want to get a feel for how fast the CPU was doing floating point functions. Personally, I think this is great because I don't like to wade through scaled code (such as UAVDevboard). I like the code to look like equations from a conference paper - i.e., very little change just because it becomes onboard code. Anyway, here are my results - maybe there is a better forum to communicate this - let me know. If you know the timing for a function or short snippet of code, you can be aware of any really time inefficient or unoptimized approaches. Our use of "atan2" for example has to be conservative because it takes about 0.2 milliseconds to do one - fortunately we don't have to do many of them. It all adds up and I show below how easy it is to add some temporary code that calls "millis()" to do your own checks. Sure, we can kill the software timing issues with faster CPUs but I am truly impressed with the amount of code that we are executing on these small processors. Benchmarking small pieces and then the whole is a good practice. You guys are doing a fantastic job - I used to do autopilot flight simulations for Boeing in Seattle and it is important to be getting these 100Hz types of cycle times.
    _________________________________________________________________
    Arduino 328 @16Mhz (ArduPilot) (is that the correct clock rate??)

    Single Precision(32-bit)Floating point timings:

    Add 8 usec
    Sub 8 usec
    Mul 5 usec
    Div 5 usec

    conversion from long to float => 8 usec
    conversion from float to long => 1 usec

    Cos 105 usec
    Sin 104 usec
    atan 175 usec
    Atan2 195 usec
    sq 10 usec
    sqrt 45 usec

    Code Sequence timings

    get_distance => 125 usec (3 adds, 2 muls, 2 sq, 1 sqrt + converts from long to float + mallocs)

    Test code for get_distance:
    float dlat = (float)Mylong1 - (float)Mylong2;
    float dlong = ((float)Mylong2 - (float)Mylong1) * 0.9876f;
    Myfloat3 = sqrt(sq(dlat) + sq(dlong)) * .01113195f;

    NOTE: For accuracy, might want to subtract the longs first then convert to float

    get_bearing => 220 usec (2 subs, 1 atan2, float to long converts)
    Test code for get_bearing:
    Mylong3 = 18000 + (long)atan2((float)(Mylong1 - Mylong2) * scaleLongDown, (float)(Mylong1 - Mylong2)) * 5729.57795f;


    Test Loop Code
    ________________

    //DHW instruction timing in Output telemetry section of Ardupilot 2.5

    slow_loopTimer = millis();
    for(Myi=0; Myi<1000; Myi++)
    {
    //Example - get_distance

    float dlat = (float)Mylong1 - (float)Mylong2;
    float dlong = ((float)Mylong2 - (float)Mylong1) * 0.9876f;
    Myfloat3 = sqrt(sq(dlat) + sq(dlong)) * .01113195f;

    }
    MyDeltaMillisec = millis() - slow_loopTimer;
    Serial.println(MyDeltaMillisec,DEC);

    ----------------------------------------------------
    Hope this is of some value to you developers
    Dave
  • Dave Whittington February 28, 2010 at 10:05am
    Mark,
    Could you please post a code snippet of the PID loop function for V251. I just want to see how the code has changed so as to have a common function which I think is a great idea!!

    Dave
  • Dave Whittington February 28, 2010 at 10:03am
    Jason,
    I think I might have found another problem in the get_distance code as far as precision. The lines that I am referring too are as follows:
    float dlat = (float)loc2->lat - (float)loc1->lat;
    float dlong = ((float)loc2->lng - (float)loc1->lng) * scaleLongDown;
    return sqrt(sq(dlat) + sq(dlong)) * .01113195;
    It looks like you are converting to floats BEFORE you do the subtraction (lat/lon) - I think this cuts down the precision to just 7 digits instead of the full 10. I think it might cause a 30->40 foot error. I'm not really sure but I wanted you to check it.
    Thanks for all the 2.5 testing!!!!
    Dave
  • Dave Whittington February 22, 2010 at 6:13pm
    Jason,
    I downloaded the latest 2.5 but didn't see any changes in the areas that I was concerned about (ch1_max being too small and the calc_attitude_roll first line).
    Are there any problems with using an older Futaba R156F receiver (just 72mHz)?
  • Developer
    Jason Short February 22, 2010 at 4:28pm
    Hi Dave,
    Check out the latest release. That should hopefully clear up those issues you mentioned.
    Jason
This reply was deleted.

Activity