diff --git a/.github/workflows/stylechecker.yml b/.github/workflows/stylechecker.yml
index 25eb4ed..fbd7383 100644
--- a/.github/workflows/stylechecker.yml
+++ b/.github/workflows/stylechecker.yml
@@ -16,7 +16,7 @@ jobs:
     - name: Install recent uncrustify
       run: |
         # we need this because of a bug introduced in https://github.com/uncrustify/uncrustify/pull/3655
-        git clone https://github.com/uncrustify/uncrustify.git --branch uncrustify-0.75.1
+        git clone https://github.com/uncrustify/uncrustify.git --branch uncrustify-0.79.0
         cd uncrustify && mkdir build && cd build
         cmake .. && sudo make install && cd ../..
     - name: Run uncrustify
diff --git a/inc/mlx90632.h b/inc/mlx90632.h
index bdb0bcf..64c0461 100644
--- a/inc/mlx90632.h
+++ b/inc/mlx90632.h
@@ -61,7 +61,7 @@
 
 /* BIT, GENMASK and ARRAY_SIZE macros are imported from kernel */
 #ifndef BIT
-#define BIT(x)(1UL << (x))
+#define BIT(x)(1U << (x))
 #endif
 #ifndef GENMASK
 #ifndef BITS_PER_LONG
@@ -69,7 +69,7 @@
 #define BITS_PER_LONG 64 /**< Define how many bits per long your CPU has */
 #endif
 #define GENMASK(h, l) \
-    (((~0UL) << (l)) & (~0UL >> (BITS_PER_LONG - 1 - (h))))
+        ((((1U << h) - 1) | (1U << h)) & ~((1U << l) - 1))
 #endif
 #ifndef ARRAY_SIZE
 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(arr[0])) /**< Return number of elements in array */
