It appears you have solved the major issues - impressive. We are working on a similar project. I will suggest your manual mode vs. autonomous mode issue is a matter of timing and level of degree - so 2 factor. An experienced pilot in manual mode will adjust based on real-world inputs (everything) while autonomous mode control is making changes that are too radical and potentially too quick for the flight path, flight conditions, etc.
We programmed a feed-back loop to better define our autonomous mode control calls. We defined a matrix of flight conditions (temp, elevation, wind factor, humidity, ...) and also developed a list of tests to auto-determine payload (max power - time duration for altitude delta [taking into consideration the flight conditions above]). This provided us a base line to sample real-world manual control decisions and use in the autonomous mode.
Yes, this is rather intense, but if you have gotten this far - you can handle it.