 		depthInMeter = buhlmann.calculateDepthFromPressure(pressureInMillibar);
 	}
 
+	//Check for sensor error
+	if (abs(depthInMeter-previousDepthInMeter) > 20) {
+		//This is a sensor error - skip it!!!
+		return;
+	} else {
+		previousDepthInMeter = depthInMeter;
+	}
+
 	//Draw the data to the screen
 	view.drawCurrentTemperature(temperatureInCelsius);
 	view.drawCurrentPressure(pressureInMillibar);


byte safetyStopState = SAFETY_STOP_NOT_STARTED;

unsigned long diveDurationInSeconds;
unsigned long safetyStopDurationInSeconds;
unsigned long testPreviousDiveDurationInSeconds;

SimpleTimer diveDurationTimer;

MAX17043 batteryMonitor;
float batterySoc = 255;


	tft.InitLCD();

    // Set the function, which will be called in every 5th second, if the timer is enabled
	diveDurationTimer.setTimer(5000, diveUnderWater, diveDurationTimer.RUN_FOREVER);
	diveDurationTimer.disable(diveDurationTimer.RUN_FOREVER);

	displayScreen(MENU_SCREEN);
