Added I gain
Added IDEL_THROTTLE, reduced Z_DISTANCE_LIM to 1.5meter
Erased test code
Moved the ENABLE/DISABLE Mode1-3 under the safety switch condition
Now setpoint will follow true value when throttlecut, increased idle throttle. Tested flight worked but need to add sensitivity