@@ -163,6 +163,8 @@ typedef enum mlx90632_meas_e {
 /* Start of burst measurement options */
 #define MLX90632_START_BURST_MEAS MLX90632_CFG_SOB(1)
 #define MLX90632_BURST_MEAS_NOT_PENDING MLX90632_CFG_SOB(0)
+/* Start of single measurement option */
+#define MLX90632_START_SINGLE_MEAS (1 << MLX90632_CFG_SOC_SHIFT)
 
 /* Device status register - volatile */
 #define MLX90632_REG_STATUS 0x3fff /**< Device status register */
@@ -208,7 +210,48 @@ typedef enum mlx90632_meas_e {
  * Masks the old register and shifts the new value in
  */
 #define MLX90632_NEW_REG_VALUE(old_reg, new_value, h, l) \
-    ((old_reg & (0xFFFF ^ GENMASK(h, l))) | (new_value << MLX90632_EE_REFRESH_RATE_SHIFT))
+        ((old_reg & (0xFFFF ^ GENMASK(h, l))) | (new_value << MLX90632_EE_REFRESH_RATE_SHIFT))
+
+/** Read raw ambient and object temperature only when measurement data is ready
+ *
+ * Read raw ambient and object temperatures without waiting. This values still need
+ * to be pre-processed via @link mlx90632_preprocess_temp_ambient @endlink and @link
+ * mlx90632_preprocess_temp_object @endlink functions and then processed via @link
+ * mlx90632_calc_temp_ambient @endlink and @link mlx90632_calc_temp_object @endlink
+ * to retrieve values in milliCelsius. This function assumes that measurement cycle
+ * has been triggered and completed via @link mlx90632_start_measurement @endlink or
+ * @link mlx90632_trigger_measurement @endlink or @link mlx90632_wait_for_measurement @endlink,
+ * and channel position has been checked via @link mlx90632_get_channel_position @endlink
+ * in case of continuous mode. This function assumes that measurement cycle has been
+ * triggered and completed via @link mlx90632_start_measurement_burst @endlink, or @link
+ * mlx90632_trigger_measurement_burst @endlink and @link mlx90632_wait_for_measurement_burst
+ * @endlink with sufficient time in between needed to refresh the whole measurement table
+ * in case of burst mode. This function assumes that measurement cycle has been
+ * triggered and completed via @link mlx90632_trigger_measurement_single @endlink and
+ * @link mlx90632_wait_for_measurement @endlink with sufficient time in between needed to
+ * refresh a single measurement in case of single mode (must be triggered and completed
+ * two times after power-up before calling this function).
+ *
+ * @param[in] channel_position Channel position where new (recently updated) measurement can be found,
+ *                             usually return value of @link mlx90632_start_measurement @endlink or
+ *                             @link mlx90632_wait_for_measurement @endlink or @link
+ *                             mlx90632_get_channel_position @endlink in case of continuous mode,
+ *                             2 in case of burst mode, and return value of @link
+ *                             mlx90632_wait_for_measurement @endlink or @link
+ *                             mlx90632_get_channel_position @endlink in case of single mode
+ * @param[out] ambient_new_raw Pointer to where new raw ambient temperature is written
+ * @param[out] object_new_raw Pointer to where new raw object temperature is written
+ * @param[out] ambient_old_raw Pointer to where old raw ambient temperature is written
+ * @param[out] object_old_raw Pointer to where old raw object temperature is written
+ *
+ * @retval 0 Successfully read both temperatures
+ * @retval <0 Something went wrong. Check errno.h for more details
+ *
+ * @note This function is not blocking!
+ */
+int32_t mlx90632_read_temp_raw_wo_wait(int32_t channel_position,
+                                       int16_t *ambient_new_raw, int16_t *ambient_old_raw,
+                                       int16_t *object_new_raw, int16_t *object_old_raw);
 
 /** Read raw ambient and object temperature
  *
@@ -368,6 +411,30 @@ double mlx90632_calc_temp_object_reflected(int32_t object, int32_t ambient, doub
  */
 int32_t mlx90632_init(void);
 
+/** Trigger measurement for mlx90632
+ *
+ * Trigger measurement cycle. It does not read anything, just triggers measurement.
+ *
+ * @retval <0 Something failed. Check errno.h for more information
+ * @retval 0 Successfully triggered and started measuremeent
+ *
+ * @note This function is not blocking!
+ */
+int32_t mlx90632_trigger_measurement(void);
+
+/** Wait for measurement to complete for mlx90632
+ *
+ * Wait for measurement data to be ready. It does not read anything, just completes measurement.
+ * This function assumes that measurement cycle has been triggered via @link
+ * mlx90632_trigger_measurement @endlink or @link mlx90632_trigger_measurement_single @endlink.
+ *
+ * @retval <0 Something failed. Check errno.h for more information
+ * @retval >=0 Channel position where new (recently updated) measurement can be found
+ *
+ * @note This function is using usleep so it is blocking!
+ */
+int32_t mlx90632_wait_for_measurement(void);
+
 /** Trigger start measurement for mlx90632
  *
  * Trigger measurement cycle and wait for data to be ready. It does not read anything, just triggers and completes.
@@ -377,7 +444,7 @@ int32_t mlx90632_init(void);
  *
  * @note This function is using usleep so it is blocking!
  */
-int mlx90632_start_measurement(void);
+int32_t mlx90632_start_measurement(void);
 
 /** Set emissivity which is retained in single variable.
  *
@@ -391,6 +458,33 @@ void mlx90632_set_emissivity(double value);
  */
 double mlx90632_get_emissivity(void);
 
+/** Trigger burst measurement for mlx90632
+ *
+ * Trigger a full measurement cycle. It does not read anything, just triggers measurement.
+ * The SOB bit is set so that the complete measurement table is re-freshed.
+ *
+ * @note The SOB bit is cleared internally by the mlx90632 immediately after the measurement has started.
+ *
+ * @retval <0 Something failed. Check errno.h for more information
+ * @retval 0 Successfully triggered and started measuremeent
+ *
+ * @note This function is not blocking!
+ */
+int32_t mlx90632_trigger_measurement_burst(void);
+
+/** Wait for burst measurement to complete for mlx90632
+ *
+ * Wait for measurement data to be ready. It does not read anything, just completes measurement.
+ * This function assumes that burst measurement cycle has been triggered via @link
+ * mlx90632_trigger_measurement_burst @endlink.
+ *
+ * @retval <0 Something failed. Check errno.h for more information
+ * @retval 0 New data is available and waiting to be processed
+ *
+ * @note This function is using usleep so it is blocking!
+ */
+int32_t mlx90632_wait_for_measurement_burst(void);
+
 /** Trigger start of burst measurement for mlx90632
  *
  * Trigger a single measurement cycle and wait for data to be ready. It does not read anything, just triggers and completes.
@@ -407,6 +501,20 @@ double mlx90632_get_emissivity(void);
  */
 int32_t mlx90632_start_measurement_burst(void);
 
+/** Trigger single measurement for mlx90632
+ *
+ * Trigger a single measurement cycle. It does not read anything, just triggers measurement.
+ * The SOC bit is set so that the single measurement is triggered.
+ *
+ * @note The SOC bit is cleared internally by the mlx90632 immediately after the measurement has started.
+ *
+ * @retval <0 Something failed. Check errno.h for more information
+ * @retval 0 Successfully triggered and started measuremeent
+ *
+ * @note This function is not blocking!
+ */
+int32_t mlx90632_trigger_measurement_single(void);
+
 /** Reads the refresh rate and calculates the time needed for a single measurment from the EEPROM settings.
  *
  * @param[in] meas Measurement to read the frefresh rate for
@@ -453,11 +561,18 @@ int32_t mlx90632_set_refresh_rate(mlx90632_meas_t measRate);
  */
 mlx90632_meas_t mlx90632_get_refresh_rate(void);
 
+/** Gets the channel position where new (recently updated) measurement can be found
+ *
+ * @retval <0 Something failed. Check errno.h for more information
+ * @retval >=0 Channel position where new (recently updated) measurement can be found
+ */
+int32_t mlx90632_get_channel_position(void);
+
 ///@}
 
 #ifdef TEST
 int32_t mlx90632_read_temp_ambient_raw(int16_t *ambient_new_raw, int16_t *ambient_old_raw);
-int32_t mlx90632_read_temp_object_raw(int32_t start_measurement_ret,
+int32_t mlx90632_read_temp_object_raw(int32_t channel_position,
                                       int16_t *object_new_raw, int16_t *object_old_raw);
 
 #endif
diff --git a/inc/mlx90632_extended_meas.h b/inc/mlx90632_extended_meas.h
index 84c5de6..bb68c28 100644
--- a/inc/mlx90632_extended_meas.h
+++ b/inc/mlx90632_extended_meas.h
@@ -24,6 +24,37 @@
 #ifndef _MLX90632_EXTENDED_MEAS_LIB_
 #define _MLX90632_EXTENDED_MEAS_LIB_
 
+/** Read raw ambient and object temperature for extended range only when measurement data is ready
+ *
+ * Read raw ambient and object temperatures without waiting. This values still need
+ * to be pre-processed via @link mlx90632_preprocess_temp_ambient_extended @endlink and @link
+ * mlx90632_preprocess_temp_object_extended @endlink functions and then processed via @link
+ * mlx90632_calc_temp_ambient_extended @endlink and @link mlx90632_calc_temp_object_extended @endlink
+ * to retrieve values in milliCelsius. This function assumes that measurement cycle
+ * has been triggered and completed via @link mlx90632_start_measurement @endlink or
+ * @link mlx90632_trigger_measurement @endlink or @link mlx90632_wait_for_measurement @endlink,
+ * and channel position has been checked via @link mlx90632_get_channel_position @endlink
+ * in case of continuous mode. This function assumes that measurement cycle has been
+ * triggered and completed via @link mlx90632_start_measurement_burst @endlink, or @link
+ * mlx90632_trigger_measurement_burst @endlink and @link mlx90632_wait_for_measurement_burst
+ * @endlink with sufficient time in between needed to refresh the whole measurement table
+ * in case of burst mode. This function assumes that measurement cycle has been
+ * triggered and completed via @link mlx90632_trigger_measurement_single @endlink and
+ * @link mlx90632_wait_for_measurement @endlink with sufficient time in between needed to
+ * refresh a single measurement in case of single mode (must be triggered and completed
+ * three times after power-up before calling this function).
+ *
+ * @param[out] ambient_new_raw Pointer to where new raw ambient temperature is written
+ * @param[out] object_new_raw Pointer to where new raw object temperature is written
+ * @param[out] ambient_old_raw Pointer to where old raw ambient temperature is written
+ *
+ * @retval 0 Successfully read both temperatures
+ * @retval <0 Something went wrong. Check errno.h for more details
+ *
+ * @note This function is not blocking!
+ */
+int32_t mlx90632_read_temp_raw_extended_wo_wait(int16_t *ambient_new_raw, int16_t *ambient_old_raw, int16_t *object_new_raw);
+
 /** Read raw ambient and object temperature for extended range
  *
  * Trigger and read raw ambient and object temperatures. This values still need
diff --git a/src/mlx90632.c b/src/mlx90632.c
index 6a6c8dc..65433c8 100644
--- a/src/mlx90632.c
+++ b/src/mlx90632.c
@@ -44,18 +44,25 @@ static const char mlx90632version[] __attribute__((used)) = { VERSION };
 #define STATIC static
 #endif
 
-int mlx90632_start_measurement(void)
+int32_t mlx90632_trigger_measurement(void)
 {
-    int ret, tries = MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES;
     uint16_t reg_status;
+    int32_t ret;
 
     ret = mlx90632_i2c_read(MLX90632_REG_STATUS, &reg_status);
     if (ret < 0)
         return ret;
 
     ret = mlx90632_i2c_write(MLX90632_REG_STATUS, reg_status & (~MLX90632_STAT_DATA_RDY));
-    if (ret < 0)
-        return ret;
+
+    return ret;
+}
+
+int32_t mlx90632_wait_for_measurement(void)
+{
+    int tries = MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES;
+    uint16_t reg_status;
+    int32_t ret;
 
     while (tries-- > 0)
     {
@@ -80,6 +87,18 @@ int mlx90632_start_measurement(void)
     return (reg_status & MLX90632_STAT_CYCLE_POS) >> 2;
 }
 
+int32_t mlx90632_start_measurement(void)
+{
+    int32_t ret = mlx90632_trigger_measurement();
+
+    if (ret < 0)
+        return ret;
+
+    ret = mlx90632_wait_for_measurement();
+
+    return ret;
+}
+
 /** Based on @link mlx90632_start_measurement @endlink return value fill channel_new and channel_old
  * variables with proper values. This is to provide a bit more flexibility in case other channels are
  * returned and need a bit more mingeling than usual maths can provide.
@@ -152,14 +171,20 @@ STATIC int32_t mlx90632_read_temp_ambient_raw(int16_t *ambient_new_raw, int16_t
  * calculation functions. Because one value is newer than other (see @link mlx90632_start_measurement @endlink) this
  * function provides dynamics based on return value of the @link mlx90632_start_measurement @endlink.
  *
- * @param[in] start_measurement_ret Return value of @link mlx90632_start_measurement @endlink
+ * @param[in] channel_position Channel position where new (recently updated) measurement can be found,
+ *                             usually return value of @link mlx90632_start_measurement @endlink or
+ *                             @link mlx90632_wait_for_measurement @endlink or @link
+ *                             mlx90632_get_channel_position @endlink in case of continuous mode,
+ *                             2 in case of burst mode, and return value of @link
+ *                             mlx90632_wait_for_measurement @endlink or @link
+ *                             mlx90632_get_channel_position @endlink in case of single mode
  * @param[out] *object_new_raw Pointer to memory location where average of new object values from sensor is stored
  * @param[out] *object_old_raw Pointer to memory location where average of old object values from sensor is stored
  *
  * @retval 0 Successfully read both values
  * @retval <0 Something went wrong. Check errno.h for more details.
  */
-STATIC int32_t mlx90632_read_temp_object_raw(int32_t start_measurement_ret,
+STATIC int32_t mlx90632_read_temp_object_raw(int32_t channel_position,
                                              int16_t *object_new_raw, int16_t *object_old_raw)
 {
     int32_t ret;
@@ -167,7 +192,7 @@ STATIC int32_t mlx90632_read_temp_object_raw(int32_t start_measurement_ret,
     int16_t read;
     uint8_t channel, channel_old;
 
-    ret = mlx90632_channel_new_select(start_measurement_ret, &channel, &channel_old);
+    ret = mlx90632_channel_new_select(channel_position, &channel, &channel_old);
     if (ret != 0)
         return -EINVAL;
 
@@ -195,46 +220,49 @@ STATIC int32_t mlx90632_read_temp_object_raw(int32_t start_measurement_ret,
     return ret;
 }
 
-int32_t mlx90632_read_temp_raw(int16_t *ambient_new_raw, int16_t *ambient_old_raw,
-                               int16_t *object_new_raw, int16_t *object_old_raw)
+int32_t mlx90632_read_temp_raw_wo_wait(int32_t channel_position,
+                                       int16_t *ambient_new_raw, int16_t *ambient_old_raw,
+                                       int16_t *object_new_raw, int16_t *object_old_raw)
 {
-    int32_t ret, start_measurement_ret;
-
-    // trigger and wait for measurement to complete
-    start_measurement_ret = mlx90632_start_measurement();
-    if (start_measurement_ret < 0)
-        return start_measurement_ret;
-
     /** Read new and old **ambient** values from sensor */
-    ret = mlx90632_read_temp_ambient_raw(ambient_new_raw, ambient_old_raw);
+    int32_t ret = mlx90632_read_temp_ambient_raw(ambient_new_raw, ambient_old_raw);
+
     if (ret < 0)
         return ret;
 
     /** Read new and old **object** values from sensor */
-    ret = mlx90632_read_temp_object_raw(start_measurement_ret, object_new_raw, object_old_raw);
+    ret = mlx90632_read_temp_object_raw(channel_position, object_new_raw, object_old_raw);
 
     return ret;
 }
 
+int32_t mlx90632_read_temp_raw(int16_t *ambient_new_raw, int16_t *ambient_old_raw,
+                               int16_t *object_new_raw, int16_t *object_old_raw)
+{
+    // trigger and wait for measurement to complete
+    int32_t start_measurement_ret = mlx90632_start_measurement();
+
+    if (start_measurement_ret < 0)
+        return start_measurement_ret;
+
+    /** Read raw ambient and object temperature */
+    return mlx90632_read_temp_raw_wo_wait(start_measurement_ret, ambient_new_raw, ambient_old_raw,
+                                          object_new_raw, object_old_raw);
+}
+
 int32_t mlx90632_read_temp_raw_burst(int16_t *ambient_new_raw, int16_t *ambient_old_raw,
                                      int16_t *object_new_raw, int16_t *object_old_raw)
 {
-    int32_t ret, start_measurement_ret;
+    int32_t start_measurement_ret;
 
     // trigger and wait for measurement to complete
     start_measurement_ret = mlx90632_start_measurement_burst();
     if (start_measurement_ret < 0)
         return start_measurement_ret;
 
-    /** Read new and old **ambient** values from sensor */
-    ret = mlx90632_read_temp_ambient_raw(ambient_new_raw, ambient_old_raw);
-    if (ret < 0)
-        return ret;
-
-    /** Read new and old **object** values from sensor */
-    ret = mlx90632_read_temp_object_raw(2, object_new_raw, object_old_raw);
-
-    return ret;
+    /** Read raw ambient and object temperature */
+    return mlx90632_read_temp_raw_wo_wait(2, ambient_new_raw, ambient_old_raw,
+                                          object_new_raw, object_old_raw);
 }
 
 
@@ -564,11 +592,10 @@ int32_t mlx90632_calculate_dataset_ready_time(void)
     return refresh_time;
 }
 
-int32_t mlx90632_start_measurement_burst(void)
+int32_t mlx90632_trigger_measurement_burst(void)
 {
-    int32_t ret;
-    int tries = MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES;
     uint16_t reg;
+    int32_t ret;
 
     ret = mlx90632_i2c_read(MLX90632_REG_CTRL, &reg);
     if (ret < 0)
@@ -577,20 +604,22 @@ int32_t mlx90632_start_measurement_burst(void)
     reg |= MLX90632_START_BURST_MEAS;
 
     ret = mlx90632_i2c_write(MLX90632_REG_CTRL, reg);
-    if (ret < 0)
-        return ret;
 
-    ret = mlx90632_calculate_dataset_ready_time();
-    if (ret < 0)
-        return ret;
-    msleep(ret); /* Waiting for refresh of all the measurement tables */
+    return ret;
+}
+
+int32_t mlx90632_wait_for_measurement_burst(void)
+{
+    int tries = MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES;
+    uint16_t reg_status;
+    int32_t ret;
 
     while (tries-- > 0)
     {
-        ret = mlx90632_i2c_read(MLX90632_REG_STATUS, &reg);
+        ret = mlx90632_i2c_read(MLX90632_REG_STATUS, &reg_status);
         if (ret < 0)
             return ret;
-        if ((reg & MLX90632_STAT_BUSY) == 0)
+        if ((reg_status & MLX90632_STAT_BUSY) == 0)
             break;
         /* minimum wait time to complete measurement
          * should be calculated according to refresh rate
@@ -608,6 +637,43 @@ int32_t mlx90632_start_measurement_burst(void)
     return 0;
 }
 
+int32_t mlx90632_start_measurement_burst(void)
+{
+    int32_t ret = mlx90632_trigger_measurement_burst();
+
+    if (ret < 0)
+        return ret;
+
+    ret = mlx90632_calculate_dataset_ready_time();
+    if (ret < 0)
+        return ret;
+    msleep(ret); /* Waiting for refresh of all the measurement tables */
+
+    ret = mlx90632_wait_for_measurement_burst();
+
+    return ret;
+}
+
+int32_t mlx90632_trigger_measurement_single(void)
+{
+    uint16_t reg;
+    int32_t ret;
+
+    // Clear NEW_DATA flag
+    ret = mlx90632_trigger_measurement();
+    if (ret < 0)
+        return ret;
+
+    ret = mlx90632_i2c_read(MLX90632_REG_CTRL, &reg);
+    if (ret < 0)
+        return ret;
+
+    reg |= MLX90632_START_SINGLE_MEAS;
+
+    ret = mlx90632_i2c_write(MLX90632_REG_CTRL, reg);
+
+    return ret;
+}
 
 STATIC int32_t mlx90632_unlock_eeporm()
 {
@@ -704,4 +770,16 @@ mlx90632_meas_t mlx90632_get_refresh_rate(void)
     return (mlx90632_meas_t)MLX90632_REFRESH_RATE(meas1);
 }
 
+int32_t mlx90632_get_channel_position(void)
+{
+    uint16_t reg_status;
+    int32_t ret;
+
+    ret = mlx90632_i2c_read(MLX90632_REG_STATUS, &reg_status);
+    if (ret < 0)
+        return ret;
+
+    return (reg_status & MLX90632_STAT_CYCLE_POS) >> 2;
+}
+
 ///@}
diff --git a/src/mlx90632_extended_meas.c b/src/mlx90632_extended_meas.c
index 2634542..9a6319c 100644
--- a/src/mlx90632_extended_meas.c
+++ b/src/mlx90632_extended_meas.c
@@ -130,9 +130,23 @@ STATIC int32_t mlx90632_read_temp_object_raw_extended(int16_t *object_new_raw)
     return ret;
 }
 
+int32_t mlx90632_read_temp_raw_extended_wo_wait(int16_t *ambient_new_raw, int16_t *ambient_old_raw, int16_t *object_new_raw)
+{
+    /** Read new and old **ambient** values from sensor */
+    int32_t ret = mlx90632_read_temp_ambient_raw_extended(ambient_new_raw, ambient_old_raw);
+
+    if (ret < 0)
+        return ret;
+
+    /** Read new **object** value from sensor */
+    ret = mlx90632_read_temp_object_raw_extended(object_new_raw);
+
+    return ret;
+}
+
 int32_t mlx90632_read_temp_raw_extended(int16_t *ambient_new_raw, int16_t *ambient_old_raw, int16_t *object_new_raw)
 {
-    int32_t ret, start_measurement_ret;
+    int32_t start_measurement_ret;
     int tries = 3;
 
     // trigger and wait for measurement to complete
@@ -152,35 +166,20 @@ int32_t mlx90632_read_temp_raw_extended(int16_t *ambient_new_raw, int16_t *ambie
         return -ETIMEDOUT;
     }
 
-    /** Read new and old **ambient** values from sensor */
-    ret = mlx90632_read_temp_ambient_raw_extended(ambient_new_raw, ambient_old_raw);
-    if (ret < 0)
-        return ret;
-
-    /** Read new **object** value from sensor */
-    ret = mlx90632_read_temp_object_raw_extended(object_new_raw);
-
-    return ret;
+    /** Read raw ambient and object temperature for extended range */
+    return mlx90632_read_temp_raw_extended_wo_wait(ambient_new_raw, ambient_old_raw, object_new_raw);
 }
 
 int32_t mlx90632_read_temp_raw_extended_burst(int16_t *ambient_new_raw, int16_t *ambient_old_raw, int16_t *object_new_raw)
 {
-    int32_t ret, start_measurement_ret;
-
     // trigger and wait for measurement to complete
-    start_measurement_ret = mlx90632_start_measurement_burst();
+    int32_t start_measurement_ret = mlx90632_start_measurement_burst();
+
     if (start_measurement_ret < 0)
         return start_measurement_ret;
 
-    /** Read new and old **ambient** values from sensor */
-    ret = mlx90632_read_temp_ambient_raw_extended(ambient_new_raw, ambient_old_raw);
-    if (ret < 0)
-        return ret;
-
-    /** Read new **object** value from sensor */
-    ret = mlx90632_read_temp_object_raw_extended(object_new_raw);
-
-    return ret;
+    /** Read raw ambient and object temperature for extended range */
+    return mlx90632_read_temp_raw_extended_wo_wait(ambient_new_raw, ambient_old_raw, object_new_raw);
 }
 
 double mlx90632_preprocess_temp_ambient_extended(int16_t ambient_new_raw, int16_t ambient_old_raw, int16_t Gb)
diff --git a/test/TestDSP.c b/test/TestDSP.c
index f063c61..e77207e 100644
--- a/test/TestDSP.c
+++ b/test/TestDSP.c
@@ -159,6 +159,7 @@ void test_dsp_object(void)
 void test_dsp_object_reflected(void)
 {
     TEST_ASSERT_DOUBLE_WITHIN(0.01, dspv5_object_helper(609, 611, 22454, 23030), dspv5_object_reflected_helper(609, 611, 22454, 23030, 48.724));
+
     mlx90632_set_emissivity(0.1);
     TEST_ASSERT_DOUBLE_WITHIN(0.01, 98.141, dspv5_object_reflected_helper(609, 611, 22454, 23030, 49.66));
     TEST_ASSERT_DOUBLE_WITHIN(0.01, 143.956, dspv5_object_reflected_helper(609, 611, 22454, 23030, 40.00));
@@ -174,6 +175,7 @@ void test_dsp_object_extended(void)
     TEST_ASSERT_DOUBLE_WITHIN(0.02, 194.599, object_extended_helper(13550, 22454, 23030, 25.0));
     TEST_ASSERT_DOUBLE_WITHIN(0.02, 267.609, object_extended_helper(26900, 22454, 23030, 25.0));
     TEST_ASSERT_DOUBLE_WITHIN(0.02, 268.508, object_extended_helper(27100, 22454, 23030, 25.0));
+
     mlx90632_set_emissivity(0.1);
     TEST_ASSERT_DOUBLE_WITHIN(0.02, 98.141, object_extended_helper(305, 22454, 23030, 49.66));
     TEST_ASSERT_DOUBLE_WITHIN(0.02, 143.956, object_extended_helper(305, 22454, 23030, 40.00));
diff --git a/test/TestInit.c b/test/TestInit.c
index ec85ad2..c282871 100644
--- a/test/TestInit.c
+++ b/test/TestInit.c
@@ -47,7 +47,7 @@ void tearDown(void)
 void test_init_success(void)
 {
     uint16_t eeprom_version_mock = 0x105;
-    uint16_t reg_status_mock = 0x47; // cycle position 1 & data ready
+    uint16_t reg_status_mock = 0x00C7; // cycle position 17 & data ready
 
     // Confirm EEPROM version
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, 0);
@@ -62,10 +62,10 @@ void test_init_success(void)
     // Reset EOC and NewData
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & ~0x01, 0);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_init());
 
     // test also ID_CONSUMER
-    eeprom_version_mock = 0x205;
+    eeprom_version_mock = 0x0205;
 
     // Confirm EEPROM version
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, 0);
@@ -80,10 +80,10 @@ void test_init_success(void)
     // Reset EOC and NewData
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & ~0x01, 0);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_init());
 
     // test also another calibration id
-    eeprom_version_mock = 0x305;
+    eeprom_version_mock = 0x0305;
 
     // Confirm EEPROM version
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, 0);
@@ -98,10 +98,10 @@ void test_init_success(void)
     // Reset EOC and NewData
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & ~0x01, 0);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_init());
 
     // test extended range
