AUTO mode impossible to tune

Hello,
https://www.youtube.com/watch?v=fxmMbNAVMfs&list=UUDEvBDa5SVM1KiRdqMN8qfw
Few years we are developing USV equipped with APM 2.5. As we started there was no big problems with the APM, our usv runned very fine, and accurate. After having range problems with 3dr radios, we decided to upgrade to GSM module. We took the newest firmware, patched the GSM module stuff in the code, and flashed the APM. After that, we are unable to tune usv, for AUTO mode.
Go to here (guided mode) works almost fine, and responses in our tuning process, but when we upload some waypionts usv starts waving very hard, and almost never reaches the waypoint.
Maybe the reason is the new L1 controll algorithm? because before upgrade we used an older firmware version, but i don’t remember exactly witch version it was.

@evaldasnar,
Since you are using a custom version of the ArduRover2 firmware, which may be causing your Auto Mode tuning issues, there is no way that we can help you out.
The latest stable version of AruRover2 is 2.45 and uses the L1 navigation code. The latest beta version 2.46 b2 uses the L1 navigation code also.
I suggest that you use a GPS/compass module that does not require firmware modification to determine if it is your custom code that is causing your tuning issues.
Regards,
TCIII Autonomous Vehicle Developer