diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index c6fc81a9245b8132655e8c5b2a979e01dd066280..3601e95071280f7bee885e9b34314d9ba78141b6 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -230,7 +230,7 @@
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
                 "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
-                "_BOARD_STM32F769NI_DISCO",
+                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
                 "_ARCH_CORTEXM7_STM32F7",
                 "STM32F769xx",
                 "HSE_VALUE=25000000",
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 72516192f67d941830b02456351511511052db84..0708e2d46fba98ea4ae67a531ae8363b9415790e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -197,10 +197,7 @@ add_executable(test-general-purpose-timer src/tests/drivers/timer/test-general-p
 sbs_target(test-general-purpose-timer stm32f429zi_stm32f4discovery)
 
 add_executable(test-internal-adc src/tests/drivers/test-internal-adc.cpp)
-sbs_target(test-internal-adc stm32f407vg_stm32f4discovery)
-
-add_executable(test-internal-adc-dma src/tests/drivers/test-internal-adc-dma.cpp)
-sbs_target(test-internal-adc-dma stm32f429zi_stm32f4discovery)
+sbs_target(test-internal-adc stm32f767zi_compute_unit)
 
 add_executable(test-mavlink src/tests/drivers/test-mavlink.cpp)
 sbs_target(test-mavlink stm32f407vg_stm32f4discovery)
@@ -253,9 +250,6 @@ sbs_target(test-i2c-driver-f7 stm32f767zi_nucleo)
 add_executable(test-i2c-f7 src/tests/drivers/i2c/test-i2c.cpp)
 sbs_target(test-i2c-f7 stm32f767zi_nucleo)
 
-add_executable(test-internal-temp src/tests/drivers/test-internal-temp.cpp)
-sbs_target(test-internal-temp stm32f407vg_stm32f4discovery)
-
 #-----------------------------------------------------------------------------#
 #                               Tests - Events                                #
 #-----------------------------------------------------------------------------#
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index a3d1da7a95ea1553aeecedb6b9d4ca498c3838ef..e9578c1be5dc6f02995299aaa5c88aa90bd03ffb 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -46,7 +46,6 @@ foreach(OPT_BOARD ${BOARDS})
 
         # Drivers
         ${SBS_BASE}/src/shared/drivers/AD5204/AD5204.cpp
-        ${SBS_BASE}/src/shared/drivers/adc/InternalTemp.cpp
         ${SBS_BASE}/src/shared/drivers/adc/InternalADC.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
diff --git a/src/shared/drivers/adc/InternalADC.cpp b/src/shared/drivers/adc/InternalADC.cpp
index caf4578798cc9f740758154db88d596774b02825..45234301d3b0cba19aac5334a6e23f6ce3b14f49 100644
--- a/src/shared/drivers/adc/InternalADC.cpp
+++ b/src/shared/drivers/adc/InternalADC.cpp
@@ -25,61 +25,49 @@
 #include <drivers/timer/TimestampTimer.h>
 #include <utils/ClockUtils.h>
 
+#if defined(STM32F407xx) || defined(STM32F429xx)
+#define TEMP30_CAL_VALUE ((uint16_t*)((uint32_t)0x1FFF7A2C))
+#define TEMP110_CAL_VALUE ((uint16_t*)((uint32_t)0x1FFF7A2E))
+#define TEMP30 30
+#define TEMP110 110
+#elif defined(STM32F767xx) || defined(STM32F769xx)
+#define TEMP30_CAL_VALUE ((uint16_t*)((uint32_t)0x1FF0F44C))
+#define TEMP110_CAL_VALUE ((uint16_t*)((uint32_t)0x1FF0F44E))
+#define TEMP30 30
+#define TEMP110 110
+#else
+#warning This microcontroller does not have a calibrated temperature sensor or is not currently supported by this driver
+#define WITHOUT_CALIBRATION
+#endif
+
+#if defined(STM32F407xx) || defined(STM32F205xx)
+#define AUX_CH InternalADC::CH16
+#define VBAT_DIV 2.0f
+#elif defined(STM32F429xx) || defined(STM32F767xx) || defined(STM32F769xx)
+#define AUX_CH InternalADC::CH18
+#define VBAT_DIV 4.0f
+#endif
+
 namespace Boardcore
 {
 
-InternalADC::InternalADC(ADC_TypeDef* adc, const float supplyVoltage,
-                         const bool isUsingDMA, DMA_Stream_TypeDef* dmaStream)
-    : adc(adc), supplyVoltage(supplyVoltage), isUsingDMA(isUsingDMA),
-      dmaStream(dmaStream)
+InternalADC::InternalADC(ADC_TypeDef* adc, const float supplyVoltage)
+    : adc(adc), supplyVoltage(supplyVoltage)
 {
     resetRegisters();
     ClockUtils::enablePeripheralClock(adc);
 
-    if (isUsingDMA)
+    for (int i = 0; i < CH_NUM; i++)
     {
-        ClockUtils::enablePeripheralClock(dma);
-
-        // Find the DMA stream number
-        streamNum = ((((uint32_t)&dmaStream) & 0xFF) - 0x10) / 0x18;
-
-        // Check which registers to use
-        statusReg    = (streamNum < 4 ? &(dma->LISR) : &(dma->HISR));
-        clearFlagReg = (streamNum < 4 ? &(dma->LIFCR) : &(dma->HIFCR));
-
-        // Create the masks for the status bits (this will do for both high and
-        // low registers as well as status and reset status registers)
-        switch (streamNum % 4)
-        {
-            case 0:
-                transferCompleteMask = DMA_LISR_TCIF0;
-                transferErrorMask    = DMA_LISR_TEIF0;
-                break;
-            case 1:
-                transferCompleteMask = DMA_LISR_TCIF1;
-                transferErrorMask    = DMA_LISR_TEIF1;
-                break;
-            case 2:
-                transferCompleteMask = DMA_LISR_TCIF2;
-                transferErrorMask    = DMA_LISR_TEIF2;
-                break;
-            case 3:
-                transferCompleteMask = DMA_LISR_TCIF3;
-                transferErrorMask    = DMA_LISR_TEIF3;
-                break;
-        }
+        channelsEnabled[i]   = false;
+        channelsRawValues[i] = 0;
     }
-
-    // Init indexMap
-    for (auto i = 0; i < CH_NUM; i++)
-        indexMap[i] = -1;
 }
 
 InternalADC::~InternalADC()
 {
     resetRegisters();
     ClockUtils::disablePeripheralClock(adc);
-    // Do not disable the DMA controller, other streams could use it!
 }
 
 bool InternalADC::init()
@@ -87,192 +75,122 @@ bool InternalADC::init()
     // Turn on the ADC
     adc->CR2 |= ADC_CR2_ADON;
 
-    // Set scan mode to read all the enabled channels in sequence
-    adc->CR1 |= ADC_CR1_SCAN;
-
-    if (isUsingDMA)
-    {
-        // Set the DMA peripheral address
-        dmaStream->PAR = (uint32_t) & (adc->DR);
-
-        // Set the DMA memory address
-        dmaStream->M0AR = (uint32_t)values;
-
-        // Enable DMA on ADC
-        adc->CR2 |= ADC_CR2_DMA;
+    return true;
+}
 
-        // Enable DMA stream
-        dmaStream->CR |= DMA_SxCR_EN;
+bool InternalADC::selfTest() { return true; }
 
-        // Check if we are using the DMA2 controller, otherwise it's DMA1
-        if (((uint32_t)&dmaStream & ~0xFF) == (uint32_t)DMA2_BASE)
+InternalADCData InternalADC::sampleImpl()
+{
+    for (int i = 0; i < CH_NUM; i++)
+    {
+        if (channelsEnabled[i])
         {
-            dma = DMA2;
+            channelsRawValues[i] = readChannel(static_cast<Channel>(i));
         }
-
-        // All this is because the status bits are not stored in the
-        // dmaStream registers but in the main DMA controller status
-        // register. We could not care about the transfer status but the
-        // timestamp would not match the values read after calling sample().
-
-        // We calculate this values to avoid doing it every time in
-        // sampleImpl().
     }
 
-    return true;
-}
-
-bool InternalADC::enableChannel(Channel channel, SampleTime sampleTime)
-{
-    // Check channel number
-    if (channel < CH0 || channel >= CH_NUM)
-        return false;
-
-    // Add channel to the sequence
-    if (!isUsingDMA)
+    // Prepare the auxiliary channel for the next sample
+    if (vbatLastRead)
     {
-        if (!addInjectedChannel(channel))
-            return false;
+        vbatVoltageRawValue = channelsRawValues[AUX_CH];
+
+        if (tempEnabled)
+        {
+            ADC->CCR &= ~ADC_CCR_VBATE;
+            ADC->CCR |= ADC_CCR_TSVREFE;
+            vbatLastRead = false;
+        }
     }
     else
     {
-        if (!addRegularChannel(channel))
-            return false;
+        temperatureRawValue = channelsRawValues[AUX_CH];
 
-        // Update the DMA number of data
-        dmaStream->NDTR = activeChannels;
+        if (vbatEnabled)
+        {
+            ADC->CCR |= ADC_CCR_VBATE;
+            ADC->CCR &= ~ADC_CCR_TSVREFE;
+            vbatLastRead = true;
+        }
     }
 
-    // Set channel's sample time
-    setChannelSampleTime(channel, sampleTime);
+    timestamp = TimestampTimer::getTimestamp();
 
-    return true;
+    return lastSample;
 }
 
-bool InternalADC::addRegularChannel(Channel channel)
+void InternalADC::enableChannel(Channel channel, SampleTime sampleTime)
 {
-    // Check active channels number
-    if (activeChannels >= 16)
-        return false;
+    channelsEnabled[channel] = true;
 
-    // Add the channel to the sequence
-    volatile uint32_t* sqrPtr;
-    switch (activeChannels / 6)
-    {
-        case 1:
-            sqrPtr = &(adc->SQR2);
-            break;
-        case 2:
-            sqrPtr = &(adc->SQR1);
-            break;
-        default:
-            sqrPtr = &(adc->SQR3);
-    }
-    *sqrPtr = channel << ((activeChannels % 6) * 5);
+    setChannelSampleTime(channel, sampleTime);
+}
 
-    // Update the channels number in the register
-    adc->SQR1 &= ~ADC_SQR1_L;
-    adc->SQR1 |= activeChannels << 20;
+void InternalADC::disableChannel(Channel channel)
+{
+    channelsEnabled[channel]   = false;
+    channelsRawValues[channel] = 0;
+}
 
-    // Save the index of the channel in the ADC's regular sequence
-    indexMap[channel] = activeChannels;
+void InternalADC::enableTemperature(SampleTime sampleTime)
+{
+    tempEnabled = true;
 
-    // Update the counter
-    activeChannels++;
+    ADC->CCR &= ~ADC_CCR_VBATE;
+    ADC->CCR |= ADC_CCR_TSVREFE;
+    vbatLastRead = false;
 
-    return true;
+    enableChannel(AUX_CH, sampleTime);
 }
 
-ADCData InternalADC::readChannel(Channel channel, SampleTime sampleTime)
+void InternalADC::disableTemperature() { tempEnabled = false; }
+
+void InternalADC::enableVbat(SampleTime sampleTime)
 {
-    setChannelSampleTime(channel, sampleTime);
-    startRegularConversion();
+    vbatEnabled = true;
 
-    while (!(adc->SR & ADC_SR_EOC))
-        ;
+    ADC->CCR |= ADC_CCR_VBATE;
+    ADC->CCR &= ~ADC_CCR_TSVREFE;
+    vbatLastRead = true;
 
-    return {TimestampTimer::getTimestamp(), channel,
-            static_cast<uint16_t>(adc->DR) * supplyVoltage / RESOLUTION};
+    enableChannel(AUX_CH, sampleTime);
 }
 
+void InternalADC::disableVbat() { vbatEnabled = false; }
+
 InternalADCData InternalADC::getVoltage(Channel channel)
 {
-    float voltage = 0;
-
-    if (indexMap[channel] >= 0 && indexMap[channel] < CH_NUM)
-    {
-        voltage = values[indexMap[channel]] * supplyVoltage / RESOLUTION;
-    }
-
-    return InternalADCData{timestamp, (uint8_t)channel, voltage};
+    return {timestamp, channel,
+            channelsRawValues[channel] * supplyVoltage / RESOLUTION};
 }
 
-bool InternalADC::selfTest()
+TemperatureData InternalADC::getTemperature()
 {
-    // Try a single sample and check for error
-    sample();
-
-    if (lastError != NO_ERRORS)
-    {
-        return false;
-    }
-
-    return true;
+    TemperatureData data;
+    data.temperatureTimestamp = timestamp;
+    data.temperature = temperatureRawValue * supplyVoltage / RESOLUTION;
+
+#ifdef WITHOUT_CALIBRATION
+    // Default conversion
+    data.temperature = ((data.temperature - 0.76) / 0.0025) + 25;
+#else
+    // Factory calibration
+    // Read "Temperature sensor characteristics" chapter in the datasheet
+    float voltage30  = static_cast<float>(*TEMP30_CAL_VALUE) * 3.3 / 4095;
+    float voltage110 = static_cast<float>(*TEMP110_CAL_VALUE) * 3.3 / 4095;
+    float slope      = (voltage110 - voltage30) / (TEMP110 - TEMP30);
+    data.temperature = ((data.temperature - voltage30) / slope) + TEMP30;
+#endif
+
+    return data;
 }
 
-InternalADCData InternalADC::sampleImpl()
+InternalADCData InternalADC::getVbatVoltage()
 {
-    if (!isUsingDMA)
-    {
-        startInjectedConversion();
-
-        // Wait for end of conversion
-        while (!(adc->SR & ADC_SR_JEOC))
-            ;
-
-        // Read all 4 channels (faster than read only the enabled ones)
-        values[0] = adc->JDR1;
-        values[1] = adc->JDR2;
-        values[2] = adc->JDR3;
-        values[3] = adc->JDR4;
-    }
-    else
-    {
-        // Rewrite the DMA bit in ADC CR2 (reference manual chapter 13.8.1)
-        adc->CR2 &= ~ADC_CR2_DMA;
-        adc->CR2 |= ADC_CR2_DMA;
-
-        startRegularConversion();
-
-        // This should trigger the DMA stream for each channel's conversion
-
-        // Wait for transfer end
-        while (!(*statusReg & (transferCompleteMask | transferErrorMask)))
-            ;
-
-        // Clear the transfer complete flag
-        *clearFlagReg |= transferCompleteMask;
-
-        // If and error has occurred (probably due to a higher priority stream)
-        // don't update the timestamp, the values should not have been updated
-        if (*statusReg & transferErrorMask)
-        {
-            // Clear the transfer error flag
-            *clearFlagReg |= transferErrorMask;
-
-            // Signal an error
-            lastError = DMA_ERROR;
-            return lastSample;
-        }
-    }
-
-    timestamp = TimestampTimer::getTimestamp();
-
-    return lastSample;
+    return {timestamp, AUX_CH,
+            vbatVoltageRawValue * supplyVoltage / RESOLUTION * VBAT_DIV};
 }
 
-float InternalADC::getSupplyVoltage() { return supplyVoltage; }
-
 inline void InternalADC::resetRegisters()
 {
     // Reset the ADC configuration
@@ -292,55 +210,33 @@ inline void InternalADC::resetRegisters()
     adc->JSQR  = 0;
 }
 
-inline void InternalADC::startInjectedConversion()
-{
-    adc->CR2 |= ADC_CR2_JSWSTART;
-}
-
-inline void InternalADC::startRegularConversion()
-{
-    adc->CR2 |= ADC_CR2_SWSTART;
-}
-
-inline bool InternalADC::addInjectedChannel(Channel channel)
+inline void InternalADC::setChannelSampleTime(Channel channel,
+                                              SampleTime sampleTime)
 {
-    // Check active channels number
-    if (activeChannels >= 4)
-        return false;
-
-    // Add the channel to the sequence, starting from position 4
-    adc->JSQR |= channel << (15 - activeChannels * 5);
-
-    // Update the channels number in the register
-    adc->JSQR &= 0x000FFFFF;
-    adc->JSQR |= activeChannels << 20;
-
-    // Increment the index of all enabled channels
-    for (auto i = 0; i < CH_NUM; i++)
+    if (channel <= 9)
     {
-        if (indexMap[i] >= 0)
-            indexMap[i]++;
+        adc->SMPR2 &= ~(0x7 << (channel * 3));
+        adc->SMPR2 |= sampleTime << (channel * 3);
+    }
+    else
+    {
+        adc->SMPR1 &= ~(0x7 << ((channel - 10) * 3));
+        adc->SMPR1 |= sampleTime << ((channel - 10) * 3);
     }
-
-    // Set this channel index to 0
-    indexMap[channel] = 0;
-
-    // Update the counter
-    activeChannels++;
-
-    return true;
 }
 
-inline void InternalADC::setChannelSampleTime(Channel channel,
-                                              SampleTime sampleTime)
+float InternalADC::readChannel(Channel channel)
 {
-    volatile uint32_t* smprPtr;
-    if (channel <= 9)
-        smprPtr = &(adc->SMPR2);
-    else
-        smprPtr = &(adc->SMPR1);
+    // Slect channel
+    adc->SQR3 = channel;
+
+    // Start conversion
+    adc->CR2 |= ADC_CR2_SWSTART;
+
+    while (!(adc->SR & ADC_SR_EOC))
+        ;
 
-    *smprPtr = sampleTime << (channel * 3);
+    return static_cast<uint16_t>(adc->DR);
 }
 
 }  // namespace Boardcore
diff --git a/src/shared/drivers/adc/InternalADC.h b/src/shared/drivers/adc/InternalADC.h
index ab0498619a7447080ab2efa4c6c6b7c53438e73b..8d4989420337a553284261b288ae15d9d94983bc 100644
--- a/src/shared/drivers/adc/InternalADC.h
+++ b/src/shared/drivers/adc/InternalADC.h
@@ -35,36 +35,12 @@ namespace Boardcore
  *
  * Allows conversions on multiple channels with per-channel sample time.
  *
- * The driver configures the ADC by itself, including the clock.
+ * The driver uses basic ADC features, that is the single conversion mode.
+ * A previous version of the driver featured injected and regular channels with
+ * also DMA. Since ADC conversion time is very low, the driver has been
+ * simplified to provide better usability and simplier implementation.
  *
- * This driver implements the following ADC's features:
- * - injected channels: a list of up to 4 channels that can be converted
- * - scan mode with regular channels: up to 16 channels converted in the same
- * sequence as they are enabled
- * - DMA: Used in scan mode to transfer data from the peripheral data register
- *
- * Note that only the DMA2 controller can be used with the ADC in stm32f4
- * microcontrollers.
- *
- * When you don't use the DMA, the driver works with the injected channels.
- * When a conversion is started the values are stored in 4 specific data
- * register, without the need to use DMA.
- *
- * If you need more than 4 channels you must use DMA. When the regular group
- * of channels is converted, the data will be stored sequentially in a single
- * data register and DMA is needed to save the value between each conversion in
- * another memory location.
- *
- * When using DMA the driver will configure only the memory addresses, you must
- * configure the stream properly!
- *
- * Examples of how to use the internal ADC driver and set up the DMA stream
- * can be found in the test files (test-internal-adc*).
- *
- * The ADC has also other features as value offsets for injected channels,
- * interrupts (such as dma transfer and watchdog), conversion triggering with
- * timers and multi adc mode.
- * This features are not implemented to keep the driver simple.
+ * @warning This driver has been tested on f205, f407, f429, f767 and f769
  */
 class InternalADC : public Sensor<InternalADCData>
 {
@@ -98,10 +74,12 @@ public:
 
     /**
      * @brief Conversion sample time. See reference manual.
+     *
+     * CYCLES_3 is not exposed because in 12-bit mode the minimum is 15
      */
     enum SampleTime : uint8_t
     {
-        CYCLES_3   = 0x0,
+        // CYCLES_3   = 0x0,
         CYCLES_15  = 0x1,
         CYCLES_28  = 0x2,
         CYCLES_56  = 0x3,
@@ -113,84 +91,64 @@ public:
 
     /**
      * @brief Resets the ADC configuration and automatically enables the
-     * peripheral clock (also for the dma channel if used).
+     * peripheral clock.
      */
-    explicit InternalADC(ADC_TypeDef* adc, const float supplyVoltage = 5.0,
-                         const bool isUsingDMA   = false,
-                         DMA_Stream_TypeDef* dma = DMA1_Stream0);
+    explicit InternalADC(ADC_TypeDef* adc, const float supplyVoltage = 3.3);
 
     ~InternalADC();
 
     /**
      * @brief ADC Initialization.
      *
-     * The ADC clock must be set beforehand as well as GPIO configuration
-     * and DMA if used. Also the clock for the analog circuitry should be set
-     * accordingly to the device datasheet.
+     * The ADC clock must be set beforehand as well as GPIO configuration. Also
+     * the clock for the analog circuitry should be set accordingly to the
+     * device datasheet.
      */
     bool init() override;
 
-    /**
-     * @brief Make a regular conversion for the specified channel.
-     */
-    ADCData readChannel(Channel channel, SampleTime sampleTime = CYCLES_3);
+    bool selfTest() override;
 
-    bool enableChannel(Channel channel, SampleTime sampleTime = CYCLES_3);
+    InternalADCData sampleImpl() override;
 
-    bool addRegularChannel(Channel channel);
+    void enableChannel(Channel channel, SampleTime sampleTime = CYCLES_480);
 
-    InternalADCData getVoltage(Channel channel);
+    void disableChannel(Channel channel);
 
-    bool selfTest() override;
+    void enableTemperature(SampleTime sampleTime = CYCLES_480);
 
-    InternalADCData sampleImpl() override;
+    void disableTemperature();
 
-    float getSupplyVoltage();
+    void enableVbat(SampleTime sampleTime = CYCLES_480);
 
-private:
-    inline void resetRegisters();
+    void disableVbat();
 
-    inline void startInjectedConversion();
+    InternalADCData getVoltage(Channel channel);
 
-    inline void startRegularConversion();
+    TemperatureData getTemperature();
 
-    inline bool addInjectedChannel(Channel channel);
+    InternalADCData getVbatVoltage();
 
-    inline void setChannelSampleTime(Channel channel, SampleTime sampleTime);
+private:
+    void resetRegisters();
 
-    ADC_TypeDef* adc;
-    const float supplyVoltage = 5.0;
+    void setChannelSampleTime(Channel channel, SampleTime sampleTime);
 
-    uint8_t activeChannels = 0;
-    uint64_t timestamp     = 0;
+    float readChannel(Channel channel);
 
-    // Raw value used by DMA
-    uint16_t values[CH_NUM] = {};
+    ADC_TypeDef* adc;
+    const float supplyVoltage;
 
-    // Maps the channel number with the index in the ADC's regular sequence
-    int8_t indexMap[CH_NUM];
+    bool channelsEnabled[CH_NUM];
+    bool tempEnabled  = false;
+    bool vbatEnabled  = false;
+    bool vbatLastRead = false;
 
-    /**
-     * @brief Determines whether to use regular channels or injected channels
-     *
-     * We'll use up to 4 injected channel by default and up to 16 channels when
-     * using DMA.
-     *
-     * The differentiation is necessary because whiteout DMA it is much simpler
-     * to use injected channel for multichannel readings. Otherwise we would
-     * need to handle each channel's end of conversion interrupt or go through
-     */
-    const bool isUsingDMA;
-    DMA_Stream_TypeDef* dmaStream;
-    DMA_TypeDef* dma = DMA2;
-    uint8_t streamNum;
-    uint32_t transferCompleteMask;
-    uint32_t transferErrorMask;
-    volatile uint32_t* statusReg;
-    volatile uint32_t* clearFlagReg;
-
-    static constexpr int INJECTED_CHANNEL_N = 4;
-    static constexpr int RESOLUTION         = 4095;  ///< 12 bits
+    float channelsRawValues[CH_NUM];
+    float temperatureRawValue = 0;
+    float vbatVoltageRawValue = 0;
+    uint64_t timestamp        = 0;
+
+    static constexpr int RESOLUTION = 4095;  ///< 12 bits
 };
 
 }  // namespace Boardcore
diff --git a/src/shared/drivers/adc/InternalTemp.cpp b/src/shared/drivers/adc/InternalTemp.cpp
deleted file mode 100644
index a1b6765e54c845a3e7bc0095ef2732cc86ef5a28..0000000000000000000000000000000000000000
--- a/src/shared/drivers/adc/InternalTemp.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Giulia Ghirardini
- *
- * 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 "InternalTemp.h"
-
-#if defined(STM32F407xx) || defined(STM32F429xx)
-#define TEMP30_CAL_VALUE ((uint16_t*)((uint32_t)0x1FFF7A2C))
-#define TEMP110_CAL_VALUE ((uint16_t*)((uint32_t)0x1FFF7A2E))
-#define TEMP30 30
-#define TEMP110 110
-#elif defined(STM32F767xx) || defined(STM32F769xx)
-#define TEMP30_CAL_VALUE ((uint16_t*)((uint32_t)0x1FF0F44C))
-#define TEMP110_CAL_VALUE ((uint16_t*)((uint32_t)0x1FF0F44E))
-#define TEMP30 30
-#define TEMP110 110
-#else
-#warning This microcontroller does not have a calibrated temperature sensor or is not currently supported by this driver
-#define WITHOUT_CALIBRATION
-#endif
-
-namespace Boardcore
-{
-
-InternalTemp::InternalTemp(InternalADC::SampleTime sampleTime,
-                           const float supplyVoltage)
-    : adc(ADC1, supplyVoltage), sampleTime(sampleTime)
-{
-}
-
-bool InternalTemp::init()
-{
-    bool result = adc.init();
-
-#if defined(STM32F407xx) || defined(STM32F205xx)
-    adc.addRegularChannel(InternalADC::CH16);
-#elif defined(STM32F429xx) || defined(STM32F767xx) || defined(STM32F769xx)
-    adc.addRegularChannel(InternalADC::CH18);
-#endif
-
-    ADC->CCR &= ~ADC_CCR_VBATE;
-    ADC->CCR |= ADC_CCR_TSVREFE;
-
-    return result;
-}
-
-bool InternalTemp::selfTest() { return adc.selfTest(); }
-
-InternalTempData InternalTemp::sampleImpl()
-{
-#if defined(STM32F407xx) || defined(STM32F205xx)
-    auto adcData = adc.readChannel(InternalADC::CH16, sampleTime);
-#elif defined(STM32F429xx) || defined(STM32F767xx) || defined(STM32F769xx)
-    auto adcData = adc.readChannel(InternalADC::CH18, sampleTime);
-#endif
-
-    InternalTempData data;
-    data.temperatureTimestamp = adcData.voltageTimestamp;
-
-#ifdef WITHOUT_CALIBRATION
-    // Default conversion
-    data.temperature = ((adcData.voltage - 0.76) / 0.0025) + 25;
-#else
-    // Factory calibration
-    float voltage30  = static_cast<float>(*TEMP30_CAL_VALUE) * 3.3 / 4095;
-    float voltage110 = static_cast<float>(*TEMP110_CAL_VALUE) * 3.3 / 4095;
-    float slope      = (voltage110 - voltage30) / (TEMP110 - TEMP30);
-    data.temperature = ((adcData.voltage - voltage30) / slope) + TEMP30;
-#endif
-
-    return data;
-}
-
-}  // namespace Boardcore
diff --git a/src/shared/drivers/adc/InternalTemp.h b/src/shared/drivers/adc/InternalTemp.h
deleted file mode 100644
index a8324a5227385a8b933e9c48a58ce6632155beb1..0000000000000000000000000000000000000000
--- a/src/shared/drivers/adc/InternalTemp.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Giulia Ghirardini
- *
- * 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 <drivers/adc/InternalADC.h>
-
-#include "InternalTempData.h"
-
-namespace Boardcore
-{
-
-class InternalTemp : public Sensor<InternalTempData>
-{
-public:
-    explicit InternalTemp(
-        InternalADC::SampleTime sampleTime = InternalADC::CYCLES_480,
-        const float supplyVoltage          = 5.0);
-
-    bool init() override;
-
-    bool selfTest() override;
-
-    InternalTempData sampleImpl() override;
-
-    // InternalTempData addRegularChannel(InternalADC::Channel channel);
-
-private:
-    InternalADC adc;
-    InternalADC::SampleTime sampleTime;
-};
-
-}  // namespace Boardcore
diff --git a/src/shared/drivers/adc/InternalTempData.h b/src/shared/drivers/adc/InternalTempData.h
deleted file mode 100644
index 96f709033d5cdcadb60b4d3e53f19d2da7901b90..0000000000000000000000000000000000000000
--- a/src/shared/drivers/adc/InternalTempData.h
+++ /dev/null
@@ -1,42 +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 <sensors/SensorData.h>
-
-namespace Boardcore
-{
-
-struct InternalTempData : public TemperatureData
-{
-    InternalTempData() : TemperatureData{0, 0} {}
-
-    static std::string header() { return "timestamp,temperature\n"; }
-
-    void print(std::ostream& os) const
-    {
-        os << temperatureTimestamp << "," << temperature << "\n";
-    }
-};
-
-}  // namespace Boardcore
diff --git a/src/tests/drivers/test-internal-adc.cpp b/src/tests/drivers/test-internal-adc.cpp
index 351505ab0319d204b2de7dd34ee438158cf75af5..e4da6359da6feb10f247d34947455025e0f6526c 100644
--- a/src/tests/drivers/test-internal-adc.cpp
+++ b/src/tests/drivers/test-internal-adc.cpp
@@ -24,6 +24,7 @@
 #include <drivers/timer/TimestampTimer.h>
 #include <miosix.h>
 
+using namespace miosix;
 using namespace Boardcore;
 
 int main()
@@ -34,23 +35,26 @@ int main()
     // maximum frequency the analog circuitry supports and compare it with the
     // parent clock
 
-    InternalADC adc(ADC3, 3.3);
-    adc.enableChannel(InternalADC::CH4);  // PF6
+    InternalADC adc(ADC1, 3.3);
     adc.enableChannel(InternalADC::CH5);  // PF7
     adc.enableChannel(InternalADC::CH6);  // PF8
+    adc.enableTemperature();
+    adc.enableVbat();
     adc.init();
 
     printf("Configuration completed\n");
 
-    while (1)
+    while (true)
     {
         adc.sample();
 
-        printf("CH4:%1.3f\tCH5:%1.3f\tCH6:%1.3f\n",
-               adc.getVoltage(InternalADC::CH4).voltage,
-               adc.getVoltage(InternalADC::CH5).voltage,
-               adc.getVoltage(InternalADC::CH6).voltage);
+        printf(
+            "CH5: %1.3f\tCH6: %1.3f\tCH18: %1.3f\tTemp: %1.3f\tVbat: %1.3f\n",
+            adc.getVoltage(InternalADC::CH5).voltage,
+            adc.getVoltage(InternalADC::CH6).voltage,
+            adc.getVoltage(InternalADC::CH18).voltage,
+            adc.getTemperature().temperature, adc.getVbatVoltage().voltage);
 
-        miosix::delayMs(1000);
+        delayMs(1000);
     }
 }