-    eeprom_version_mock = 0x505;
+    eeprom_version_mock = 0x0505;
 
     // Confirm EEPROM version
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, 0);
@@ -116,34 +116,34 @@ void test_init_success(void)
     // Reset EOC and NewData
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & ~0x01, 0);
 
-    TEST_ASSERT_EQUAL_INT(ERANGE, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(ERANGE, mlx90632_init());
 }
 
 void test_init_wrong_eeprom_version(void)
 {
-    uint16_t eeprom_version_mock = 0x103;
+    uint16_t eeprom_version_mock = 0x0103;
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&eeprom_version_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPROTONOSUPPORT, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(-EPROTONOSUPPORT, mlx90632_init());
 }
 
 void test_init_i2c_read_fails(void)
 {
-    uint16_t eeprom_version_mock = 0x103;
+    uint16_t eeprom_version_mock = 0x0103;
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, -EBUSY);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
 
-    TEST_ASSERT_EQUAL_INT(-EBUSY, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(-EBUSY, mlx90632_init());
 }
 
 void test_init_i2c_read_fails2(void)
 {
-    uint16_t eeprom_version_mock = 0x105;
-    uint16_t reg_status_mock = 0x47; // cycle position 1 & data ready
+    uint16_t eeprom_version_mock = 0x0105;
+    uint16_t reg_status_mock = 0x00C7; // cycle position 17 & data ready
 
     // Confirm EEPROM version
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, 0);
@@ -153,15 +153,14 @@ void test_init_i2c_read_fails2(void)
     // Read REG_STATUS
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_init());
 }
 
 void test_init_i2c_read_fails3(void)
 {
-    uint16_t eeprom_version_mock = 0x105;
-    uint16_t reg_status_mock = 0x47; // cycle position 1 & data ready
+    uint16_t eeprom_version_mock = 0x0105;
+    uint16_t reg_status_mock = 0x00C7; // cycle position 17 & data ready
 
     // Confirm EEPROM version
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_VERSION, &eeprom_version_mock, 0);
@@ -177,7 +176,7 @@ void test_init_i2c_read_fails3(void)
     // Reset NewData
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & ~0x01, -EPERM);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_init());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_init());
 }
 
 ///@}
