From db60bbc46b4a237697df0204d7d0acdeffb4d640 Mon Sep 17 00:00:00 2001
From: alexklimaj <alex@arkelectron.com>
Date: Tue, 31 Oct 2023 10:25:29 -0600
Subject: [PATCH] dronecan: gps add noise, jamming, and spoofing data

---
 src/drivers/uavcan/sensors/gnss.cpp           | 28 ++++++++++++++++---
 src/drivers/uavcan/sensors/gnss.hpp           |  4 ++-
 .../uavcannode/Publishers/GnssFix2.hpp        |  9 ++++--
 3 files changed, 34 insertions(+), 7 deletions(-)

diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp
index ddbc7907e6..49c033e43b 100644
--- a/src/drivers/uavcan/sensors/gnss.cpp
+++ b/src/drivers/uavcan/sensors/gnss.cpp
@@ -155,7 +155,7 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 	float vel_cov[9];
 	msg.velocity_covariance.unpackSquareMatrix(vel_cov);
 
-	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov, NAN, NAN, NAN);
+	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov, NAN, NAN, NAN, -1, -1, 0, 0);
 }
 
 void
@@ -299,9 +299,16 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	float heading_offset = NAN;
 	float heading_accuracy = NAN;
 
+	int32_t noise_per_ms = -1;
+	int32_t jamming_indicator = -1;
+	uint8_t jamming_state = 0;
+	uint8_t spoofing_state = 0;
+
 	// Use ecef_position_velocity for now... There is no heading field
 	if (!msg.ecef_position_velocity.empty()) {
-		heading = msg.ecef_position_velocity[0].velocity_xyz[0];
+		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[0])) {
+			heading = msg.ecef_position_velocity[0].velocity_xyz[0];
+		}
 
 		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
 			heading_offset = msg.ecef_position_velocity[0].velocity_xyz[1];
@@ -310,10 +317,16 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
 			heading_accuracy = msg.ecef_position_velocity[0].velocity_xyz[2];
 		}
+
+		noise_per_ms = msg.ecef_position_velocity[0].position_xyz_mm[0];
+		jamming_indicator = msg.ecef_position_velocity[0].position_xyz_mm[1];
+
+		jamming_state = msg.ecef_position_velocity[0].position_xyz_mm[2] >> 8;
+		spoofing_state = msg.ecef_position_velocity[0].position_xyz_mm[2] & 0xFF;
 	}
 
 	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
-		     heading_accuracy);
+		     heading_accuracy, noise_per_ms, jamming_indicator, jamming_state, spoofing_state);
 }
 
 template <typename FixType>
@@ -322,7 +335,9 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 				    const float (&pos_cov)[9], const float (&vel_cov)[9],
 				    const bool valid_pos_cov, const bool valid_vel_cov,
 				    const float heading, const float heading_offset,
-				    const float heading_accuracy)
+				    const float heading_accuracy, const int32_t noise_per_ms,
+				    const int32_t jamming_indicator, const uint8_t jamming_state,
+				    const uint8_t spoofing_state)
 {
 	sensor_gps_s report{};
 	report.device_id = get_device_id();
@@ -452,6 +467,11 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	report.heading_offset = heading_offset;
 	report.heading_accuracy = heading_accuracy;
 
+	report.noise_per_ms = noise_per_ms;
+	report.jamming_indicator = jamming_indicator;
+	report.jamming_state = jamming_state;
+	report.spoofing_state = spoofing_state;
+
 	publish(msg.getSrcNodeID().get(), &report);
 }
 
diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp
index 5b65f57bb6..f2f28eb5bc 100644
--- a/src/drivers/uavcan/sensors/gnss.hpp
+++ b/src/drivers/uavcan/sensors/gnss.hpp
@@ -89,7 +89,9 @@ private:
 			  const float (&pos_cov)[9], const float (&vel_cov)[9],
 			  const bool valid_pos_cov, const bool valid_vel_cov,
 			  const float heading, const float heading_offset,
-			  const float heading_accuracy);
+			  const float heading_accuracy, const int32_t noise_per_ms,
+			  const int32_t jamming_indicator, const uint8_t jamming_state,
+			  const uint8_t spoofing_state);
 
 	void handleInjectDataTopic();
 	bool PublishRTCMStream(const uint8_t *data, size_t data_len);
diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp
index 5cd977a1bf..4f3223ec48 100644
--- a/src/drivers/uavcannode/Publishers/GnssFix2.hpp
+++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp
@@ -127,6 +127,11 @@ public:
 			ecefpositionvelocity.velocity_xyz[1] = NAN;
 			ecefpositionvelocity.velocity_xyz[2] = NAN;
 
+			// Use ecef_position_velocity for now... There are no fields for these
+			ecefpositionvelocity.position_xyz_mm[0] = gps.noise_per_ms;
+			ecefpositionvelocity.position_xyz_mm[1] = gps.jamming_indicator;
+			ecefpositionvelocity.position_xyz_mm[2] = (gps.jamming_state << 8) | gps.spoofing_state;
+
 			// Use ecef_position_velocity for now... There is no heading field
 			if (!std::isnan(gps.heading)) {
 				ecefpositionvelocity.velocity_xyz[0] = gps.heading;
@@ -138,10 +143,10 @@ public:
 				if (!std::isnan(gps.heading_accuracy)) {
 					ecefpositionvelocity.velocity_xyz[2] = gps.heading_accuracy;
 				}
-
-				fix2.ecef_position_velocity.push_back(ecefpositionvelocity);
 			}
 
+			fix2.ecef_position_velocity.push_back(ecefpositionvelocity);
+
 			uavcan::Publisher<uavcan::equipment::gnss::Fix2>::broadcast(fix2);
 
 			// ensure callback is registered