Add altitude data to PID_rc_pass_command
Gap function with altitude setpoint accumulator working