I would like to Fix the Altitude Hold so it works properly.

Currently, terrain following using Altitude Hold doesn't work. You engage it and it over-corrects or under-corrects as you fly and your altitude varies all over the place. A real helicopter behaves differently.
I'm a Mechanical Engineer. I've built and programmed simulators myself which were infinitely more stable than EECH is. EECH appears to use a defective proportional feedback loop. It should be using a full PID (Proportional-Integration-Differentiation) loop. I think the original programmers weren't versed in the physics and simply didn't know how to program it properly.

We can fix it. The input parameters are available within the program, you need three things: the current radar altitude, the current vertical velocity, and a time code for reference. The correction calculation is used to adjust the Cyclic to move the helicopter towards the correct altitude and keep it there without overshooting. That behavior is called Critical Damping. Currently there is very little damping at all, which is why it over corrects and hits the ground a lot.

Is there anyone on the team who can direct me in the code to the correct subroutines so I can dig in and take a look at it under the hood?