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

Require speed to set coordinates

parent 96ed9f80
......@@ -53,12 +53,7 @@ static Coordinates targeted_destination;
static float targeted_radius = DEFAULT_RADIUS;
static Coordinates projected_destination;
static const float DEFAULT_SPEED = 17;
static float last_override_altitude;
static float last_override_speed;
static bool do_projection = false;
static bool first_coordinate_entered = false;
static bool is_in_air = false;
// Logs functions
......@@ -167,9 +162,6 @@ static int doReposition(float la, float lo, float radius, float y) {
}
static void doReposition_async(float la, float lo, float radius, float y) {
if (!first_coordinate_entered) {
first_coordinate_entered = true;
}
std::thread(doReposition, la, lo, radius, y).detach();
}
......@@ -191,34 +183,26 @@ static long long int getTimestamp() {
return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
}
static int doOverride(float altitude, float speed, const char* failure_msg) {
static int doOverride(float altitude, float airspeed, const char* failure_msg) {
if (!mavsdk_started)
return -1;
if (!first_coordinate_entered) {
log_error("Not overriding altitude before user sets coordinates");
return -1;
}
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_OVERRIDE;
command.param1 = 1 | 2 | 4 | 8;
command.param2 = 1 | 2 | 4 | 8;
command.param3 = altitude;
command.param4 = speed;
command.param4 = airspeed;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, failure_msg);
}
static int setAltitude(float altitude) {
last_override_altitude = altitude;
return doOverride(altitude, last_override_speed, "Setting altitude failed");
}
static void setAltitude_async(float altitude) {
std::thread(setAltitude, altitude).detach();
static void doOverride_async(float altitude, float airspeed) {
std::thread(
doOverride, altitude, airspeed, "Overriding altitude and speed failed"
).detach();
}
void updateLogAndProjection(void) {
......@@ -236,7 +220,6 @@ void updateLogAndProjection(void) {
<< metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
log(oss.str());
if (first_coordinate_entered) {
if (do_projection) {
updateProjection(current_position[LATITUDE],
current_position[LONGITUDE]);
......@@ -249,7 +232,6 @@ void updateLogAndProjection(void) {
);
}
}
}
}
// Connexion management functions
......@@ -318,8 +300,6 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
} while(isnan(abs_altitude) || abs_altitude == 0);
origin = telemetry->position();
last_override_altitude = origin.absolute_altitude_m + DEFAULT_OVERRIDE_ALTITUDE;
last_override_speed = DEFAULT_SPEED;
log("MAVSDK started...");
mavsdk_started = 1;
......@@ -400,38 +380,30 @@ int triggerParachute(void) {
// Flight management functions
void loiter(double la, double lo, float a, float radius) {
targeted_destination.latitude = la;
targeted_destination.longitude = lo;
void loiter(double latitude, double longitude, float altitude,
float radius, float speed) {
targeted_destination.latitude = latitude;
targeted_destination.longitude = longitude;
targeted_radius = radius;
do_projection = false;
doReposition_async((float)la, (float)lo, radius, 0);
setAltitude_async(a);
}
static int setAirSpeed(float airspeed) {
last_override_speed = airspeed;
return doOverride(last_override_altitude, airspeed,
"Setting airspeed failed");
}
void setAirSpeed_async(float airspeed) {
std::thread(setAirSpeed, airspeed).detach();
doReposition_async((float)latitude, (float)longitude, radius, 0);
doOverride_async(altitude, speed);
}
void setTargetCoordinates(double la, double lo, float a) {
void setTargetCoordinates(double latitude, double longitude, float altitude,
float speed) {
if (!mavsdk_started) {
log_error("Mavsdk not started");
return;
}
targeted_destination.latitude = la;
targeted_destination.longitude = lo;
targeted_destination.latitude = latitude;
targeted_destination.longitude = longitude;
do_projection = true;
updateProjection(current_position[0], current_position[1]);
setAltitude_async(a);
doOverride_async(altitude, speed);
}
// Information functions
......
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