diff --git a/software/pmc/src/adc.c b/software/pmc/src/adc.c
index 04f3a672..4bafb405 100644
--- a/software/pmc/src/adc.c
+++ b/software/pmc/src/adc.c
@@ -24,8 +24,17 @@ void adc_init()
     ADC_REGS->ADC_REFCTRL = ADC_REFCTRL_REFCOMP_Msk | ADC_REFCTRL_REFSEL_INT1V;
     ADC_REGS->ADC_CTRLB = ADC_CTRLB_PRESCALER_DIV4 | ADC_CTRLB_RESSEL_12BIT;
     ADC_REGS->ADC_CTRLA = ADC_CTRLA_ENABLE_Msk;
+    ADC_REGS->ADC_INPUTCTRL = ADC_INPUTCTRL_GAIN_1X | ADC_INPUTCTRL_MUXNEG_IOGND | ADC_INPUTCTRL_MUXPOS_BANDGAP;
+    ADC_REGS->ADC_SAMPCTRL = ADC_SAMPCTRL_SAMPLEN(0x3F);
 
     while(ADC_REGS->ADC_STATUS & ADC_STATUS_SYNCBUSY_Msk);
+
+    // Do a dummy read
+    ADC_REGS->ADC_SWTRIG = ADC_SWTRIG_START_Msk;
+
+    while(!(ADC_REGS->ADC_INTFLAG & ADC_INTFLAG_RESRDY_Msk));
+
+    REG_DISCARD(&(ADC_REGS->ADC_RESULT));
 }
 
 static uint16_t adc_read_iovdd()
@@ -43,19 +52,6 @@ static uint16_t adc_read_iovdd()
 
     return usResult;
 }
-uint32_t adc_get_iovdd()
-{
-    uint16_t usResult = adc_read_iovdd();
-
-    return (uint32_t)usResult * 1000UL * 4UL / 4095UL;
-}
-float adc_getf_iovdd()
-{
-    uint16_t usResult = adc_read_iovdd();
-    static const float fCorr = 1000.f / 4095.f * 4.f;
-
-    return (float)usResult * fCorr;
-}
 static uint16_t adc_read_corevdd()
 {
     ADC_REGS->ADC_INPUTCTRL = ADC_INPUTCTRL_GAIN_1X | ADC_INPUTCTRL_MUXNEG_GND | ADC_INPUTCTRL_MUXPOS_SCALEDCOREVCC;
@@ -71,19 +67,6 @@ static uint16_t adc_read_corevdd()
 
     return usResult;
 }
-uint32_t adc_get_corevdd()
-{
-    uint16_t usResult = adc_read_corevdd();
-
-    return (uint32_t)usResult * 1000UL * 4UL / 4095UL;
-}
-float adc_getf_corevdd()
-{
-    uint16_t usResult = adc_read_corevdd();
-    static const float fCorr = 1000.f / 4095.f * 4.f;
-
-    return (float)usResult * fCorr;
-}
 static uint16_t adc_read_vbus()
 {
     ADC_REGS->ADC_INPUTCTRL = ADC_INPUTCTRL_GAIN_1X | ADC_INPUTCTRL_MUXNEG_GND | ADC_INPUTCTRL_MUXPOS(ADC_VBUS_VSENSE_CHAN);
@@ -99,19 +82,6 @@ static uint16_t adc_read_vbus()
 
     return usResult;
 }
-uint32_t adc_get_vbus()
-{
-    uint16_t usResult = adc_read_vbus();
-
-    return (uint64_t)usResult * 1000UL * (ADC_VBUS_VSENSE_DIV_RT + ADC_VBUS_VSENSE_DIV_RB) / (ADC_VBUS_VSENSE_DIV_RB * 4095UL);
-}
-float adc_getf_vbus()
-{
-    uint16_t usResult = adc_read_vbus();
-    static const float fCorr = 1000.f / 4095.f / ADC_VBUS_VSENSE_DIV;
-
-    return (float)usResult * fCorr;
-}
 static uint16_t adc_read_vin()
 {
     ADC_REGS->ADC_INPUTCTRL = ADC_INPUTCTRL_GAIN_1X | ADC_INPUTCTRL_MUXNEG_GND | ADC_INPUTCTRL_MUXPOS(ADC_VIN_VSENSE_CHAN);
@@ -127,19 +97,6 @@ static uint16_t adc_read_vin()
 
     return usResult;
 }
-uint32_t adc_get_vin()
-{
-    uint16_t usResult = adc_read_vin();
-
-    return (uint64_t)usResult * 1000UL * (ADC_VIN_VSENSE_DIV_RT + ADC_VIN_VSENSE_DIV_RB) / (ADC_VIN_VSENSE_DIV_RB * 4095UL);
-}
-float adc_getf_vin()
-{
-    uint16_t usResult = adc_read_vin();
-    static const float fCorr = 1000.f / 4095.f / ADC_VIN_VSENSE_DIV;
-
-    return (float)usResult * fCorr;
-}
 static uint16_t adc_read_12v0()
 {
     ADC_REGS->ADC_INPUTCTRL = ADC_INPUTCTRL_GAIN_1X | ADC_INPUTCTRL_MUXNEG_GND | ADC_INPUTCTRL_MUXPOS(ADC_12V0_VSENSE_CHAN);
@@ -155,19 +112,6 @@ static uint16_t adc_read_12v0()
 
     return usResult;
 }
-uint32_t adc_get_12v0()
-{
-    uint16_t usResult = adc_read_12v0();
-
-    return (uint64_t)usResult * 1000UL * (ADC_12V0_VSENSE_DIV_RT + ADC_12V0_VSENSE_DIV_RB) / (ADC_12V0_VSENSE_DIV_RB * 4095UL);
-}
-float adc_getf_12v0()
-{
-    uint16_t usResult = adc_read_12v0();
-    static const float fCorr = 1000.f / 4095.f / ADC_12V0_VSENSE_DIV;
-
-    return (float)usResult * fCorr;
-}
 static uint16_t adc_read_vext()
 {
     ADC_REGS->ADC_INPUTCTRL = ADC_INPUTCTRL_GAIN_1X | ADC_INPUTCTRL_MUXNEG_GND | ADC_INPUTCTRL_MUXPOS(ADC_VEXT_VSENSE_CHAN);
@@ -183,19 +127,6 @@ static uint16_t adc_read_vext()
 
     return usResult;
 }
-uint32_t adc_get_vext()
-{
-    uint16_t usResult = adc_read_vext();
-
-    return (uint64_t)usResult * 1000UL * (ADC_VEXT_VSENSE_DIV_RT + ADC_VEXT_VSENSE_DIV_RB) / (ADC_VEXT_VSENSE_DIV_RB * 4095UL);
-}
-float adc_getf_vext()
-{
-    uint16_t usResult = adc_read_vext();
-    static const float fCorr = 1000.f / 4095.f / ADC_VEXT_VSENSE_DIV;
-
-    return (float)usResult * fCorr;
-}
 
 static uint16_t adc_read_temperature()
 {
@@ -212,51 +143,134 @@ static uint16_t adc_read_temperature()
 
     return usResult;
 }
-uint32_t adc_get_temperature()
+
+uint32_t adc_get_iovdd()
+{
+    uint16_t usResult = adc_read_iovdd();
+
+    return (uint32_t)usResult * 1000UL * 4UL / 4095UL;
+}
+uint32_t adc_get_corevdd()
+{
+    uint16_t usResult = adc_read_corevdd();
+
+    return (uint32_t)usResult * 1000UL * 4UL / 4095UL;
+}
+uint32_t adc_get_vbus()
+{
+    uint16_t usResult = adc_read_vbus();
+
+    return (uint64_t)usResult * 1000UL * (ADC_VBUS_VSENSE_DIV_RT + ADC_VBUS_VSENSE_DIV_RB) / (ADC_VBUS_VSENSE_DIV_RB * 4095UL);
+}
+uint32_t adc_get_vin()
+{
+    uint16_t usResult = adc_read_vin();
+
+    return (uint64_t)usResult * 1000UL * (ADC_VIN_VSENSE_DIV_RT + ADC_VIN_VSENSE_DIV_RB) / (ADC_VIN_VSENSE_DIV_RB * 4095UL);
+}
+uint32_t adc_get_12v0()
+{
+    uint16_t usResult = adc_read_12v0();
+
+    return (uint64_t)usResult * 1000UL * (ADC_12V0_VSENSE_DIV_RT + ADC_12V0_VSENSE_DIV_RB) / (ADC_12V0_VSENSE_DIV_RB * 4095UL);
+}
+uint32_t adc_get_vext()
+{
+    uint16_t usResult = adc_read_vext();
+
+    return (uint64_t)usResult * 1000UL * (ADC_VEXT_VSENSE_DIV_RT + ADC_VEXT_VSENSE_DIV_RB) / (ADC_VEXT_VSENSE_DIV_RB * 4095UL);
+}
+
+int32_t adc_get_temperature()
 {
     static uint8_t ubCalibInit = 0;
-    static uint32_t ulRoomTemp;
-    static uint32_t ulRoom1V0;
-    static uint32_t ulRoomVoltage;
-    static uint32_t ulHotTemp;
-    static uint32_t ulHot1V0;
-    static uint32_t ulHotVoltage;
+    static int32_t lRoomTemp;
+    static int32_t lRoom1V0;
+    static int32_t lRoomVoltage;
+    static int32_t lHotTemp;
+    static int32_t lHot1V0;
+    static int32_t lHotVoltage;
 
     if(!ubCalibInit)
     {
         uint8_t ubRoomTempInt = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_INT_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_INT_Pos;
         uint8_t ubRoomTempDec = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_DEC_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_DEC_Pos;
-        ulRoomTemp = (uint32_t)ubRoomTempInt * 1000UL + (uint32_t)ubRoomTempDec * 100UL;
+        lRoomTemp = (uint32_t)ubRoomTempInt * 1000UL + (uint32_t)ubRoomTempDec * 100UL;
 
         int8_t bRoom1V0 = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_INT1V_VAL_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_INT1V_VAL_Pos;
-        ulRoom1V0 = 1000UL - bRoom1V0;
+        lRoom1V0 = 1000UL - bRoom1V0;
 
         uint16_t usRoomADCCode = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_1 & FUSES_TEMP_LOG_WORD_1_NVMCTRL_ROOM_ADC_VAL_Msk) >> FUSES_TEMP_LOG_WORD_1_NVMCTRL_ROOM_ADC_VAL_Pos;
-        ulRoomVoltage = (uint32_t)usRoomADCCode * ulRoom1V0 / 4095UL;
+        lRoomVoltage = (uint32_t)usRoomADCCode * lRoom1V0 / 4095UL;
 
         uint8_t ubHotTempInt = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_HOT_TEMP_VAL_INT_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_HOT_TEMP_VAL_INT_Pos;
         uint8_t ubHotTempDec = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_HOT_TEMP_VAL_DEC_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_HOT_TEMP_VAL_DEC_Pos;
-        ulHotTemp = (uint32_t)ubHotTempInt * 1000UL + (uint32_t)ubHotTempDec * 100UL;
+        lHotTemp = (uint32_t)ubHotTempInt * 1000UL + (uint32_t)ubHotTempDec * 100UL;
 
         int8_t bHot1V0 = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_1 & FUSES_TEMP_LOG_WORD_1_NVMCTRL_HOT_INT1V_VAL_Msk) >> FUSES_TEMP_LOG_WORD_1_NVMCTRL_HOT_INT1V_VAL_Pos;
