[Multirotors only] Small improvements to the vertical position controller

[JulioCesarMatias]

This is a new approach to Altitude-Hold, which consists of making the Z Position control algorithm more intelligent.

• Adds a new PI controller for Acceleration control (Similar to the old Velocity PID, but without the weight of the Accelerometer).
• Do Sqrt Controller 4 times to improve vertical control accuracy (PosZ, Shape PosZ, Shape VelZ and Land).
• Adds an Over-Speed calculation (Calculated increased maximum acceleration and jerk if over speed condition is detected).
• Changes in TakeOff, using the relaxation of the integral term of the Z Acceleration PID controller in conjunction with a constant time (Decays the output acceleration by 97% every half second to achieve a smooth transition to zero requested acceleration).
• Different approach to Land control based on Velocity Z PID controller Error (aka Velocity Z Control Ratio).
• Adds FeedForward component for Z Velocity and Z Acceleration.

All vertical control P and PID controllers contain the following filters (which are as macros):

#define NAV_MC_VEL_Z_ERROR_CUT_HZ 5 // Low-Pass filter on velocity P error in Alt-Hold
#define NAV_MC_ACC_Z_ERROR_CUT_HZ 20 // Low-Pass filter on acceleration PID error in Alt-Hold

And it also contains some other macros that have also been added:

#define NAV_MC_INTEGRAL_RELAX_TC_Z 0.10f // Acceleration PID Integral relaxation time constant is used to decay the I term to 3% in half a second.
#define NAV_MC_OVERSPEED_GAIN_Z 2.0f // Gain controlling rate at which z-axis speed is brought back within velMaxDownCms and velMaxUpCms range ("max_manual_climb_rate" and "max_auto_climb_rate" params).

The commands to configure Altitude-Hold mode are available in the PID Tunning tab, or via CLI. Below is the list:

Param Name: nav_mc_pos_z_p
Description:Position Z controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller

Param Name: nav_mc_vel_z_p
Description:Velocity Z controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller

Param Name: nav_mc_acc_z_p
Description:Acceleration Z controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output

Param Name: nav_mc_acc_z_i
Description:Acceleration Z controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration

Param Name: nav_mc_acc_z_d
Description:Acceleration Z controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration

Param Name: nav_jerk_z
Description:Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target