From 9431b65a1756d34b75e6934abc0bc4b1487a04fe Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sat, 1 Jul 2023 17:14:14 +0200
Subject: [PATCH] [ADS131] Implemented new 8 channels driver and removed unused
 driver

---
 cmake/boardcore.cmake                         |   1 +
 src/shared/sensors/ADS131M04/ADS131M04.cpp    |  70 ++-
 .../sensors/ADS131M04/ADS131M04HighFreq.cpp   | 166 -------
 .../sensors/ADS131M04/ADS131M04HighFreq.h     |  96 ----
 src/shared/sensors/ADS131M08/ADS131M08.cpp    | 422 ++++++++++++++++++
 src/shared/sensors/ADS131M08/ADS131M08.h      | 335 ++++++++++++++
 .../ADS131M08Data.h}                          |  40 +-
 src/tests/sensors/test-ads131m08.cpp          |  80 ++++
 8 files changed, 908 insertions(+), 302 deletions(-)
 delete mode 100644 src/shared/sensors/ADS131M04/ADS131M04HighFreq.cpp
 delete mode 100644 src/shared/sensors/ADS131M04/ADS131M04HighFreq.h
 create mode 100644 src/shared/sensors/ADS131M08/ADS131M08.cpp
 create mode 100644 src/shared/sensors/ADS131M08/ADS131M08.h
 rename src/shared/sensors/{ADS131M04/ADS131M04HighFreqData.h => ADS131M08/ADS131M08Data.h} (50%)
 create mode 100644 src/tests/sensors/test-ads131m08.cpp

diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 6fd4ba2d0..3e29187ee 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -84,6 +84,7 @@ foreach(OPT_BOARD ${BOARDS})
         # Sensors
         ${SBS_BASE}/src/shared/sensors/ADS1118/ADS1118.cpp
         ${SBS_BASE}/src/shared/sensors/ADS131M04/ADS131M04.cpp
+        ${SBS_BASE}/src/shared/sensors/ADS131M08/ADS131M08.cpp
         ${SBS_BASE}/src/shared/sensors/BME280/BME280.cpp
         ${SBS_BASE}/src/shared/sensors/BME280/BME280I2C.cpp
         ${SBS_BASE}/src/shared/sensors/BMP280/BMP280.cpp
