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