diff --git a/test/TestRead.c b/test/TestRead.c
index 4935a40..582c2c5 100644
--- a/test/TestRead.c
+++ b/test/TestRead.c
@@ -53,11 +53,124 @@ void tearDown(void)
 {
 }
 
+/** Test trigger measurement.
+ */
+void test_trigger_measurement_success(void)
+{
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
+
+    // Trigger measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
+
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_trigger_measurement());
+}
+
+/** Test different failure paths when triggering measurement.
+ */
+void test_trigger_measurement_errors(void)
+{
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
+
+    // Trigger measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement());
+
+    // Trigger measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), -EPERM);
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement());
+}
+
+/** Test wait for measurement with data ready.
+ */
+void test_wait_for_measurement_success(void)
+{
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
+
+    // Wait for measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    TEST_ASSERT_EQUAL_INT32(1, mlx90632_wait_for_measurement());
+}
+
+/** Test wait for measurement function, if data is ready and if it is not it waits one usleep period before retry.
+ *
+ * First we simulate data not ready with bit0 not set. After one period we flip the bit0 to 1 to indicate
+ * data ready and wait for measurement should complete with success.
+ */
+void test_wait_for_measurement_one_wait(void)
+{
+    uint16_t reg_status_mock = 0x0C86; // cycle position 1 & data not ready
+    uint16_t reg_status_mock1 = 0x0087; // cycle position 1 & data ready
+
+    // Wait for measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    usleep_Expect(10000, 11000);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock1, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock1);
+
+    TEST_ASSERT_EQUAL_INT32(1, mlx90632_wait_for_measurement());
+}
+
+/** Test failure path when waiting for measurement.
+ */
+void test_wait_for_measurement_error(void)
+{
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
+
+    // Wait for measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_wait_for_measurement());
+}
+
+/** Test sensor timeouts while waiting for measurement.
+ *
+ * If this happens in real life it means that after 100 tries sensor still did not indicate data ready, which probably
+ * points to much larger problem than a simple timeout. Timeout is only valid if usleep is a lot shorter than default
+ * values
+ */
+void test_wait_for_measurement_timeout(void)
+{
+    uint16_t reg_status_mock = 0x0C86; // cycle position 1 & data not ready
+    int i;
+
+    for (i = 0; i < MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES; ++i)
+    {
+        // Wait for measurement expectations
+        mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+        mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+        mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+        usleep_Expect(10000, 11000);
+    }
+
+    TEST_ASSERT_EQUAL_INT32(-ETIMEDOUT, mlx90632_wait_for_measurement());
+}
+
 /** Test start measurement with data ready.
  */
 void test_start_measurement_success(void)
 {
-    uint16_t reg_status_mock = 0x87; // cycle position 1 & data ready
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
@@ -70,7 +183,7 @@ void test_start_measurement_success(void)
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    TEST_ASSERT_EQUAL_INT(1, mlx90632_start_measurement());
+    TEST_ASSERT_EQUAL_INT32(1, mlx90632_start_measurement());
 }
 
 /** Test start measurement function, if data is ready and if it is not it waits one usleep period before retry.
@@ -80,8 +193,8 @@ void test_start_measurement_success(void)
  */
 void test_start_measurement_one_wait(void)
 {
-    uint16_t reg_status_mock = 0x86; // cycle position 1 & data NOT READY
-    uint16_t reg_status_mock1 = 0x87; // cycle position 1 & data READY
+    uint16_t reg_status_mock = 0x0C86; // cycle position 1 & data not reaady
+    uint16_t reg_status_mock1 = 0x0087; // cycle position 1 & data ready
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
@@ -100,20 +213,19 @@ void test_start_measurement_one_wait(void)
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock1);
 
-    TEST_ASSERT_EQUAL_INT(1, mlx90632_start_measurement());
+    TEST_ASSERT_EQUAL_INT32(1, mlx90632_start_measurement());
 }
 
 /** Test different failure paths in start_measurement */
-void test_start_measurement_busy_i2c(void)
+void test_start_measurement_errors(void)
 {
-    uint16_t reg_status_mock = 0x06;
+    uint16_t reg_status_mock = 0x0C86; // cycle position 1 & data not reaady
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_start_measurement());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_start_measurement());
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
@@ -122,7 +234,7 @@ void test_start_measurement_busy_i2c(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), -EPERM);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_start_measurement());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_start_measurement());
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
@@ -134,7 +246,7 @@ void test_start_measurement_busy_i2c(void)
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_start_measurement());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_start_measurement());
 }
 
 /** Test sensor timeouts while start_measure.
@@ -145,32 +257,135 @@ void test_start_measurement_busy_i2c(void)
  */
 void test_start_measurement_timeout(void)
 {
-    uint16_t reg_status_first_mock = 0x06;
-    uint16_t reg_status_mock[MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES];
+    uint16_t reg_status_mock = 0x0C06; // cycle position 1 & data not ready
     int i;
 
-    for (i = 0; i < MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES; ++i)
-        reg_status_mock[i] = 0x0006; // cycle position 1 & data NOT READY through whole array!
-
     // Start measurement expectations
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_first_mock, 0);
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_first_mock);
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_first_mock & (~MLX90632_STAT_DATA_RDY), 0);
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
 
     for (i = 0; i < MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES; ++i)
     {
-        mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock[i], 0);
+        mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
         mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-        mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock[i]);
+        mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
         usleep_Expect(10000, 11000);
     }
 
-    TEST_ASSERT_EQUAL_INT(-ETIMEDOUT, mlx90632_start_measurement());
+    TEST_ASSERT_EQUAL_INT32(-ETIMEDOUT, mlx90632_start_measurement());
+}
+
+
+/** Test read temperature from sensor without waiting procedure.
+ *
+ * Read ambient and object raw values considering channel 1 as new. Channel 2 values are read as old,
+ * while channel 1 values are read as new.
+ */
+void test_read_temp_raw_wo_wait_ch1_success(void)
+{
+    int16_t ambient_new_mock = 22454;
+    int16_t ambient_old_mock = 23030;
+    int16_t object_new_mock = 150;
+    int16_t object_old_mock = 150;
+
+    // Read Ambient raw expectations (based on cycle position 1)
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(1), (uint16_t*)&ambient_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(2), (uint16_t*)&ambient_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_old_mock);
+
+    // Read Object raw expectations (based on cycle position 1)
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(1), (uint16_t*)&object_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(1), (uint16_t*)&object_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(2), (uint16_t*)&object_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(2), (uint16_t*)&object_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
+
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_wo_wait(1, &ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+
+    // Confirm all values are as expected
+    TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
+    TEST_ASSERT_EQUAL_INT16(ambient_old_mock, ambient_old_raw);
+    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_raw);
+    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_raw);
 }
 
+/** Test read temperature from sensor without waiting procedure.
+ *
+ * Read ambient and object raw values considering channel 2 as new. Channel 1 values are read as old,
+ * while channel 2 values are read as new.
+ */
+void test_read_temp_raw_wo_wait_ch2_success(void)
+{
+    int16_t ambient_new_mock = 22454;
+    int16_t ambient_old_mock = 23030;
+    int16_t object_new_mock = 150;
+    int16_t object_old_mock = 150;
+
+    // Read Ambient raw expectations (based on cycle position 2)
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(1), (uint16_t*)&ambient_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(2), (uint16_t*)&ambient_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_old_mock);
+
+    // Read Object raw expectations (based on cycle position 2)
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(2), (uint16_t*)&object_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(2), (uint16_t*)&object_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(1), (uint16_t*)&object_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(1), (uint16_t*)&object_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
+
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_wo_wait(2, &ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+
+    // Confirm all values are as expected
+    TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
+    TEST_ASSERT_EQUAL_INT16(ambient_old_mock, ambient_old_raw);
+    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_raw);
+    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_raw);
+}
+
+/** Test failure path when reading temperature from sensor without waiting procedure.
+ */
+void test_read_temp_raw_wo_wait_error(void)
+{
+    int16_t ambient_new_mock = 22454;
+
+    // Read Ambient raw expectations (based on cycle position 1)
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(1), (uint16_t*)&ambient_new_mock, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_wo_wait(1, &ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+}
 
 /** Test whole start and read temperature from sensor procedure.
  *
@@ -180,7 +395,7 @@ void test_start_measurement_timeout(void)
  */
 void test_read_temp_raw_ch1_success(void)
 {
-    uint16_t reg_status_mock = 0x87; // cycle position 1 & data ready
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
     int16_t ambient_new_mock = 22454;
     int16_t ambient_old_mock = 23030;
     int16_t object_new_mock = 150;
@@ -224,13 +439,13 @@ void test_read_temp_raw_ch1_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
     TEST_ASSERT_EQUAL_INT16(ambient_old_mock, ambient_old_raw);
-    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_mock);
-    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_mock);
+    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_raw);
+    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_raw);
 }
 
 /** Test whole start and read temperature from sensor procedure.
@@ -241,7 +456,7 @@ void test_read_temp_raw_ch1_success(void)
  */
 void test_read_temp_raw_ch2_success(void)
 {
-    uint16_t reg_status_mock = 0x8b; // cycle position 2 & data ready
+    uint16_t reg_status_mock = 0x008B; // cycle position 2 & data ready
     int16_t ambient_new_mock = 22454;
     int16_t ambient_old_mock = 23030;
     int16_t object_new_mock = 150;
@@ -285,25 +500,26 @@ void test_read_temp_raw_ch2_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
     TEST_ASSERT_EQUAL_INT16(ambient_old_mock, ambient_old_raw);
-    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_mock);
-    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_mock);
+    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_raw);
+    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_raw);
 }
 
 /** Test Error output on whole start and read temperature from sensor procedure. */
 void test_read_temp_raw_errors(void)
 {
-    uint16_t reg_status_mock = 0x87; // cycle position 1 & data ready
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
     int16_t ambient_new_mock = 22454;
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
@@ -319,7 +535,8 @@ void test_read_temp_raw_errors(void)
     // Read Ambient raw expectations (based on above cycle position)
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(1), (uint16_t*)&ambient_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
 }
 
 /** Test reading channel 1 ambient values from sensor.
@@ -341,7 +558,7 @@ void test_read_ambient_values_ch1_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
@@ -367,7 +584,7 @@ void test_read_ambient_values_ch2_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
@@ -383,7 +600,8 @@ void test_read_ambient_values_errors(void)
     // First read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(1), (uint16_t*)&ambient_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
 
     // Second read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(1), (uint16_t*)&ambient_new_mock, 0);
@@ -392,7 +610,8 @@ void test_read_ambient_values_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(2), (uint16_t*)&ambient_old_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_ambient_raw(&ambient_new_raw, &ambient_old_raw));
 }
 
 /** Test reading channel 1 object values from sensor.
@@ -422,10 +641,10 @@ void test_read_object_values_ch1_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
 
-    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_mock);
-    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_mock);
+    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_raw);
+    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_raw);
 }
 
 /** Test reading channel 2 object values from sensor.
@@ -455,10 +674,10 @@ void test_read_object_values_ch2_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_object_raw(2, &object_new_raw, &object_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_object_raw(2, &object_new_raw, &object_old_raw));
 
-    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_mock);
-    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_mock);
+    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_raw);
+    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_raw);
 }
 
 /** Test error outputs when reading object values. */