diff --git a/src/tests/drivers/test-internal-temp.cpp b/src/tests/drivers/test-internal-temp.cpp
deleted file mode 100644
index f4da3994af806a68643a1bb3b014f1291058ccd0..0000000000000000000000000000000000000000
--- a/src/tests/drivers/test-internal-temp.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Giulia Ghirardini
- *
- * 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/adc/InternalTemp.h>
-#include <drivers/timer/TimestampTimer.h>
-#include <miosix.h>
-#include <utils/ClockUtils.h>
-
-using namespace Boardcore;
-
-int main()
-{
-    ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1;
-
-    InternalTemp temp(InternalADC::CYCLES_480, 3.0);
-    temp.init();
-
-    for (;;)
-    {
-        temp.sample();
-        printf("Temperature: %2.3f\n", temp.getLastSample().temperature);
-
-        miosix::delayMs(1000);
-    }
-}
diff --git a/src/tests/test-serial.cpp b/src/tests/test-serial.cpp
index 337f69a0075405d59fcfa50e36c02885c884563c..b1d27f59703eb56a8c43a8fb2abf2f036add5d3d 100644
--- a/src/tests/test-serial.cpp
+++ b/src/tests/test-serial.cpp
@@ -20,155 +20,20 @@
  * THE SOFTWARE.
  */
 
