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

Stop using blocking overrides

parent 6645734a
......@@ -48,6 +48,10 @@ static Telemetry::Position origin;
static Coordinates targeted_destination;
static Coordinates projected_destination;
static const float DEFAULT_SPEED = 16;
static float last_override_altitude;
static float last_override_speed;
// Logs functions
static void log(std::string message) {
......@@ -236,6 +240,8 @@ 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;
last_override_speed = DEFAULT_SPEED;
log("MAVSDK started...");
mavsdk_started = 1;
......@@ -295,7 +301,8 @@ int takeOffAndWait(void) {
while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
return doAction(&Action::transition_to_fixedwing, "Transition to fixedwing failed");
return doAction(&Action::transition_to_fixedwing,
"Transition to fixedwing failed");
}
int triggerParachute(void) {
......@@ -305,7 +312,8 @@ int triggerParachute(void) {
log("Landing...");
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_PARACHUTE;
command.param1 = 2; //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
//see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
command.param1 = 2;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Parachute release failed");
......@@ -313,19 +321,25 @@ int triggerParachute(void) {
// Flight management functions
static int setAltitude(float altitude) {
static int doOverride(float altitude, float speed, const char* failure_msg) {
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.param2 = 1 | 2 | 4 | 8;
command.param3 = altitude;
command.param4 = speed;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Setting altitude failed");
return doMavlinkCommand(command, failure_msg);
}
static int setAltitude(float altitude) {
last_override_altitude = altitude;
return doOverride(altitude, last_override_speed, "Setting altitude failed");
}
int loiter(double la, double lo, float a, float radius) {
......@@ -335,18 +349,9 @@ int loiter(double la, double lo, float a, float radius) {
}
int setAirSpeed(float airspeed) {
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_OVERRIDE;
command.param1 = 1 | 2 | 4 | 8;
command.param2 = 2 | 4 | 8;
command.param4 = airspeed;
command.target_sysid = mavlink_passthrough->get_target_sysid();
command.target_compid = mavlink_passthrough->get_target_compid();
return doMavlinkCommand(command, "Setting airspeed failed");
last_override_speed = airspeed;
return doOverride(last_override_altitude, airspeed,
"Setting airspeed failed");
}
int setTargetCoordinates(double la, double lo, float a) {
......@@ -405,7 +410,8 @@ double *getPositionArray(void) {
float *getSpeedArray(void) {
Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
float *speedArray =
(float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
speedArray[0] = getYaw();
speedArray[1] = metrics.airspeed_m_s;
speedArray[2] = metrics.climb_rate_m_s;
......
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