diff --git a/src/shared/sensors/ADS131M04/ADS131M04.cpp b/src/shared/sensors/ADS131M04/ADS131M04.cpp
index 8c7a46e2d..3b7387c41 100644
--- a/src/shared/sensors/ADS131M04/ADS131M04.cpp
+++ b/src/shared/sensors/ADS131M04/ADS131M04.cpp
@@ -142,25 +142,24 @@ void ADS131M04::setChannelOffset(Channel channel, uint32_t offset)
 {
     // Set the correct registers based on the selected channel
     Registers regMSB, regLSB;
-    if (static_cast<int>(channel) == 0)
+    switch (static_cast<int>(channel))
     {
-        regMSB = Registers::REG_CH0_OCAL_MSB;
-        regLSB = Registers::REG_CH0_OCAL_LSB;
-    }
-    else if (static_cast<int>(channel) == 1)
-    {
-        regMSB = Registers::REG_CH0_OCAL_MSB;
-        regLSB = Registers::REG_CH0_OCAL_LSB;
-    }
-    else if (static_cast<int>(channel) == 1)
-    {
-        regMSB = Registers::REG_CH0_OCAL_MSB;
-        regLSB = Registers::REG_CH0_OCAL_LSB;
-    }
-    else
-    {
-        regMSB = Registers::REG_CH0_OCAL_MSB;
-        regLSB = Registers::REG_CH0_OCAL_LSB;
+        case 0:
+            regMSB = Registers::REG_CH0_OCAL_MSB;
+            regLSB = Registers::REG_CH0_OCAL_LSB;
+            break;
+        case 1:
+            regMSB = Registers::REG_CH1_OCAL_MSB;
+            regLSB = Registers::REG_CH1_OCAL_LSB;
+            break;
+        case 2:
+            regMSB = Registers::REG_CH2_OCAL_MSB;
+            regLSB = Registers::REG_CH2_OCAL_LSB;
+            break;
+        default:
+            regMSB = Registers::REG_CH3_OCAL_MSB;
+            regLSB = Registers::REG_CH3_OCAL_LSB;
+            break;
     }
 
     writeRegister(regMSB, static_cast<uint16_t>(offset) >> 16);
@@ -175,25 +174,24 @@ void ADS131M04::setChannelGainCalibration(Channel channel, double gain)
 
     // Set the correct registers based on the selected channel
     Registers regMSB, regLSB;
-    if (static_cast<int>(channel) == 0)
-    {
-        regMSB = Registers::REG_CH0_GCAL_MSB;
-        regLSB = Registers::REG_CH0_GCAL_LSB;
-    }
-    else if (static_cast<int>(channel) == 1)
-    {
-        regMSB = Registers::REG_CH0_GCAL_MSB;
-        regLSB = Registers::REG_CH0_GCAL_LSB;
-    }
-    else if (static_cast<int>(channel) == 1)
-    {
-        regMSB = Registers::REG_CH0_GCAL_MSB;
-        regLSB = Registers::REG_CH0_GCAL_LSB;
-    }
-    else
+    switch (static_cast<int>(channel))
     {
-        regMSB = Registers::REG_CH0_GCAL_MSB;
-        regLSB = Registers::REG_CH0_GCAL_LSB;
+        case 0:
+            regMSB = Registers::REG_CH0_GCAL_MSB;
+            regLSB = Registers::REG_CH0_GCAL_LSB;
+            break;
+        case 1:
+            regMSB = Registers::REG_CH1_GCAL_MSB;
+            regLSB = Registers::REG_CH1_GCAL_LSB;
+            break;
+        case 2:
+            regMSB = Registers::REG_CH2_GCAL_MSB;
+            regLSB = Registers::REG_CH2_GCAL_LSB;
+            break;
+        default:
+            regMSB = Registers::REG_CH3_GCAL_MSB;
+            regLSB = Registers::REG_CH3_GCAL_LSB;
+            break;
     }
 
     // Compute the correct gain register parameter
diff --git a/src/shared/sensors/ADS131M04/ADS131M04HighFreq.cpp b/src/shared/sensors/ADS131M04/ADS131M04HighFreq.cpp
deleted file mode 100644
index 7c56c6947..000000000
--- a/src/shared/sensors/ADS131M04/ADS131M04HighFreq.cpp
+++ /dev/null
@@ -1,166 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "ADS131M04HighFreq.h"
-
-#include <fmt/format.h>
-#include <kernel/scheduler/scheduler.h>
-
-#include <thread>
-
-namespace Boardcore
-{
-
-ADS131M04HighFreq::ADS131M04HighFreq(SPISlave spiSlave, SPIType *spi,
-                                     DMAStream rxStream,
-                                     DMAStream::Channel dmaChannel,
-                                     SPISignalGenerator spiSignalGenerator,
-                                     int bufSize, const std::string logFileName)
-    : ADS131M04(spiSlave), spi(spi), rxStream(rxStream), dmaChannel(dmaChannel),
-      spiSignalGenerator(spiSignalGenerator), bufSize(bufSize),
-      logFileName(logFileName)
-{
-    // Allocate the buffers
-    buffer1 = static_cast<ADS131M04HighFreqData *>(
-        malloc(bufSize * sizeof(ADS131M04HighFreqData)));
-    buffer2 = static_cast<ADS131M04HighFreqData *>(
-        malloc(bufSize * sizeof(ADS131M04HighFreqData)));
-}
-
-void ADS131M04HighFreq::startHighFreqSampling()
-{
-    // Setup spi as a slave
-    spi.reset();
-    spi.set16BitFrameFormat();
-    spi.setMode(spiSlave.config.mode);
-    spi.enableRxDMARequest();
-    spi.enable();
-
-    // Setup DMA stream
-    rxStream.reset();
-    rxStream.setPeripheralAddress(const_cast<uint32_t *>(&(spi.getSpi()->DR)));
-    rxStream.setMemory0Address(reinterpret_cast<uint32_t *>(buffer1));
-    rxStream.setMemory1Address(reinterpret_cast<uint32_t *>(buffer2));
-    rxStream.setMemoryDataSize(DMAStream::MemoryDataSize::HALF_WORD);
-    rxStream.setPeripheralDataSize(DMAStream::PeripheralDataSize::HALF_WORD);
-    rxStream.setNumberOfDataItems(bufSize * sizeof(ADS131M04HighFreqData) / 2);
-    rxStream.setStreamChannel(dmaChannel);
-    rxStream.setStreamPriorityLevel(DMAStream::PriorityLevel::VERY_HIGH);
-    rxStream.enableMemoryIncrement();
-    rxStream.enableTransferCompleteInterrupt();
-    rxStream.enableCircularMode();
-    rxStream.enableDoubleBufferMode();
-    rxStream.enable();
-
-    // Start the clock generator
-    spiSignalGenerator.configure();
-    spiSignalGenerator.enable();
-
-    highFreqSamplingStarted = true;
-}
-
-void ADS131M04HighFreq::stopHighFreqSampling()
-{
-    // Stop the clock generator
-    spiSignalGenerator.disable();
-
-    highFreqSamplingStarted = false;
-}
-
-void ADS131M04HighFreq::resumeHighFreqSampling()
-{
-    // Stop the clock generator
-    spiSignalGenerator.enable();
-
-    highFreqSamplingStarted = true;
-}
-
-void ADS131M04HighFreq::startLogging() { start(); }
-
-ADS131M04Data ADS131M04HighFreq::sampleImpl()
-{
-    if (highFreqSamplingStarted)
-    {
-        // TODO
-        return ADS131M04Data{};
-    }
-    else
-    {
-        return ADS131M04::sampleImpl();
-    }
-}
-
-void ADS131M04HighFreq::run()
-{
-    std::string filename = fmt::format("/sd/{}.bin", logFileName);
-    remove(filename.c_str());
-    FILE *logFile = fopen(filename.c_str(), "a");
-
-    if (logFile == 0)
-    {
-        LOG_ERR(logger, "Could not open file!");
-        return;
-    }
-
-    while (!shouldStop())
-    {
-        miosix::FastInterruptDisableLock dLock;
-        loggerThread = miosix::Thread::getCurrentThread();
-
-        while (loggerThread != 0)
-        {
-            loggerThread->IRQwait();
-            {
-                miosix::FastInterruptEnableLock eLock(dLock);
-                miosix::Thread::yield();
-            }
-        }
-
-        if ((DMA2_Stream0->CR & DMA_SxCR_CT) == 0)
-        {
-            fwrite(buffer2, sizeof(ADS131M04HighFreqData), bufSize, logFile);
-        }
-        else
-        {
-            fwrite(buffer1, sizeof(ADS131M04HighFreqData), bufSize, logFile);
-        }
-    }
-
-    fclose(logFile);
-}
-
-void ADS131M04HighFreq::handleTransferCompleteInterrupt()
-{
-    // Wake up the logger thread
-    if (loggerThread)
-    {
-        loggerThread->IRQwakeup();
-        if (loggerThread->IRQgetPriority() >
-            miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
-        {
-            miosix::Scheduler::IRQfindNextThread();
-        }
-        loggerThread = nullptr;
-    }
-}
-
-}  // namespace Boardcore
diff --git a/src/shared/sensors/ADS131M04/ADS131M04HighFreq.h b/src/shared/sensors/ADS131M04/ADS131M04HighFreq.h
deleted file mode 100644
index 988512e0a..000000000
--- a/src/shared/sensors/ADS131M04/ADS131M04HighFreq.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
-
-#include <ActiveObject.h>
-#include <drivers/dma/DMAStream.h>
-#include <drivers/spi/SPISignalGenerator.h>
-
-#include "ADS131M04.h"
-#include "ADS131M04HighFreqData.h"
-
-namespace Boardcore
-{
-
-/**
- * @brief Driver for ADS131M04 in combination with DMA and timers.
- */
-class ADS131M04HighFreq : public ADS131M04, private ActiveObject
-{
-public:
-    ADS131M04HighFreqData* buffer1 = nullptr;
-    ADS131M04HighFreqData* buffer2 = nullptr;
-
-    ADS131M04HighFreq(SPISlave spiSlave, SPIType* spi, DMAStream rxStream,
-                      DMAStream::Channel dmaChannel,
-                      SPISignalGenerator spiSignalGenerator, int bufSize,
-                      std::string logFileName);
-
-    /**
-     * @brief Configures DMA and SPI peripherals and starts sampling by enablig
-     * the SPI signal generator.
-     */
-    void startHighFreqSampling();
-
-    /**
-     * @brief Stops the high frequency sampling by disabling the SPI signal
-     * generator.
-     *
-     * After calling this function DMA and SPI are still correctly configured
-     * and transaction can be restarted with resumeHighFreqSampling.
-     */
-    void stopHighFreqSampling();
-
-    /**
-     * @brief Resumed the high frequency sampling by enabling the SPI signal
-     * generator.
-     *
-     * DMA and SPI must be already configured, otherwise no data will be read.
-     */
-    void resumeHighFreqSampling();
-
-    void startLogging();
-
-    void waitDataAndThenLog(void*);
-
-    void handleTransferCompleteInterrupt();
-
-protected:
-    ADS131M04Data sampleImpl() override;
-
-    void run() override;
-
-private:
-    SPI spi;
-    DMAStream rxStream;
-    DMAStream::Channel dmaChannel;
-    SPISignalGenerator spiSignalGenerator;
-    int bufSize;
-    std::string logFileName;
-
-    bool highFreqSamplingStarted = false;
-
-    miosix::Thread* loggerThread = nullptr;
-};
-
-}  // namespace Boardcore
diff --git a/src/shared/sensors/ADS131M08/ADS131M08.cpp b/src/shared/sensors/ADS131M08/ADS131M08.cpp
new file mode 100644
index 000000000..9ea5204cc
--- /dev/null
+++ b/src/shared/sensors/ADS131M08/ADS131M08.cpp
@@ -0,0 +1,422 @@
+/* Copyright (c) 2020 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "ADS131M08.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <util/crc16.h>
+#include <utils/CRC.h>
+
+using namespace Boardcore::ADS131M08RegisterBitMasks;
+
+namespace Boardcore
+{
+
+ADS131M08::ADS131M08(SPIBusInterface &bus, miosix::GpioPin cs,
+                     SPIBusConfig config)
+    : ADS131M08(SPISlave(bus, cs, config))
+{
+}
+
+ADS131M08::ADS131M08(SPISlave spiSlave) : spiSlave(spiSlave)
+{
+    // Reset the configuration
+    channelsPGAGain[0] = PGA::PGA_1;
+    channelsPGAGain[1] = PGA::PGA_1;
+    channelsPGAGain[2] = PGA::PGA_1;
+    channelsPGAGain[3] = PGA::PGA_1;
+    channelsPGAGain[4] = PGA::PGA_1;
+    channelsPGAGain[5] = PGA::PGA_1;
+    channelsPGAGain[6] = PGA::PGA_1;
+    channelsPGAGain[7] = PGA::PGA_1;
+}
+
+SPIBusConfig ADS131M08::getDefaultSPIConfig()
+{
+    SPIBusConfig spiConfig{};
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_64;
+    spiConfig.mode         = SPI::Mode::MODE_1;
+    return spiConfig;
+}
+
+void ADS131M08::setOversamplingRatio(OversamplingRatio ratio)
+{
+    changeRegister(Registers::REG_CLOCK, static_cast<uint16_t>(ratio),
+                   REG_CLOCK_OSR);
+}
+
+bool ADS131M08::init()
+{
+    if (!reset())
+    {
+        lastError = SensorErrors::INIT_FAIL;
+        LOG_ERR(logger, "Initialization failed");
+        return false;
+    }
+
+    return true;
+}
+
+bool ADS131M08::reset()
+{
+    // Write all the communication frame which is maximum 6 24bit words
+    uint8_t data[18] = {0};
+
+    // Prepare the command
+    data[1] = static_cast<uint16_t>(Commands::RESET);
+
+    SPITransaction transaction(spiSlave);
+    transaction.write(data, sizeof(data));
+    miosix::delayUs(10);
+    transaction.read(data, 3);
+
+    uint16_t response = data[0] << 8 | data[1];
+
+    // Check for the correct response
+    if (response != 0xFF24)
+    {
+        lastError = SensorErrors::COMMAND_FAILED;
+        LOG_ERR(logger, "Reset command failed, response was {:X}", response);
+        return false;
+    }
+
+    return true;
+}
+
+void ADS131M08::calibrateOffset()
+{
+    int32_t averageValues[8] = {0};
+
+    // Sample the channels and average the samples
+    for (int i = 0; i < 1000; i++)
+    {
+        sample();
+        miosix::delayUs(1);
+
+        for (int ii = 0; ii < 4; ii++)
+            averageValues[ii] +=
+                lastSample.voltage[ii] /
+                PGA_LSB_SIZE[static_cast<uint16_t>(channelsPGAGain[ii])];
+    }
+
+    for (int i = 0; i < 4; i++)
+        averageValues[i] /= 1000;
+
+    // Configure the offset registers
+    writeRegister(Registers::REG_CH0_OCAL_MSB, averageValues[0] >> 8);
+    writeRegister(Registers::REG_CH0_OCAL_LSB, averageValues[0] << 16);
+    writeRegister(Registers::REG_CH1_OCAL_MSB, averageValues[1] >> 8);
+    writeRegister(Registers::REG_CH1_OCAL_LSB, averageValues[1] << 16);
+    writeRegister(Registers::REG_CH2_OCAL_MSB, averageValues[2] >> 8);
+    writeRegister(Registers::REG_CH2_OCAL_LSB, averageValues[2] << 16);
+    writeRegister(Registers::REG_CH3_OCAL_MSB, averageValues[3] >> 8);
+    writeRegister(Registers::REG_CH3_OCAL_LSB, averageValues[3] << 16);
+    writeRegister(Registers::REG_CH3_OCAL_MSB, averageValues[4] >> 8);
+    writeRegister(Registers::REG_CH3_OCAL_LSB, averageValues[4] << 16);
+    writeRegister(Registers::REG_CH3_OCAL_MSB, averageValues[5] >> 8);
+    writeRegister(Registers::REG_CH3_OCAL_LSB, averageValues[5] << 16);
+    writeRegister(Registers::REG_CH3_OCAL_MSB, averageValues[6] >> 8);
+    writeRegister(Registers::REG_CH3_OCAL_LSB, averageValues[6] << 16);
+    writeRegister(Registers::REG_CH3_OCAL_MSB, averageValues[7] >> 8);
+    writeRegister(Registers::REG_CH3_OCAL_LSB, averageValues[7] << 16);
+}
+
+void ADS131M08::setChannelPGA(Channel channel, PGA gain)
+{
+    channelsPGAGain[static_cast<int>(channel)] = gain;
+
+    if (channel <= Channel::CHANNEL_3)
+    {
+        int shift = static_cast<int>(channel) * 4;
+        changeRegister(Registers::REG_GAIN_1,
+                       static_cast<uint16_t>(gain) << shift,
+                       REG_GAIN_PGAGAIN0 << shift);
+    }
+    else
+    {
+        int shift = (static_cast<int>(channel) - 4) * 4;
+        changeRegister(Registers::REG_GAIN_2,
+                       static_cast<uint16_t>(gain) << shift,
+                       REG_GAIN_PGAGAIN0 << shift);
+    }
+}
+
+void ADS131M08::setChannelOffset(Channel channel, uint32_t offset)
+{
+    // Set the correct registers based on the selected channel
+    Registers regMSB, regLSB;
+    switch (static_cast<int>(channel))
+    {
+        case 0:
+            regMSB = Registers::REG_CH0_OCAL_MSB;
+            regLSB = Registers::REG_CH0_OCAL_LSB;
+            break;
+        case 1:
+            regMSB = Registers::REG_CH1_OCAL_MSB;
+            regLSB = Registers::REG_CH1_OCAL_LSB;
+            break;
+        case 2:
+            regMSB = Registers::REG_CH2_OCAL_MSB;
+            regLSB = Registers::REG_CH2_OCAL_LSB;
+            break;
+        case 3:
+            regMSB = Registers::REG_CH3_OCAL_MSB;
+            regLSB = Registers::REG_CH3_OCAL_LSB;
+            break;
+        case 4:
+            regMSB = Registers::REG_CH4_OCAL_MSB;
+            regLSB = Registers::REG_CH4_OCAL_LSB;
+            break;
+        case 5:
+            regMSB = Registers::REG_CH5_OCAL_MSB;
+            regLSB = Registers::REG_CH5_OCAL_LSB;
+            break;
+        case 6:
+            regMSB = Registers::REG_CH6_OCAL_MSB;
+            regLSB = Registers::REG_CH6_OCAL_LSB;
+            break;
+        default:
+            regMSB = Registers::REG_CH7_OCAL_MSB;
+            regLSB = Registers::REG_CH7_OCAL_LSB;
+            break;
+    }
+
+    writeRegister(regMSB, static_cast<uint16_t>(offset) >> 16);
+    writeRegister(regLSB, static_cast<uint16_t>(offset) << 8);
+}
+
+void ADS131M08::setChannelGainCalibration(Channel channel, double gain)
+{
+    // Check gain value
+    if (gain < 0 || gain > 2)
+        return;
+
+    // Set the correct registers based on the selected channel
+    Registers regMSB, regLSB;
+    switch (static_cast<int>(channel))
+    {
+        case 0:
+            regMSB = Registers::REG_CH0_GCAL_MSB;
+            regLSB = Registers::REG_CH0_GCAL_LSB;
+            break;
+        case 1:
+            regMSB = Registers::REG_CH1_GCAL_MSB;
+            regLSB = Registers::REG_CH1_GCAL_LSB;
+            break;
+        case 2:
+            regMSB = Registers::REG_CH2_GCAL_MSB;
+            regLSB = Registers::REG_CH2_GCAL_LSB;
+            break;
+        case 3:
+            regMSB = Registers::REG_CH3_GCAL_MSB;
+            regLSB = Registers::REG_CH3_GCAL_LSB;
+            break;
+        case 4:
+            regMSB = Registers::REG_CH4_GCAL_MSB;
+            regLSB = Registers::REG_CH4_GCAL_LSB;
+            break;
+        case 5:
+            regMSB = Registers::REG_CH5_GCAL_MSB;
+            regLSB = Registers::REG_CH5_GCAL_LSB;
+            break;
+        case 6:
+            regMSB = Registers::REG_CH6_GCAL_MSB;
+            regLSB = Registers::REG_CH6_GCAL_LSB;
+            break;
+        default:
+            regMSB = Registers::REG_CH7_GCAL_MSB;
+            regLSB = Registers::REG_CH7_GCAL_LSB;
+            break;
+    }
+
+    // Compute the correct gain register parameter
+    uint32_t rawGain = gain * 8388608;  // 2^23
+
+    writeRegister(regMSB, rawGain >> 16);
+    writeRegister(regLSB, rawGain << 8);
+}
+
+void ADS131M08::enableChannel(Channel channel)
+{
+    changeRegister(Registers::REG_CLOCK, 1 << (static_cast<int>(channel) + 8),
+                   1 << (static_cast<int>(channel) + 8));
+}
+
+void ADS131M08::disableChannel(Channel channel)
+{
+    changeRegister(Registers::REG_CLOCK, 0,
+                   1 << (static_cast<int>(channel) + 8));
+}
+
+void ADS131M08::enableGlobalChopMode()
+{
+    changeRegister(Registers::REG_CFG, ADS131M08RegisterBitMasks::REG_CFG_GC_EN,
+                   ADS131M08RegisterBitMasks::REG_CFG_GC_EN);
+}
+
+void ADS131M08::disableGlobalChopMode()
+{
+    changeRegister(Registers::REG_CFG, 0,
+                   ADS131M08RegisterBitMasks::REG_CFG_GC_EN);
+}
+
+ADCData ADS131M08::getVoltage(Channel channel)
+{
+    return {lastSample.timestamp, static_cast<uint8_t>(channel),
+            lastSample.voltage[static_cast<uint8_t>(channel)]};
+}
+
+bool ADS131M08::selfTest()
+{
+    // TODO
+    return true;
+}
+
+ADS131M08Data ADS131M08::sampleImpl()
+{
+    // Send the NULL command and read response
+    uint8_t data[30] = {0};
+
+    data[0] = static_cast<uint16_t>(Commands::NULL_CMD) >> 8;
+    data[1] = static_cast<uint16_t>(Commands::NULL_CMD);
+
+    SPITransaction transaction(spiSlave);
+    transaction.transfer(data, sizeof(data));
+
+    // Extract each channel value
+    int32_t rawValue[8];
+    rawValue[0] = static_cast<uint32_t>(data[3]) << 16 |
+                  static_cast<uint32_t>(data[4]) << 8 |
+                  static_cast<uint32_t>(data[5]);
+    rawValue[1] = static_cast<uint32_t>(data[6]) << 16 |
+                  static_cast<uint32_t>(data[7]) << 8 |
+                  static_cast<uint32_t>(data[8]);
+    rawValue[2] = static_cast<uint32_t>(data[9]) << 16 |
+                  static_cast<uint32_t>(data[10]) << 8 |
+                  static_cast<uint32_t>(data[11]);
+    rawValue[3] = static_cast<uint32_t>(data[12]) << 16 |
+                  static_cast<uint32_t>(data[13]) << 8 |
+                  static_cast<uint32_t>(data[14]);
+    rawValue[4] = static_cast<uint32_t>(data[15]) << 16 |
+                  static_cast<uint32_t>(data[16]) << 8 |
+                  static_cast<uint32_t>(data[17]);
+    rawValue[5] = static_cast<uint32_t>(data[18]) << 16 |
+                  static_cast<uint32_t>(data[19]) << 8 |
+                  static_cast<uint32_t>(data[20]);
+    rawValue[6] = static_cast<uint32_t>(data[21]) << 16 |
+                  static_cast<uint32_t>(data[22]) << 8 |
+                  static_cast<uint32_t>(data[23]);
+    rawValue[7] = static_cast<uint32_t>(data[24]) << 16 |
+                  static_cast<uint32_t>(data[25]) << 8 |
+                  static_cast<uint32_t>(data[26]);
+
+    // Extract and verify the CRC
+    uint16_t dataCrc =
+        static_cast<uint32_t>(data[27]) << 8 | static_cast<uint32_t>(data[28]);
+    uint16_t calculatedCrc = CRCUtils::crc16(data, sizeof(data) - 3);
+
+    if (dataCrc != calculatedCrc)
+    {
+        lastError = SensorErrors::BUS_FAULT;
+        LOG_ERR(logger, "Failed CRC check during sensor sampling");
+
+        // Return and don't convert the corrupted data
+        return lastSample;
+    }
+
+    // Set the two complement
+    for (int i = 0; i < 8; i++)
+    {
+        // Check for the sign bit
+        if (rawValue[i] & 0x800000)
+            rawValue[i] |= 0xFF000000;  // Extend the sign bit
+    }
+
+    // Convert values
+    ADS131M08Data adcData;
+    adcData.timestamp = TimestampTimer::getTimestamp();
+    for (int i = 0; i < 8; i++)
+    {
+        adcData.voltage[i] =
+            rawValue[i] *
+            PGA_LSB_SIZE[static_cast<uint16_t>(channelsPGAGain[i])];
+    }
+
+    return adcData;
+}
+
+uint16_t ADS131M08::readRegister(Registers reg)
+{
+    uint8_t data[3] = {0};
+
+    // Prepare the command
+    data[0] = static_cast<uint16_t>(Commands::RREG) >> 8 |
+              static_cast<uint16_t>(reg) >> 1;
+    data[1] = static_cast<uint16_t>(reg) << 7;
+
+    SPITransaction transaction(spiSlave);
+    transaction.write(data, sizeof(data));
+    transaction.read(data, sizeof(data));
+
+    return data[0] << 8 | data[1];
+}
+
+void ADS131M08::writeRegister(Registers reg, uint16_t data)
+{
+    uint8_t writeCommand[6] = {0};
+
+    // Prepare the command
+    writeCommand[0] = static_cast<uint16_t>(Commands::WREG) >> 8 |
+                      static_cast<uint16_t>(reg) >> 1;
+    writeCommand[1] = static_cast<uint16_t>(reg) << 7;
+    writeCommand[3] = data >> 8;
+    writeCommand[4] = data;
+
+    SPITransaction transaction(spiSlave);
+    transaction.write(writeCommand, sizeof(writeCommand));
+
+    // Check response
+    transaction.read(writeCommand, 3);
+    uint16_t response = writeCommand[0] << 8 | writeCommand[1];
+    if (response != (0x4000 | (static_cast<uint16_t>(reg) << 7)))
+    {
+        lastError = SensorErrors::COMMAND_FAILED;
+        LOG_ERR(logger, "Write command failed, response was {:X}", response);
+    }
+}
+
+void ADS131M08::changeRegister(Registers reg, uint16_t newValue, uint16_t mask)
+{
+    // Read the clock register
+    uint16_t regValue = readRegister(reg);
+
+    // Remove the OSR configuration
+    regValue &= ~mask;
+
+    // Set the OSR
+    regValue |= newValue;
+
+    // Write the new value
+    writeRegister(reg, regValue);
+}
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/ADS131M08/ADS131M08.h b/src/shared/sensors/ADS131M08/ADS131M08.h
new file mode 100644
index 000000000..0d2d3c733
--- /dev/null
+++ b/src/shared/sensors/ADS131M08/ADS131M08.h
@@ -0,0 +1,335 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <diagnostic/PrintLogger.h>
+#include <drivers/spi/SPIDriver.h>
+#include <sensors/Sensor.h>
+
+#include "ADS131M08Data.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Driver for ADS131M08 8 simultaneous channels adc.
+ *
+ * The ADS131M08 is a four-channel, simultaneously-sampling, 24-bit,
+ * delta-sigma (ΔΣ), analog-to-digital converter (ADC). The individual ADC
+ * channels can be independently configured depending on the sensor input. A
+ * low-noise, programmable gain amplifier (PGA) provides gains ranging from 1
+ * to 128 to amplify low-level signals.
+ *
+ * Each channel on the ADS131M08 contains a digital decimation filter that
+ * demodulates the output of the ΔΣ modulators. Offset and gain calibration
+ * registers can be programmed to automatically adjust output samples for
+ * measured offset and gain errors.
+ *
+ * The device features a "global-chop mode" to reduce offset error and offset
+ * drift inherent to the device due to mismatch in the internal circuitry to
+ * very low levels. When global-chop mode is enabled by setting the GC_EN bit in
+ * the GLOBAL_CHOP_CFG register, the device uses the conversion results from two
+ * consecutive internal conversions taken with opposite input polarity to cancel
+ * the device offset voltage.
+ *
+ * Each channel has a dedicated input multiplexer that controls which signals
+ * are routed to the ADC channels:
+ * - The analog input pins corresponding to the given channel
+ * - AGND, which is helpfu for offset calibraton
+ * - Positive DC test signal
+ * - Negative DC test signal
+ *
+ * Each channel also feature an integrated programmable gain amplifier (PGA)
+ * that provides gains of 1 to 128. Each channel has an independent PGA.
+ *
+ * The device communicates via SPI, the maximum allowed frequency is 25MHz.
+ *
+ * The ADC can work on 3 different power configurations depending on the clock
+ * frequency:
+ * - High-resulution: 250-32KSPS
+ * - Low-power: 125-16KSPS
+ * - Very-low-power: 62.5-8KSPS
+ * You will probably have the ADC in high resolution mode.
+ */
+class ADS131M08 : public Sensor<ADS131M08Data>
+{
+public:
+    /**
+     * @brief ADC's oversampling ratio configurations.
+     *
+     * The OSR determins the output data rate, depending on the master clock
+     * frequency.
+     *
+     * ODR = f_CLK / 2 / OSR
+     *
+     * On Skyward's boards an 8.192MHz clock is used.
+     */
+    enum class OversamplingRatio : uint16_t
+    {
+        OSR_128   = 0,         // ODR is 32KHz
+        OSR_256   = 0x1 << 2,  // ODR is 16KHz
+        OSR_512   = 0x2 << 2,  // ODR is 8KHz
+        OSR_1024  = 0x3 << 2,  // ODR is 4KHz
+        OSR_2048  = 0x4 << 2,  // ODR is 2KHz
+        OSR_4096  = 0x5 << 2,  // ODR is 1KHz
+        OSR_8192  = 0x6 << 2,  // ODR is 500Hz
+        OSR_16256 = 0x7 << 2   // ODR is 250Hz
+    };
+
+    enum class PGA : uint16_t
+    {
+        PGA_1   = 0,    ///< Full scale resolution is ±1.2V
+        PGA_2   = 0x1,  ///< Full scale resolution is ±600mV
+        PGA_4   = 0x2,  ///< Full scale resolution is ±300mV
+        PGA_8   = 0x3,  ///< Full scale resolution is ±150mV
+        PGA_16  = 0x4,  ///< Full scale resolution is ±75mV
+        PGA_32  = 0x5,  ///< Full scale resolution is ±37.5mV
+        PGA_64  = 0x6,  ///< Full scale resolution is ±18.75mV
+        PGA_128 = 0x7   ///< Full scale resolution is ±9.375mV
+    };
+
+    enum class Channel : uint8_t
+    {
+        CHANNEL_0 = 0,
+        CHANNEL_1 = 1,
+        CHANNEL_2 = 2,
+        CHANNEL_3 = 3,
+        CHANNEL_4 = 3,
+        CHANNEL_5 = 3,
+        CHANNEL_6 = 3,
+        CHANNEL_7 = 3
+    };
+
+    ADS131M08(SPIBusInterface &bus, miosix::GpioPin cs,
+              SPIBusConfig config = getDefaultSPIConfig());
+
+    explicit ADS131M08(SPISlave spiSlave);
+
+    /**
+     * Constructs the default config for SPI Bus.
+     *
+     * @returns The default SPIBusConfig.
+     */
+    static SPIBusConfig getDefaultSPIConfig();
+
+    bool init() override;
+
+    bool reset();
+
+    /**
+     * @brief Samples each channel, averages the samples and applies offset
+     * compensation in the device.
+     */
+    void calibrateOffset();
+
+    void setOversamplingRatio(OversamplingRatio ratio);
+
+    void setChannelPGA(Channel channel, PGA gain);
+
+    /**
+     * @brief Sets the channel offset.
+     *
+     * Note that the device offset is a 24bit two complement.
+     */
+    void setChannelOffset(Channel channel, uint32_t offset);
+
+    /**
+     * @brief Sets the channel gain calibration.
+     *
+     * The ADS131M08 corrects for gain errors by multiplying the ADC conversion
+     * result using the gain calibration registers.
+     * The gain calibration value is interpreted as a 24bit unsigned. The values
+     * corresponds to n * (1/2^23), ranging from 0 to 2 - (1/2^23).
+     *
+     * This function accepts a value between 0 and 2, it then compute the
+     * correct gain register value.
+     *
+     * @param gain Must be between 0 and 2.
+     */
+    void setChannelGainCalibration(Channel channel, double gain);
+
+    void enableChannel(Channel channel);
+
+    void disableChannel(Channel channel);
+
+    void enableGlobalChopMode();
+
+    void disableGlobalChopMode();
+
+    ADCData getVoltage(Channel channel);
+
+    bool selfTest() override;
+
+protected:
+    ADS131M08Data sampleImpl() override;
+
+private:
+    enum class Registers : uint16_t
+    {
+        // Device settings and indicators
+        REG_ID     = 0,
+        REG_STATUS = 0x1,
+
+        // Global settings across channels
+        REG_MODE        = 0x2,
+        REG_CLOCK       = 0x3,
+        REG_GAIN_1      = 0x4,
+        REG_GAIN_2      = 0x5,
+        REG_CFG         = 0x6,
+        REG_THRSHLD_MSB = 0x7,
+        REG_THRSHLD_LSB = 0x8,
+
+        // Channel specific settings
+        REG_CH0_CFG      = 0x9,
+        REG_CH0_OCAL_MSB = 0xA,
+        REG_CH0_OCAL_LSB = 0xB,
+        REG_CH0_GCAL_MSB = 0xC,
+        REG_CH0_GCAL_LSB = 0xD,
+        REG_CH1_CFG      = 0xE,
+        REG_CH1_OCAL_MSB = 0xF,
+        REG_CH1_OCAL_LSB = 0x10,
+        REG_CH1_GCAL_MSB = 0x11,
+        REG_CH1_GCAL_LSB = 0x12,
+        REG_CH2_CFG      = 0x13,
+        REG_CH2_OCAL_MSB = 0x14,
+        REG_CH2_OCAL_LSB = 0x15,
+        REG_CH2_GCAL_MSB = 0x16,
+        REG_CH2_GCAL_LSB = 0x17,
+        REG_CH3_CFG      = 0x18,
+        REG_CH3_OCAL_MSB = 0x19,
+        REG_CH3_OCAL_LSB = 0x1A,
+        REG_CH3_GCAL_MSB = 0x1B,
+        REG_CH3_GCAL_LSB = 0x1C,
+        REG_CH4_CFG      = 0x1D,
+        REG_CH4_OCAL_MSB = 0x1E,
+        REG_CH4_OCAL_LSB = 0x1F,
+        REG_CH4_GCAL_MSB = 0x20,
+        REG_CH4_GCAL_LSB = 0x21,
+        REG_CH5_CFG      = 0x22,
+        REG_CH5_OCAL_MSB = 0x23,
+        REG_CH5_OCAL_LSB = 0x24,
+        REG_CH5_GCAL_MSB = 0x25,
+        REG_CH5_GCAL_LSB = 0x26,
+        REG_CH6_CFG      = 0x27,
+        REG_CH6_OCAL_MSB = 0x28,
+        REG_CH6_OCAL_LSB = 0x29,
+        REG_CH6_GCAL_MSB = 0x2A,
+        REG_CH6_GCAL_LSB = 0x2B,
+        REG_CH7_CFG      = 0x2C,
+        REG_CH7_OCAL_MSB = 0x2D,
+        REG_CH7_OCAL_LSB = 0x2E,
+        REG_CH7_GCAL_MSB = 0x2F,
+        REG_CH7_GCAL_LSB = 0x30,
+
+        // Register map CRC
+        REG_REGMAP_CRC = 0x3E
+    };
+
+    uint16_t readRegister(Registers reg);
+
+    void writeRegister(Registers reg, uint16_t data);
+
+    void changeRegister(Registers reg, uint16_t newValue, uint16_t mask);
+
+    enum class Commands : uint16_t
+    {
+        NULL_CMD = 0,
+        RESET    = 0x11,
+        STANDBY  = 0x22,
+        WAKEUP   = 0x33,
+        LOCK     = 0x555,
+        UNLOCK   = 0x655,
+        RREG     = 0xA000,
+        WREG     = 0x6000
+    };
+
+    ///< Digit value in mV for each pga configurations
+    const float PGA_LSB_SIZE[8] = {143.0511e-9, 71.5256e-9, 35.7628e-9,
+                                   17.8814e-9,  8.9407e-9,  4.4703e-9,
+                                   2.2352e-9,   1.1176e-9};
+
+    PGA channelsPGAGain[8] = {PGA::PGA_1};
+
+protected:
+    SPISlave spiSlave;
+
+    PrintLogger logger = Logging::getLogger("ads131m08");
+};
+
+namespace ADS131M08RegisterBitMasks
+{
+
+// Status register
+constexpr uint16_t REG_STATUS_LOCK     = 1 << 15;
+constexpr uint16_t REG_STATUS_F_RESYNC = 1 << 14;
+constexpr uint16_t REG_STATUS_REG_MAP  = 1 << 13;
+constexpr uint16_t REG_STATUS_CRC_ERR  = 1 << 12;
+constexpr uint16_t REG_STATUS_CRC_TYPE = 1 << 11;
+constexpr uint16_t REG_STATUS_RESET    = 1 << 10;
+constexpr uint16_t REG_STATUS_WLENGTH  = 3 << 8;
+constexpr uint16_t REG_STATUS_DRDY3    = 1 << 3;
+constexpr uint16_t REG_STATUS_DRDY2    = 1 << 2;
+constexpr uint16_t REG_STATUS_DRDY1    = 1 << 1;
+constexpr uint16_t REG_STATUS_DRDY0    = 1;
+
+// Mode register
+constexpr uint16_t REG_MODE_REG_CRC_EN = 1 << 13;
+constexpr uint16_t REG_MODE_RX_CRC_EN  = 1 << 12;
+constexpr uint16_t REG_MODE_CRC_TYPE   = 1 << 11;
+constexpr uint16_t REG_MODE_RESET      = 1 << 10;
+constexpr uint16_t REG_MODE_WLENGTH    = 3 << 8;
+constexpr uint16_t REG_MODE_TIMEOUT    = 1 << 4;
+constexpr uint16_t REG_MODE_DRDY_SEL   = 3 << 2;
+constexpr uint16_t REG_MODE_DRDY_HiZ   = 1 << 1;
+constexpr uint16_t REG_MODE_DRDY_FMT   = 1 << 0;
+
+// Clock register
+constexpr uint16_t REG_CLOCK_CH3_EN = 1 << 11;
+constexpr uint16_t REG_CLOCK_CH2_EN = 1 << 10;
+constexpr uint16_t REG_CLOCK_CH1_EN = 1 << 9;
+constexpr uint16_t REG_CLOCK_CH0_EN = 1 << 8;
+constexpr uint16_t REG_CLOCK_OSR    = 7 << 2;
+constexpr uint16_t REG_CLOCK_PWR    = 3;
+
+// Gain register
+constexpr uint16_t REG_GAIN_PGAGAIN3 = 7 << 12;
+constexpr uint16_t REG_GAIN_PGAGAIN2 = 7 << 8;
+constexpr uint16_t REG_GAIN_PGAGAIN1 = 7 << 4;
+constexpr uint16_t REG_GAIN_PGAGAIN0 = 7;
+
+// Configuration register
+constexpr uint16_t REG_CFG_GC_DLY   = 0xF << 9;
+constexpr uint16_t REG_CFG_GC_EN    = 1 << 8;
+constexpr uint16_t REG_CFG_CD_ALLCH = 1 << 7;
+constexpr uint16_t REG_CFG_CD_NUM   = 7 << 4;
+constexpr uint16_t REG_CFG_CD_LEN   = 7 << 1;
+constexpr uint16_t REG_CFG_CD_EN    = 1;
+
+// Channel configuration register
+constexpr uint16_t REG_CHx_CFG_PHASE     = 0x3FF << 6;
+constexpr uint16_t REG_CHx_CFG_DCBLK_DIS = 1 << 2;
+constexpr uint16_t REG_CHx_CFG_MUX       = 3;
+
+}  // namespace ADS131M08RegisterBitMasks
+
+}  // namespace Boardcore
diff --git a/src/shared/sensors/ADS131M04/ADS131M04HighFreqData.h b/src/shared/sensors/ADS131M08/ADS131M08Data.h
similarity index 50%
rename from src/shared/sensors/ADS131M04/ADS131M04HighFreqData.h
rename to src/shared/sensors/ADS131M08/ADS131M08Data.h
index 74806a448..355bcd4dc 100644
--- a/src/shared/sensors/ADS131M04/ADS131M04HighFreqData.h
+++ b/src/shared/sensors/ADS131M08/ADS131M08Data.h
@@ -24,14 +24,46 @@
 
 #include <stdint.h>
 
+#include <ostream>
+
 namespace Boardcore
 {
 
-struct ADS131M04HighFreqData
+struct ADS131M08Data
 {
-    uint16_t status;
-    uint8_t rawData[12];
-    uint16_t crc;
+    uint64_t timestamp;
+    float voltage[8];
+
+    ADS131M08Data() : ADS131M08Data{0, 0, 0, 0, 0, 0, 0, 0, 0} {}
+
+    ADS131M08Data(uint64_t timestamp, float voltageCh1, float voltageCh2,
+                  float voltageCh3, float voltageCh4, float voltageCh5,
+                  float voltageCh6, float voltageCh7, float voltageCh8)
+        : timestamp(timestamp)
+    {
+        voltage[0] = voltageCh1;
+        voltage[1] = voltageCh2;
+        voltage[2] = voltageCh3;
+        voltage[3] = voltageCh4;
+        voltage[4] = voltageCh5;
+        voltage[5] = voltageCh6;
+        voltage[6] = voltageCh7;
+        voltage[7] = voltageCh8;
+    }
+
+    static std::string header()
+    {
+        return "timestamp,voltage_channel_1,voltage_channel_2,voltage_channel_"
+               "3,voltage_channel_4,voltage_channel_5,voltage_channel_6,"
+               "voltage_channel_7,voltage_channel_8\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << voltage[0] << "," << voltage[1] << ","
+           << voltage[2] << "," << voltage[3] << "," << voltage[4] << ","
+           << voltage[5] << "," << voltage[6] << "," << voltage[7] << "\n";
+    }
 };
 
 }  // namespace Boardcore
diff --git a/src/tests/sensors/test-ads131m08.cpp b/src/tests/sensors/test-ads131m08.cpp
new file mode 100644
index 000000000..5deb400da
--- /dev/null
+++ b/src/tests/sensors/test-ads131m08.cpp
@@ -0,0 +1,80 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <drivers/spi/SPIDriver.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+GpioPin sckPin  = GpioPin(GPIOA_BASE, 5);
+GpioPin misoPin = GpioPin(GPIOA_BASE, 6);
+GpioPin mosiPin = GpioPin(GPIOA_BASE, 7);
+GpioPin csPin   = GpioPin(GPIOA_BASE, 3);
+
+void initBoard()
+{
+    // Setup gpio pins
+    csPin.mode(Mode::OUTPUT);
+    csPin.high();
+    sckPin.mode(Mode::ALTERNATE);
+    sckPin.alternateFunction(5);
+    misoPin.mode(Mode::ALTERNATE);
+    misoPin.alternateFunction(5);
+    mosiPin.mode(Mode::ALTERNATE);
+    mosiPin.alternateFunction(5);
+}
+
+int main()
+{
+    // Enable SPI clock and set gpios
+    initBoard();
+
+    // SPI configuration setup
+    SPIBus spiBus(SPI1);
+    SPISlave spiSlave(spiBus, csPin, ADS131M08::getDefaultSPIConfig());
+
+    // Device initialization
+    ADS131M08 ads131(spiSlave);
+
+    // Initialize the device
+    ads131.init();
+    ads131.enableGlobalChopMode();
+    ads131.setOversamplingRatio(ADS131M08::OversamplingRatio::OSR_16256);
+    ads131.calibrateOffset();
+
+    while (true)
+    {
+        ads131.sample();
+
+        ADS131M08Data data = ads131.getLastSample();
+
+        printf(
+            "% 2.5f\t% 2.5f\t% 2.5f\t% 2.5f\t% 2.5f\t% 2.5f\t% 2.5f\t% 2.5f\n",
+            data.voltage[0], data.voltage[1], data.voltage[2], data.voltage[3],
+            data.voltage[4], data.voltage[5], data.voltage[6], data.voltage[7]);
+
+        delayMs(50);
+    }
+}
-- 
GitLab