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

Remove setTargetCoordinates' `going_through` param

Now `setTargetCoordinates` always go through the desired point by updating
regulary the projected coordinates.
Function `loiter` is now defined for the purpose where `setTargetCoordinates`
was called with `going_through` parameter set to `false` (to loiter around a
point without going through it).
parent 726e36fc
......@@ -20,6 +20,11 @@ using std::this_thread::sleep_for;
#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour
struct Coordinates {
double latitude;
double longitude;
};
static std::ofstream log_file_fd;
static Mavsdk _mavsdk;
......@@ -33,11 +38,17 @@ static std::future<std::shared_ptr<System>> fut;
static const float DEFAULT_RADIUS = 100;
static const float EARTH_RADIUS = 6371000;
static const float ADDED_DISTANCE = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
static const double COS_ADDED_DISTANCE = cos(ADDED_DISTANCE);
static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
static int mavsdk_started = 0;
static uint64_t init_timestamp;
static Telemetry::Position origin;
static Coordinates targeted_destination;
static Coordinates projected_destination;
// Logs functions
static void log(std::string message) {
......@@ -80,7 +91,7 @@ static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string
return 0;
}
// Connexion management functions
// Coordinates related functions
static double toRad(double angle) {
return M_PI * angle / 180;
......@@ -90,6 +101,74 @@ static double toDeg(double angle) {
return 180 * angle / M_PI;
}
/*
* compute the bearing angle between point 1 and point 2
* the bearing angle is the direction to take to go from point 1 to point 2
* values are in [-π; π] where 0 is North
*/
static double bearing(double lat1, double lon1, double lat2, double lon2) {
double lat1_rad = toRad(lat1);
double lat2_rad = toRad(lat2);
double dL = toRad(lon2 - lon1);
double x = cos(lat2_rad) * sin(dL);
double y = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(dL);
return atan2(x, y);
}
/*
* To ensure that a drone goes through the target point and to avoid loitering
* trajectory, the target point is projected to a distance equal to twice the
* default loiter radius.
*/
static Coordinates getProjectionCoordinates(Coordinates destination, Coordinates position) {
double laRad = toRad(destination.latitude);
double sinLa = sin(laRad);
double cosLa = cos(laRad);
double loRad = toRad(destination.longitude);
double bearing_angle = bearing(position.latitude, position.longitude,
targeted_destination.latitude,
targeted_destination.longitude);
double newLa = asin(sinLa * COS_ADDED_DISTANCE
+ cosLa * SIN_ADDED_DISTANCE * cos(bearing_angle));
double newLo = loRad + atan2(sin(bearing_angle) * SIN_ADDED_DISTANCE * cosLa,
COS_ADDED_DISTANCE - sinLa * sin(newLa));
Coordinates projected{ toDeg(newLa), toDeg(newLo) };
return projected;
}
static int doReposition(float la, float lo, float radius, float y) {
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_REPOSITION;
command.param1 = -1; // Ground speed, -1 for default
command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
command.param3 = radius; // loiter radius
command.param4 = y; // loiter direction, 0: clockwise 1: counter clockwise
command.param5 = la;
command.param6 = lo;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Entering loiter mode failed");
}
static void updateProjection(double current_lat, double current_lon) {
Coordinates position{ current_lat, current_lon };
projected_destination = getProjectionCoordinates(targeted_destination,
position);
return doReposition(
(float)projected_destination.latitude,
(float)projected_destination.longitude,
DEFAULT_RADIUS,
0
);
}
// Connexion management functions
int start(const char * ip, int port, const char * log_file, int timeout) {
std::string remote_ip(ip);
ConnectionResult connection_result;
......@@ -149,6 +228,9 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
<< telemetry->attitude_euler().yaw_deg << ";"
<< gps.velocity_m_s << ";" << telemetry->fixedwing_metrics().climb_rate_m_s;
log(oss.str());
if (projected_destination.latitude != 0 || targeted_destination.latitude != 0)
updateProjection(gps.latitude_deg, gps.longitude_deg);
});
return 0;
}
......@@ -218,32 +300,25 @@ int triggerParachute(void) {
// Flight management functions
static int doReposition(float la, float lo, float radius, float y) {
static int setAltitude(float altitude) {
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_REPOSITION;
command.param1 = -1; // Ground speed, -1 for default
command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
command.param3 = radius; // loiter radius
command.param4 = y; // loiter direction, 0: clockwise 1: counter clockwise
command.param5 = la;
command.param6 = lo;
command.command = MAV_CMD_DO_OVERRIDE;
command.param1 = 1 | 2 | 4 | 8;
command.param2 = 1 | 2 | 8;
command.param3 = altitude;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Entering loiter mode failed");
return doMavlinkCommand(command, "Setting altitude failed");
}
int loiter(float radius) {
Telemetry::Position position = telemetry->position();
return doReposition(
(float)position.latitude_deg,
(float)position.longitude_deg,
radius,
0
);
int loiter(double la, double lo, float a, float radius) {
int result = doReposition((float)la, (float)lo, radius, 0);
result |= setAltitude(a);
return result;
}
int setAirspeed(float airspeed) {
......@@ -261,65 +336,16 @@ int setAirspeed(float airspeed) {
return doMavlinkCommand(command, "Setting airspeed failed");
}
static int setAltitude(float altitude) {
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_OVERRIDE;
command.param1 = 1 | 2 | 4 | 8;
command.param2 = 1 | 2 | 8;
command.param3 = altitude;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Setting altitude failed");
}
/*
* compute the bearing angle between point 1 and point 2
* the bearing angle is the direction to take to go from point 1 to point 2
* values are in [-π; π] where 0 is North
*/
static double bearing(double lat1, double lon1, double lat2, double lon2) {
double lat1_rad = toRad(lat1);
double lat2_rad = toRad(lat2);
double dL = toRad(lon2 - lon1);
double x = cos(lat2_rad) * sin(dL);
double y = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(dL);
return atan2(x, y);
}
int setTargetCoordinates(double la, double lo, float a, bool going_through) {
int setTargetCoordinates(double la, double lo, float a) {
int result;
if (!mavsdk_started)
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();
targeted_destination.latitude = la;
targeted_destination.longitude = lo;
b = bearing(position.latitude_deg, position.longitude_deg, la, lo);
newLa = asin(
sin(laRad) * cos(addedDistance)
+ cos(laRad) * sin(addedDistance) * cos(b)
);
newLo = loRad + atan2(
sin(b) * sin(addedDistance) * cos(laRad),
cos(addedDistance) - sin(laRad) * sin(newLa)
);
result = doReposition(
(float)toDeg(newLa),
(float)toDeg(newLo),
DEFAULT_RADIUS,
0
);
} else {
result = doReposition((float)la, (float)lo, DEFAULT_RADIUS, 0);
}
result = updateProjection(position.latitude_deg, position.longitude_deg);
result |= setAltitude(a);
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