Elevation data

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<std::mutex> lock(_lastGPSMutex);
-		_lastGPS = gpscoords_t{lat, lon};
+		_lastGPS = gpscoords_t{lat, lon, alt};
 	}
 }
 

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();
 			}

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

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());

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;
@@ -219,6 +220,7 @@ void TelemReportTask::sendTelemetry() {
 					{"orientZ", quat.z()},
 					{"lon", lon},
 					{"lat", lat},
+					{"alt", alt},
 					{"recency", recency}};
 		_server.sendJSON(Constants::MC_PROTOCOL_NAME, msg);
 	}

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<std::mutex> guard(gpsMutex);
 	lastGPS = {coords};
 }