-        ulHot1V0 = 1000UL - bHot1V0;
+        lHot1V0 = 1000UL - bHot1V0;
 
         uint16_t usHotADCCode = (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_1 & FUSES_TEMP_LOG_WORD_1_NVMCTRL_HOT_ADC_VAL_Msk) >> FUSES_TEMP_LOG_WORD_1_NVMCTRL_HOT_ADC_VAL_Pos;
-        ulHotVoltage = (uint32_t)usHotADCCode * ulHot1V0 / 4095UL;
+        lHotVoltage = (uint32_t)usHotADCCode * lHot1V0 / 4095UL;
 
         ubCalibInit = 1;
     }
 
-    uint16_t usResult = adc_read_temperature();
+    int32_t lResult = (int32_t)adc_read_temperature();
+
+    int32_t lCoarseVoltage = lResult * 1000UL / 4095UL;
+    int32_t lCoarseTemp = (lCoarseVoltage - lRoomVoltage) * (lHotTemp - lRoomTemp) / (lHotVoltage - lRoomVoltage) + lRoomTemp;
+    int32_t l1V0 = (lHot1V0 - lRoom1V0) * (lCoarseTemp - lRoomTemp) / (lHotTemp - lRoomTemp) + lRoom1V0;
+    int32_t lFineVoltage = lResult * l1V0 / 4095UL;
+    int32_t lFineTemp = (lFineVoltage - lRoomVoltage) * (lHotTemp - lRoomTemp) / (lHotVoltage - lRoomVoltage) + lRoomTemp;
+
+    return lFineTemp;
+}
 
-    uint32_t ulCoarseVoltage = (uint32_t)usResult * 1000UL / 4095UL;
-    uint32_t ulCoarseTemp = (ulCoarseVoltage - ulRoomVoltage) * (ulHotTemp - ulRoomTemp) / (ulHotVoltage - ulRoomVoltage) + ulRoomTemp;
-    uint32_t ul1V0 = (ulHot1V0 - ulRoom1V0) * (ulCoarseTemp - ulRoomTemp) / (ulHotTemp - ulRoomTemp) + ulRoom1V0;
-    uint32_t ulFineVoltage = (uint32_t)usResult * ul1V0 / 4095UL;
-    uint32_t ulFineTemp = (ulFineVoltage - ulRoomVoltage) * (ulHotTemp - ulRoomTemp) / (ulHotVoltage - ulRoomVoltage) + ulRoomTemp;
+#ifndef ADC_DISABLE_FLOAT
+float adc_getf_iovdd()
+{
+    uint16_t usResult = adc_read_iovdd();
+    static const float fCorr = 1000.f / 4095.f * 4.f;
+
+    return (float)usResult * fCorr;
+}
+float adc_getf_corevdd()
+{
+    uint16_t usResult = adc_read_corevdd();
+    static const float fCorr = 1000.f / 4095.f * 4.f;
+
+    return (float)usResult * fCorr;
+}
+float adc_getf_vbus()
+{
+    uint16_t usResult = adc_read_vbus();
+    static const float fCorr = 1000.f / 4095.f / ADC_VBUS_VSENSE_DIV;
+
+    return (float)usResult * fCorr;
+}
+float adc_getf_vin()
+{
+    uint16_t usResult = adc_read_vin();
+    static const float fCorr = 1000.f / 4095.f / ADC_VIN_VSENSE_DIV;
 
-    return ulFineTemp;
+    return (float)usResult * fCorr;
 }
+float adc_getf_12v0()
+{
+    uint16_t usResult = adc_read_12v0();
+    static const float fCorr = 1000.f / 4095.f / ADC_12V0_VSENSE_DIV;
+
+    return (float)usResult * fCorr;
+}
+float adc_getf_vext()
+{
+    uint16_t usResult = adc_read_vext();
+    static const float fCorr = 1000.f / 4095.f / ADC_VEXT_VSENSE_DIV;
+
+    return (float)usResult * fCorr;
+}
+
 float adc_getf_temperature()
 {
     static uint8_t ubCalibInit = 0;
@@ -301,4 +315,5 @@ float adc_getf_temperature()
     float fFineTemp = (fFineVoltage - fRoomVoltage) * (fHotTemp - fRoomTemp) / (fHotVoltage - fRoomVoltage) + fRoomTemp;
 
     return fFineTemp;
-}
\ No newline at end of file
+}
+#endif
\ No newline at end of file
diff --git a/software/pmc/src/include/adc.h b/software/pmc/src/include/adc.h
index 648d535e..d0ad69b0 100644
--- a/software/pmc/src/include/adc.h
+++ b/software/pmc/src/include/adc.h
@@ -22,22 +22,28 @@
 #define ADC_12V0_VSENSE_CHAN    4 // PA04
 #define ADC_VEXT_VSENSE_CHAN    5 // PA05
 
+#define ADC_DISABLE_FLOAT 1
+
 void adc_init();
 
 uint32_t adc_get_iovdd();
-float adc_getf_iovdd();
 uint32_t adc_get_corevdd();
-float adc_getf_corevdd();
 uint32_t adc_get_vbus();
-float adc_getf_vbus();
 uint32_t adc_get_vin();
-float adc_getf_vin();
 uint32_t adc_get_12v0();
-float adc_getf_12v0();
 uint32_t adc_get_vext();
+
+int32_t adc_get_temperature();
+
+#ifndef ADC_DISABLE_FLOAT
+float adc_getf_iovdd();
+float adc_getf_corevdd();
+float adc_getf_vbus();
+float adc_getf_vin();
+float adc_getf_12v0();
 float adc_getf_vext();
 
-uint32_t adc_get_temperature();
 float adc_getf_temperature();
+#endif
 
 #endif // __ADC_H__
\ No newline at end of file
diff --git a/software/pmc/src/include/sercom.h b/software/pmc/src/include/sercom.h
index 7bcba796..464a842b 100644
--- a/software/pmc/src/include/sercom.h
+++ b/software/pmc/src/include/sercom.h
@@ -10,8 +10,10 @@
 #include "utils.h"
 #include "atomic.h"
 
-#define SERCOM_I2C_NORMAL 0
-#define SERCOM_I2C_FAST 1
+#define SERCOM_I2C_NORMAL 100000 // Hz
+#define SERCOM_I2C_FAST   400000 // Hz
+
+#define SERCOM_I2C_TRISE_DEFAULT 100 // ns
 
 #define SERCOM_I2C_RESTART 0
 #define SERCOM_I2C_STOP 1
@@ -31,7 +33,7 @@ typedef uint8_t (* sercom_i2c_slave_rx_data_isr_t)(uint8_t);
 #define SERCOM0_FIFO_SIZE            256   // Only relevant when in UART mode
 
 #ifdef SERCOM0_MODE_I2C_MASTER
-void sercom0_i2c_master_init(uint8_t ubMode);
+void sercom0_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise);
 uint8_t sercom0_i2c_master_transmit(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop);
 static inline uint8_t sercom0_i2c_master_write(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop)
 {
@@ -119,7 +121,7 @@ static inline void sercom0_uart_read(uint8_t *pubDst, uint32_t ulSize)
 #define SERCOM1_FIFO_SIZE            256   // Only relevant when in UART mode
 
 #ifdef SERCOM1_MODE_I2C_MASTER
-void sercom1_i2c_master_init(uint8_t ubMode);
+void sercom1_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise);
 uint8_t sercom1_i2c_master_transmit(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop);
 static inline uint8_t sercom1_i2c_master_write(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop)
 {
@@ -207,7 +209,7 @@ static inline void sercom1_uart_read(uint8_t *pubDst, uint32_t ulSize)
 #define SERCOM2_FIFO_SIZE            256   // Only relevant when in UART mode
 
 #ifdef SERCOM2_MODE_I2C_MASTER
-void sercom2_i2c_master_init(uint8_t ubMode);
+void sercom2_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise);
 uint8_t sercom2_i2c_master_transmit(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop);
 static inline uint8_t sercom2_i2c_master_write(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop)
 {
@@ -295,7 +297,7 @@ static inline void sercom2_uart_read(uint8_t *pubDst, uint32_t ulSize)
 #define SERCOM3_FIFO_SIZE            256   // Only relevant when in UART mode
 
 #ifdef SERCOM3_MODE_I2C_MASTER
-void sercom3_i2c_master_init(uint8_t ubMode);
+void sercom3_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise);
 uint8_t sercom3_i2c_master_transmit(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop);
 static inline uint8_t sercom3_i2c_master_write(uint8_t ubAddress, uint8_t *pubSrc, uint32_t ulCount, uint8_t ubStop)
 {
diff --git a/software/pmc/src/include/utils.h b/software/pmc/src/include/utils.h
index e8279e7e..ff9826f7 100644
--- a/software/pmc/src/include/utils.h
+++ b/software/pmc/src/include/utils.h
@@ -29,6 +29,21 @@
 // Macro to get the bit value
 #define BIT(x) (1 << (x))
 
+// Math
+#define ABS(x)                  ((x) < 0 ? (-(x)) : (x))
+#define D_ABS(x, y)             ((x) > (y) ? ((x) - (y)) : ((y) - (x)))
+#define MIN(x, y)               (((x) < (y)) ? (x) : (y))
+#define MIN_T(t, x, y)          (t)MIN((t)(x), (t)(y))
+#define MAX(x, y)               (((x) > (y)) ? (x) : (y))
+#define MAX_T(t, x, y)          (t)MAX((t)(x), (t)(y))
+#define CLAMP(x, min, max)      (MAX(MIN((x), (max)), (min)))
+#define CLAMP_T(t, x, min, max) (t)CLAMP((t)(x), (t)(min), (t)(max))
+#define DIV_CEIL(x,y)       (((x) + (y) - 1) / (y))
+#define DIV_ROUND(x, y)     (((x) + (y) / 2) / (y))
+
+// Array size
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+
 // Printf macros to print bits
 #define UINT8BITSTR         "%c%c%c%c%c%c%c%c"
 #define UINT16BITSTR        UINT8BITSTR UINT8BITSTR
@@ -39,7 +54,4 @@
 #define UINT322BITSTR(b)    UINT162BITSTR(((b) >> 16) & 0xFFFF), UINT162BITSTR(((b) >> 0) & 0xFFFF)
 #define UINT642BITSTR(b)    UINT322BITSTR(((b) >> 32) & 0xFFFFFFFF), UINT322BITSTR(((b) >> 0) & 0xFFFFFFFF)
 
-// Absolute value of
-#define ABS(a)      ((a) < 0 ? (-(a)) : (a))
-
 #endif  // __UTILS_H__
diff --git a/software/pmc/src/include/wdt.h b/software/pmc/src/include/wdt.h
index 242e4eb3..547cede5 100644
--- a/software/pmc/src/include/wdt.h
+++ b/software/pmc/src/include/wdt.h
@@ -13,8 +13,8 @@ void wdt_init(uint8_t ubTimeoutCycles, uint8_t ubWarningCycles);
 void wdt_enable();
 void wdt_disable();
 void wdt_set_warning_isr(wdt_warning_isr_t pfISR);
-float wdt_get_timeout_period();
-float wdt_get_warning_period();
+uint32_t wdt_get_timeout_period();
+uint32_t wdt_get_warning_period();
 void wdt_feed();
 
 #endif // __WDT_H__
\ No newline at end of file
diff --git a/software/pmc/src/main.c b/software/pmc/src/main.c
index 6b795c2a..66e62fca 100644
--- a/software/pmc/src/main.c
+++ b/software/pmc/src/main.c
@@ -289,7 +289,7 @@ void i2c_slave_register_init()
         I2C_SLAVE_REGISTER              (uint32_t, I2C_SLAVE_REGISTER_CORE_VOLTAGE)             = 0;
         I2C_SLAVE_REGISTER_WRITE_MASK   (uint32_t, I2C_SLAVE_REGISTER_CORE_VOLTAGE)             = 0x00000000;
         I2C_SLAVE_REGISTER_READ_MASK    (uint32_t, I2C_SLAVE_REGISTER_CORE_VOLTAGE)             = 0xFFFFFFFF;
-        I2C_SLAVE_REGISTER              (uint32_t, I2C_SLAVE_REGISTER_TEMP)                     = 0;
+        I2C_SLAVE_REGISTER              (int32_t,  I2C_SLAVE_REGISTER_TEMP)                     = 0;
         I2C_SLAVE_REGISTER_WRITE_MASK   (uint32_t, I2C_SLAVE_REGISTER_TEMP)                     = 0x00000000;
         I2C_SLAVE_REGISTER_READ_MASK    (uint32_t, I2C_SLAVE_REGISTER_TEMP)                     = 0xFFFFFFFF;
         I2C_SLAVE_REGISTER              (uint64_t, I2C_SLAVE_REGISTER_UPTIME)                   = g_ullSystemTick / 1000;
@@ -451,7 +451,7 @@ int init()
     adc_init(); // Init ADC
 
     sercom0_spi_init(1000000, 0, SERCOM_SPI_MSB_FIRST, 0, 2);
-    sercom1_i2c_master_init(SERCOM_I2C_FAST);
+    sercom1_i2c_master_init(SERCOM_I2C_FAST, SERCOM_I2C_TRISE_DEFAULT);
     sercom3_uart_init(500000, SERCOM_USART_INT_CTRLA_DORD_LSB | SERCOM_USART_INT_CTRLA_FORM_USART_FRAME_NO_PARITY | SERCOM_USART_INT_CTRLB_SBMODE_1_BIT | SERCOM_SPIM_CTRLB_CHSIZE_8_BIT, 0, 1);
 
     // sercom1_i2c_slave_init(I2C_SLAVE_ADDRESS); // Initialized as slave later, after LT7182S startup is done
@@ -473,43 +473,50 @@ int init()
     DBGPRINTLN_CTX("IcyRadio PMC v%lu (%s %s)!", BUILD_VERSION, __DATE__, __TIME__);
     DBGPRINTLN_CTX("Core: %s", szDeviceCoreName);
     DBGPRINTLN_CTX("Device: %s", szDeviceName);
-    DBGPRINTLN_CTX("Device Revision: %hhu", get_device_revision());
-    DBGPRINTLN_CTX("Calibration temperature: %hhu.%hhu C", (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_INT_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_INT_Pos, (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_DEC_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_DEC_Pos);
-    DBGPRINTLN_CTX("Flash Size: %hu kB", nvmctrl_get_flash_size() >> 10);
-    DBGPRINTLN_CTX("RAM Size: %hu kB", HRAMC0_SIZE >> 10);
-    DBGPRINTLN_CTX("Free RAM: %lu B", get_free_ram());
+    DBGPRINTLN_CTX("Revision: %hhu", get_device_revision());
+    DBGPRINTLN_CTX("Cal Temp: %hhu.%hhu C", (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_INT_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_INT_Pos, (TEMP_LOG_FUSES_REGS->FUSES_TEMP_LOG_WORD_0 & FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_DEC_Msk) >> FUSES_TEMP_LOG_WORD_0_NVMCTRL_ROOM_TEMP_VAL_DEC_Pos);
+    DBGPRINTLN_CTX("Flash: %hu kB", nvmctrl_get_flash_size() >> 10);
+    DBGPRINTLN_CTX("RAM: %hu kB (Free: %lu B)", HRAMC0_SIZE >> 10, get_free_ram());
     DBGPRINTLN_CTX("Unique ID: %08X-%08X-%08X-%08X", ulUniqueID[0], ulUniqueID[1], ulUniqueID[2], ulUniqueID[3]);
 
-    DBGPRINTLN_CTX("RMU - Reset cause: %s", pm_get_reset_reason_string(pm_get_reset_reason()));
+    DBGPRINTLN_CTX("RMU:");
+    DBGPRINTLN_CTX("  Reset cause: %s", pm_get_reset_reason_string(pm_get_reset_reason()));
 
-    DBGPRINTLN_CTX("ADC - IOVDD Voltage: %.2f (%u) mV", adc_getf_iovdd(), adc_get_iovdd());
-    DBGPRINTLN_CTX("ADC - Core Voltage: %.2f (%u) mV", adc_getf_corevdd(), adc_get_corevdd());
-    DBGPRINTLN_CTX("ADC - VIN Voltage: %.2f (%u) mV", adc_getf_vin(), adc_get_vin());
-    DBGPRINTLN_CTX("ADC - VEXT Voltage: %.2f (%u) mV", adc_getf_vext(), adc_get_vext());
-    DBGPRINTLN_CTX("ADC - 12V0 Voltage: %.2f (%u) mV", adc_getf_12v0(), adc_get_12v0());
-    DBGPRINTLN_CTX("ADC - VBUS Voltage: %.2f (%u) mV", adc_getf_vbus(), adc_get_vbus());
-    DBGPRINTLN_CTX("ADC - Temperature: %.2f (%u / 1000) C", adc_getf_temperature(), adc_get_temperature());
+    int32_t lTemp = adc_get_temperature();
+
+    DBGPRINTLN_CTX("ADC:");
+    DBGPRINTLN_CTX("  IOVDD: %u mV", adc_get_iovdd());
+    DBGPRINTLN_CTX("  Core: %u mV", adc_get_corevdd());
+    DBGPRINTLN_CTX("  VIN: %u mV", adc_get_vin());
+    DBGPRINTLN_CTX("  VEXT: %u mV", adc_get_vext());
+    DBGPRINTLN_CTX("  12V0: %u mV", adc_get_12v0());
+    DBGPRINTLN_CTX("  VBUS: %u mV", adc_get_vbus());
+    DBGPRINTLN_CTX("  Temp: %d.%02u C", lTemp / 1000, ABS(lTemp % 1000));
 
     nvmctrl_config_waitstates(PM_CPU_CLOCK_FREQ, adc_get_iovdd()); // Optimize flash wait states for frequency and voltage
 
-    DBGPRINTLN_CTX("SYSCTRL - XOSC Oscillator: %.3f MHz", (float)SYSCTRL_XOSC_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("SYSCTRL - OSC8M Oscillator: %.3f MHz", (float)SYSCTRL_OSC8M_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("SYSCTRL - DFLL48M Clock: %.3f MHz", (float)SYSCTRL_DFLL48M_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("PM - MAIN Clock: %.3f MHz", (float)PM_MAIN_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("PM - CPU Clock: %.3f MHz", (float)PM_CPU_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("PM - AHB Clock: %.3f MHz", (float)PM_AHB_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("PM - APBA Clock: %.3f MHz", (float)PM_APBA_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("PM - APBB Clock: %.3f MHz", (float)PM_APBB_CLOCK_FREQ / 1000000);
-    DBGPRINTLN_CTX("PM - APBC Clock: %.3f MHz", (float)PM_APBC_CLOCK_FREQ / 1000000);
+    DBGPRINTLN_CTX("SYSCTRL:");
+    DBGPRINTLN_CTX("  XOSC: %u Hz", SYSCTRL_XOSC_CLOCK_FREQ);
+    DBGPRINTLN_CTX("  OSC8M: %u Hz", SYSCTRL_OSC8M_CLOCK_FREQ);
+    DBGPRINTLN_CTX("  DFLL48M: %u Hz", SYSCTRL_DFLL48M_CLOCK_FREQ);
+
+    DBGPRINTLN_CTX("PM:");
+    DBGPRINTLN_CTX("  MAIN: %u Hz", PM_MAIN_CLOCK_FREQ);
+    DBGPRINTLN_CTX("  CPU: %u Hz", PM_CPU_CLOCK_FREQ);
+    DBGPRINTLN_CTX("  AHB: %u Hz", PM_AHB_CLOCK_FREQ);
+    DBGPRINTLN_CTX("  APBA: %u Hz", PM_APBA_CLOCK_FREQ);
+    DBGPRINTLN_CTX("  APBB: %u Hz", PM_APBB_CLOCK_FREQ);
+    DBGPRINTLN_CTX("  APBC: %u Hz", PM_APBC_CLOCK_FREQ);
 
-    DBGPRINTLN_CTX("WDT - Timeout period: %.3f ms", wdt_get_timeout_period());
-    DBGPRINTLN_CTX("WDT - Warning period: %.3f ms", wdt_get_warning_period());
+    DBGPRINTLN_CTX("WDT:");
+    DBGPRINTLN_CTX("  Timeout: %u ns", wdt_get_timeout_period());
+    DBGPRINTLN_CTX("  Warn: %u ns", wdt_get_warning_period());
 
-    DBGPRINTLN_CTX("Scanning I2C bus 1...");
+    // DBGPRINTLN_CTX("Scanning I2C bus 1...");
 
-    for(uint8_t a = 0x08; a < 0x78; a++)
-        if(sercom1_i2c_master_write(a, NULL, 0, SERCOM_I2C_STOP))
-            DBGPRINTLN_CTX("  Address 0x%02X ACKed!", a);
+    // for(uint8_t a = 0x08; a < 0x78; a++)
+    //     if(sercom1_i2c_master_write(a, NULL, 0, SERCOM_I2C_STOP))
+    //         DBGPRINTLN_CTX("  Address 0x%02X ACKed!", a);
 
     if(lt7182s_init())
         DBGPRINTLN_CTX("LT7182S init OK!");
@@ -527,23 +534,25 @@ int main()
     uint8_t ubMFRBuf[16];
     uint8_t ubMFRLen = 0;
 
+    DBGPRINTLN_CTX("LT7182S:");
+
     if(!lt7182s_read_mfr_id(szMFRBuf, sizeof(szMFRBuf)))
-        DBGPRINTLN_CTX("Failed to read LT7182S manufacturer IF!");
+        DBGPRINTLN_CTX("Failed to read manufacturer IF!");
     else
-        DBGPRINTLN_CTX("LT7182S MFR ID: %s", szMFRBuf);
+        DBGPRINTLN_CTX("  MFR ID: %s", szMFRBuf);
 
     if(!lt7182s_read_mfr_model(szMFRBuf, sizeof(szMFRBuf)))
-        DBGPRINTLN_CTX("Failed to read LT7182S manufacturer model!");
+        DBGPRINTLN_CTX("Failed to read manufacturer model!");
     else
-        DBGPRINTLN_CTX("LT7182S MFR Model: %s", szMFRBuf);
+        DBGPRINTLN_CTX("  MFR Model: %s", szMFRBuf);
 
     if(!(ubMFRLen = lt7182s_read_mfr_revision(ubMFRBuf, sizeof(ubMFRBuf))))
     {
-        DBGPRINTLN_CTX("Failed to read LT7182S manufacturer revision!");
+        DBGPRINTLN_CTX("Failed to read manufacturer revision!");
     }
     else
     {
-        DBGPRINT_CTX("LT7182S MFR Revision: ");
+        DBGPRINT_CTX("  MFR Revision: ");
 
         for(uint8_t i = 0; i < ubMFRLen; i++)
             DBGPRINT("%02X", ubMFRBuf[i]);
@@ -553,11 +562,11 @@ int main()
 
     if(!(ubMFRLen = lt7182s_read_mfr_serial(ubMFRBuf, sizeof(ubMFRBuf))))
     {
-        DBGPRINTLN_CTX("Failed to read LT7182S manufacturer serial!");
+        DBGPRINTLN_CTX("Failed to read manufacturer serial!");
     }
     else
     {
-        DBGPRINT_CTX("LT7182S MFR Serial: ");
+        DBGPRINT_CTX("  MFR Serial: ");
 
         for(uint8_t i = 0; i < ubMFRLen; i++)
             DBGPRINT("%02X", ubMFRBuf[i]);
@@ -565,7 +574,7 @@ int main()
         DBGPRINTLN("");
     }
 
-    DBGPRINTLN_CTX("LT7182S MFR Special ID: 0x%04X", lt7182s_read_mfr_special_id());
+    DBGPRINTLN_CTX("  MFR Special ID: 0x%04X", lt7182s_read_mfr_special_id());
 
     // Configure Channel 0
     lt7182s_set_operation(0, LT7182S_OPERATION_TURN_OFF_IMMED);
@@ -603,108 +612,78 @@ int main()
     lt7182s_set_vout_uv_warn(1, 0.95f); // 0.95 V Undervoltage Warning
     lt7182s_set_iout_oc_warn(1, 3.f); // 3 A Output Overcurrent Warning TODO: Up up
 
-    DBGPRINTLN_CTX("LT7182S Channel #0 VIN: %.3f V (Peak: %.3f V)", lt7182s_read_vin(0), lt7182s_read_vin_peak(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VIN ON Voltage: %.3f V", lt7182s_get_vin_on(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VIN OFF Voltage: %.3f V", lt7182s_get_vin_off(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VIN UV Warning Voltage: %.3f V", lt7182s_get_vin_uv_warn(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 IIN: %.3f A", lt7182s_read_iin(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 IIN OC Warning Current: %.3f A", lt7182s_get_iin_oc_warn(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VOUT: %.3f V (Set: %.3f V, Peak: %.3f V)", lt7182s_read_vout(0), lt7182s_get_vout(0), lt7182s_read_vout_peak(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 Maximum VOUT: %.3f V", lt7182s_get_vout_max(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 Margin High VOUT: %.3f V", lt7182s_get_vout_margin_high(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 Margin Low VOUT: %.3f V", lt7182s_get_vout_margin_low(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VOUT Transition Rate: %.3f V/ms", lt7182s_get_vout_transition_rate(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VOUT OV Fault Voltage: %.3f V", lt7182s_get_vout_ov_fault(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VOUT OV Warning Voltage: %.3f V", lt7182s_get_vout_ov_warn(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VOUT UV Fault Voltage: %.3f V", lt7182s_get_vout_uv_fault(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 VOUT UV Warning Voltage: %.3f V", lt7182s_get_vout_uv_warn(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 IOUT: %.3f A (Peak: %.3f A)", lt7182s_read_iout(0), lt7182s_read_iout_peak(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 IOUT OC Warning Current: %.3f A", lt7182s_get_iout_oc_warn(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 FREQ: %.3f kHz", lt7182s_read_freq(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 POUT: %.3f W", lt7182s_read_pout(0));
-    DBGPRINTLN_CTX("LT7182S Channel #0 ITH Voltage: %.3f V", lt7182s_read_ith(0));
-
-    uint16_t usPWMConfig = lt7182s_get_pwm_config(0);
-
-    DBGPRINTLN_CTX("LT7182S Channel #0 PWM Config:");
-    DBGPRINTLN_CTX("  LT7182S Channel #0 GMEA: %.3f uS", l17182s_pwm_config_parse_gmea(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #0 Positive ILIM: %.3f A", l17182s_pwm_config_parse_pos_ilim(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #0 Negative ILIM: %.3f A", l17182s_pwm_config_parse_neg_ilim(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #0 CITH: %.3f pF", l17182s_pwm_config_parse_cith(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #0 RITH: %.3f kOhm", l17182s_pwm_config_parse_rith(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #0 Pulse Skipping: %s", l17182s_pwm_config_parse_pulse_skipping(usPWMConfig) ? "Enabled" : "Disabled");
-    DBGPRINTLN_CTX("  LT7182S Channel #0 FCM at TOFF: %s", l17182s_pwm_config_parse_fcm_at_toff(usPWMConfig) ? "Enabled" : "Disabled");
-    DBGPRINTLN_CTX("  LT7182S Channel #0 Low VOUT Mode: %s", l17182s_pwm_config_parse_low_vout_mode(usPWMConfig) ? "Enabled" : "Disabled");
-    DBGPRINTLN_CTX("  LT7182S Channel #0 PWM Phase: %.3f deg", lt7182s_get_pwm_phase(0));
-    DBGPRINTLN_CTX("------------------------------");
-    DBGPRINTLN_CTX("LT7182S Channel #1 VIN: %.3f V (Peak: %.3f V)", lt7182s_read_vin(1), lt7182s_read_vin_peak(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VIN ON Voltage: %.3f V", lt7182s_get_vin_on(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VIN OFF Voltage: %.3f V", lt7182s_get_vin_off(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VIN UV Warning Voltage: %.3f V", lt7182s_get_vin_uv_warn(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 IIN: %.3f A", lt7182s_read_iin(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 IIN OC Warning Current: %.3f A", lt7182s_get_iin_oc_warn(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VOUT: %.3f V (Set: %.3f V, Peak: %.3f V)", lt7182s_read_vout(1), lt7182s_get_vout(1), lt7182s_read_vout_peak(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 Maximum VOUT: %.3f V", lt7182s_get_vout_max(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 Margin High VOUT: %.3f V", lt7182s_get_vout_margin_high(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 Margin Low VOUT: %.3f V", lt7182s_get_vout_margin_low(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VOUT Transition Rate: %.3f V/ms", lt7182s_get_vout_transition_rate(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VOUT OV Fault Voltage: %.3f V", lt7182s_get_vout_ov_fault(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VOUT OV Warning Voltage: %.3f V", lt7182s_get_vout_ov_warn(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VOUT UV Fault Voltage: %.3f V", lt7182s_get_vout_uv_fault(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 VOUT UV Warning Voltage: %.3f V", lt7182s_get_vout_uv_warn(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 IOUT: %.3f A (Peak: %.3f A)", lt7182s_read_iout(1), lt7182s_read_iout_peak(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 IOUT OC Warning Current: %.3f A", lt7182s_get_iout_oc_warn(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 FREQ: %.3f kHz", lt7182s_read_freq(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 POUT: %.3f W", lt7182s_read_pout(1));
-    DBGPRINTLN_CTX("LT7182S Channel #1 ITH Voltage: %.3f V", lt7182s_read_ith(1));
-
-    usPWMConfig = lt7182s_get_pwm_config(1);
-
-    DBGPRINTLN_CTX("LT7182S Channel #1 PWM Config:");
-    DBGPRINTLN_CTX("  LT7182S Channel #1 GMEA: %.3f uS", l17182s_pwm_config_parse_gmea(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #1 Positive ILIM: %.3f A", l17182s_pwm_config_parse_pos_ilim(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #1 Negative ILIM: %.3f A", l17182s_pwm_config_parse_neg_ilim(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #1 CITH: %.3f pF", l17182s_pwm_config_parse_cith(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #1 RITH: %.3f kOhm", l17182s_pwm_config_parse_rith(usPWMConfig));
-    DBGPRINTLN_CTX("  LT7182S Channel #1 Pulse Skipping: %s", l17182s_pwm_config_parse_pulse_skipping(usPWMConfig) ? "Enabled" : "Disabled");
-    DBGPRINTLN_CTX("  LT7182S Channel #1 FCM at TOFF: %s", l17182s_pwm_config_parse_fcm_at_toff(usPWMConfig) ? "Enabled" : "Disabled");
-    DBGPRINTLN_CTX("  LT7182S Channel #1 Low VOUT Mode: %s", l17182s_pwm_config_parse_low_vout_mode(usPWMConfig) ? "Enabled" : "Disabled");
-    DBGPRINTLN_CTX("  LT7182S Channel #1 PWM Phase: %.3f deg", lt7182s_get_pwm_phase(1));
-    DBGPRINTLN_CTX("------------------------------");
-    DBGPRINTLN_CTX("LT7182S EXTVCC Voltage: %.3f V", lt7182s_read_extvcc());
-    DBGPRINTLN_CTX("LT7182S Temperature: %.3f C (Peak: %.3f C)", lt7182s_read_temperature(), lt7182s_read_temperature_peak());
-    DBGPRINTLN_CTX("LT7182S OT Fault Temperature: %.3f C", lt7182s_get_ot_fault());
-    DBGPRINTLN_CTX("LT7182S OT Warning Temperature: %.3f C", lt7182s_get_ot_warn());
-    DBGPRINTLN_CTX("LT7182S Frequency: %.3f kHz", lt7182s_get_freq());
+    for(uint8_t i = 0; i < 2; i++)
+    {
+        DBGPRINTLN_CTX("  Channel #%hhu:", i);
+        DBGPRINTLN_CTX("    VIN: %.3f V (Pk: %.3f V)", lt7182s_read_vin(i), lt7182s_read_vin_peak(i));
+        DBGPRINTLN_CTX("    VIN ON: %.3f V", lt7182s_get_vin_on(i));
+        DBGPRINTLN_CTX("    VIN OFF: %.3f V", lt7182s_get_vin_off(i));
+        DBGPRINTLN_CTX("    VIN UV Warn: %.3f V", lt7182s_get_vin_uv_warn(i));
+        DBGPRINTLN_CTX("    IIN: %.3f A", lt7182s_read_iin(i));
+        DBGPRINTLN_CTX("    IIN OC Warn: %.3f A", lt7182s_get_iin_oc_warn(i));
+        DBGPRINTLN_CTX("    VOUT: %.3f V (Set: %.3f V, Pk: %.3f V)", lt7182s_read_vout(i), lt7182s_get_vout(i), lt7182s_read_vout_peak(i));
+        DBGPRINTLN_CTX("    VOUT Max: %.3f V", lt7182s_get_vout_max(i));
+        DBGPRINTLN_CTX("    VOUT Mg High: %.3f V", lt7182s_get_vout_margin_high(i));
+        DBGPRINTLN_CTX("    VOUT Mg Low: %.3f V", lt7182s_get_vout_margin_low(i));
+        DBGPRINTLN_CTX("    VOUT Tr Rate: %.3f V/ms", lt7182s_get_vout_transition_rate(i));
+        DBGPRINTLN_CTX("    VOUT OV Fault: %.3f V", lt7182s_get_vout_ov_fault(i));
+        DBGPRINTLN_CTX("    VOUT OV Warn: %.3f V", lt7182s_get_vout_ov_warn(i));
+        DBGPRINTLN_CTX("    VOUT UV Fault: %.3f V", lt7182s_get_vout_uv_fault(i));
+        DBGPRINTLN_CTX("    VOUT UV Warn: %.3f V", lt7182s_get_vout_uv_warn(i));
+        DBGPRINTLN_CTX("    IOUT: %.3f A (Pk: %.3f A)", lt7182s_read_iout(i), lt7182s_read_iout_peak(i));
+        DBGPRINTLN_CTX("    IOUT OC Warn: %.3f A", lt7182s_get_iout_oc_warn(i));
+        DBGPRINTLN_CTX("    FREQ: %.3f kHz", lt7182s_read_freq(i));
+        DBGPRINTLN_CTX("    POUT: %.3f W", lt7182s_read_pout(i));
+        DBGPRINTLN_CTX("    VITH: %.3f V", lt7182s_read_ith(i));
+
+        uint16_t usPWMConfig = lt7182s_get_pwm_config(i);
+
+        DBGPRINTLN_CTX("    PWM Config:");
+        DBGPRINTLN_CTX("      GMEA: %.3f uS", l17182s_pwm_config_parse_gmea(usPWMConfig));
+        DBGPRINTLN_CTX("      ILIM Pos: %.3f A", l17182s_pwm_config_parse_pos_ilim(usPWMConfig));
+        DBGPRINTLN_CTX("      ILIM Neg: %.3f A", l17182s_pwm_config_parse_neg_ilim(usPWMConfig));
+        DBGPRINTLN_CTX("      CITH: %.3f pF", l17182s_pwm_config_parse_cith(usPWMConfig));
+        DBGPRINTLN_CTX("      RITH: %.3f kOhm", l17182s_pwm_config_parse_rith(usPWMConfig));
+        DBGPRINTLN_CTX("      Pulse Skip: %s", l17182s_pwm_config_parse_pulse_skipping(usPWMConfig) ? "Enabled" : "Disabled");
+        DBGPRINTLN_CTX("      FCM at TOFF: %s", l17182s_pwm_config_parse_fcm_at_toff(usPWMConfig) ? "Enabled" : "Disabled");
+        DBGPRINTLN_CTX("      Low VOUT: %s", l17182s_pwm_config_parse_low_vout_mode(usPWMConfig) ? "Enabled" : "Disabled");
+        DBGPRINTLN_CTX("      Phase: %.3f deg", lt7182s_get_pwm_phase(i));
+    }
+
+    DBGPRINTLN_CTX("  EXTVCC: %.3f V", lt7182s_read_extvcc());
+    DBGPRINTLN_CTX("  Temp: %.3f C (Pk: %.3f C)", lt7182s_read_temperature(), lt7182s_read_temperature_peak());
+    DBGPRINTLN_CTX("  OT Fault Temp: %.3f C", lt7182s_get_ot_fault());
+    DBGPRINTLN_CTX("  OT Warn Temp: %.3f C", lt7182s_get_ot_warn());
+    DBGPRINTLN_CTX("  Freq: %.3f kHz", lt7182s_get_freq());
 
     //// UPD test
-    UPD350_SELECT();
-    delay_ms(50);
-    UPD350_UNSELECT();
-    delay_ms(500);
+    // UPD350_SELECT();
+    // delay_ms(50);
+    // UPD350_UNSELECT();
+    // delay_ms(500);
 
-    uint8_t buf[8];
+    // uint8_t buf[8];
 
-    UPD350_SELECT();
+    // UPD350_SELECT();
 
-    sercom0_spi_write_byte(0x0B, 0);
+    // sercom0_spi_write_byte(0x0B, 0);
 
-    sercom0_spi_write_byte(0x00, 0);
-    sercom0_spi_write_byte(0x00, 0);
+    // sercom0_spi_write_byte(0x00, 0);
+    // sercom0_spi_write_byte(0x00, 0);
 
-    sercom0_spi_write_byte(0x00, 1);
+    // sercom0_spi_write_byte(0x00, 1);
 
-    sercom0_spi_read(buf, 8, 0x00);
+    // sercom0_spi_read(buf, 8, 0x00);
 
-    UPD350_UNSELECT();
+    // UPD350_UNSELECT();
 
-    DBGPRINTLN_CTX("UPD350 ID: 0x%08X", *(uint32_t *)buf);
-    DBGPRINTLN_CTX("UPD350 USB Vendor ID: 0x%04X", *(uint16_t *)(buf + 4));
-    DBGPRINTLN_CTX("UPD350 USB Product ID: 0x%04X", *(uint16_t *)(buf + 6));
+    // DBGPRINTLN_CTX("UPD350 ID: 0x%08X", *(uint32_t *)buf);
+    // DBGPRINTLN_CTX("UPD350 USB Vendor ID: 0x%04X", *(uint16_t *)(buf + 4));
+    // DBGPRINTLN_CTX("UPD350 USB Product ID: 0x%04X", *(uint16_t *)(buf + 6));
     //// End UPD test
 
-    DBGPRINTLN_CTX("Enabling power source in 500 ms...");
-    delay_ms(500);
+    DBGPRINTLN_CTX("Checking power sources in 100 ms...");
+    delay_ms(100);
 
     if(!pmc_check_input_voltages())
     {
@@ -725,7 +704,7 @@ int main()
     DBGPRINTLN_CTX("Enabling 5V0 and 1V0 in 100 ms...");
     delay_ms(100);
 
-    lt7182s_set_operation(1, LT7182S_OPERATION_ON);
+    lt7182s_set_operation(1, LT7182S_OPERATION_ON); // First enable 1V0, it is held off by the 5V0 PG line
     lt7182s_set_operation(0, LT7182S_OPERATION_ON);
     delay_ms(50);
 
@@ -748,24 +727,25 @@ int main()
             lt7182s_set_operation(0, LT7182S_OPERATION_TURN_OFF_IMMED);
             lt7182s_set_operation(1, LT7182S_OPERATION_TURN_OFF_IMMED);
 
-            DBGPRINTLN_CTX("LT7182S Channel #%hhu failed to turn on! Status word: 0x%04X", i, usStatus);
+            DBGPRINTLN_CTX("LT7182S Channel #%hhu failed to turn on!", i);
 
-            DBGPRINTLN_CTX("  VOUT Status: 0x%02X", ubStatusVOUT);
-            DBGPRINTLN_CTX("  IOUT Status: 0x%02X", ubStatusIOUT);
-            DBGPRINTLN_CTX("  Input Status: 0x%02X", ubStatusInput);
-            DBGPRINTLN_CTX("  Temperature Status: 0x%02X", ubStatusTemperature);
-            DBGPRINTLN_CTX("  CML Status: 0x%02X", ubStatusCML);
-            DBGPRINTLN_CTX("  MFR Specific Status: 0x%02X", ubStatusMFRSpecific);
-            DBGPRINTLN_CTX("  MFR Common Status: 0x%02X", ubStatusMFRCommon);
-            DBGPRINTLN_CTX("  MFR Pads Status: 0x%04X", usStatusMFRPads);
-            DBGPRINTLN_CTX("  MFR Pin Config Status: 0x%02X", ubStatusMFRPinConfig);
+            DBGPRINTLN_CTX("Status (0x%04X):", usStatus);
+            DBGPRINTLN_CTX("  VOUT: 0x%02X", ubStatusVOUT);
+            DBGPRINTLN_CTX("  IOUT: 0x%02X", ubStatusIOUT);
+            DBGPRINTLN_CTX("  Input: 0x%02X", ubStatusInput);
+            DBGPRINTLN_CTX("  Temperature: 0x%02X", ubStatusTemperature);
+            DBGPRINTLN_CTX("  CML: 0x%02X", ubStatusCML);
+            DBGPRINTLN_CTX("  MFR Specific: 0x%02X", ubStatusMFRSpecific);
+            DBGPRINTLN_CTX("  MFR Common: 0x%02X", ubStatusMFRCommon);
+            DBGPRINTLN_CTX("  MFR Pads: 0x%04X", usStatusMFRPads);
+            DBGPRINTLN_CTX("  MFR Pin Config: 0x%02X", ubStatusMFRPinConfig);
 
             // TODO: Further investigate status bits to determine the cause
 
             while(1)
             {
                 LED_TOGGLE();
-                delay_ms(100);
+                delay_ms(500);
             }
         }
     }
@@ -775,19 +755,15 @@ int main()
 
     sercom1_i2c_slave_init(I2C_SLAVE_ADDRESS);
 
-    DBGPRINTLN_CTX("Configuring FPGA in 500 ms...");
-    delay_ms(500);
+    DBGPRINTLN_CTX("Releasing FPGA init in 100 ms...");
+    delay_ms(100);
 
-    FPGA_INIT_DEASSERT();
-    delay_ms(50);
-    FPGA_INIT_ASSERT();
-    delay_ms(50);
     FPGA_INIT_DEASSERT();
 
     if(!PCIE_12V0_STATUS()) // CM4 supported only when not using the PCIe slot
     {
-        DBGPRINTLN_CTX("Turning CM4 ON in 500 ms...");
-        delay_ms(500);
+        DBGPRINTLN_CTX("Turning CM4 ON in 200 ms...");
+        delay_ms(200);
 
         //CM4_USB_OTG_DEVICE();
         //CM4_BTLDR_ENABLE();
@@ -821,9 +797,6 @@ int main()
     {
         wdt_feed();
 
-        if(!NO_VOLTAGE_ENABLED() && pmc_check_input_voltages())
-            DBGPRINTLN_CTX("Power source changed to: %s", VEXT_STATUS() ? "VEXT" : PCIE_12V0_STATUS() ? "PCIe 12V0" : VBUS_SNK_STATUS() ? "VBUS" : "UNKNOWN");
-
         // if(I2C_SLAVE_REGISTER(uint8_t, I2C_SLAVE_REGISTER_CONFIG) & BIT(7))
         // {
         //     delay_ms(20);
@@ -840,17 +813,28 @@ int main()
         }
 
         static uint64_t ullLastHeartBeat = 0;
+        static uint64_t ullLastPowerSourceChange = 0;
         static uint64_t ullLastCM4Check = 0;
         static uint64_t ullLastTelemetryUpdate = 0;
 
-        if((g_ullSystemTick > 0 && ullLastHeartBeat == 0) || g_ullSystemTick - ullLastHeartBeat > 1000)
+        if((g_ullSystemTick > 0 && ullLastHeartBeat == 0) || g_ullSystemTick - ullLastHeartBeat > 5000)
         {
             ullLastHeartBeat = g_ullSystemTick;
 
             LED_TOGGLE();
 
             if(LED_STATUS())
-                ullLastHeartBeat -= 900;
+                ullLastHeartBeat -= 4900;
+        }
+
+        if((g_ullSystemTick > 0 && ullLastPowerSourceChange == 0) || g_ullSystemTick - ullLastPowerSourceChange > 1000)
+        {
+            if(!NO_VOLTAGE_ENABLED() && pmc_check_input_voltages())
+            {
+                DBGPRINTLN_CTX("Power source changed to: %s", VEXT_STATUS() ? "VEXT" : PCIE_12V0_STATUS() ? "PCIe 12V0" : VBUS_SNK_STATUS() ? "VBUS" : "UNKNOWN");
+
+                ullLastPowerSourceChange = g_ullSystemTick; // Hysteresis
+            }
         }
 
         if((g_ullSystemTick > 0 && ullLastCM4Check == 0) || g_ullSystemTick - ullLastCM4Check > 500)
@@ -892,16 +876,13 @@ int main()
             ullLastTelemetryUpdate = g_ullSystemTick;
 
             // System Temperatures
-            uint32_t ulTemp = adc_get_temperature();
+            int32_t lTemp = adc_get_temperature();
 
             ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
             {
-                I2C_SLAVE_REGISTER(uint32_t, I2C_SLAVE_REGISTER_TEMP) = ulTemp;
+                I2C_SLAVE_REGISTER(int32_t, I2C_SLAVE_REGISTER_TEMP) = lTemp;
             }
 
-            // DBGPRINTLN_CTX("----------------------------------");
-            // DBGPRINTLN_CTX("Temperature: %lu.%02lu C", ulTemp / 1000, ulTemp % 1000);
-
             // System Voltages/Currents
             uint32_t ulIOVDD = adc_get_iovdd();
             uint32_t ulCoreVDD = adc_get_corevdd();
@@ -916,35 +897,32 @@ int main()
                 I2C_SLAVE_REGISTER(uint32_t, I2C_SLAVE_REGISTER_CORE_VOLTAGE) = ulCoreVDD;
             }
 
-            // DBGPRINTLN_CTX("----------------------------------");
-            // DBGPRINTLN_CTX("IOVDD Voltage: %u mV", ulIOVDD);
-            // DBGPRINTLN_CTX("Core Voltage: %u mV", ulCoreVDD);
-            // DBGPRINTLN_CTX("VIN Voltage: %u mV", ulVIN);
-            // DBGPRINTLN_CTX("VEXT Voltage: %u mV", ulVEXT);
-            // DBGPRINTLN_CTX("12V0 Voltage: %u mV", ul12V0);
-            // DBGPRINTLN_CTX("VBUS Voltage: %u mV", ulVBUS);
-
-            // DBGPRINTLN_CTX("------------------------------");
-            // DBGPRINTLN_CTX("LT7182S Channel #0 VIN: %.3f V (Peak: %.3f V)", lt7182s_read_vin(0), lt7182s_read_vin_peak(0));
-            // DBGPRINTLN_CTX("LT7182S Channel #0 IIN: %.3f A", lt7182s_read_iin(0));
-            // DBGPRINTLN_CTX("LT7182S Channel #0 VOUT: %.3f V (Set: %.3f V, Peak: %.3f V)", lt7182s_read_vout(0), lt7182s_get_vout(0), lt7182s_read_vout_peak(0));
-            // DBGPRINTLN_CTX("LT7182S Channel #0 IOUT: %.3f A (Peak: %.3f A)", lt7182s_read_iout(0), lt7182s_read_iout_peak(0));
-            // DBGPRINTLN_CTX("LT7182S Channel #0 FREQ: %.3f kHz", lt7182s_read_freq(0));
-            // DBGPRINTLN_CTX("LT7182S Channel #0 POUT: %.3f W", lt7182s_read_pout(0));
-            // DBGPRINTLN_CTX("LT7182S Channel #0 ITH Voltage: %.3f V", lt7182s_read_ith(0));
-            // DBGPRINTLN_CTX("------------------------------");
-            // DBGPRINTLN_CTX("LT7182S Channel #1 VIN: %.3f V (Peak: %.3f V)", lt7182s_read_vin(1), lt7182s_read_vin_peak(1));
-            // DBGPRINTLN_CTX("LT7182S Channel #1 IIN: %.3f A", lt7182s_read_iin(1));
-            // DBGPRINTLN_CTX("LT7182S Channel #1 VOUT: %.3f V (Set: %.3f V, Peak: %.3f V)", lt7182s_read_vout(1), lt7182s_get_vout(1), lt7182s_read_vout_peak(1));
-            // DBGPRINTLN_CTX("LT7182S Channel #1 IOUT: %.3f A (Peak: %.3f A)", lt7182s_read_iout(1), lt7182s_read_iout_peak(1));
-            // DBGPRINTLN_CTX("LT7182S Channel #1 FREQ: %.3f kHz", lt7182s_read_freq(1));
-            // DBGPRINTLN_CTX("LT7182S Channel #1 POUT: %.3f W", lt7182s_read_pout(1));
-            // DBGPRINTLN_CTX("LT7182S Channel #1 ITH Voltage: %.3f V", lt7182s_read_ith(1));
-            // DBGPRINTLN_CTX("------------------------------");
-            // DBGPRINTLN_CTX("LT7182S EXTVCC Voltage: %.3f V", lt7182s_read_extvcc());
-            // DBGPRINTLN_CTX("LT7182S Temperature: %.3f C (Peak: %.3f C)", lt7182s_read_temperature(), lt7182s_read_temperature_peak());
-            // DBGPRINTLN_CTX("------------------------------");
-            // DBGPRINTLN_CTX("RPi CM4 State: %s", CM4_RUNNING() ? "Running" : "OFF");
+            // DBGPRINTLN("------------------------------");
+            // DBGPRINTLN("IOVDD: %u mV", ulIOVDD);
+            // DBGPRINTLN("Core: %u mV", ulCoreVDD);
+            // DBGPRINTLN("VIN: %u mV", ulVIN);
+            // DBGPRINTLN("VEXT: %u mV", ulVEXT);
+            // DBGPRINTLN("12V0: %u mV", ul12V0);
+            // DBGPRINTLN("VBUS: %u mV", ulVBUS);
+            // DBGPRINTLN("Temp: %d.%02u C", lTemp / 1000, ABS(lTemp % 1000));
+
+            // DBGPRINTLN("LT7182S:");
+
+            // for(uint8_t i = 0; i < 2; i++)
+            // {
+            //     DBGPRINTLN("  Channel #%hhu:", i);
+            //     DBGPRINTLN("    VIN: %.3f V (Pk: %.3f V)", lt7182s_read_vin(i), lt7182s_read_vin_peak(i));
+            //     DBGPRINTLN("    IIN: %.3f A", lt7182s_read_iin(i));
+            //     DBGPRINTLN("    VOUT: %.3f V (Set: %.3f V, Pk: %.3f V)", lt7182s_read_vout(i), lt7182s_get_vout(i), lt7182s_read_vout_peak(i));
+            //     DBGPRINTLN("    IOUT: %.3f A (Pk: %.3f A)", lt7182s_read_iout(i), lt7182s_read_iout_peak(i));
+            //     DBGPRINTLN("    FREQ: %.3f kHz", lt7182s_read_freq(i));
+            //     DBGPRINTLN("    POUT: %.3f W", lt7182s_read_pout(i));
+            //     DBGPRINTLN("    VITH: %.3f V", lt7182s_read_ith(i));
+            // }
+
+            // DBGPRINTLN("  EXTVCC: %.3f V", lt7182s_read_extvcc());
+            // DBGPRINTLN("  Temp: %.3f C (Pk: %.3f C)", lt7182s_read_temperature(), lt7182s_read_temperature_peak());
+            // DBGPRINTLN("CM4: %s", CM4_RUNNING() ? "Running" : "OFF");
         }
     }
 
diff --git a/software/pmc/src/sercom.c b/software/pmc/src/sercom.c
index 5eac17b7..52c656cc 100644
--- a/software/pmc/src/sercom.c
+++ b/software/pmc/src/sercom.c
@@ -7,7 +7,7 @@ void _putchar(char ch)
 
 
 #ifdef SERCOM0_MODE_I2C_MASTER
-void sercom0_i2c_master_init(uint8_t ubMode)
+void sercom0_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise)
 {
     pm_apbc_clock_gate(PM_APBCMASK_SERCOM0_Msk, 1);
 
@@ -19,19 +19,20 @@ void sercom0_i2c_master_init(uint8_t ubMode)
 
     SERCOM0_REGS->I2CM.SERCOM_CTRLA = SERCOM_I2CM_CTRLA_LOWTOUT_Msk | SERCOM_I2CM_CTRLA_INACTOUT_55US | SERCOM_I2CM_CTRLA_SDAHOLD_450NS | SERCOM_I2CM_CTRLA_MODE_I2C_MASTER;
 
-    float fI2CFreq = ubMode == SERCOM_I2C_FAST ? 400000.f : 100000.f;
-    float fSrcFreq = (float)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM0_CORE_Val];
-    float fBaud = (fSrcFreq / fI2CFreq) - ((fSrcFreq * (100.0f / 1000000000.0f)) + 10.0f);
-    uint32_t ulBaud = (uint32_t)fBaud;
+    uint32_t ulSourceFreq = GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM0_CORE_Val];
+    int32_t lBaud = (10U * ulSourceFreq / ulSCLFreq) - 100;
 
-    if(ulBaud > (0xFF * 2))
-        ulBaud = 0xFF;
-    else if(ulBaud <= 1)
-        ulBaud = 1;
-    else
-        ulBaud /= 2;
+    if(ulTRise)
+        lBaud -= ulSourceFreq / (100000000U / ulTRise);
+
+    lBaud = DIV_ROUND(lBaud, 20);
+
+    if(lBaud > 255)
+        lBaud = 255;
+    else if(lBaud <= 1)
+        lBaud = 1;
 
-    SERCOM0_REGS->I2CM.SERCOM_BAUD = ulBaud;
+    SERCOM0_REGS->I2CM.SERCOM_BAUD = (uint8_t)lBaud;
 
     while(SERCOM0_REGS->I2CM.SERCOM_STATUS & SERCOM_I2CM_STATUS_SYNCBUSY_Msk);
 
@@ -305,7 +306,14 @@ void sercom0_uart_init(uint32_t ulBaud, uint32_t ulFrameSettings, uint8_t ubRXPO
 
     SERCOM0_REGS->USART_INT.SERCOM_CTRLA = (ulFrameSettings & SERCOM_USART_INT_CTRLA_DORD_Msk) | SERCOM_USART_INT_CTRLA_CMODE_ASYNC | (ulFrameSettings & SERCOM_USART_INT_CTRLA_FORM_Msk) | SERCOM_USART_INT_CTRLA_RXPO(ubRXPO) | SERCOM_USART_INT_CTRLA_TXPO(ubTXPO) | SERCOM_USART_INT_CTRLA_MODE_USART_INT_CLK;
     SERCOM0_REGS->USART_INT.SERCOM_CTRLB = SERCOM_USART_INT_CTRLB_RXEN_Msk | SERCOM_USART_INT_CTRLB_TXEN_Msk | (ulFrameSettings & SERCOM_USART_INT_CTRLB_PMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_SBMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_CHSIZE_Msk);
-    SERCOM0_REGS->USART_INT.SERCOM_BAUD = (uint16_t)(65536 * (1 - 16 * ((float)ulBaud / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM0_CORE_Val])));
+
+    uint64_t ullBaud = 65536U * 100U - 65536ULL * 16ULL * 100ULL * (uint64_t)ulBaud / (uint64_t)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM0_CORE_Val];
+    ullBaud = DIV_ROUND(ullBaud, 100);
+
+    if(ullBaud > 65535)
+        ullBaud = 65535;
+
+    SERCOM0_REGS->USART_INT.SERCOM_BAUD = (uint16_t)ullBaud;
 
     while(SERCOM0_REGS->USART_INT.SERCOM_STATUS & SERCOM_USART_INT_STATUS_SYNCBUSY_Msk);
 
@@ -372,7 +380,7 @@ void _sercom0_isr()
 
 
 #ifdef SERCOM1_MODE_I2C_MASTER
-void sercom1_i2c_master_init(uint8_t ubMode)
+void sercom1_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise)
 {
     pm_apbc_clock_gate(PM_APBCMASK_SERCOM1_Msk, 1);
 
@@ -384,19 +392,20 @@ void sercom1_i2c_master_init(uint8_t ubMode)
 
     SERCOM1_REGS->I2CM.SERCOM_CTRLA = SERCOM_I2CM_CTRLA_LOWTOUT_Msk | SERCOM_I2CM_CTRLA_INACTOUT_55US | SERCOM_I2CM_CTRLA_SDAHOLD_450NS | SERCOM_I2CM_CTRLA_MODE_I2C_MASTER;
 
-    float fI2CFreq = ubMode == SERCOM_I2C_FAST ? 400000.f : 100000.f;
-    float fSrcFreq = (float)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM1_CORE_Val];
-    float fBaud = (fSrcFreq / fI2CFreq) - ((fSrcFreq * (100.0f / 1000000000.0f)) + 10.0f);
-    uint32_t ulBaud = (uint32_t)fBaud;
+    uint32_t ulSourceFreq = GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM1_CORE_Val];
+    int32_t lBaud = (10U * ulSourceFreq / ulSCLFreq) - 100;
 
-    if(ulBaud > (0xFF * 2))
-        ulBaud = 0xFF;
-    else if(ulBaud <= 1)
-        ulBaud = 1;
-    else
-        ulBaud /= 2;
+    if(ulTRise)
+        lBaud -= ulSourceFreq / (100000000U / ulTRise);
+
+    lBaud = DIV_ROUND(lBaud, 20);
+
+    if(lBaud > 255)
+        lBaud = 255;
+    else if(lBaud <= 1)
+        lBaud = 1;
 
-    SERCOM1_REGS->I2CM.SERCOM_BAUD = ulBaud;
+    SERCOM1_REGS->I2CM.SERCOM_BAUD = (uint8_t)lBaud;
 
     while(SERCOM1_REGS->I2CM.SERCOM_STATUS & SERCOM_I2CM_STATUS_SYNCBUSY_Msk);
 
@@ -670,7 +679,14 @@ void sercom1_uart_init(uint32_t ulBaud, uint32_t ulFrameSettings, uint8_t ubRXPO
 
     SERCOM1_REGS->USART_INT.SERCOM_CTRLA = (ulFrameSettings & SERCOM_USART_INT_CTRLA_DORD_Msk) | SERCOM_USART_INT_CTRLA_CMODE_ASYNC | (ulFrameSettings & SERCOM_USART_INT_CTRLA_FORM_Msk) | SERCOM_USART_INT_CTRLA_RXPO(ubRXPO) | SERCOM_USART_INT_CTRLA_TXPO(ubTXPO) | SERCOM_USART_INT_CTRLA_MODE_USART_INT_CLK;
     SERCOM1_REGS->USART_INT.SERCOM_CTRLB = SERCOM_USART_INT_CTRLB_RXEN_Msk | SERCOM_USART_INT_CTRLB_TXEN_Msk | (ulFrameSettings & SERCOM_USART_INT_CTRLB_PMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_SBMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_CHSIZE_Msk);
-    SERCOM1_REGS->USART_INT.SERCOM_BAUD = (uint16_t)(65536 * (1 - 16 * ((float)ulBaud / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM1_CORE_Val])));
+
+    uint64_t ullBaud = 65536U * 100U - 65536ULL * 16ULL * 100ULL * (uint64_t)ulBaud / (uint64_t)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM1_CORE_Val];
+    ullBaud = DIV_ROUND(ullBaud, 100);
+
+    if(ullBaud > 65535)
+        ullBaud = 65535;
+
+    SERCOM1_REGS->USART_INT.SERCOM_BAUD = (uint16_t)ullBaud;
 
     while(SERCOM1_REGS->USART_INT.SERCOM_STATUS & SERCOM_USART_INT_STATUS_SYNCBUSY_Msk);
 
@@ -737,7 +753,7 @@ void _sercom1_isr()
 
 
 #ifdef SERCOM2_MODE_I2C_MASTER
-void sercom2_i2c_master_init(uint8_t ubMode)
+void sercom2_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise)
 {
     pm_apbc_clock_gate(PM_APBCMASK_SERCOM2_Msk, 1);
 
@@ -749,19 +765,20 @@ void sercom2_i2c_master_init(uint8_t ubMode)
 
     SERCOM2_REGS->I2CM.SERCOM_CTRLA = SERCOM_I2CM_CTRLA_LOWTOUT_Msk | SERCOM_I2CM_CTRLA_INACTOUT_55US | SERCOM_I2CM_CTRLA_SDAHOLD_450NS | SERCOM_I2CM_CTRLA_MODE_I2C_MASTER;
 
-    float fI2CFreq = ubMode == SERCOM_I2C_FAST ? 400000.f : 100000.f;
-    float fSrcFreq = (float)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM2_CORE_Val];
-    float fBaud = (fSrcFreq / fI2CFreq) - ((fSrcFreq * (100.0f / 1000000000.0f)) + 10.0f);
-    uint32_t ulBaud = (uint32_t)fBaud;
+    uint32_t ulSourceFreq = GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM2_CORE_Val];
+    int32_t lBaud = (10U * ulSourceFreq / ulSCLFreq) - 100;
 
-    if(ulBaud > (0xFF * 2))
-        ulBaud = 0xFF;
-    else if(ulBaud <= 1)
-        ulBaud = 1;
-    else
-        ulBaud /= 2;
+    if(ulTRise)
+        lBaud -= ulSourceFreq / (100000000U / ulTRise);
+
+    lBaud = DIV_ROUND(lBaud, 20);
+
+    if(lBaud > 255)
+        lBaud = 255;
+    else if(lBaud <= 1)
+        lBaud = 1;
 
-    SERCOM2_REGS->I2CM.SERCOM_BAUD = ulBaud;
+    SERCOM2_REGS->I2CM.SERCOM_BAUD = (uint8_t)lBaud;
 
     while(SERCOM2_REGS->I2CM.SERCOM_STATUS & SERCOM_I2CM_STATUS_SYNCBUSY_Msk);
 
@@ -1035,7 +1052,14 @@ void sercom2_uart_init(uint32_t ulBaud, uint32_t ulFrameSettings, uint8_t ubRXPO
 
     SERCOM2_REGS->USART_INT.SERCOM_CTRLA = (ulFrameSettings & SERCOM_USART_INT_CTRLA_DORD_Msk) | SERCOM_USART_INT_CTRLA_CMODE_ASYNC | (ulFrameSettings & SERCOM_USART_INT_CTRLA_FORM_Msk) | SERCOM_USART_INT_CTRLA_RXPO(ubRXPO) | SERCOM_USART_INT_CTRLA_TXPO(ubTXPO) | SERCOM_USART_INT_CTRLA_MODE_USART_INT_CLK;
     SERCOM2_REGS->USART_INT.SERCOM_CTRLB = SERCOM_USART_INT_CTRLB_RXEN_Msk | SERCOM_USART_INT_CTRLB_TXEN_Msk | (ulFrameSettings & SERCOM_USART_INT_CTRLB_PMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_SBMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_CHSIZE_Msk);
-    SERCOM2_REGS->USART_INT.SERCOM_BAUD = (uint16_t)(65536 * (1 - 16 * ((float)ulBaud / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM2_CORE_Val])));
+
+    uint64_t ullBaud = 65536U * 100U - 65536ULL * 16ULL * 100ULL * (uint64_t)ulBaud / (uint64_t)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM2_CORE_Val];
+    ullBaud = DIV_ROUND(ullBaud, 100);
+
+    if(ullBaud > 65535)
+        ullBaud = 65535;
+
+    SERCOM2_REGS->USART_INT.SERCOM_BAUD = (uint16_t)ullBaud;
 
     while(SERCOM2_REGS->USART_INT.SERCOM_STATUS & SERCOM_USART_INT_STATUS_SYNCBUSY_Msk);
 
@@ -1102,7 +1126,7 @@ void _sercom2_isr()
 
 
 #ifdef SERCOM3_MODE_I2C_MASTER
-void sercom3_i2c_master_init(uint8_t ubMode)
+void sercom3_i2c_master_init(uint32_t ulSCLFreq, uint32_t ulTRise)
 {
     pm_apbc_clock_gate(PM_APBCMASK_SERCOM3_Msk, 1);
 
@@ -1114,19 +1138,20 @@ void sercom3_i2c_master_init(uint8_t ubMode)
 
     SERCOM3_REGS->I2CM.SERCOM_CTRLA = SERCOM_I2CM_CTRLA_LOWTOUT_Msk | SERCOM_I2CM_CTRLA_INACTOUT_55US | SERCOM_I2CM_CTRLA_SDAHOLD_450NS | SERCOM_I2CM_CTRLA_MODE_I2C_MASTER;
 
-    float fI2CFreq = ubMode == SERCOM_I2C_FAST ? 400000.f : 100000.f;
-    float fSrcFreq = (float)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM3_CORE_Val];
-    float fBaud = (fSrcFreq / fI2CFreq) - ((fSrcFreq * (100.0f / 1000000000.0f)) + 10.0f);
-    uint32_t ulBaud = (uint32_t)fBaud;
+    uint32_t ulSourceFreq = GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM3_CORE_Val];
+    int32_t lBaud = (10U * ulSourceFreq / ulSCLFreq) - 100;
 
-    if(ulBaud > (0xFF * 2))
-        ulBaud = 0xFF;
-    else if(ulBaud <= 1)
-        ulBaud = 1;
-    else
-        ulBaud /= 2;
+    if(ulTRise)
+        lBaud -= ulSourceFreq / (100000000U / ulTRise);
+
+    lBaud = DIV_ROUND(lBaud, 20);
+
+    if(lBaud > 255)
+        lBaud = 255;
+    else if(lBaud <= 1)
+        lBaud = 1;
 
-    SERCOM3_REGS->I2CM.SERCOM_BAUD = ulBaud;
+    SERCOM3_REGS->I2CM.SERCOM_BAUD = (uint8_t)lBaud;
 
     while(SERCOM3_REGS->I2CM.SERCOM_STATUS & SERCOM_I2CM_STATUS_SYNCBUSY_Msk);
 
@@ -1400,7 +1425,14 @@ void sercom3_uart_init(uint32_t ulBaud, uint32_t ulFrameSettings, uint8_t ubRXPO
 
     SERCOM3_REGS->USART_INT.SERCOM_CTRLA = (ulFrameSettings & SERCOM_USART_INT_CTRLA_DORD_Msk) | SERCOM_USART_INT_CTRLA_CMODE_ASYNC | (ulFrameSettings & SERCOM_USART_INT_CTRLA_FORM_Msk) | SERCOM_USART_INT_CTRLA_RXPO(ubRXPO) | SERCOM_USART_INT_CTRLA_TXPO(ubTXPO) | SERCOM_USART_INT_CTRLA_MODE_USART_INT_CLK;
     SERCOM3_REGS->USART_INT.SERCOM_CTRLB = SERCOM_USART_INT_CTRLB_RXEN_Msk | SERCOM_USART_INT_CTRLB_TXEN_Msk | (ulFrameSettings & SERCOM_USART_INT_CTRLB_PMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_SBMODE_Msk) | (ulFrameSettings & SERCOM_USART_INT_CTRLB_CHSIZE_Msk);
-    SERCOM3_REGS->USART_INT.SERCOM_BAUD = (uint16_t)(65536 * (1 - 16 * ((float)ulBaud / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM3_CORE_Val])));
+
+    uint64_t ullBaud = 65536U * 100U - 65536ULL * 16ULL * 100ULL * (uint64_t)ulBaud / (uint64_t)GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_SERCOM3_CORE_Val];
+    ullBaud = DIV_ROUND(ullBaud, 100);
+
+    if(ullBaud > 65535)
+        ullBaud = 65535;
+
+    SERCOM3_REGS->USART_INT.SERCOM_BAUD = (uint16_t)ullBaud;
 
     while(SERCOM3_REGS->USART_INT.SERCOM_STATUS & SERCOM_USART_INT_STATUS_SYNCBUSY_Msk);
 
diff --git a/software/pmc/src/wdt.c b/software/pmc/src/wdt.c
index 8c9a23c6..41048e3b 100644
--- a/software/pmc/src/wdt.c
+++ b/software/pmc/src/wdt.c
@@ -64,17 +64,17 @@ void wdt_set_warning_isr(wdt_warning_isr_t pfISR)
 {
     pfWarningISR = pfISR;
 }
-float wdt_get_timeout_period()
+uint32_t wdt_get_timeout_period()
 {
     uint8_t ubPeriod = (WDT_REGS->WDT_CONFIG & WDT_CONFIG_PER_Msk) >> WDT_CONFIG_PER_Pos;
 
-    return (float)(1 << (ubPeriod + 3)) / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_WDT_Val] * 1000.f;
+    return (1000000000ULL * (1 << (ubPeriod + 3))) / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_WDT_Val];
 }
-float wdt_get_warning_period()
+uint32_t wdt_get_warning_period()
 {
     uint8_t ubPeriod = (WDT_REGS->WDT_EWCTRL & WDT_EWCTRL_EWOFFSET_Msk) >> WDT_EWCTRL_EWOFFSET_Pos;
 
-    return (float)(1 << (ubPeriod + 3)) / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_WDT_Val] * 1000.f;
+    return (1000000000ULL * (1 << (ubPeriod + 3))) / GCLK_CLOCK_FREQ[GCLK_CLKCTRL_ID_WDT_Val];
 }
 void wdt_feed()
 {