From a2430d7904b409e68fc1dc4d6985b9c7172f1bc1 Mon Sep 17 00:00:00 2001 From: Leo-Paul Geneau <leo-paul.geneau@nexedi.com> Date: Mon, 1 Aug 2022 17:26:56 +0200 Subject: [PATCH] Subscribe to positions from GPS - use positions provided by the GPS to get an accurate timestamp - remove telemetry variables --- include/mavsdk_wrapper.h | 6 -- mavsdk_wrapper.cpp | 121 +++++++++++---------------------------- 2 files changed, 33 insertions(+), 94 deletions(-) diff --git a/include/mavsdk_wrapper.h b/include/mavsdk_wrapper.h index bdfede3..4087270 100644 --- a/include/mavsdk_wrapper.h +++ b/include/mavsdk_wrapper.h @@ -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); diff --git a/mavsdk_wrapper.cpp b/mavsdk_wrapper.cpp index 55c8e21..5a10818 100644 --- a/mavsdk_wrapper.cpp +++ b/mavsdk_wrapper.cpp @@ -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; - - oss << mavsdk_getTimestamp() << "; " - << mavsdk_getLatitude() << "; " << mavsdk_getLongitude() << "; " - << mavsdk_getAltitude() << "; " << mavsdk_getAltitudeRel() << "; " - << mavsdk_getPitch() << "; " << mavsdk_getRoll() << "; " << mavsdk_getYaw() << "; " - << mavsdk_getAirspeed() << "; " << mavsdk_getThrottle() << "; " << mavsdk_getClimbRate(); + Telemetry::EulerAngle euler_angle = telemetry->attitude_euler(); + Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics(); + Telemetry::Position position = telemetry->position(); + + 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) { -- 2.30.9