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

Subscribe to positions from GPS

- use positions provided by the GPS to get an accurate timestamp
- remove telemetry variables
parent 55abbce6
......@@ -32,14 +32,8 @@ double mavsdk_getInitialLatitude(void);
double mavsdk_getInitialLongitude(void);
double mavsdk_getLatitude(void);
double mavsdk_getLongitude(void);
uint64_t mavsdk_getTimestamp(void);
double mavsdk_getTakeOffAltitude(void);
float mavsdk_getPitch(void);
float mavsdk_getRoll(void);
float mavsdk_getYaw(void);
float mavsdk_getAirspeed(void);
float mavsdk_getClimbRate(void);
float mavsdk_getThrottle(void);
int mavsdk_healthAllOk(void);
bool mavsdk_isInManualMode(void);
int mavsdk_landed(void);
......
......@@ -34,12 +34,6 @@ static std::shared_ptr<System> msystem;
static auto prom = std::promise<std::shared_ptr<System>>{};
static std::future<std::shared_ptr<System>> fut;
static Telemetry::FlightMode flight_mode;
static Telemetry::EulerAngle angle;
static Telemetry::FixedwingMetrics metric;
static Telemetry::Position position;
static uint64_t timestamp_us;
static const double EARTH_RADIUS = 6371000.F;
static bool autocontinue = false;
......@@ -101,6 +95,7 @@ static double toRad(double angle) {
int mavsdk_start(const char * url, const char * log_file, int timeout) {
std::string connection_url(url);
ConnectionResult connection_result;
float abs_altitude;
log_file_fd.open(log_file);
connection_result = _mavsdk.add_any_connection(connection_url);
......@@ -135,46 +130,27 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
manual_control = new ManualControl(msystem);
mavlink_passthrough = new MavlinkPassthrough(msystem);
log("Subscribing to flight mode...");
// Subscribe to receive updates on flight mode. You can find out whether FollowMe is active.
telemetry->subscribe_flight_mode([](Telemetry::FlightMode _flight_mode) {
if(flight_mode != _flight_mode) {
flight_mode = _flight_mode;
}
});
log("Subscribing to Euler angle...");
telemetry->subscribe_attitude_euler([](Telemetry::EulerAngle euler_angle) {
angle = euler_angle;
});
log("Subscribing to Fixedwing Metrics...");
telemetry->subscribe_fixedwing_metrics([](Telemetry::FixedwingMetrics m) {
metric = m;
});
log("Subscribing to position...");
telemetry->subscribe_position([](Telemetry::Position pos) {
timestamp_us = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch()
).count();
log("Subscribing to raw GPS...");
telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
std::ostringstream oss;
position = pos;
Telemetry::EulerAngle euler_angle = telemetry->attitude_euler();
Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
Telemetry::Position position = telemetry->position();
oss << mavsdk_getTimestamp() << "; "
<< mavsdk_getLatitude() << "; " << mavsdk_getLongitude() << "; "
<< mavsdk_getAltitude() << "; " << mavsdk_getAltitudeRel() << "; "
<< mavsdk_getPitch() << "; " << mavsdk_getRoll() << "; " << mavsdk_getYaw() << "; "
<< mavsdk_getAirspeed() << "; " << mavsdk_getThrottle() << "; " << mavsdk_getClimbRate();
oss << gps.timestamp_us << "; "
<< gps.latitude_deg << "; " << gps.longitude_deg << "; "
<< gps.absolute_altitude_m << "; " << position.relative_altitude_m << "; "
<< euler_angle.pitch_deg << "; " << euler_angle.roll_deg << "; " << euler_angle.yaw_deg << "; "
<< metrics.airspeed_m_s << "; " << metrics.throttle_percentage << "; " << metrics.climb_rate_m_s;
log(oss.str());
});
while(isnan(position.absolute_altitude_m)) {
do {
log("Waiting for first telemetry");
sleep_for(seconds(1));
}
origin = position;
abs_altitude = mavsdk_getAltitude();
} while(isnan(abs_altitude) || abs_altitude == 0);
origin = telemetry->position();
xy_ratio = std::cos(toRad(origin.latitude_deg));
log("MAVSDK started...");
......@@ -184,17 +160,16 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
}
int mavsdk_stop() {
if(!mavsdk_started)
if (!mavsdk_started)
return -1;
if(!mavsdk_landed()) {
if (!mavsdk_landed()) {
log_error("You must land first !");
return -1;
}
if(doAction(&Action::shutdown, "Shutdown failed")) {
if (doAction(&Action::shutdown, "Shutdown failed"))
return -1;
}
// Delete pointers
delete action;
......@@ -227,46 +202,40 @@ int mavsdk_arm(void) {
int mavsdk_land(void) {
log("Landing...");
if(doAction(&Action::terminate, "Land failed")) {
if (doAction(&Action::terminate, "Land failed"))
return -1;
}
// Check if vehicle is still in air
while(telemetry->in_air()) {
while (telemetry->in_air()) {
log("Vehicle is landing...");
sleep_for(seconds(1));
}
log("Landed!");
while(telemetry->armed()) {
while (telemetry->armed())
sleep_for(seconds(1));
}
log("Finished...");
return 0;
}
int mavsdk_takeOff(void) {
if(doAction(&Action::takeoff, "Takeoff failed")) {
if (doAction(&Action::takeoff, "Takeoff failed"))
return -1;
}
while(flight_mode != Telemetry::FlightMode::Takeoff) {
while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
}
log("Taking off...");
return 0;
}
int mavsdk_takeOffAndWait(void) {
if(mavsdk_takeOff() < 0) {
if (mavsdk_takeOff() < 0)
return -1;
}
while(flight_mode == Telemetry::FlightMode::Takeoff) {
while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
sleep_for(seconds(1));
}
return 0;
}
......@@ -304,7 +273,7 @@ int mavsdk_loiter(float radius) {
}
int mavsdk_setAirspeed(float airspeed) {
if(!mavsdk_started)
if (!mavsdk_started)
return -1;
MavlinkPassthrough::CommandLong command;
......@@ -402,11 +371,11 @@ int mavsdk_setManualControlInput(void) {
// Information functions
float mavsdk_getAltitude(void) {
return position.absolute_altitude_m;
return telemetry->position().absolute_altitude_m;
}
float mavsdk_getAltitudeRel(void) {
return position.relative_altitude_m;
return telemetry->position().relative_altitude_m;
}
float mavsdk_getInitialAltitude(void) {
......@@ -422,49 +391,25 @@ double mavsdk_getInitialLongitude(void) {
}
double mavsdk_getLatitude(void) {
return position.latitude_deg;
return telemetry->position().latitude_deg;
}
double mavsdk_getLongitude(void) {
return position.longitude_deg;
}
uint64_t mavsdk_getTimestamp(void) {
return timestamp_us;
return telemetry->position().longitude_deg;
}
double mavsdk_getTakeOffAltitude(void) {
const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
if(response.first != Action::Result::Success) {
if (response.first != Action::Result::Success) {
log_error_from_result("Get takeoff altitude failed", response.first);
return -1;
}
return response.second;
}
float mavsdk_getPitch(void) {
return angle.pitch_deg;
}
float mavsdk_getRoll(void) {
return angle.roll_deg;
}
float mavsdk_getYaw(void) {
return angle.yaw_deg;
}
float mavsdk_getAirspeed(void) {
return metric.airspeed_m_s;
}
float mavsdk_getClimbRate(void) {
return metric.climb_rate_m_s;
}
float mavsdk_getThrottle(void) {
return metric.throttle_percentage;
return telemetry->attitude_euler().yaw_deg;
}
int mavsdk_healthAllOk(void) {
......
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