From 774c3804428928d7d705b5acdafd28c7f3b7af69 Mon Sep 17 00:00:00 2001
From: Pavel Bobov <pb@envirover.com>
Date: Mon, 5 Apr 2021 20:37:07 -0700
Subject: [PATCH] #109 Use eph and epv HIGH_LATENCY2 fields to report roll and
 pitch

---
 .../main/java/com/envirover/uvhub/StateCodec.java | 14 ++++++++++----
 .../envirover/spl/rest/UVTracksResourceV3.java    | 15 +++++++++++++--
 2 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/UVHub/src/main/java/com/envirover/uvhub/StateCodec.java b/UVHub/src/main/java/com/envirover/uvhub/StateCodec.java
index a68afd2..2ac3aad 100644
--- a/UVHub/src/main/java/com/envirover/uvhub/StateCodec.java
+++ b/UVHub/src/main/java/com/envirover/uvhub/StateCodec.java
@@ -93,8 +93,8 @@ private static MAVLinkMessage getGpsRawIntMsg(msg_high_latency2 state) {
         msg.lat = state.latitude;
         msg.lon = state.longitude;
         msg.alt = state.altitude * 1000;
-        msg.eph = state.eph;
-        msg.epv = state.epv;
+        //msg.eph = state.eph;
+        //msg.epv = state.epv;
         //msg.satellites_visible = 4;
         msg.fix_type = GPS_FIX_TYPE.GPS_FIX_TYPE_3D_FIX;
         return msg;
@@ -104,8 +104,14 @@ private static MAVLinkMessage getAttitudeMsg(msg_high_latency2 state) {
         msg_attitude msg = new msg_attitude();
         msg.sysid = state.sysid;
         msg.yaw = (float) Math.toRadians(state.heading * 2);
-        msg.pitch = 0;
-        msg.roll = 0;
+        msg.pitch = (float) Math.toRadians(state.epv * 2);
+        if (msg.pitch > Math.PI) {
+            msg.pitch -= 2 * Math.PI;
+        }
+        msg.roll = (float) Math.toRadians(state.eph * 2);
+        if (msg.roll > Math.PI) {
+            msg.roll -= 2 * Math.PI;
+        }
         return msg;
     }
 
diff --git a/UVTracks/src/main/java/com/envirover/spl/rest/UVTracksResourceV3.java b/UVTracks/src/main/java/com/envirover/spl/rest/UVTracksResourceV3.java
index ebe1a68..90a4403 100644
--- a/UVTracks/src/main/java/com/envirover/spl/rest/UVTracksResourceV3.java
+++ b/UVTracks/src/main/java/com/envirover/spl/rest/UVTracksResourceV3.java
@@ -323,8 +323,16 @@ private static Feature reportToPointFeature(StateReport reportedState)
         properties.put("longitude", (Integer)properties.get("longitude") / 10e6);
         properties.put("altitude", (Short)properties.get("altitude") * 1.0);
         properties.put("target_altitude", (Short)properties.get("target_altitude") * 1.0);
-        properties.put("epv", (Short)properties.get("epv") / 10.0);
-        properties.put("eph", (Short)properties.get("eph") / 10.0);
+        double pitch = (Short)properties.get("epv") * 2.0;
+        if (pitch > 180.0) {
+            pitch -= 360;
+        }
+        properties.put("pitch", pitch);
+        double roll = (Short)properties.get("eph") * 2.0;
+        if (roll > 180) {
+            roll -= 360;
+        }
+        properties.put("roll", roll);
         properties.put("target_distance", (Integer)properties.get("target_distance") * 1.0);
         properties.put("airspeed", (Short)properties.get("airspeed") / 5.0);
         properties.put("airspeed_sp", (Short)properties.get("airspeed_sp") / 5.0);
@@ -337,6 +345,9 @@ private static Feature reportToPointFeature(StateReport reportedState)
         properties.put("temperature_air", (Byte)properties.get("temperature_air") * 1.0);
         properties.put("battery_voltage", (Byte)properties.get("custom0") * 1.0);
         properties.put("battery2_voltage", (Byte)properties.get("custom1") * 1.0);
+        properties.put("channel", (Byte)properties.get("custom2"));
+        properties.remove("eph");
+        properties.remove("epv");
         properties.remove("custom0");
         properties.remove("custom1");
         properties.remove("custom2");