diff --git a/examples/test_ms_ir_thermometer/test_ms_ir_thermometer.ino b/examples/test_ms_ir_thermometer/test_ms_ir_thermometer.ino
index 95bb881..67b177e 100644
--- a/examples/test_ms_ir_thermometer/test_ms_ir_thermometer.ino
+++ b/examples/test_ms_ir_thermometer/test_ms_ir_thermometer.ino
@@ -71,30 +71,51 @@ void loop() {
 
     case a:
       temp = irt.getAmbientTemperatureC();
-      Serial.print("Ambient temperature: ");
-      Serial.print(temp);
-      Serial.println("C");
+
+      if (temp < 0) {
+        Serial.print("Error: ");
+        Serial.println((int)temp);
+      } else {
+        Serial.print("Ambient temperature: ");
+        Serial.print(temp);
+        Serial.println("C");
+      }
       break;
 
     case t:
       temp = irt.getTargetTemperatureC();
-      Serial.print("Target temperature: ");
-      Serial.print(temp);
-      Serial.println("C");
+      if (temp < 0) {
+        Serial.print("Error: ");
+        Serial.println((int)temp);
+      } else {
+        Serial.print("Target temperature: ");
+        Serial.print(temp);
+        Serial.println("C");
+      }
       break;
 
     case A:
       temp = irt.getAmbientTemperatureF();
-      Serial.print("Ambient temperature: ");
-      Serial.print(temp);
-      Serial.println("F");
+      if (temp < 0) {
+        Serial.print("Error: ");
+        Serial.println((int)temp);
+      } else {
+        Serial.print("Ambient temperature: ");
+        Serial.print(temp);
+        Serial.println("F");
+      }
       break;
 
     case T:
       temp = irt.getTargetTemperatureF();
-      Serial.print("Target temperature: ");
-      Serial.print(temp);
-      Serial.println("F");
+      if (temp < 0) {
+        Serial.print("Error: ");
+        Serial.println((int)temp);
+      } else {
+        Serial.print("Target temperature: ");
+        Serial.print(temp);
+        Serial.println("F");
+      }
       break;
 
     case p:
diff --git a/src/LmHtIrSeekerV2.cpp b/src/LmHtIrSeekerV2.cpp
index 8883572..153b8bc 100644
--- a/src/LmHtIrSeekerV2.cpp
+++ b/src/LmHtIrSeekerV2.cpp
@@ -67,10 +67,10 @@ int HtIrSeekerV2::getDirection(int mode)
 {
 	switch (switchMode(mode)) {
 	case DC:
-		return i2c.readByte(HISV2_REG_DC_DIR);
+		return i2c.getByte(HISV2_REG_DC_DIR);
 
 	case AC:
-		return i2c.readByte(HISV2_REG_AC_DIR);
+		return i2c.getByte(HISV2_REG_AC_DIR);
 	}
 
 	return -1;
@@ -134,10 +134,10 @@ int HtIrSeekerV2::getSensorValue(unsigned int id, int mode)
 
 	switch (switchMode(mode)) {
 	case DC:
-		return i2c.readByte(HISV2_REG_DC_VAL + id);
+		return i2c.getByte(HISV2_REG_DC_VAL + id);
 
 	case AC:
-		return i2c.readByte(HISV2_REG_AC_VAL + id);
+		return i2c.getByte(HISV2_REG_AC_VAL + id);
 	}
 
 	return -1;
@@ -161,7 +161,7 @@ int HtIrSeekerV2::getSensorValues(int values[], size_t n_values, int mode)
 
 int HtIrSeekerV2::getAverage(void)
 {
-	return i2c.readByte(HISV2_REG_DC_AVG);
+	return i2c.getByte(HISV2_REG_DC_AVG);
 }
 
 int HtIrSeekerV2::switchMode(int mode)
diff --git a/src/LmI2C.cpp b/src/LmI2C.cpp
index 8772f82..3fd0660 100644
--- a/src/LmI2C.cpp
+++ b/src/LmI2C.cpp
@@ -81,35 +81,42 @@ int I2C::readStr(int reg, char *val, size_t len)
 	return read(reg, val, len - 1);
 }
 
-int I2C::readByte(int reg)
+int I2C::readByte(uint8_t *val, int reg)
 {
-	char val[1];
-	size_t len = sizeof(val) / sizeof(val[0]);
+	return read(reg, val, 1);
+}
+
+int I2C::getByte(int reg)
+{
+	uint8_t val;
 	int ret;
 
-	ret = read(reg, val, len);
+	ret = I2C::readByte(&val, reg);
 
 	if (ret < 0)
-		return ret;
+		return -1;
 
-	return val[0];
+	return val;
 }
 
-int I2C::readWord(int reg)
+int I2C::readWord(uint16_t *word, int reg)
 {
-	uint16_t word = 0;
+	uint8_t val;
 	int ret;
 
+	if(!word)
+		return -1;
+
 	for (int b = 0; b < 2; b++) {
-		ret = readByte(reg + b);
+		ret = readByte(&val, reg + b);
 
 		if (ret < 0)
 			return ret;
 
-		word |= ((uint16_t)ret << (8 * b));
+		*word |= ((uint16_t)val << (8 * b));
 	}
 
-	return word;
+	return 0;
 }
 
 int I2C::write(int reg, char *val, size_t len)
diff --git a/src/LmI2C.h b/src/LmI2C.h
index 2c16292..7e01b63 100644
--- a/src/LmI2C.h
+++ b/src/LmI2C.h
@@ -13,6 +13,7 @@
 //! @endcond
 
 #include <stddef.h>
+#include <stdint.h>
 
 class I2C
 {
@@ -22,8 +23,9 @@ class I2C
 		I2C(void);
 		int read(int reg, char *val, size_t len);
 		int readStr(int reg, char *val, size_t len);
-		int readByte(int reg);
-		int readWord(int reg);
+		int readByte(uint8_t *val, int reg);
+		int getByte(int reg);
+		int readWord(uint16_t *word, int reg);
 		int write(int reg, char *val, size_t len);
 		int getVersion(char *version, size_t len);
 		int getVendorId(char *vendor, size_t len);
diff --git a/src/LmMsIRThermometer.cpp b/src/LmMsIRThermometer.cpp
index 5a7d63e..97b7281 100644
--- a/src/LmMsIRThermometer.cpp
+++ b/src/LmMsIRThermometer.cpp
@@ -23,22 +23,35 @@ MsIRThermometer::MsIRThermometer(void)
 	addr = 0x2A / 2;
 }
 
+float MsIRThermometer::getTemperature(int reg)
+{
+	uint16_t word;
+	int ret;
+
+	ret = I2C::readWord(&word, reg);
+
+	if(ret < 0)
+		return -1;
+
+	return (float)word / 100.;
+}
+
 float MsIRThermometer::getAmbientTemperatureC(void)
 {
-	return (float)I2C::readWord(MS_IRT_AMB_TEMP_C) / 100.;
+	return getTemperature(MS_IRT_AMB_TEMP_C);
 }
 
 float MsIRThermometer::getTargetTemperatureC(void)
 {
-	return (float)I2C::readWord(MS_IRT_TGT_TEMP_C) / 100.;
+	return getTemperature(MS_IRT_TGT_TEMP_C);
 }
 
 float MsIRThermometer::getAmbientTemperatureF(void)
 {
-	return (float)I2C::readWord(MS_IRT_AMB_TEMP_F) / 100.;
+	return getTemperature(MS_IRT_AMB_TEMP_F);
 }
 
 float MsIRThermometer::getTargetTemperatureF(void)
 {
-	return (float)I2C::readWord(MS_IRT_TGT_TEMP_F) / 100.;
+	return getTemperature(MS_IRT_TGT_TEMP_F);
 }
diff --git a/src/LmMsIRThermometer.h b/src/LmMsIRThermometer.h
index 1e49432..6b31323 100644
--- a/src/LmMsIRThermometer.h
+++ b/src/LmMsIRThermometer.h
@@ -30,6 +30,9 @@ class MsIRThermometer: public MsI2cSensor
 		float getTargetTemperatureC(void);
 		float getAmbientTemperatureF(void);
 		float getTargetTemperatureF(void);
+
+	private:
+		float getTemperature(int reg);
 };
 
 //! @cond SuppressGuard
