Skip to content

Commit

Permalink
#109 Use eph and epv HIGH_LATENCY2 fields to report roll and pitch
Browse files Browse the repository at this point in the history
  • Loading branch information
envirover committed Apr 6, 2021
1 parent 773d524 commit 774c380
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 6 deletions.
14 changes: 10 additions & 4 deletions UVHub/src/main/java/com/envirover/uvhub/StateCodec.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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");
Expand Down

0 comments on commit 774c380

Please sign in to comment.