Changed gpscoords_t in navtypes.h to contain information about altitu…

…de. Also changed the json that MissionControlTasks.cpp to contain information about altitude.

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,24 +201,27 @@ 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;
 		if (gps.isValid()) {
 			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()},
 					{"orientY", quat.y()},
 					{"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};
 }