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
Replies
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
_________________________________________________________________
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
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
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
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)?
Check out the latest release. That should hopefully clear up those issues you mentioned.
Jason