@@ -470,7 +689,8 @@ void test_read_object_values_errors(void)
     // First read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(1), (uint16_t*)&object_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
 
     // Second read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(1), (uint16_t*)&object_new_mock, 0);
@@ -479,7 +699,8 @@ void test_read_object_values_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(1), (uint16_t*)&object_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
 
     // Third read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(1), (uint16_t*)&object_new_mock, 0);
@@ -492,7 +713,8 @@ void test_read_object_values_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(2), (uint16_t*)&object_old_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
 
     // Forth and last read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(1), (uint16_t*)&object_new_mock, 0);
@@ -509,14 +731,15 @@ void test_read_object_values_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(2), (uint16_t*)&object_old_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw(1, &object_new_raw, &object_old_raw));
 }
 
 /** Test error outputs when reading object values. */
 void test_read_object_error_ch(void)
 {
     // Input retval is invalid
-    TEST_ASSERT_EQUAL_INT(-EINVAL, mlx90632_read_temp_object_raw(3, &object_new_raw, &object_old_raw));
+    TEST_ASSERT_EQUAL_INT32(-EINVAL, mlx90632_read_temp_object_raw(3, &object_new_raw, &object_old_raw));
 }
 
 /** Test reading ambient values from sensor in extended range measurements.
@@ -537,7 +760,7 @@ void test_read_ambient_values_extended_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_ambient_raw_extended(&ambient_new_raw, &ambient_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_ambient_raw_extended(&ambient_new_raw, &ambient_old_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
@@ -553,7 +776,8 @@ void test_read_ambient_values_extended_errors(void)
     // First read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_ambient_raw_extended(&ambient_new_raw, &ambient_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_ambient_raw_extended(&ambient_new_raw, &ambient_old_raw));
 
     // Second read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, 0);
@@ -562,7 +786,8 @@ void test_read_ambient_values_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(18), (uint16_t*)&ambient_old_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_ambient_raw_extended(&ambient_new_raw, &ambient_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_ambient_raw_extended(&ambient_new_raw, &ambient_old_raw));
 }
 
 void test_read_object_values_extended_success(void)
@@ -600,7 +825,7 @@ void test_read_object_values_extended_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_object_raw_extended(&object_new_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(287, object_new_raw);
@@ -620,9 +845,8 @@ void test_read_object_values_extended_errors(void)
     // First read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_l1);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
 
     // Second read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, 0);
@@ -631,9 +855,8 @@ void test_read_object_values_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(17), (uint16_t*)&object_mock_b1, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_b1);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
 
     // Third read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, 0);
@@ -646,9 +869,8 @@ void test_read_object_values_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(18), (uint16_t*)&object_mock_b2, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_b2);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
 
     // 4th read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, 0);
@@ -665,9 +887,8 @@ void test_read_object_values_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(18), (uint16_t*)&object_mock_l2, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_l2);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
 
     // 5th read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, 0);
@@ -688,9 +909,8 @@ void test_read_object_values_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(19), (uint16_t*)&object_mock_v1, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v1);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
 
     // 6th read fails
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, 0);
@@ -715,9 +935,8 @@ void test_read_object_values_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(19), (uint16_t*)&object_mock_v2, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_object_raw_extended(&object_new_raw));
 
     // Data overflow
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, 0);
@@ -745,7 +964,92 @@ void test_read_object_values_extended_errors(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(-EINVAL, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EINVAL, mlx90632_read_temp_object_raw_extended(&object_new_raw));
+}
+
+/** Test read extended temperature from sensor without waiting procedure.
+ */
+void test_read_temp_raw_extended_wo_wait_success(void)
+{
+    int16_t ambient_new_mock = 22454;
+    int16_t ambient_old_mock = 23030;
+    int16_t object_mock_l1 = 250;
+    int16_t object_mock_l2 = 260;
+    int16_t object_mock_b1 = -25;
+    int16_t object_mock_b2 = -35;
+    int16_t object_mock_v1 = 4;
+    int16_t object_mock_v2 = -2;
+
+    // Read Ambient raw expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(18), (uint16_t*)&ambient_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_old_mock);
+
+    // Read Object raw expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_l1);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(17), (uint16_t*)&object_mock_b1, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_b1);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(18), (uint16_t*)&object_mock_b2, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_b2);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(18), (uint16_t*)&object_mock_l2, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_l2);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(19), (uint16_t*)&object_mock_v1, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v1);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_2(19), (uint16_t*)&object_mock_v2, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
+
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_extended_wo_wait(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+
+    // Confirm all values are as expected
+    TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
+    TEST_ASSERT_EQUAL_INT16(ambient_old_mock, ambient_old_raw);
+    TEST_ASSERT_EQUAL_INT16(287, object_new_raw);
+}
+
+/** Test different failure paths when reading extended temperature from sensor without waiting procedure.
+ */
+void test_read_temp_raw_extended_wo_wait_errors(void)
+{
+    int16_t ambient_new_mock = 22454;
+    int16_t ambient_old_mock = 23030;
+    int16_t object_mock_l1 = 250;
+
+    // Read Ambient raw expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_extended_wo_wait(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+
+    // Read Ambient raw expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_new_mock);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(18), (uint16_t*)&ambient_old_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_old_mock);
+
+    // Read Object raw expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_1(17), (uint16_t*)&object_mock_l1, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_extended_wo_wait(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 }
 
 /** Test whole start and read extended temperature from sensor procedure.
@@ -754,9 +1058,9 @@ void test_read_object_values_extended_errors(void)
  */
 void test_read_temp_raw_extended_success(void)
 {
-    uint16_t reg_status_mock1 = 0xC5; // cycle position 17 & data ready
-    uint16_t reg_status_mock2 = 0xC9; // cycle position 18 & data ready
-    uint16_t reg_status_mock3 = 0xCF; // cycle position 19 & data ready
+    uint16_t reg_status_mock1 = 0x00C5; // cycle position 17 & data ready
+    uint16_t reg_status_mock2 = 0x00C9; // cycle position 18 & data ready
+    uint16_t reg_status_mock3 = 0x00CF; // cycle position 19 & data ready
     int16_t ambient_new_mock = 22454;
     int16_t ambient_old_mock = 23030;
     int16_t object_mock_l1 = 250;
@@ -813,7 +1117,7 @@ void test_read_temp_raw_extended_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
 
     // Trigger the read_temp_raw_extended function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
@@ -877,7 +1181,7 @@ void test_read_temp_raw_extended_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
 
     // Trigger the read_temp_raw_extended function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
@@ -951,7 +1255,7 @@ void test_read_temp_raw_extended_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
 
     // Trigger the read_temp_raw_extended function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
@@ -965,20 +1269,18 @@ void test_read_temp_raw_extended_success(void)
  */
 void test_read_temp_raw_extended_errors(void)
 {
-    uint16_t reg_status_mock1 = 0xC5; // cycle position 17 & data ready
-    uint16_t reg_status_mock2 = 0xC9; // cycle position 18 & data ready
-    uint16_t reg_status_mock3 = 0xCF; // cycle position 19 & data ready
+    uint16_t reg_status_mock1 = 0x00C5; // cycle position 17 & data ready
+    uint16_t reg_status_mock2 = 0x00CF; // cycle position 19 & data ready
     int16_t ambient_new_mock = 22454;
 
     // 1st read returns an error
     //Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock1, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock1);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 
-    // 3 reads are not enough to reach the end of meas
+    // read_ambient_raw_extended returns an error
     //Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock2, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
@@ -990,64 +1292,149 @@ void test_read_temp_raw_extended_errors(void)
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock2);
 
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock2, 0);
+    // Read Ambient raw expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock2);
 
-    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock2 & (~MLX90632_STAT_DATA_RDY), 0);
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+}
 
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock2, 0);
+/** Test retries exhaust on whole start and read extended temperature from sensor procedure.
+ */
+void test_wait_for_measurement_retries_exhaust(void)
+{
+    uint16_t reg_status_mock = 0x0CC9; // cycle position 18 & data ready
+
+    // 3 reads are not enough to reach the end of meas
+    //Start measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock2);
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock2, 0);
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock2);
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock2 & (~MLX90632_STAT_DATA_RDY), 0);
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock2, 0);
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock2);
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock2, 0);
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock2);
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock2 & (~MLX90632_STAT_DATA_RDY), 0);
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
 
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock2, 0);
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock2);
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    TEST_ASSERT_EQUAL_INT(-ETIMEDOUT, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-ETIMEDOUT, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+}
 
