acsl-mipt / drone-games Goto Github PK

Координаты точек маршрута имеют вид:
1.0046790641871626 1.0009882945059274 121.0
1.0050389922015597 1.0007187596801768 131.0
1.004769046190762 1.0003593798252748 115.0
1.0044091181763648 1.000628914659671 105.0
1.0041391721655668 1.000269534843038 115.0
1.004499100179964 1.0 105.0
1.0048590281943612 0.9997304651273407 120.0
1.0045890821835632 0.99937108532305 115.0

Глобальные координаты коптера в точке взлёта выглядят вот так:

Слева высота из топика global_position/global равна 232. Справа - преобразованная в программе высота равна 216. Я осведомлён, что высота, выдаваемая MAVROS, задана в элипсоиде WGS-84. Я преобразую её в высоту над уровнем моря(AMSL), вычисляя разницу с помощью этой функции, а затем отнимая эту разницу от эллипсоидной высоты. Однако даже после такого преобразования точки маршрута оказываются ниже уровня земли, и дрон при попытке приблизиться к любой точке врезается в землю.