RTL does not keep altiture when using airspeed

[GoogleCodeExporter]

What steps will reproduce the problem?
1. define AIRSPEED_SENSOR ENABLED
2. set RTL
3. altitude keeps dropping

What is the expected output? What do you see instead?
altitude to stay at the set altitude

What version of the product are you using? On what operating system?

Please provide any additional information below.
in Atitude.pde
#if AIRSPEED_SENSOR == 1
void calc_nav_pitch()
{
    // Calculate the Pitch of the plane
    // --------------------------------
    nav_pitch = -PID(airspeed_error, dTnav, CASE_NAV_PITCH_ASP, 1.0);
    nav_pitch = constrain(nav_pitch, pitch_min, pitch_max);

}
#endif

I change it to 

#if AIRSPEED_SENSOR == 1
void calc_nav_pitch()
{
    // Calculate the Pitch of the plane
    // --------------------------------
    nav_pitch = PID(altitude_error, dTnav, CASE_NAV_PITCH_ALT, 1.0);
    nav_pitch = constrain(nav_pitch, pitch_min, pitch_max);
}
#endif

It does work but I do not understand what we were trying to achieve here.

Original issue reported on code.google.com by mbada...@gmail.com on 12 Oct 2010 at 8:51

[GoogleCodeExporter]

It appears you've changed nav_pitch to respond using the pitch-altitude 
settings rather than the pitch-airspeed settings.  This isn't what we want to 
do, but it seems to indicate that the problem is in calculating airspeed_error, 
or the gain values for pitch-airspeed.  Are you able to determine the values of 
airspeed_error when the plane is below the desired altitude in RTL mode with 
the original code?

Also, the default designed behavior is for the plane to follow a straight line 
between the point at which RTL is engaged and the point directly above home at 
an altitude stored in EEPROM.  Could you 1) confirm that your plane is not 
doing this and 2) copy the results of test -> waypoints in the setup interface?

Original comment by bjpcalt...@gmail.com on 12 Oct 2010 at 9:32

[GoogleCodeExporter]

I have to check in light of what you say but it is getting late for me. I 'll 
check tomorrow.

I suppose I just do not understand who is keeping the altitude : calc_nav_pitch 
or calc_throttle or something else ?

The thing I am quite sure of is that the plane is going straight to altitude 0 
at home.
I'll recheck twice tomorrow.

Original comment by mbada...@gmail.com on 12 Oct 2010 at 10:05

[GoogleCodeExporter]

Sounds good.  Are you actually observing the plane going straight to the point 
(home at zero altitude), or is the plane simply losing altitude as it heads 
toward home?  The latter sounds like a PID/control issue while the former 
sounds like a logic/settings issue.  If you started your plane at point A and 
altitude 100 then engaged RTL, do you expect it would hit the ground near home 
or not near home?  Then, if you started your plane at point A and altitude 300 
and engaged RTL, do you expect the plane would hit the ground in roughly the 
same place, or roughly 3x as far from point A?

There are two different ways that the APM controls pitch and throttle.  When 
the airspeed sensor is present, the plane pitches to maintain airspeed and 
adjusts throttle to maintain energy/altitude.  This is how a real pilot would 
do it; if his plane was getting too slow, he wouldn't maintain pitch and 
throttle up because that adjustment would take too long and he might be in 
danger of stalling while it was taking place.  Instead, he would dive to regain 
airspeed and then apply throttle because he would be too low after the dive.  
So, the short answer is that, as designed, calc_throttle is keeping the 
altitude when the airspeed sensor is present.  But calc_nav_pitch will cause 
short-term changes in altitude as it corrects airspeed.

Original comment by bjpcalt...@gmail.com on 12 Oct 2010 at 10:16

[GoogleCodeExporter]

I have specified "hold current altitude" which order the plane to go home with 
the same altitude as it is currently.

Here is my log with only the RTL part.
beginning
GPS:0,1,0,47.9119796,2.1909058,292.70,292.70,45.53,45.04
the altitude is 292.70
NTUN:45.06,4521,231.59,231.59,0.00,4.79,0.91,
and the altitude_error is 0.00

At the end 
GPS:0,1,0,47.8943862,2.1577906,172.60,172.50,48.02,224.54
Altitude is 172.60
NTUN:224.59,4872,231.94,231.94,120.20,1.28,0.96,
and the altitude error is 120.20

NB : I changed for my debug purposes NTUN field 2 from wp_distance to airspeed, 
eg on the last trace the airspeed ws 48.72 m/s (hey, it is a real plane in 
xplane)

result of test->waypoints 
Hold current altitude
1 waypoints
Hit radius: 25
Loiter radius: 20

