Implement SCD41 logic in a separate file

include/sensor.h

@@ -0,0 +1,14 @@
+#ifndef SENSOR_H_
+#define SENSOR_H_
+
+#include <SensirionI2cScd4x.h>
+
+void handle_error(int code, char *msg);
+
+void initialize_sensor(SensirionI2cScd4x &sensor, int error_code,
+                       char *error_msg);
+
+void read_values(SensirionI2cScd4x &sensor, float *data, int error_code,
+                 char *error_msg);
+
+#endif // SENSOR_H_

src/sensor.cpp

@@ -0,0 +1,49 @@
+#include "sensor.h"
+#include <Wire.h>
+
+// TODO Trigger a reset when an error occurs
+void handle_error(int code, char *msg) {
+    if (code) {
+        errorToString(code, msg, sizeof msg);
+        exit(1);
+    }
+}
+
+void initialize_sensor(SensirionI2cScd4x &sensor, int error_code,
+                       char *error_msg) {
+    Wire.begin();
+    sensor.begin(Wire, SCD41_I2C_ADDR_62);
+    Serial.println("Starting sensor initialization");
+
+    error_code = sensor.wakeUp();
+    handle_error(error_code, error_msg);
+    error_code = sensor.stopPeriodicMeasurement();
+    handle_error(error_code, error_msg);
+    error_code = sensor.reinit();
+    handle_error(error_code, error_msg);
+    error_code = sensor.startPeriodicMeasurement();
+    handle_error(error_code, error_msg);
+
+    Serial.println("The sensor was initialized properly");
+};
+
+void read_values(SensirionI2cScd4x &sensor, float *data, int error_code,
+                 char *error_msg) {
+    bool data_available = false;
+    short unsigned int co2_concentration = 0.0;
+
+    error_code = sensor.getDataReadyStatus(data_available);
+    handle_error(error_code, error_msg);
+
+    while (!data_available) {
+        delay(100);
+        error_code = sensor.getDataReadyStatus(data_available);
+        handle_error(error_code, error_msg);
+    }
+
+    Serial.println("Data fetched from the sensor");
+
+    error_code = sensor.readMeasurement(co2_concentration, data[1], data[2]);
+    data[0] = co2_concentration;
+    handle_error(error_code, error_msg);
+}