Commit c9acd773 authored by Thomas Gambier's avatar Thomas Gambier 🚴🏼

Fix bearing

See merge request !4
parents 460a8cf2 e4b06e47
......@@ -36,11 +36,9 @@ typedef struct {
int type;
UA_Byte builtInType;
union {
UA_DateTime (*getTimestamp)(void);
UA_Double (*getDouble)(void);
UA_Float (*getFloat)(void);
UA_String (*getString)(void);
UA_UInt32 (*getUint32)(void);
} getter;
} VariableData;
......
......@@ -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);
}
......
......@@ -194,12 +194,6 @@ VariableData pubsub_get_value(UA_String identifier) {
case UA_TYPES_FLOAT:
*(UA_Float*)var_details.value = droneVariableArray[i].getter.getFloat();
break;
case UA_TYPES_UINT32:
*(UA_UInt32*)var_details.value = droneVariableArray[i].getter.getUint32();
break;
case UA_TYPES_DATETIME:
*(UA_DateTime*)var_details.value = droneVariableArray[i].getter.getTimestamp();
break;
case UA_TYPES_STRING:
*(UA_String*)var_details.value = droneVariableArray[i].getter.getString();
break;
......
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