diff --git a/src/LmMsLineLeaderV2.cpp b/src/LmMsLineLeaderV2.cpp
index b1c07e9..463ae2e 100644
--- a/src/LmMsLineLeaderV2.cpp
+++ b/src/LmMsLineLeaderV2.cpp
@@ -83,22 +83,22 @@ int MsLineLeaderV2::getVoltage(uint16_t *readings, size_t len)
 
 int MsLineLeaderV2::getAverage(void)
 {
-	return I2C::readByte(MLLV2_AVG);
+	return I2C::getByte(MLLV2_AVG);
 }
 
 int MsLineLeaderV2::getSteering(void)
 {
-	return I2C::readByte(MLLV2_STEER);
+	return I2C::getByte(MLLV2_STEER);
 }
 
 int MsLineLeaderV2::getResult(void)
 {
-	return I2C::readByte(MLLV2_RESULT);
+	return I2C::getByte(MLLV2_RESULT);
 }
 
 int MsLineLeaderV2::getSetpoint(void)
 {
-	return I2C::readByte(MLLV2_SETPT);
+	return I2C::getByte(MLLV2_SETPT);
 }
 
 int MsLineLeaderV2::setSetpoint(char setpoint)
@@ -108,7 +108,7 @@ int MsLineLeaderV2::setSetpoint(char setpoint)
 
 int MsLineLeaderV2::getKp(void)
 {
-	return I2C::readByte(MLLV2_KP);
+	return I2C::getByte(MLLV2_KP);
 }
 
 int MsLineLeaderV2::setKp(char k_p)