-// PA14 -> OSC32_IN
-// PA15 -> OSC32_OUT
-
 #include <miosix.h>
 
 using namespace miosix;
 
-GpioPin mco1(GPIOA_BASE, 8);
-GpioPin mco2(GPIOC_BASE, 9);
-
-typedef struct RTC_Date
-{
-    int day;
-    int month;
-    int year;
-} RTC_Date;
-typedef struct RTC_Time
-{
-    int hour;
-    int minutes;
-    int seconds;
-} RTC_Time;
-
 int main()
 {
-    // RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
-    // RCC_SYNC();
-
-    // mco1.alternateFunction(0);
-    // mco1.speed(Speed::_100MHz);
-    // mco2.alternateFunction(0);
-    // mco2.speed(Speed::_100MHz);
-    // // mco1.mode(Mode::OUTPUT);
-    // // mco1.high();
-
-    // RCC->CFGR |= RCC_CFGR_MCO1;
-    // RCC->CFGR |= RCC_CFGR_MCO2_1;
-
-    for (int i = 5; i > 0; i--)
-    {
-        printf("Waiting... %d seconds left\n", i);
-        Thread::sleep(1000);
-    }
-
-    // Enable clock to RTC and PWR peripherals
-    RCC->APB1ENR |= RCC_APB1ENR_RTCEN;
-    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
-    RCC_SYNC();
-
-    // Disable backup domain write protection
-    PWR->CR1 |= PWR_CR1_DBP;
-
-    // Enable the LSE clock
-    RCC->BDCR |= RCC_BDCR_LSEDRV;    // High drive
-    RCC->BDCR |= RCC_BDCR_LSEON;     // Enable LSE clock
-    RCC->BDCR |= RCC_BDCR_RTCSEL_0;  // LSE as RTC clock
-    RCC->BDCR |= RCC_BDCR_RTCEN;     // Enable RTC clock
-    RCC_SYNC();
-
-    while ((RCC->BDCR & RCC_BDCR_LSERDY) == 0)
-    {
-        printf("LSE clock status %d\n", (RCC->BDCR & RCC_BDCR_LSERDY) != 0);
-        Thread::sleep(250);
-    }
-    printf("LSE clock now stable\n");
-
-    // Enable access to RTC registers
-    RTC->WPR = 0xCA;
-    RTC->WPR = 0x53;
-
-    // Enter RTC initialization mode
-    RTC->ISR |= RTC_ISR_INIT;
-    while (!(RTC->ISR & RTC_ISR_INITF))
-    {
-        printf("Waiting for RTC to enter initialization mode 0x%lx\n",
-               RTC->ISR);
-        Thread::sleep(250);
-    }
-
-    // The RTC prescalers are configured by default for a 32.768KHz clock
-    // f_CK_APRE = f_RTCCLK  / (PREDIV_A + 1) = f_RTCCLK / (127 + 1)
-    // f_CK_SPRE = f_CK_APRE / (PREDIV_B + 1) = f_RTCCLK / (255 + 1)
-    RTC->PRER |= 0x7f7fff;
-
-    // Configure the time
-    uint32_t ht = 17 / 10;
-    uint32_t hu = 17 % 10;
-    uint32_t mt = 15 / 10;
-    uint32_t mu = 15 % 10;
-    uint32_t st = 30 / 10;
-    uint32_t su = 30 % 10;
-    RTC->TR     = ht << 20 | hu << 16 | mt << 12 | mu << 8 | st << 4 | su << 0;
-
-    // Configure the date
-    uint32_t yt = 23 / 10;
-    uint32_t yu = 23 % 10;
-    uint32_t wd = 1;
-    mt          = 3 / 10;
-    mu          = 3 % 10;
-    uint32_t dt = 27 / 10;
-    uint32_t du = 27 % 10;
-    RTC->DR =
-        yt << 20 | yu << 16 | wd << 13 | mt << 12 | mu << 8 | dt << 4 | du << 0;
-
-    // Exit RTC initialization mode
-    RTC->ISR &= ~RTC_ISR_INIT;
-
-    // Reactivate the write protection
-    RTC->WPR = 0xFF;
-    PWR->CR1 &= ~PWR_CR1_DBP;
-
-    Thread::sleep(1000);
-
-    RTC_Time time;
-    RTC_Date date;
-
     while (true)
     {
-        uint32_t rtc_tr = RTC->TR;
-        uint32_t rtc_dr = RTC->DR;
-
-        int hour_tens    = (rtc_tr & RTC_TR_HT_Msk) >> RTC_TR_HT_Pos;
-        int hour_units   = (rtc_tr & RTC_TR_HU_Msk) >> RTC_TR_HU_Pos;
-        time.hour        = (hour_tens * 10) + hour_units;
-        int mins_tens    = (rtc_tr & RTC_TR_MNT_Msk) >> RTC_TR_MNT_Pos;
-        int mins_units   = (rtc_tr & RTC_TR_MNU_Msk) >> RTC_TR_MNU_Pos;
-        time.minutes     = (mins_tens * 10) + mins_units;
-        int second_tens  = (rtc_tr & RTC_TR_ST_Msk) >> RTC_TR_ST_Pos;
-        int second_units = (rtc_tr & RTC_TR_SU_Msk) >> RTC_TR_SU_Pos;
-        time.seconds     = (second_tens * 10) + second_units;
-
-        int day_tens   = (rtc_dr & RTC_DR_DT_Msk) >> RTC_DR_DT_Pos;
-        int day_units  = (rtc_dr & RTC_DR_DU_Msk) >> RTC_DR_DU_Pos;
-        date.day       = (day_tens * 10) + day_units;
-        int month_tens = (rtc_dr & RTC_DR_MT_Msk) >> RTC_DR_MT_Pos;
-        int month_unit = (rtc_dr & RTC_DR_MU_Msk) >> RTC_DR_MU_Pos;
-        date.month     = (month_tens * 10) + month_unit;
-        int year_tens  = (rtc_dr & RTC_DR_YT_Msk) >> RTC_DR_YT_Pos;
-        int year_unit  = (rtc_dr & RTC_DR_YU_Msk) >> RTC_DR_YU_Pos;
-        date.year      = (year_tens * 10) + year_unit + 2000;
-
-        printf("%04d/%02d/%02d %02d:%02d:%02d\n", date.year, date.month,
-               date.day, time.hour, time.minutes, time.seconds);
+        ledOn();
+        printf("Serial is working!\n");
+        Thread::sleep(1000);
+        ledOff();
         Thread::sleep(1000);
     }
 
-    // while (true)
-    // {
-    //     printf("I'm alive!\n");
-    //     Thread::sleep(1000);
-    // }
+    return 0;
 }