SIH MAVLink established, rendering jerky

src/me/drton/jmavsim/MAVLinkDisplayOnly.java

@@ -0,0 +1,82 @@
+package me.drton.jmavsim;
+
+import me.drton.jmavlib.conversion.RotationConversion;
+import me.drton.jmavlib.mavlink.MAVLinkMessage;
+import me.drton.jmavlib.mavlink.MAVLinkSchema;
+import me.drton.jmavsim.vehicle.AbstractVehicle;
+
+import javax.vecmath.*;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.List;
+import java.util.ArrayList;
+// import java.util.Calendar;
+// import java.util.TimeZone;
+
+/**
+ * MAVLinkDisplayOnly is MAVLink bridge between AbstractVehicle and autopilot connected via MAVLink.
+ * MAVLinkDisplayOnly should have the same sysID as the autopilot, but different componentId.
+ * It reads HIL_STATE_QUATERNION from the MAVLink and displays the vehicle position and attitude.
+ */
+public class MAVLinkDisplayOnly extends MAVLinkHILSystem {
+
+    private boolean fistMsg=true;       // to detect the first MAVLink message
+    private double lat0;                // initial latitude (radians)
+    private double lon0;                // initial longitude (radians)
+    private double alt0;                // initial altitude (meters)
+    private double lat;                 // geodetic latitude (radians)
+    private double lon;                 // geodetic longitude (radians)
+    private double alt;                  // above sea level (meters)
+    private double [] quat={0.0,0.0,0.0,0.0};   // unit quaternion for attitude representation
+    private Double [] control = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+    private static final double EARTH_RADIUS=6371000.0;    // earth radius in meters
+
+    /**
+     * Create MAVLinkDisplayOnly, MAVLink system that sends simulated sensors to autopilot and passes controls from
+     * autopilot to simulator
+     *
+     * @param sysId       SysId of simulator should be the same as autopilot
+     * @param componentId ComponentId of simulator should be different from autopilot
+     * @param vehicle     vehicle to connect
+     */
+    public MAVLinkDisplayOnly(MAVLinkSchema schema, int sysId, int componentId, AbstractVehicle vehicle) {
+        super(schema, sysId, componentId, vehicle);
+    }
+
+    @Override
+    public void handleMessage(MAVLinkMessage msg) {
+        if ("HIL_STATE_QUATERNION".equals(msg.getMsgName())) {
+
+            if (fistMsg) {
+                fistMsg=false;
+                // we take the fist received position as initial position
+                lat0=Math.toRadians(msg.getDouble("lat")*1e-7);
+                lon0=Math.toRadians(msg.getDouble("lon")*1e-7);
+                alt0=msg.getDouble("alt")*1e-3;
+            }
+            for (int i = 0; i < 4; ++i) {
+                quat[i] = ((Number)((Object[])msg.get("attitude_quaternion"))[i]).doubleValue();
+                // System.out.println("["+quat[0]+","+quat[1]+","+quat[2]+","+quat[3]+"]");
+            }        
+            lat=Math.toRadians(msg.getDouble("lat")*1e-7);
+            lon=Math.toRadians(msg.getDouble("lon")*1e-7);
+            alt=msg.getDouble("alt")*1e-3;
+
+            Vector3d pos = new Vector3d(EARTH_RADIUS*(lat-lat0),EARTH_RADIUS*(lon-lon0)*Math.cos(lat0),alt0-alt);
+            double [] euler = RotationConversion.eulerAnglesByQuaternion(quat);
+            Matrix3d dcm = new Matrix3d(RotationConversion.rotationMatrixByEulerAngles(euler[0],euler[1],euler[2]));   
+
+            vehicle.setControl(Arrays.asList(control));     // set 0 throttles
+            vehicle.setPosition(pos);   // we want ideally a local pos groundtruth
+            vehicle.setRotation(dcm); 
+            vehicle.updateBranchGroup();
+
+            // System.out.println("pos=["+(int)(pos.x*1000)+", "+(int)(pos.y*1000)+", "+(int)(pos.z*1000)+"]");
+        }
+    }
+
+    @Override
+    public void update(long t, boolean paused) {
+        super.update(t, true);
+   }
+}

src/me/drton/jmavsim/MAVLinkHILSystem.java

