From d365336bcf13e8c3345057772159d5aa74d3222c Mon Sep 17 00:00:00 2001 From: PatrickRung Date: Mon, 4 Nov 2024 22:24:08 -0800 Subject: [PATCH 1/4] Changed gpscoords_t in navtypes.h to contain information about altitude. Also changed the json that MissionControlTasks.cpp to contain information about altitude. --- src/navtypes.h | 2 ++ src/network/MissionControlProtocol.cpp | 4 ++-- src/network/MissionControlTasks.cpp | 5 ++++- src/world_interface/simulator_interface.cpp | 2 +- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/navtypes.h b/src/navtypes.h index 5ac7670f..ea2ebb4e 100644 --- a/src/navtypes.h +++ b/src/navtypes.h @@ -12,6 +12,8 @@ struct gpscoords_t { double lat; /** the longitude of the gps coordinate, in degrees */ double lon; + /** the altitude of the gps coordinate */ + double alt; }; /* @brief Euler angles to represent orientation. Rotation ordering is XYZ extrinsic diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index cec6d264..daa03481 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -271,11 +271,11 @@ static bool validateWaypointNavRequest(const json& j) { void MissionControlProtocol::handleWaypointNavRequest(const json& j) { float latitude = j["latitude"]; float longitude = j["longitude"]; + float altitude = j["altitude"]; bool isApproximate = j["isApproximate"]; bool isGate = j["isGate"]; - if (Globals::AUTONOMOUS && !isApproximate && !isGate) { - navtypes::gpscoords_t coords = {latitude, longitude}; + navtypes::gpscoords_t coords = {latitude, longitude, altitude}; auto target = robot::gpsToMeters(coords); if (target) { _autonomous_task.start(target.value()); diff --git a/src/network/MissionControlTasks.cpp b/src/network/MissionControlTasks.cpp index 7b4cf874..24de37d5 100644 --- a/src/network/MissionControlTasks.cpp +++ b/src/network/MissionControlTasks.cpp @@ -201,10 +201,11 @@ void TelemReportTask::sendTelemetry() { util::to_string(gps.isValid()).c_str()); if (imu.isValid()) { Eigen::Quaterniond quat = imu.getData(); - double lon = 0, lat = 0; + double lon = 0, lat = 0, alt = 0; if (gps.isValid()) { lon = gps.getData().lon; lat = gps.getData().lat; + alt = gps.getData().alt; } double imuRecency = util::durationToSec(dataclock::now() - imu.getTime()); double recency = imuRecency; @@ -212,6 +213,7 @@ void TelemReportTask::sendTelemetry() { double gpsRecency = util::durationToSec(dataclock::now() - gps.getTime()); recency = std::max(recency, gpsRecency); } + json msg = {{"type", ROVER_POS_REP_TYPE}, {"orientW", quat.w()}, {"orientX", quat.x()}, @@ -219,6 +221,7 @@ void TelemReportTask::sendTelemetry() { {"orientZ", quat.z()}, {"lon", lon}, {"lat", lat}, + {"alt", alt}, {"recency", recency}}; _server.sendJSON(Constants::MC_PROTOCOL_NAME, msg); } diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index f8348baf..aea1d1ac 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -129,7 +129,7 @@ void initMotors() { } void handleGPS(json msg) { - gpscoords_t coords{msg["latitude"], msg["longitude"]}; + gpscoords_t coords{msg["latitude"], msg["longitude"], msg["altitude"]}; std::lock_guard guard(gpsMutex); lastGPS = {coords}; } From c8680e94146056ccb25f1b9bfc229aba5d44e36f Mon Sep 17 00:00:00 2001 From: PatrickRung Date: Mon, 4 Nov 2024 22:42:48 -0800 Subject: [PATCH 2/4] Fixed issue with _lastGPS not having a altitude value in ArduPilotProtocol.cpp. --- src/ardupilot/ArduPilotProtocol.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/ardupilot/ArduPilotProtocol.cpp b/src/ardupilot/ArduPilotProtocol.cpp index b8108b4b..0fb036e1 100644 --- a/src/ardupilot/ArduPilotProtocol.cpp +++ b/src/ardupilot/ArduPilotProtocol.cpp @@ -57,15 +57,17 @@ void ArduPilotProtocol::clientDisconnected() { bool ArduPilotProtocol::validateGPSRequest(const json& j) { return util::validateKey(j, "lat", val_t::number_float) && - util::validateKey(j, "lon", val_t::number_float); + util::validateKey(j, "lon", val_t::number_float) && + util::validateKey(j, "alt", val_t::number_float); } void ArduPilotProtocol::handleGPSRequest(const json& j) { double lat = j["lat"]; double lon = j["lon"]; + double alt = j["alt"]; { std::lock_guard lock(_lastGPSMutex); - _lastGPS = gpscoords_t{lat, lon}; + _lastGPS = gpscoords_t{lat, lon, alt}; } } From 413b6819b3d5e90441464ae322fcb6a6b4c5d42d Mon Sep 17 00:00:00 2001 From: PatrickRung Date: Fri, 8 Nov 2024 15:31:51 -0800 Subject: [PATCH 3/4] Removed unnecessary line break --- src/network/MissionControlTasks.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/network/MissionControlTasks.cpp b/src/network/MissionControlTasks.cpp index 24de37d5..89f9a1a0 100644 --- a/src/network/MissionControlTasks.cpp +++ b/src/network/MissionControlTasks.cpp @@ -213,7 +213,6 @@ void TelemReportTask::sendTelemetry() { double gpsRecency = util::durationToSec(dataclock::now() - gps.getTime()); recency = std::max(recency, gpsRecency); } - json msg = {{"type", ROVER_POS_REP_TYPE}, {"orientW", quat.w()}, {"orientX", quat.x()}, From bad512f7e86c804244120e6957ec53dece081c45 Mon Sep 17 00:00:00 2001 From: PatrickRung Date: Sat, 16 Nov 2024 00:47:37 -0800 Subject: [PATCH 4/4] Changed most recents coords in read_usb_gps.cpp to contain altitude data so that altitude data is sent on from the gps on the rover outside of the simulation --- src/gps/usb_gps/read_usb_gps.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/gps/usb_gps/read_usb_gps.cpp b/src/gps/usb_gps/read_usb_gps.cpp index ab961298..454d52cb 100644 --- a/src/gps/usb_gps/read_usb_gps.cpp +++ b/src/gps/usb_gps/read_usb_gps.cpp @@ -60,14 +60,15 @@ void gps_loop() { } else { double lat = newdata->fix.latitude; double lon = newdata->fix.longitude; - LOG_F(2, "Received fresh GPS data: (lat %.6f, lon %.6f).", lat, lon); + double alt = newdata->fix.altitude; + LOG_F(2, "Received fresh GPS data: (lat %.6f, lon %.6f).", lat, lon, alt); gps_mutex.lock(); if (!has_fix) { // This is our first fix has_fix = true; } - most_recent_coords = {lat, lon}; + most_recent_coords = {lat, lon, alt}; gps_time = dataclock::now(); gps_mutex.unlock(); }