package me.drton.jmavsim;

import me.drton.jmavlib.geo.GlobalPositionProjector;
import me.drton.jmavlib.processing.DelayLine;

import javax.vecmath.Matrix3d;
import javax.vecmath.Vector3d;

/**
 * User: ton Date: 27.11.13 Time: 19:06
 */
public class SimpleSensors implements Sensors {
    private DynamicObject object;
    private GlobalPositionProjector globalProjector = new GlobalPositionProjector();
    private DelayLine<GNSSReport> gpsDelayLine = new DelayLine<GNSSReport>();
    private long gpsStartTime = 0;
    private long gpsInterval = 200;
    private long gpsLast = 0;
    private GNSSReport gps = new GNSSReport();
    private boolean gpsUpdated = false;
    private double pressureAltOffset = 0.0;

    @Override
    public void setObject(DynamicObject object) {
        this.object = object;
        globalProjector.init(object.getWorld().getGlobalReference());
    }

    public void setGPSStartTime(long time) {
        gpsStartTime = time;
    }

    public void setGPSDelay(long delay) {
        gpsDelayLine.setDelay(delay);
    }

    public void setGPSInterval(long gpsInterval) {
        this.gpsInterval = gpsInterval;
    }

    public void setPressureAltOffset(double pressureAltOffset) {
        this.pressureAltOffset = pressureAltOffset;
    }

    @Override
    public Vector3d getAcc() {
        Vector3d accBody = new Vector3d(object.getAcceleration());
        accBody.sub(object.getWorld().getEnvironment().getG());
        Matrix3d rot = new Matrix3d(object.getRotation());
        rot.transpose();
        rot.transform(accBody);
        return accBody;
    }

    @Override
    public Vector3d getGyro() {
        return object.getRotationRate();
    }

    @Override
    public Vector3d getMag() {
        Vector3d mag = new Vector3d(object.getWorld().getEnvironment().getMagField(object.getPosition()));
        Matrix3d rot = new Matrix3d(object.getRotation());
        rot.transpose();
        rot.transform(mag);
        return mag;
    }

    @Override
    public double getPressureAlt() {
        return -object.getPosition().z + pressureAltOffset;
    }

    @Override
    public GNSSReport getGNSS() {
        return gps;
    }

    @Override
    public boolean isGPSUpdated() {
        boolean res = gpsUpdated;
        gpsUpdated = false;
        return res;
    }

    @Override
    public void update(long t) {
        // GPS
        if (t > gpsStartTime && t > gpsLast + gpsInterval) {
            gpsLast = t;
            gpsUpdated = true;
            GNSSReport gpsCurrent = new GNSSReport();
            Vector3d pos = object.getPosition();
            gpsCurrent.position = globalProjector.reproject(new double[]{pos.x, pos.y, pos.z});
            gpsCurrent.eph = 1.0;
            gpsCurrent.epv = 1.0;
            gpsCurrent.velocity = new Vector3d(object.getVelocity());
            gpsCurrent.fix = 3;
            gpsCurrent.time = System.currentTimeMillis() * 1000;
            gps = gpsDelayLine.getOutput(t, gpsCurrent);
        }
    }
}

@@ -872,32 +872,582 @@
 ence());%0A    %7D%0A%0A
+    public Vector3d addZeroMeanNoise(Vector3d vIn, double var) %7B%0A        double x0;%0A        double b0, b1;%0A%0A        do %7B%0A            b0 = Math.random();%0A            b1 = Math.random();%0A        %7D while (b0 %3C= Float.intBitsToFloat(0x1));%0A%0A        x0 = java.lang.Math.sqrt(-2.0 * java.lang.Math.log(b1)) * java.lang.Math.cos(Math.PI * 2.0 * b1);%0A%0A        double n = x0 * (var * var);%0A%0A        if (java.lang.Double.isFinite(n)) %7B%0A            return new Vector3d(vIn.x + n, vIn.y + n, vIn.z + n);%0A        %7D else %7B%0A            return vIn;%0A        %7D%0A    %7D%0A%0A
     public void 
@@ -1471,24 +1471,24 @@
 ong time) %7B%0A
-
         gpsS
@@ -2108,24 +2108,74 @@
 m(accBody);%0A
+        accBody = addZeroMeanNoise(accBody, 0.8);%0A
         retu
@@ -2254,16 +2254,33 @@
  return 
+addZeroMeanNoise(
 object.g
@@ -2295,16 +2295,22 @@
 onRate()
+, 0.5)
 ;%0A    %7D%0A
@@ -2589,19 +2589,42 @@
  return 
-mag
+addZeroMeanNoise(mag, 0.3)
 ;%0A    %7D%0A

Add zero-mean noise to simulator to satisfy sensor validation routines in PX4