@@ -118,7 +118,7 @@ int MsLineLeaderV2::setKp(char k_p)
 
 int MsLineLeaderV2::getKi(void)
 {
-	return I2C::readByte(MLLV2_KI);
+	return I2C::getByte(MLLV2_KI);
 }
 
 int MsLineLeaderV2::setKi(char k_i)
@@ -128,7 +128,7 @@ int MsLineLeaderV2::setKi(char k_i)
 
 int MsLineLeaderV2::getKd(void)
 {
-	return I2C::readByte(MLLV2_KD);
+	return I2C::getByte(MLLV2_KD);
 }
 
 int MsLineLeaderV2::setKd(char k_d)
@@ -138,7 +138,7 @@ int MsLineLeaderV2::setKd(char k_d)
 
 int MsLineLeaderV2::getKpDiv(void)
 {
-	return I2C::readByte(MLLV2_KP_DIV);
+	return I2C::getByte(MLLV2_KP_DIV);
 }
 
 int MsLineLeaderV2::setKpDiv(char k_p_div)
@@ -148,7 +148,7 @@ int MsLineLeaderV2::setKpDiv(char k_p_div)
 
 int MsLineLeaderV2::getKiDiv(void)
 {
-	return I2C::readByte(MLLV2_KI_DIV);
+	return I2C::getByte(MLLV2_KI_DIV);
 }
 
 int MsLineLeaderV2::setKiDiv(char k_i_div)
@@ -158,7 +158,7 @@ int MsLineLeaderV2::setKiDiv(char k_i_div)
 
 int MsLineLeaderV2::getKdDiv(void)
 {
-	return I2C::readByte(MLLV2_KD_DIV);
+	return I2C::getByte(MLLV2_KD_DIV);
 }
 
 int MsLineLeaderV2::setKdDiv(char k_d_div)