-    // read_ambient_raw_extended returns an error
-    //Start measurement expectations
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock3, 0);
+/** Test trigger burst measurement.
+ */
+void test_trigger_measurement_burst_success(void)
+{
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+
+    // Trigger burst measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock3);
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
 
-    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock3 & (~MLX90632_STAT_DATA_RDY), 0);
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock | MLX90632_START_BURST_MEAS, 0);
 
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock3, 0);
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_trigger_measurement_burst());
+}
+
+/** Test different failure paths when triggering burst measurement.
+ */
+void test_trigger_measurement_burst_errors(void)
+{
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+
+    // Trigger burst measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock3);
 
-    // Read Ambient raw expectations
-    mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, -EPERM);
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement_burst());
+
+    // Trigger burst measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock | MLX90632_START_BURST_MEAS, -EPERM);
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement_burst());
+}
+
+/** Test wait for burst measurement with data ready.
+ */
+void test_wait_for_measurement_burst_success(void)
+{
+    uint16_t reg_status_mock = 0x000B; // cycle position 2 & data ready & device not busy
+
+    // Wait for burst measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_wait_for_measurement_burst());
+}
+
+/** Test failure path when waiting for burst measurement.
+ */
+void test_wait_for_measurement_burst_error(void)
+{
+    uint16_t reg_status_mock = 0x000B; // cycle position 2 & data ready & device not busy
+
+    // Wait for burst measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_new_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw_extended(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_wait_for_measurement_burst());
+}
+
+/** Test sensor timeouts while while waiting for burst measurement.
+ *
+ * If this happens in real life it means that after some tries sensor still did not indicate data ready, which probably
+ * points to much larger problem than a simple timeout. Timeout is only valid if usleep is a lot shorter than default
+ * values
+ */
+void test_wait_for_measurement_burst_timeout(void)
+{
+    uint16_t reg_status_mock = 0x0C06; // cycle position 1 & device busy
+    int i;
+
+    for (i = 0; i < MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES; ++i)
+    {
+        // Wait for measurement expectations
+        mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+        mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+        mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+        usleep_Expect(10000, 11000);
+    }
+
+    TEST_ASSERT_EQUAL_INT32(-ETIMEDOUT, mlx90632_wait_for_measurement_burst());
 }
 
 /** Test start sleeping step measurement.
  */
 void test_start_measurement_burst_success(void)
 {
-    uint16_t reg_ctrl_mock = 0x0002; // ctrl_reg value medical sleeping step meas selected
-    uint16_t reg_status_mock = 0x010B;  // cycle position 2 & data ready & device is not busy
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+    uint16_t reg_status_mock = 0x000B; // cycle position 2 & data ready & device not busy
     uint16_t meas1_mock = 0x820D;
     uint16_t meas2_mock = 0x821D;
 
@@ -1076,22 +1463,21 @@ void test_start_measurement_burst_success(void)
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_start_measurement_burst());
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_start_measurement_burst());
 }
 
 void test_start_measurement_burst_errors(void)
 {
-    uint16_t reg_ctrl_mock = 0x0002; // ctrl_reg value medical sleeping step meas selected
-    uint16_t reg_status_mock = 0x010B;  // cycle position 2 & data ready & device is not busy
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+    uint16_t reg_status_mock = 0x000B; // cycle position 2 & data ready & device not busy
     uint16_t meas1_mock = 0x820D;
     uint16_t meas2_mock = 0x821D;
 
     //mlx90632_reg_ctrl read error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_start_measurement_burst());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_start_measurement_burst());
 
     //mlx90632_reg_ctrl write error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
@@ -1100,7 +1486,7 @@ void test_start_measurement_burst_errors(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock | MLX90632_START_BURST_MEAS, -EPERM);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_start_measurement_burst());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_start_measurement_burst());
 
     //calculate measurement time error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
@@ -1111,9 +1497,8 @@ void test_start_measurement_burst_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_start_measurement_burst());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_start_measurement_burst());
 
     //mlx90632_reg_status read error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
@@ -1138,9 +1523,8 @@ void test_start_measurement_burst_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_start_measurement_burst());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_start_measurement_burst());
 }
 
 /** Test sensor timeouts while start_measure.
@@ -1151,15 +1535,12 @@ void test_start_measurement_burst_errors(void)
  */
 void test_start_measurement_burst_timeout(void)
 {
-    uint16_t reg_ctrl_mock = 0x0002;
-    uint16_t reg_status_mock[MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES];
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+    uint16_t reg_status_mock = 0x0C06; // cycle position 1 & device busy
     uint16_t meas1_mock = 0x820D;
     uint16_t meas2_mock = 0x821D;
     int i;
 
-    for (i = 0; i < MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES; ++i)
-        reg_status_mock[i] = 0x0C06; // cycle position 1 & device busy for all samples!
-
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
@@ -1183,14 +1564,87 @@ void test_start_measurement_burst_timeout(void)
 
     for (i = 0; i < MLX90632_MAX_NUMBER_MESUREMENT_READ_TRIES; ++i)
     {
-        mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock[i], 0);
+        mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
         mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-        mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock[i]);
+        mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
 
         usleep_Expect(10000, 11000);
     }
 
-    TEST_ASSERT_EQUAL_INT(-ETIMEDOUT, mlx90632_start_measurement_burst());
+    TEST_ASSERT_EQUAL_INT32(-ETIMEDOUT, mlx90632_start_measurement_burst());
+}
+
+/** Test trigger single measurement.
+ */
+void test_trigger_measurement_single_success(void)
+{
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+
+    // Trigger single measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock | MLX90632_START_SINGLE_MEAS, 0);
+
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_trigger_measurement_single());
+}
+
+/** Test failure path when triggering single measurement.
+ */
+void test_trigger_measurement_single_errors(void)
+{
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+
+    // Trigger single measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement_single());
+
+    // Trigger single measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), -EPERM);
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement_single());
+
+    // Trigger single measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement_single());
+
+    // Trigger single measurement expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock & (~MLX90632_STAT_DATA_RDY), 0);
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
+
+    mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock | MLX90632_START_SINGLE_MEAS, -EPERM);
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_trigger_measurement_single());
 }
 
 /** Test whole start and read extended temperature in sleeping step mode from sensor procedure.
@@ -1199,8 +1653,8 @@ void test_start_measurement_burst_timeout(void)
  */
 void test_read_temp_raw_extended_burst_success(void)
 {
-    uint16_t reg_ctrl_mock = 0x0112; // ctrl_reg value extended sleeping step meas selected
-    uint16_t reg_status_mock = 0x01CF;  // cycle position 19 & data ready & device is not busy
+    uint16_t reg_ctrl_mock = 0x0112; // extended sleeping step meas selected
+    uint16_t reg_status_mock = 0x01CF; // cycle position 19 & data ready & device not busy
     int16_t ambient_new_mock = 22454;
     int16_t ambient_old_mock = 23030;
     int16_t object_mock_l1 = 250;
@@ -1277,7 +1731,7 @@ void test_read_temp_raw_extended_burst_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_mock_v2);
 
     // Trigger the read_temp_raw_extended function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_raw_extended_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_extended_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