MSG: command #: 0 id: 16 p1: 0 p2: 11730 p3: 478942108 p4: 21574566
MSG: command #: 1 id: 0 p1: 0 p2: 376249088 p3: -1223842816 p4: 21

Original comment by mbada...@gmail.com on 13 Oct 2010 at 4:32

Attachments:

• rtl.txt

[GoogleCodeExporter]

I made some changes to the trunk last night; could you check to see if the 
problem still occurs after r1252?

Original comment by bjpcalt...@gmail.com on 13 Oct 2010 at 4:38

[GoogleCodeExporter]

looks better so far especially FBW_B.
but for RTL with airpseed and xplane , I suppose there is still a problem with 
airspeed_energy_error no being calculated 
in ArduPilotMega.pde read_airspeed should be called to read 
airspeed_energy_error
#if GPS_PROTOCOL == GPS_PROTOCOL_IMU
        GPS.update();
        airspeed        = (float)GPS.airspeed;  //airspeed is * 100 coming in from Xplane for accuracy

        if(digitalRead(SLIDE_SWITCH_PIN) == 0) {
            read_AHRS();
            roll_sensor     = -roll_sensor;
            pitch_sensor    = -pitch_sensor;
            //yaw_sensor      = -yaw_sensor;
        }else{
            roll_sensor     = GPS.roll_sensor;
            pitch_sensor    = GPS.pitch_sensor;
            yaw_sensor      = GPS.ground_course;
        }

#else
        // Read Airspeed
        // -------------
        #if AIRSPEED_SENSOR == 1
        read_airspeed();
        #endif

Original comment by mbada...@gmail.com on 13 Oct 2010 at 5:19

[GoogleCodeExporter]

moreover it seems I have some conversion issue
airspeed_cruise: 5000 airspeed: 4159 airspeed_energy_error: 1
where it should be 388.875

Original comment by mbada...@gmail.com on 13 Oct 2010 at 5:48

[GoogleCodeExporter]

Thanks -- good point in comment 6.  I'm not entirely sure how I was able to fly 
a successful mission in XPlane given that.  I will look into comment 7 as well.

Original comment by bjpcalt...@gmail.com on 13 Oct 2010 at 5:53

[GoogleCodeExporter]

the boost code was doing the job in RTL 8)

Original comment by mbada...@gmail.com on 13 Oct 2010 at 6:07

[GoogleCodeExporter]

looks better like this in my test case
airspeed_energy_error = (((long)airspeed_cruise * (long)airspeed_cruise) - 
((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to 
avoid floating point calculation

Original comment by mbada...@gmail.com on 13 Oct 2010 at 6:16

[GoogleCodeExporter]

Agreed -- that's exactly the change I made :)  In the process of testing.

Original comment by bjpcalt...@gmail.com on 13 Oct 2010 at 6:23

[GoogleCodeExporter]

just tested with your last night svn code + the zero_airspeed for RTL (comment 
6)  and the "long" modif (comment 10) and it works like a charm now. Not 
loosing altitude anymore in RTL

Original comment by mbada...@gmail.com on 13 Oct 2010 at 6:37

[GoogleCodeExporter]

This issue was closed by revision r1253.

Original comment by bjpcalt...@gmail.com on 13 Oct 2010 at 9:16

• Changed state: Fixed

[GoogleCodeExporter]

[deleted comment]

[GoogleCodeExporter]

airspeed_energy_error is still calculated in two different places because we 
want plane to behave differently with respect to airspeed in FBW_B as compared 
to all other modes.  In FBW_B, the throttle channel sets the target airspeed 
rather than always having the target airspeed be airspeed_cruise.

Original comment by bjpcalt...@gmail.com on 13 Oct 2010 at 10:55

[GoogleCodeExporter]

[deleted comment]

[GoogleCodeExporter]

Not at all -- thanks for pointing it out.  I'll try to commit the change later 
today.

Original comment by bjpcalt...@gmail.com on 14 Oct 2010 at 7:29

• Changed state: Accepted

[GoogleCodeExporter]

ERRATA : I change my comment , I had a mistake 
sorry to be a bugger but the formula is not the same and there is a casting 
error in ArduPilotMega.pde line 787

airspeed_energy_error = (long)(((long)airspeed_error * (long)airspeed_error) - 
(airspeed * airspeed))/20000;

should be like in navigation.pde

airspeed_energy_error = (long)(((long)airspeed_error * (long)airspeed_error) - 
((long)airspeed * (long)airspeed))/20000;

Original comment by mbada...@gmail.com on 14 Oct 2010 at 7:36

[GoogleCodeExporter]

I think r1258 fixes this.  Let me know if you find any other problems.

Original comment by bjpcalt...@gmail.com on 14 Oct 2010 at 11:17

• Changed state: Fixed