Well I was bored, so I attempted to fix it, let me know if its correct.
#define MAX_CLIMB_SUM_ERR 100
#define MAX_PITCH_CLIMB_SUM_ERR 100
/* Computes desired_gaz and desired_pitch from desired_climb */
void climb_pid_run ( void ) {
float err = estimator_z_dot - desired_climb;
if (auto_pitch) { /* gaz constant */
desired_gaz = nav_desired_gaz;
desired_pitch = climb_pitch_pgain * (err + climb_pitch_igain *
climb_pitch_sum_err);
if (desired_pitch > max_pitch)
desired_pitch = max_pitch;
if (desired_pitch < min_pitch)
desired_pitch = min_pitch;
climb_pitch_sum_err += err;
if (climb_pitch_sum_err > MAX_PITCH_CLIMB_SUM_ERR)
climb_pitch_sum_err =
MAX_PITCH_CLIMB_SUM_ERR;
if (climb_pitch_sum_err < - MAX_PITCH_CLIMB_SUM_ERR)
climb_pitch_sum_err = -
MAX_PITCH_CLIMB_SUM_ERR;
} else { /* pitch almost constant */
/* pitch offset for climb */
pitch_of_vz = desired_climb * pitch_of_vz_pgain;
float fgaz = climb_pgain * (err + climb_igain * climb_sum_err) +
CLIMB_LEVEL_GAZ + CLIMB_GAZ_OF_CLIMB*desired_climb;
climb_sum_err += err;
if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err =
MAX_CLIMB_SUM_ERR;
if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = -
MAX_CLIMB_SUM_ERR;
desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ);
desired_pitch = nav_pitch + pitch_of_vz;
}
}
Quoting address@hidden:
Pascal,
Nice work and looks good. However it seems like the
climb_pitch_sum_err is
missing. should have lines similar to this
climb_sum_err += err;
if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err =
MAX_CLIMB_SUM_ERR;
if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = -
MAX_CLIMB_SUM_ERR;
Quoting Pascal <address@hidden>:
Hi,
A new longitudinal mode have been added in the autopilot; its aim is to
control the vertical climb only using the pitch, with a constant
throttle command. This mode is compatible with the "alt" (the default),
"climb" and "glide" vertical mode's.
Syntax in a flight plan: set the "pitch" value to "auto" along with a
"gaz" value (required).
For example:
<go pitch="auto" gaz="0.75" wp="1"/>
or, if you want to crash ASAP (roll=30°, vertical speed=-2 m/s, full
throttle)
<attitude roll="30" vmode="climb" climb="-2" pitch="auto"
gaz="1.0"
until="FALSE"/>
New parameters are needed in the airframe description to handle this
mode:
In the PID section, some limits on the desired pitch:
<define name="MAX_PITCH" value="0.25"/>
<define name="MIN_PITCH" value="-0.35"/>
and in the ALT section, two gains for the new controller. Unit of PGAIN
is rad/(m/s) .
<define name="PITCH_PGAIN" value="-0.1"/>
<define name="PITCH_IGAIN" value="1.0"/>
Some details: The source code has been patched in pid.c:climb_pid_run()
: The "auto_pitch" boolean flag is used to select the standard
controller (FALSE, desired_gaz computed from desired_climb,
desired_pitch almost constant) or the new one (TRUE, desired_gaz
constant, desired_pitch computed from desired_climb).
Warning: the single test on this new code has been done with gcc: We
cannot simulate it with the current simulator. So be ready to switch
back to auto1 mode ...
Comments are welcome.
--Pascal
_______________________________________________
Paparazzi-devel mailing list
address@hidden
http://lists.nongnu.org/mailman/listinfo/paparazzi-devel
_______________________________________________
Paparazzi-devel mailing list
address@hidden
http://lists.nongnu.org/mailman/listinfo/paparazzi-devel
_______________________________________________
Paparazzi-devel mailing list
address@hidden
http://lists.nongnu.org/mailman/listinfo/paparazzi-devel