@@ -18,8 +18,8 @@
  * MAVLinkHILSystem should have the same sysID as the autopilot, but different componentId.
  */
 public class MAVLinkHILSystem extends MAVLinkSystem {
-    private Simulator simulator;
-    private AbstractVehicle vehicle;
+    protected Simulator simulator;
+    protected AbstractVehicle vehicle;
     private boolean gotHeartBeat = false;
     private boolean inited = false;
     private boolean stopped = false;

src/me/drton/jmavsim/Simulator.java

@@ -53,6 +53,7 @@ private static enum Port {
     public static boolean   LOG_TO_STDOUT         =
         true;   // send System.out messages to stdout (console) as well as any custom handlers (see SystemOutHandler)
     public static boolean DEBUG_MODE = false;
+    public static boolean DISPLAY_ONLY = false; // display HIL_STATE_QUATERNION from the autopilot, simulation engine disabled
 
     public static final int    DEFAULT_SIM_RATE = 250; // Hz
     public static final double    DEFAULT_SPEED_FACTOR = 1.0;
@@ -196,7 +197,7 @@ public Simulator() throws IOException, InterruptedException {
             connCommon.addSkipMessage(schema.getMessageDefinition("HIL_ACTUATOR_CONTROLS").id);
             connCommon.addSkipMessage(schema.getMessageDefinition("HIL_SENSOR").id);
             connCommon.addSkipMessage(schema.getMessageDefinition("HIL_GPS").id);
-            connCommon.addSkipMessage(schema.getMessageDefinition("HIL_STATE_QUATERNION").id);
+            // connCommon.addSkipMessage(schema.getMessageDefinition("HIL_STATE_QUATERNION").id);
         }
         world.addObject(connCommon);
 
@@ -269,7 +270,14 @@ public Simulator() throws IOException, InterruptedException {
 
         // Create MAVLink HIL system
         // SysId should be the same as autopilot, ComponentId should be different!
-        hilSystem = new MAVLinkHILSystem(schema, autopilotSysId, 51, vehicle);
+        if (DISPLAY_ONLY){
+            // paused=true;
+            vehicle.setIgnoreGravity(true);
+            vehicle.setIgnoreWind(true);
+            hilSystem = new MAVLinkDisplayOnly(schema, autopilotSysId, 51, vehicle);
+        } else {
+            hilSystem = new MAVLinkHILSystem(schema, autopilotSysId, 51, vehicle);
+        }
         hilSystem.setSimulator(this);
         //hilSystem.setHeartbeatInterval(0);
         connHIL.addNode(hilSystem);
@@ -564,6 +572,7 @@ public void advanceTime() {
     public final static String RATE_STRING = "-r <Hz>";
     public final static String SPEED_FACTOR_STRING = "-f";
     public final static String LOCKSTEP_STRING = "-lockstep";
+    public final static String DISPLAY_ONLY_STRING = "-disponly";    
     public final static String CMD_STRING =
         "java [-Xmx512m] -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator";
     public final static String CMD_STRING_JAR = "java [-Xmx512m] -jar jmavsim_run.jar";
@@ -580,7 +589,8 @@ public void advanceTime() {
                                               GUI_MAX_STRING + "] [" +
                                               GUI_VIEW_STRING + "] [" +
                                               REP_STRING + "] [" +
-                                              PRINT_INDICATION_STRING + "]";
+                                              PRINT_INDICATION_STRING + "] [" +
+                                              DISPLAY_ONLY_STRING + "]";
 
     public static void main(String[] args)
     throws InterruptedException, IOException {
@@ -784,6 +794,8 @@ public static void main(String[] args)
                     System.err.println("-view requires an argument: " + GUI_VIEW_STRING);
                     return;
                 }
+            } else if (arg.equals(DISPLAY_ONLY_STRING)) {
+                DISPLAY_ONLY = true;    // // display HIL_STATE_QUATERNION from the autopilot, simulation engine disabled
             } else if (arg.equals("-automag")) {
                 DO_MAG_FIELD_LOOKUP = true;
             } else if (arg.equals("-rep")) {
@@ -869,6 +881,9 @@ public static void handleHelpFlag() {
         System.out.println(PRINT_INDICATION_STRING);
         System.out.println("      Monitor (echo) all/selected MAVLink messages to the console.");
         System.out.println("      If no MsgIDs are specified, all messages are monitored.");
+        System.out.println(DISPLAY_ONLY_STRING);        
+        System.out.println("      Disable the simulation engine.");
+        System.out.println("      Display the autopilot states from HIL_STATE_QUATERNION.");
         System.out.println("");
         System.out.println("Key commands (in the visualizer window):");
         System.out.println("");