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

Fix autocontinue and angles > 180

Fix autocontinue to go straight
Fix angle difference when absolute value is greater than 180
parent 93ef2d6c
......@@ -37,6 +37,7 @@ static std::future<std::shared_ptr<System>> fut;
static const double EARTH_RADIUS = 6371000.F;
static bool autocontinue = false;
static double initialBearing;
static double previousBearing;
static double xy_ratio;
static double target_latitude;
......@@ -329,8 +330,9 @@ int mavsdk_setTargetCoordinates(double la, double lo, float a) {
target_latitude = la;
target_longitude = lo;
previousBearing = bearing(position.latitude_deg, position.longitude_deg,
la, lo);
initialBearing = previousBearing = bearing(position.latitude_deg,
position.longitude_deg,
la, lo);
autocontinue = false;
if (!mavsdk_isInManualMode()) {
......@@ -351,19 +353,35 @@ static int setManualControlInput(float x, float y, float z, float r) {
}
int mavsdk_setManualControlInput(void) {
if (autocontinue)
return setManualControlInput(0, 0, 0, 0);
double b;
Telemetry::Position position = telemetry->position();
double b = bearing(position.latitude_deg, position.longitude_deg,
target_latitude, target_longitude);
if (abs(b - previousBearing) > 160) {
autocontinue = true;
if (autocontinue) {
b = initialBearing;
} else {
previousBearing = b;
Telemetry::Position position = telemetry->position();
b = bearing(position.latitude_deg, position.longitude_deg,
target_latitude, target_longitude);
/*
If there is a difference of more than 160° (180° for a half circle and 20° of imprecision)
between the required bearing and the one some milliseconds ago,
it means that the target is now behind the drone so the drone just went over the target.
In this case, we don't follow the target anymore, we simply keep the same trajectory.
*/
if (abs(b - previousBearing) > 160) {
autocontinue = true;
} else {
previousBearing = b;
}
}
float angle_diff = (float)b - mavsdk_getYaw();
if (angle_diff < -180) {
angle_diff += 360;
} else if (angle_diff > 180) {
angle_diff -= 360;
}
return setManualControlInput(0, (angle_diff > 0 ? 1 : -1) * std::min(abs(angle_diff), 30.0f) / 30, 0, 0);
}
......
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