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

Comply with qjs-wrapper v2.0

See merge request !1
parents ba7c9205 f98da690
CXXFLAGS=-std=gnu++17 -pipe -Wall -Wextra -Wpedantic -Werror -Wno-overlength-strings -Wno-unused-parameter -Wformat -Wformat-security -Wformat-nonliteral -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden
LIBS=-lmavsdk
LIB_NAME := libautopilotwrapper.so
SRCS := mavsdk_wrapper.cpp
OBJS := mavsdk_wrapper.o
all: $(LIB_NAME)
%.o:
$(LIB_NAME): $(OBJS)
$(CC) $(LDFLAGS) -shared -o $@ $^ $(LIBS)
install: all
mkdir -p "$(DESTDIR)$(prefix)/lib"
install -m644 $(LIB_NAME) "$(DESTDIR)$(prefix)/lib"
clean:
$(RM) -f $(OBJS) $(LIB_NAME)
# mavsdk-wrapper # c-astral-wrapper
This project is holding the c-astral's autopilot specific source code that will wrap the functions from [MAVSDK](https://mavsdk.mavlink.io/main/en/index.html).
\ No newline at end of file
#include <chrono>
#include <cmath>
#include <cstdint>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream>
#include <fstream>
#include <functional>
#include <future>
#include <memory>
#include <thread>
#include "autopilot_wrapper.h"
using namespace mavsdk;
using std::chrono::seconds;
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;
static Telemetry * telemetry;
static Action * action;
static MavlinkPassthrough * mavlink_passthrough;
static std::shared_ptr<System> msystem;
static auto prom = std::promise<std::shared_ptr<System>>{};
static std::future<std::shared_ptr<System>> fut;
static const float DEFAULT_RADIUS = 100;
static const float DEFAULT_OVERRIDE_ALTITUDE = 30;
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 Telemetry::Position origin;
static int64_t current_position[POSITION_ARRAY_SIZE] = {0};
static int64_t timestamp_difference = 0;
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
static void log(std::string message) {
log_file_fd << message << std::endl;
}
static void log_error(std::string message) {
log(ERROR_CONSOLE_TEXT + message + NORMAL_CONSOLE_TEXT);
}
template <class Enumeration>
static void log_error_from_result(std::string message, Enumeration result) {
std::ostringstream oss;
oss << message << ": " << result;
log_error(oss.str());
}
// Action functions
static int doAction(Action::Result (Action::*toDo)(void) const, std::string failure_message) {
if(!mavsdk_started) {
log_error("Mavsdk not started");
return -1;
}
const Action::Result result = (action->*toDo)();
if(result != Action::Result::Success) {
log_error_from_result(failure_message, result);
return -1;
}
return 0;
}
static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string failure_message) {
const MavlinkPassthrough::Result result = mavlink_passthrough->send_command_long(command);
if(result != MavlinkPassthrough::Result::Success) {
log_error_from_result(failure_message, result);
return -1;
}
return 0;
}
// Coordinates related functions
static double toRad(double angle) {
return M_PI * angle / 180;
}
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 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();
}
static void updateProjection(int64_t current_lat, int64_t current_lon) {
Coordinates position{ (double)current_lat / 1e7, (double)current_lon / 1e7 };
projected_destination = getProjectionCoordinates(targeted_destination,
position);
doReposition_async(
(float)projected_destination.latitude,
(float)projected_destination.longitude,
DEFAULT_RADIUS,
0
);
}
static long long int getTimestamp() {
const auto now = std::chrono::system_clock::now();
return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
}
static int doOverride(float altitude, float speed, 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.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();
}
void updateLogAndProjection(void) {
std::ostringstream oss;
Telemetry::FixedwingMetrics metrics;
if(mavsdk_started) {
metrics = telemetry->fixedwing_metrics();
oss << current_position[4] << ";"
<< current_position[0] << ";" << current_position[1] << ";"
<< current_position[2] << ";" << current_position[3] << ";"
<< telemetry->attitude_euler().yaw_deg << ";"
<< metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
log(oss.str());
if (first_coordinate_entered) {
if (do_projection) {
updateProjection(current_position[0], current_position[1]);
} else {
doReposition_async(
(float)targeted_destination.latitude,
(float)targeted_destination.longitude,
targeted_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;
float abs_altitude;
log_file_fd.open(log_file);
connection_result = _mavsdk.add_udp_connection(remote_ip, port);
if (connection_result != ConnectionResult::Success) {
log_error_from_result("Connection failed", connection_result);
return -1;
}
log("Waiting to discover msystem...");
fut = prom.get_future();
_mavsdk.subscribe_on_new_system([]() {
auto msystem_tmp = _mavsdk.systems().back();
if (msystem_tmp->has_autopilot()) {
log("Discovered autopilot");
// Unsubscribe again as we only want to find one system.
_mavsdk.subscribe_on_new_system(nullptr);
prom.set_value(msystem_tmp);
}
});
if (fut.wait_for(seconds(timeout)) == std::future_status::timeout) {
log_error("No autopilot found, exiting.");
return -1;
}
msystem = fut.get();
telemetry = new Telemetry(msystem);
action = new Action(msystem);
mavlink_passthrough = new MavlinkPassthrough(msystem);
mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, [](const mavlink_message_t& message) {
mavlink_global_position_int_t position;
mavlink_msg_global_position_int_decode(&message, &position);
if (timestamp_difference == 0)
timestamp_difference = getTimestamp() - position.time_boot_ms;
current_position[0] = position.lat;
current_position[1] = position.lon;
current_position[2] = position.alt;
current_position[3] = position.relative_alt;
current_position[4] = position.time_boot_ms + timestamp_difference;
});
telemetry->subscribe_landed_state([](const Telemetry::LandedState landed_state) {
if (landed_state == Telemetry::LandedState::InAir) {
is_in_air = true;
}
});
do {
log("Waiting for first telemetry");
sleep_for(seconds(1));
abs_altitude = getAltitude();
} 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;
log("timestamp (ms);latitude (°E7);longitude (°E7);AMSL (mm);rel altitude (mm);yaw (°);air speed (m/s);climb rate (m/s)");
return 0;
}
int stop() {
// Delete pointers
delete action;
delete mavlink_passthrough;
delete telemetry;
log_file_fd << std::flush;
log_file_fd.close();
return 0;
}
int reboot() {
return doAction(&Action::reboot, "Rebooting failed");
}
// Flight state management functions
int arm(void) {
if(!mavsdk_started)
return -1;
if (telemetry->armed()) {
return 0;
}
while(!telemetry->health().is_home_position_ok) {
log("Waiting for home position to be set");
sleep_for(seconds(1));
}
log("Arming...");
return doAction(&Action::arm, "Arming failed");
}
int takeOff(void) {
action->set_takeoff_altitude(DEFAULT_OVERRIDE_ALTITUDE);
if (doAction(&Action::takeoff, "Takeoff failed"))
return -1;
return 0;
}
int takeOffAndWait(void) {
if (takeOff() < 0)
return -1;
while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
return doAction(&Action::transition_to_fixedwing,
"Transition to fixedwing failed");
}
int triggerParachute(void) {
if (!mavsdk_started)
return -1;
log("Landing...");
MavlinkPassthrough::CommandLong command;
command.command = MAV_CMD_DO_PARACHUTE;
//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");
}
// Flight management functions
void loiter(double la, double lo, float a, float radius) {
targeted_destination.latitude = la;
targeted_destination.longitude = lo;
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();
}
void setTargetCoordinates(double la, double lo, float a) {
if (!mavsdk_started) {
log_error("Mavsdk not started");
return;
}
targeted_destination.latitude = la;
targeted_destination.longitude = lo;
do_projection = true;
updateProjection(current_position[0], current_position[1]);
setAltitude_async(a);
}
// Information functions
float getAltitude(void) {
return (float)(current_position[2] / 1000);
}
float getInitialAltitude(void) {
return origin.absolute_altitude_m;
}
double getInitialLatitude(void) {
return origin.latitude_deg;
}
double getInitialLongitude(void) {
return origin.longitude_deg;
}
int64_t *getPositionArray(void) {
int64_t *positionArray = (int64_t*) malloc(POSITION_ARRAY_SIZE * sizeof(int64_t));
memcpy(positionArray, current_position, POSITION_ARRAY_SIZE * sizeof(int64_t));
return positionArray;
}
float *getSpeedArray(void) {
Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
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;
return speedArray;
}
double getTakeOffAltitude(void) {
const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
if (response.first != Action::Result::Success) {
log_error_from_result("Get takeoff altitude failed", response.first);
return -1;
}
return response.second;
}
float getYaw(void) {
return telemetry->attitude_euler().yaw_deg;
}
float getSpeed(void) {
return telemetry->fixedwing_metrics().airspeed_m_s;
}
float getClimbRate(void) {
return telemetry->fixedwing_metrics().climb_rate_m_s;
}
int gpsIsOk(void) {
Telemetry::FixType fixType = telemetry->gps_info().fix_type;
return (fixType != Telemetry::FixType::NoGps) && (fixType != Telemetry::FixType::NoFix);
}
int healthAllOk(void) {
return telemetry->health_all_ok();
}
int isLanding(void) {
if (is_in_air && telemetry->landed_state() != Telemetry::LandedState::InAir)
return 1;
return 0;
}
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