I have no luck flying at preset altitude , or RTL at set altitude when airspeed is enabled.
By looking at the source, I am not even sure it it's expected to happen:
static void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
if (g.airspeed_enabled == true) {
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
} else {
nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav);
}
nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get());
}
If enabled, - pitch is set by airspeed error ? - so it pitches down if speed too low, and up - only if speed is too high ? - and no care for altitude ?
- while - if airspeed disabled - pitch is adjusted by altitude.. and it works.
Am I missing something ?
Replies
you are missing the throttle component.
a/s slow = pitch down to increase speed.
a/s fast = pitch up to wash of speed.
code for throttle
// throttle control with airspeed compensation
// -------------------------------------------
energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;
// positive energy errors make the throttle go higher
g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav);
g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out,
g.throttle_min.get(), g.throttle_max.get()); // TODO - resolve why "saved" is used here versus "current"
as you can see this is where your altitude error come into play. this is why tuning the airspeed sensor is very important.