Commit e10b89af authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

Allow classic doReposition

parent 729a9b0a
...@@ -300,15 +300,16 @@ static double bearing(double lat1, double lon1, double lat2, double lon2) { ...@@ -300,15 +300,16 @@ static double bearing(double lat1, double lon1, double lat2, double lon2) {
return atan2(x, y); return atan2(x, y);
} }
int setTargetCoordinates(double la, double lo, float a) { int setTargetCoordinates(double la, double lo, float a, bool going_through) {
double b, laRad = toRad(la), loRad = toRad(lo), newLa, newLo;
int result; int result;
float addedDistance = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
Telemetry::Position position = telemetry->position();
if (!mavsdk_started) if (!mavsdk_started)
return -1; return -1;
if (going_through) {
double b, laRad = toRad(la), loRad = toRad(lo), newLa, newLo;
float addedDistance = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
Telemetry::Position position = telemetry->position();
b = bearing(position.latitude_deg, position.longitude_deg, la, lo); b = bearing(position.latitude_deg, position.longitude_deg, la, lo);
newLa = asin( newLa = asin(
sin(laRad) * cos(addedDistance) sin(laRad) * cos(addedDistance)
...@@ -325,6 +326,10 @@ int setTargetCoordinates(double la, double lo, float a) { ...@@ -325,6 +326,10 @@ int setTargetCoordinates(double la, double lo, float a) {
DEFAULT_RADIUS, DEFAULT_RADIUS,
0 0
); );
} else {
result = doReposition((float)la, (float)lo, DEFAULT_RADIUS, 0);
}
result |= setAltitude(a); result |= setAltitude(a);
return result; return result;
} }
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment