diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53273bb8481f7b3ac5c2385fd4505fd29a0f90fe..21f28ce15cd369550ebb04743164d1fdf3927d72 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -176,7 +176,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 stm32f429zi_skyward_death_stack_x)
+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)
@@ -229,6 +229,9 @@ sbs_target(test-xbee-snd stm32f429zi_stm32f4discovery)
 add_executable(test-usart src/tests/drivers/usart/test-usart.cpp)
 sbs_target(test-usart stm32f407vg_stm32f4discovery)
 
+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 2fdea04d4a7b2628845b1bcfc62952433eee4e2b..2619281f0396c726a2cf9cb49df0d71731625f89 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -40,6 +40,7 @@ foreach(OPT_BOARD ${BOARDS})
         ${SBS_BASE}/src/shared/diagnostic/PrintLogger.cpp
 
         # Drivers
+        ${SBS_BASE}/src/shared/drivers/adc/InternalTemp.cpp
         ${SBS_BASE}/src/shared/drivers/adc/InternalADC.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/Canbus.cpp
         ${SBS_BASE}/src/shared/drivers/canbus/CanInterrupt.cpp
diff --git a/libs/mavlink-skyward-lib b/libs/mavlink-skyward-lib
index 94b57a3b5d5c200e69817066d69bc2e37cf963af..96329687cd3f44837978a91f94c84bf0a65e0d0b 160000
--- a/libs/mavlink-skyward-lib
+++ b/libs/mavlink-skyward-lib
@@ -1 +1 @@
-Subproject commit 94b57a3b5d5c200e69817066d69bc2e37cf963af
+Subproject commit 96329687cd3f44837978a91f94c84bf0a65e0d0b
diff --git a/libs/miosix-kernel b/libs/miosix-kernel
index 441aca88b44e5438e04abbef1639cd7f9df7bb37..45d44bfaf145f22262405760cb140b30587dbc70 160000
--- a/libs/miosix-kernel
+++ b/libs/miosix-kernel
@@ -1 +1 @@
-Subproject commit 441aca88b44e5438e04abbef1639cd7f9df7bb37
+Subproject commit 45d44bfaf145f22262405760cb140b30587dbc70
diff --git a/src/shared/drivers/adc/InternalTemp.cpp b/src/shared/drivers/adc/InternalTemp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89a7914062869ccf2ce41bf3e24b489284644527
--- /dev/null
+++ b/src/shared/drivers/adc/InternalTemp.cpp
@@ -0,0 +1,64 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: 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"
+
+#define TEMP110_CAL_VALUE ((uint16_t*)((uint32_t)0x1FFF7A2E))
+#define TEMP30_CAL_VALUE ((uint16_t*)((uint32_t)0x1FFF7A2C))
+#define TEMP110 110
+#define TEMP30 30
+
+namespace Boardcore
+{
+
+InternalTemp::InternalTemp(InternalADC::SampleTime sampleTime,
+                           const float supplyVoltage)
+    : adc(ADC1, supplyVoltage), sampleTime(sampleTime)
+{
+}
+
+bool InternalTemp::init()
+{
+    bool result = adc.init();
+    ADC->CCR |= ADC_CCR_TSVREFE;
+
+    return result;
+}
+
+bool InternalTemp::selfTest() { return adc.selfTest(); }
+
+InternalTempData InternalTemp::sampleImpl()
+{
+    auto adcData = adc.readChannel(InternalADC::CH16);
+    printf("%2.3f \n", adcData.voltage);
+    InternalTempData data;
+    data.temperatureTimestamp = adcData.voltageTimestamp;
+    data.temperature          = ((adcData.voltage - 0.76) / 0.0025) + 25;
+    // data.temperature          = (int32_t)(
+    //    (TEMP110 - TEMP30) /
+    //        ((float)(*TEMP110_CAL_VALUE) - (float)(*TEMP30_CAL_VALUE)) *
+    //        (adcData.voltage - (float)(*TEMP30_CAL_VALUE)) +
+    //    TEMP30);
+    return data;
+}
+
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/drivers/adc/InternalTemp.h b/src/shared/drivers/adc/InternalTemp.h
new file mode 100644
index 0000000000000000000000000000000000000000..280deb38eedd79cbb7ad8157f9193d8b7bb56b48
--- /dev/null
+++ b/src/shared/drivers/adc/InternalTemp.h
@@ -0,0 +1,51 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: 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_3,
+        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
new file mode 100644
index 0000000000000000000000000000000000000000..96f709033d5cdcadb60b4d3e53f19d2da7901b90
--- /dev/null
+++ b/src/shared/drivers/adc/InternalTempData.h
@@ -0,0 +1,42 @@
+/* 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..40bbf4f973f5a024f3911ab4e0b992089f7fbc3f 100644
--- a/src/tests/drivers/test-internal-adc.cpp
+++ b/src/tests/drivers/test-internal-adc.cpp
@@ -35,9 +35,9 @@ int main()
     // parent clock
 
     InternalADC adc(ADC3, 3.3);
-    adc.enableChannel(InternalADC::CH4);  // PF6
-    adc.enableChannel(InternalADC::CH5);  // PF7
-    adc.enableChannel(InternalADC::CH6);  // PF8
+    adc.enableChannel(InternalADC::CH18);  // PF6
+    adc.enableChannel(InternalADC::CH5);   // PF7
+    adc.enableChannel(InternalADC::CH6);   // PF8
     adc.init();
 
     printf("Configuration completed\n");
@@ -47,7 +47,7 @@ int main()
         adc.sample();
 
         printf("CH4:%1.3f\tCH5:%1.3f\tCH6:%1.3f\n",
-               adc.getVoltage(InternalADC::CH4).voltage,
+               adc.getVoltage(InternalADC::CH18).voltage,
                adc.getVoltage(InternalADC::CH5).voltage,
                adc.getVoltage(InternalADC::CH6).voltage);
 
diff --git a/src/tests/drivers/test-internal-temp.cpp b/src/tests/drivers/test-internal-temp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..209f8aadeaa16699b6ea429a282a99268d8f135f
--- /dev/null
+++ b/src/tests/drivers/test-internal-temp.cpp
@@ -0,0 +1,45 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: 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();
+    printf("Init done %ld\n",
+           ClockUtils::getAPBFrequency(ClockUtils::APB::APB2));
+
+    for (;;)
+    {
+        temp.sample();
+        printf("%2.3f test\n", temp.getLastSample().temperature);
+
+        miosix::delayMs(1000);
+    }
+}
\ No newline at end of file