@@ -1292,8 +1746,8 @@ void test_read_temp_raw_extended_burst_success(void)
  */
 void test_read_temp_raw_extended_burst_errors(void)
 {
-    uint16_t reg_ctrl_mock = 0x0112; // ctrl_reg value extended sleeping step meas selected
-    uint16_t reg_status_mock = 0x01CF;  // cycle position 19 & data ready & device is not busy
+    uint16_t reg_ctrl_mock = 0x0112; // extended sleeping step meas selected
+    uint16_t reg_status_mock = 0x01CF; // cycle position 19 & data ready & device not busy
     int16_t ambient_new_mock = 22454;
     uint16_t meas1_mock = 0x8300;
     uint16_t meas2_mock = 0x8312;
@@ -1303,9 +1757,8 @@ void test_read_temp_raw_extended_burst_errors(void)
     //Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw_extended_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_extended_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 
     // read_ambient_raw_extended returns an error
     //Start measurement expectations
@@ -1340,15 +1793,14 @@ void test_read_temp_raw_extended_burst_errors(void)
     // Read Ambient raw expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(17), (uint16_t*)&ambient_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&ambient_new_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw_extended_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_extended_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw));
 }
 
 void test_read_temp_raw_burst_success(void)
 {
-    uint16_t reg_ctrl_mock = 0x0002; // ctrl_reg value medical sleeping step meas selected
-    uint16_t reg_status_mock = 0x010B;  // cycle position 2 & data ready & device is not busy
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+    uint16_t reg_status_mock = 0x010B; // cycle position 2 & data ready & device not busy
     int16_t ambient_new_mock = 22454;
     int16_t ambient_old_mock = 23030;
     int16_t object_new_mock = 150;
@@ -1408,19 +1860,19 @@ void test_read_temp_raw_burst_success(void)
     mlx90632_i2c_read_ReturnThruPtr_value((uint16_t*)&object_old_mock);
 
     // Trigger the read_temp_raw function
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_read_temp_raw_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_read_temp_raw_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
 
     // Confirm all values are as expected
     TEST_ASSERT_EQUAL_INT16(ambient_new_mock, ambient_new_raw);
     TEST_ASSERT_EQUAL_INT16(ambient_old_mock, ambient_old_raw);
-    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_mock);
-    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_mock);
+    TEST_ASSERT_EQUAL_INT16(object_new_mock, object_new_raw);
+    TEST_ASSERT_EQUAL_INT16(object_old_mock, object_old_raw);
 }
 
 void test_read_temp_raw_burst_errors(void)
 {
-    uint16_t reg_ctrl_mock = 0x0002;    // ctrl_reg value medical sleeping step meas selected
-    uint16_t reg_status_mock = 0x010B;  // cycle position 2 & data ready & device is not busy
+    uint16_t reg_ctrl_mock = 0x0002; // medical sleeping step meas selected
+    uint16_t reg_status_mock = 0x010B; // cycle position 2 & data ready & device not busy
     int16_t ambient_new_mock = 22454;
     uint16_t meas1_mock = 0x820D;
     uint16_t meas2_mock = 0x821D;
@@ -1429,9 +1881,8 @@ void test_read_temp_raw_burst_errors(void)
     //Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
 
     // Start measurement expectations
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock, 0);
@@ -1461,12 +1912,13 @@ void test_read_temp_raw_burst_errors(void)
     // Read Ambient raw expectations (based on above cycle position)
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_RAM_3(1), (uint16_t*)&ambient_new_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_read_temp_raw_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_read_temp_raw_burst(&ambient_new_raw, &ambient_old_raw, &object_new_raw, &object_old_raw));
 }
 
 void test_calculate_dataset_ready_time_medical_success(void)
 {
-    uint16_t reg_ctrl_medb_mock = 0x0002; // ctrl_reg value medical sleeping step meas selected
+    uint16_t reg_ctrl_medb_mock = 0x0002; // medical sleeping step meas selected
     uint16_t med_meas1_mock[] = { 0x800D, 0x810D, 0x820D, 0x830D, 0x840D, 0x850D, 0x860D, 0x870D };
     uint16_t med_meas2_mock[] = { 0x801D, 0x811D, 0x821D, 0x831D, 0x841D, 0x851D, 0x861D, 0x871D };
     int med_waiting_time[] = { 4000, 2000, 1000, 500, 250, 124, 62, 30 };
@@ -1486,13 +1938,13 @@ void test_calculate_dataset_ready_time_medical_success(void)
         mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
         mlx90632_i2c_read_ReturnThruPtr_value(&med_meas2_mock[i]);
 
-        TEST_ASSERT_EQUAL_INT(med_waiting_time[i], mlx90632_calculate_dataset_ready_time());
+        TEST_ASSERT_EQUAL_INT32(med_waiting_time[i], mlx90632_calculate_dataset_ready_time());
     }
 }
 
 void test_calculate_dataset_ready_time_extended_success(void)
 {
-    uint16_t reg_ctrl_extb_mock = 0x0112; // ctrl_reg value extended sleeping step meas selected
+    uint16_t reg_ctrl_extb_mock = 0x0112; // extended sleeping step meas selected
     uint16_t ext_meas1_mock[] = { 0x8000, 0x8100, 0x8200, 0x8300, 0x8400, 0x8500, 0x8600, 0x8700 };
     uint16_t ext_meas2_mock[] = { 0x8012, 0x8112, 0x8212, 0x8312, 0x8412, 0x8512, 0x8612, 0x8712 };
     uint16_t ext_meas3_mock[] = { 0x800C, 0x810C, 0x820C, 0x830C, 0x840C, 0x850C, 0x860C, 0x870C };
@@ -1517,30 +1969,29 @@ void test_calculate_dataset_ready_time_extended_success(void)
         mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
         mlx90632_i2c_read_ReturnThruPtr_value(&ext_meas3_mock[i]);
 
-        TEST_ASSERT_EQUAL_INT(ext_waiting_time[i], mlx90632_calculate_dataset_ready_time());
+        TEST_ASSERT_EQUAL_INT32(ext_waiting_time[i], mlx90632_calculate_dataset_ready_time());
     }
 }
 
 void test_calculate_dataset_ready_time_medical_errors(void)
 {
-    uint16_t reg_ctrl_medb_mock = 0x0002; // ctrl_reg value medical sleeping step meas selected
-    uint16_t reg_ctrl_med_mock = 0x0006; // ctrl_reg value medical continuous meas selected
+    uint16_t reg_ctrl_medb_mock = 0x0002; // medical sleeping step meas selected
+    uint16_t reg_ctrl_med_mock = 0x0006; // medical continuous meas selected
     uint16_t med_meas1_mock = 0x820D;
     uint16_t med_meas2_mock = 0x821D;
 
     //get_meas_type error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_medb_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_medb_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_calculate_dataset_ready_time());
 
     //invalid meas type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_med_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_med_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EINVAL, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EINVAL, mlx90632_calculate_dataset_ready_time());
 
     //medical meas 1 error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_medb_mock, 0);
@@ -1549,9 +2000,8 @@ void test_calculate_dataset_ready_time_medical_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_MEDICAL_MEAS1, &med_meas1_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&med_meas1_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_calculate_dataset_ready_time());
 
     //medical meas 2 error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_medb_mock, 0);
@@ -1564,15 +2014,14 @@ void test_calculate_dataset_ready_time_medical_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_MEDICAL_MEAS2, &med_meas2_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&med_meas2_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_calculate_dataset_ready_time());
 }
 
 void test_calculate_dataset_ready_time_extended_errors(void)
 {
-    uint16_t reg_ctrl_extb_mock = 0x0112; // ctrl_reg value extended sleeping step meas selected
-    uint16_t reg_ctrl_ext_mock = 0x0116; // ctrl_reg value extended continuous meas selected
+    uint16_t reg_ctrl_extb_mock = 0x0112; // extended sleeping step meas selected
+    uint16_t reg_ctrl_ext_mock = 0x0116; // extended continuous meas selected
     uint16_t ext_meas1_mock = 0x8200;
     uint16_t ext_meas2_mock = 0x8212;
     uint16_t ext_meas3_mock = 0x820C;
@@ -1580,16 +2029,15 @@ void test_calculate_dataset_ready_time_extended_errors(void)
     //get_meas_type error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_extb_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_extb_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_calculate_dataset_ready_time());
 
     //invalid meas type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_ext_mock, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_ext_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EINVAL, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EINVAL, mlx90632_calculate_dataset_ready_time());
 
     //extended meas 1 error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_extb_mock, 0);
@@ -1598,9 +2046,8 @@ void test_calculate_dataset_ready_time_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_EXTENDED_MEAS1, &ext_meas1_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&ext_meas1_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_calculate_dataset_ready_time());
 
     //extended meas 2 error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_extb_mock, 0);
@@ -1613,9 +2060,8 @@ void test_calculate_dataset_ready_time_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_EXTENDED_MEAS2, &ext_meas2_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&ext_meas2_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_calculate_dataset_ready_time());
 
     //extended meas 3 error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_extb_mock, 0);
@@ -1632,9 +2078,64 @@ void test_calculate_dataset_ready_time_extended_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_EXTENDED_MEAS3, &ext_meas3_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&ext_meas3_mock);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_calculate_dataset_ready_time());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_calculate_dataset_ready_time());
+}
+
+/** Test get channel position.
+ */
+void test_get_channel_position_success(void)
+{
+    uint16_t reg_status_mock_med1 = 0x0087; // cycle position 1 & data ready
+    uint16_t reg_status_mock_med2 = 0x008B; // cycle position 2 & data ready
+    uint16_t reg_status_mock_ext1 = 0x00C5; // cycle position 17 & data ready
+    uint16_t reg_status_mock_ext2 = 0x00C9; // cycle position 18 & data ready
+    uint16_t reg_status_mock_ext3 = 0x00CF; // cycle position 19 & data ready
+
+    // Read medical measurement type
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock_med1, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock_med1);
+
+    TEST_ASSERT_EQUAL_INT32(1, mlx90632_get_channel_position());
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock_med2, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock_med2);
+
+    TEST_ASSERT_EQUAL_INT32(2, mlx90632_get_channel_position());
+
+    // Read extended measurement type
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock_ext1, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock_ext1);
+
+    TEST_ASSERT_EQUAL_INT32(17, mlx90632_get_channel_position());
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock_ext2, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock_ext2);
+
+    TEST_ASSERT_EQUAL_INT32(18, mlx90632_get_channel_position());
+
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock_ext3, 0);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+    mlx90632_i2c_read_ReturnThruPtr_value(&reg_status_mock_ext3);
+
+    TEST_ASSERT_EQUAL_INT32(19, mlx90632_get_channel_position());
+}
+
+/** Test failure path when getting channel position.
+ */
+void test_get_channel_position_error(void)
+{
+    uint16_t reg_status_mock = 0x0087; // cycle position 1 & data ready
+
+    // Get channel position expectations
+    mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, &reg_status_mock, -EPERM);
+    mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
+
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_get_channel_position());
 }
 
 void test_set_meas_type_success(void)
@@ -1672,7 +2173,7 @@ void test_set_meas_type_success(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock_ext, 0);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 
     // Switch from extended to medical measurement type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_ext, 0);
@@ -1698,7 +2199,7 @@ void test_set_meas_type_success(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock_med, 0);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_set_meas_type(MLX90632_MTYP_MEDICAL));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_set_meas_type(MLX90632_MTYP_MEDICAL));
 
     // Switch from medical to sleeping step medical measurement type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, 0);
@@ -1724,7 +2225,7 @@ void test_set_meas_type_success(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock_med2, 0);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_set_meas_type(MLX90632_MTYP_MEDICAL_BURST));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_set_meas_type(MLX90632_MTYP_MEDICAL_BURST));
 
     // Switch from medical sleeping step to extended sleeping step measurement type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med2, 0);
@@ -1750,7 +2251,7 @@ void test_set_meas_type_success(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock_ext2, 0);
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED_BURST));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED_BURST));
 }
 
 void test_set_meas_type_errors(void)
@@ -1760,14 +2261,13 @@ void test_set_meas_type_errors(void)
     uint16_t reg_ctrl_mock_step = 0xFE0D;
 
     // Invalid input parameter
-    TEST_ASSERT_EQUAL_INT(-EINVAL, mlx90632_set_meas_type(9));
+    TEST_ASSERT_EQUAL_INT32(-EINVAL, mlx90632_set_meas_type(9));
 
     // Addressed reset read current reg value error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_med);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 
     // Addressed reset write step value error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, 0);
@@ -1776,7 +2276,7 @@ void test_set_meas_type_errors(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock_step, -EPERM);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 
     // Addressed reset command error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, 0);
@@ -1787,7 +2287,7 @@ void test_set_meas_type_errors(void)
 
     mlx90632_i2c_write_ExpectAndReturn(0x3005, MLX90632_RESET_CMD, -EPERM);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 
     // Addressed reset restore value error
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, 0);
@@ -1801,7 +2301,7 @@ void test_set_meas_type_errors(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock_med, -EPERM);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 
     // First read fail
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, 0);
@@ -1817,9 +2317,8 @@ void test_set_meas_type_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_med);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 
     // First write fail
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, 0);
@@ -1839,7 +2338,7 @@ void test_set_meas_type_errors(void)
 
     mlx90632_i2c_write_ExpectAndReturn(MLX90632_REG_CTRL, reg_ctrl_mock_ext1, -EPERM);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 
     // Second read fail
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med, 0);
@@ -1861,9 +2360,8 @@ void test_set_meas_type_errors(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_ext1, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_ext1);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_meas_type(MLX90632_MTYP_EXTENDED));
 }
 
 void test_get_meas_type_success(void)
