@Override
    public synchronized void velocityChanged(float vx, float vy, float vz) {
        
        long temp = System.currentTimeMillis();
        long timeStamp = LastVelocityReadTime - temp;
        LastVelocityReadTime = temp;
        
        if(VelocityXSamples.size() >= NrOfSamples)
        {
            VelocityXSamples.removeLast();
        }
        VelocityXSamples.addFirst(new VelocitySample(vx, timeStamp));
        
        if(VelocityYSamples.size() >= NrOfSamples)
        {
            VelocityYSamples.removeLast();
        }
        VelocityYSamples.addFirst(new VelocitySample(vy, timeStamp));
        
        /* -- Moving average X -- */
        int size = VelocityXSamples.size();
        float sumX = 0;
        for(VelocitySample sample : VelocityXSamples)
        {
            sumX += sample.getSample();
        }
        
        float mavX = sumX/size;
        VelocityX.setSample(mavX);
        VelocityX.setTimeStamp(timeStamp);
        //VelocityXSamplesFiltered.addFirst(new VelocitySample(mavX, timeStamp));
        
        /* -- Moving average Y -- */
        size = VelocityYSamples.size();
        float sumY = 0;
        for(VelocitySample sample : VelocityXSamples)
        {
            sumY += sample.getSample();
        }
        
        float mavY = sumY/size;
        VelocityY.setSample(mavY);
        VelocityY.setTimeStamp(timeStamp);
        //VelocityYSamplesFiltered.addFirst(new VelocitySample(mavY, timeStamp));
        
        /* -- Calculate & store distance -- */
        float distX = mavX*(timeStamp/1000);
        DistanceX.setDistance(distX);
        
        float distY = mavY*(timeStamp/1000);
        DistanceY.setDistance(distY);
        
        String[] loggerData = {mavX+"", mavY+"", distX+"", distY+""};
        
        logger.LogData(loggerData, GetTimeSinceStart(), timeStamp);
        
        System.out.println("VX: " + mavX + " VY: " + mavY + " distX: " + distX + " distY: " + distY);
    }