gps info stuff

huskyroboticsteam · Aug 13, 2023 · b5d9c92 · b5d9c92
1 parent d24bf13
commit b5d9c92
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 12 deletions.
diff --git a/src/ardupilot/ArduPilotInterface.cpp b/src/ardupilot/ArduPilotInterface.cpp
@@ -32,4 +32,8 @@ DataPoint<double> readIMUHeading() {
 	}
 	return DataPoint<double>();
 }
+
+types::DataPoint<eulerangles_t> readIMU() {
+	return ardupilot_protocol->getIMU();
+}
 } // namespace robot
diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp
@@ -187,19 +187,30 @@ void MissionControlProtocol::handleJointPowerRequest(const json& j) {
 
 void MissionControlProtocol::sendRoverPos() {
 	auto heading = robot::readIMUHeading();
-	auto gps = robot::readGPS();
+	auto imu = robot::readIMU();
+	auto gps = gps::readGPSCoords();
 	log(LOG_DEBUG, "imu=%d, gps=%d\n", heading.isValid(), gps.isValid());
-	if (gps.isValid() && heading.isValid()) {
-		double orientX = 0.0;
-		double orientY = 0.0;
-		double orientZ = std::sin(heading.getData() / 2);
-		double orientW = std::cos(heading.getData() / 2);
-		double posX = gps.getData()[0];
-		double posY = gps.getData()[1];
+	if (heading.isValid()) {
+		auto rpy = imu.getData();
+		Eigen::Quaterniond quat(Eigen::AngleAxisd(rpy.roll, Eigen::Vector3d::UnitX()) *
+								Eigen::AngleAxisd(rpy.pitch, Eigen::Vector3d::UnitY()) *
+								Eigen::AngleAxisd(rpy.roll, Eigen::Vector3d::UnitZ()));
+		double orientX = quat.x();
+		double orientY = quat.y();
+		double orientZ = quat.z();
+		double orientW = quat.w();
+		double posX = 0, posY = 0;
+		if (gps.isValid()) {
+			posX = gps.getData().lon;
+			posY = gps.getData().lat;
+		}
 		double posZ = 0.0;
-		double gpsRecency = util::durationToSec(dataclock::now() - gps.getTime());
 		double headingRecency = util::durationToSec(dataclock::now() - heading.getTime());
-		double recency = std::max(gpsRecency, headingRecency);
+		double recency = headingRecency;
+		if (gps.isValid()) {
+			double gpsRecency = util::durationToSec(dataclock::now() - gps.getTime());
+			recency = std::max(recency, gpsRecency);
+		}
 		json msg = {{"type", ROVER_POS_REP_TYPE},
 					{"orientW", orientW},
 					{"orientX", orientX},
@@ -386,8 +397,10 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
 	this->addConnectionHandler(std::bind(&MissionControlProtocol::handleConnection, this));
 	this->addDisconnectionHandler(
 		std::bind(&MissionControlProtocol::stopAndShutdownPowerRepeat, this));
-
-	this->setPongTimeoutHandler(HEARTBEAT_TIMEOUT_PERIOD, std::bind(&MissionControlProtocol::handleHeartbeatTimeout, this));
+
+	this->setPongTimeoutHandler(
+		HEARTBEAT_TIMEOUT_PERIOD,
+		std::bind(&MissionControlProtocol::handleHeartbeatTimeout, this));
 
 	this->_streaming_running = true;
 	this->_streaming_thread = std::thread(&MissionControlProtocol::videoStreamTask, this);

diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h
@@ -165,6 +165,9 @@ types::DataPoint<navtypes::point_t> readGPS();
  */
 types::DataPoint<double> readIMUHeading();
 
+// TODO: circ hack. add impls for sim and noop
+types::DataPoint<navtypes::eulerangles_t> readIMU();
+
 /**
  * @brief Get the ground truth pose, if available. This is only available in simulation.
  *