@@ -1878,28 +2376,28 @@ void test_get_meas_type_success(void)
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_med);
 
-    TEST_ASSERT_EQUAL_INT(MLX90632_MTYP_MEDICAL, mlx90632_get_meas_type());
+    TEST_ASSERT_EQUAL_INT32(MLX90632_MTYP_MEDICAL, mlx90632_get_meas_type());
 
     // Read extended measurement type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_ext, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_ext);
 
-    TEST_ASSERT_EQUAL_INT(MLX90632_MTYP_EXTENDED, mlx90632_get_meas_type());
+    TEST_ASSERT_EQUAL_INT32(MLX90632_MTYP_EXTENDED, mlx90632_get_meas_type());
 
     // Read medical sleeping step measurement type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_med_burst, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_med_burst);
 
-    TEST_ASSERT_EQUAL_INT(MLX90632_MTYP_MEDICAL_BURST, mlx90632_get_meas_type());
+    TEST_ASSERT_EQUAL_INT32(MLX90632_MTYP_MEDICAL_BURST, mlx90632_get_meas_type());
 
     // Read extended measurement type
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_ext_burst, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_ext_burst);
 
-    TEST_ASSERT_EQUAL_INT(MLX90632_MTYP_EXTENDED_BURST, mlx90632_get_meas_type());
+    TEST_ASSERT_EQUAL_INT32(MLX90632_MTYP_EXTENDED_BURST, mlx90632_get_meas_type());
 }
 
 void test_get_meas_type_errors(void)
@@ -1910,23 +2408,22 @@ void test_get_meas_type_errors(void)
     // Error reading the register
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_inval, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_inval);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_get_meas_type());
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_get_meas_type());
 
     // Invalid measurement type data (measurement type)
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_inval, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_inval);
 
-    TEST_ASSERT_EQUAL_INT(-EINVAL, mlx90632_get_meas_type());
+    TEST_ASSERT_EQUAL_INT32(-EINVAL, mlx90632_get_meas_type());
 
     // Invalid measurement type data (operating mode)
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_CTRL, &reg_ctrl_mock_inval1, 0);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_ctrl_mock_inval1);
 
-    TEST_ASSERT_EQUAL_INT(-EINVAL, mlx90632_get_meas_type());
+    TEST_ASSERT_EQUAL_INT32(-EINVAL, mlx90632_get_meas_type());
 }
 
 ///@}
diff --git a/test/TestRefreshRate.c b/test/TestRefreshRate.c
index 52466ac..c1b6f6c 100644
--- a/test/TestRefreshRate.c
+++ b/test/TestRefreshRate.c
@@ -118,7 +118,7 @@ void assert_set_refresh_rate(mlx90632_meas_t meas, uint16_t reg_meas1, uint16_t
     expect_write_meas2_success(reg_meas2);
     expect_read_status_success_eeprom_not_busy();
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_set_refresh_rate(meas));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_set_refresh_rate(meas));
 }
 
 void test_set_refresh_rate_success(void)
@@ -141,7 +141,6 @@ void expect_read_meas1_error()
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_MEDICAL_MEAS1, &reg_meas1_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_meas1_mock);
 }
 
 void expect_unlockEEPROM_error()
@@ -158,14 +157,13 @@ void expect_read_status_error(uint16_t* reg_status_mock)
 {
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_REG_STATUS, reg_status_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(reg_status_mock);
 }
 
 void test_set_refresh_rate_error_first_read_fails(void)
 {
     expect_read_meas1_error();
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_first_unlock_erase_fails(void)
@@ -173,7 +171,7 @@ void test_set_refresh_rate_error_first_unlock_erase_fails(void)
     expect_read_meas1_success();
     expect_unlockEEPROM_error();
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_first_erase_fails(void)
@@ -182,7 +180,7 @@ void test_set_refresh_rate_error_first_erase_fails(void)
     expect_unlock_eeprom_success();
     expect_write_meas1_error(0x00);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_first_read_erase_status_fails(void)
@@ -194,7 +192,7 @@ void test_set_refresh_rate_error_first_read_erase_status_fails(void)
     expect_write_meas1_success(0x00);
     expect_read_status_error(&error);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_first_unlock_write_fails(void)
@@ -205,7 +203,7 @@ void test_set_refresh_rate_error_first_unlock_write_fails(void)
     expect_read_status_success_eeprom_not_busy();
     expect_unlockEEPROM_error();
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_first_write_fails(void)
@@ -217,7 +215,7 @@ void test_set_refresh_rate_error_first_write_fails(void)
     expect_unlock_eeprom_success();
     expect_write_meas1_error(0x870D);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_first_read_write_status_fails(void)
@@ -232,7 +230,7 @@ void test_set_refresh_rate_error_first_read_write_status_fails(void)
     expect_write_meas1_success(0x870D);
     expect_read_status_error(&error);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void expect_read_meas2_error()
@@ -241,7 +239,6 @@ void expect_read_meas2_error()
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_MEDICAL_MEAS2, &reg_meas2_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_meas2_mock);
 }
 
 void expect_write_meas2_error(uint16_t data)
@@ -261,7 +258,7 @@ void test_set_refresh_rate_error_second_read_fails(void)
 
     expect_read_meas2_error();
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_second_unlock_erase_fails(void)
@@ -277,7 +274,7 @@ void test_set_refresh_rate_error_second_unlock_erase_fails(void)
     expect_read_meas2_success();
     expect_unlockEEPROM_error();
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_second_erase_fails(void)
@@ -294,7 +291,7 @@ void test_set_refresh_rate_error_second_erase_fails(void)
     expect_unlock_eeprom_success();
     expect_write_meas2_error(0x00);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_second_read_erase_status_fails(void)
@@ -314,7 +311,7 @@ void test_set_refresh_rate_error_second_read_erase_status_fails(void)
     expect_write_meas2_success(0x00);
     expect_read_status_error(&error);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_second_unlock_write_fails(void)
@@ -333,7 +330,7 @@ void test_set_refresh_rate_error_second_unlock_write_fails(void)
     expect_read_status_success_eeprom_not_busy();
     expect_unlockEEPROM_error();
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_second_write_fails(void)
@@ -353,7 +350,7 @@ void test_set_refresh_rate_error_second_write_fails(void)
     expect_unlock_eeprom_success();
     expect_write_meas2_error(0x871D);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_error_second_read_write_status_fails(void)
@@ -376,7 +373,7 @@ void test_set_refresh_rate_error_second_read_write_status_fails(void)
     expect_write_meas2_success(0x871D);
     expect_read_status_error(&error);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void assert_get_refresh_rate(mlx90632_meas_t meas, uint16_t reg_meas1_mock)
@@ -385,7 +382,7 @@ void assert_get_refresh_rate(mlx90632_meas_t meas, uint16_t reg_meas1_mock)
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
     mlx90632_i2c_read_ReturnThruPtr_value(&reg_meas1_mock);
 
-    TEST_ASSERT_EQUAL_INT(meas, mlx90632_get_refresh_rate());
+    TEST_ASSERT_EQUAL_INT32(meas, mlx90632_get_refresh_rate());
 }
 
 void test_get_refresh_rate(void)
@@ -406,9 +403,8 @@ void test_get_refresh_rate_error(void)
 
     mlx90632_i2c_read_ExpectAndReturn(MLX90632_EE_MEDICAL_MEAS1, &reg_meas1_mock, -EPERM);
     mlx90632_i2c_read_IgnoreArg_value(); // Ignore input of mock since we use it as output
-    mlx90632_i2c_read_ReturnThruPtr_value(&reg_meas1_mock);
 
-    TEST_ASSERT_EQUAL_INT(MLX90632_MEAS_HZ_ERROR, mlx90632_get_refresh_rate());
+    TEST_ASSERT_EQUAL_INT32(MLX90632_MEAS_HZ_ERROR, mlx90632_get_refresh_rate());
 }
 
 void test_set_refresh_rate_success_status_busy_error(void)
@@ -421,7 +417,7 @@ void test_set_refresh_rate_success_status_busy_error(void)
     expect_read_status_success_eepromBusy();
     expect_read_status_error(&error);
 
-    TEST_ASSERT_EQUAL_INT(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
+    TEST_ASSERT_EQUAL_INT32(-EPERM, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_64));
 }
 
 void test_set_refresh_rate_same_as_old_meas1_doesnt_write_meas1(void)
@@ -437,7 +433,7 @@ void test_set_refresh_rate_same_as_old_meas1_doesnt_write_meas1(void)
     expect_write_meas2_success(0x801D);
     expect_read_status_success_eeprom_not_busy();
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_HALF));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_HALF));
 }
 
 void test_set_refresh_rate_same_as_old_meas2_doesnt_write_meas1(void)
@@ -453,7 +449,7 @@ void test_set_refresh_rate_same_as_old_meas2_doesnt_write_meas1(void)
     reg_meas2_mock = 0x801D;
     expect_read_meas2_success();
 
-    TEST_ASSERT_EQUAL_INT(0, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_HALF));
+    TEST_ASSERT_EQUAL_INT32(0, mlx90632_set_refresh_rate(MLX90632_MEAS_HZ_HALF));
 }
 
 ///@}
\ No newline at end of file
diff --git a/tools/uncrustify.cfg b/tools/uncrustify.cfg
index 95de9cc..9f48e1a 100644
--- a/tools/uncrustify.cfg
+++ b/tools/uncrustify.cfg
@@ -20,7 +20,7 @@ nl_for_brace			= add		# "for () {" vs "for () \n {"
 nl_else_brace			= add		# "else {" vs "else \n {"
 nl_while_brace			= add		# "while () {" vs "while () \n {"
 nl_switch_brace			= add		# "switch () {" vs "switch () \n {"
-nl_func_var_def_blk		= 1
+nl_var_def_blk_end_func_top	= 1
 nl_before_case			= 1
 nl_fcall_brace			= add		# "foo() {" vs "foo()\n{"
 nl_fdef_brace			= add		# "int foo() {" vs "int foo()\n{"