I need same routines in c or c++. You must check solution in gis software with 2cm presition. [url removed, login to view]
I will verify with the comparison. For example, I will calculate the distance according to your method and then calculate it with the formula distance = speed x time. You must take into account the altitude.