diff --git a/Boards/M5stackCore2/CMakeLists.txt b/Boards/M5stackCore2/CMakeLists.txt
index 6746e3cb..a81836b5 100644
--- a/Boards/M5stackCore2/CMakeLists.txt
+++ b/Boards/M5stackCore2/CMakeLists.txt
@@ -1,5 +1,5 @@
 idf_component_register(
-    SRC_DIRS "Source" "Source/hal" "Source/ft6x36"
+    SRC_DIRS "Source" "Source/hal" "Source/ft6x36" "Source/axp192"
     INCLUDE_DIRS "Source"
-    REQUIRES Tactility esp_lvgl_port esp_lcd esp_lcd_ili9341 driver vfs fatfs M5Unified
+    REQUIRES Tactility esp_lvgl_port esp_lcd esp_lcd_ili9341 driver vfs fatfs
 )
diff --git a/Boards/M5stackCore2/Source/InitBoot.cpp b/Boards/M5stackCore2/Source/InitBoot.cpp
index 9a7a8285..143c3dfd 100644
--- a/Boards/M5stackCore2/Source/InitBoot.cpp
+++ b/Boards/M5stackCore2/Source/InitBoot.cpp
@@ -3,7 +3,9 @@
 #include <intr_types.h>
 #include "Log.h"
 #include "hal/Core2DisplayConstants.h"
-#include "M5Unified/src/utility/AXP192_Class.hpp"
+#include "axp192/axp192.h"
+#include "hal/i2c/I2c.h"
+#include "CoreDefines.h"
 
 #define TAG "core2"
 
@@ -11,8 +13,15 @@
 #define CORE2_SPI2_PIN_MOSI GPIO_NUM_23
 #define CORE2_SPI2_PIN_MISO GPIO_NUM_38
 
-m5::I2C_Class i2c;
-m5::AXP192_Class axpDevice(0x34, 400000, &i2c);
+axp192_t axpDevice;
+
+static int32_t axpI2cRead(TT_UNUSED void* handle, uint8_t address, uint8_t reg, uint8_t* buffer, uint16_t size) {
+    return tt::hal::i2c::masterRead(I2C_NUM_0, address, reg, buffer, size, 50 / portTICK_PERIOD_MS);
+}
+
+static int32_t axpI2cWrite(TT_UNUSED void* handle, uint8_t address, uint8_t reg, const uint8_t* buffer, uint16_t size) {
+    return tt::hal::i2c::masterWrite(I2C_NUM_0, address, reg, buffer, size, 50 / portTICK_PERIOD_MS);
+}
 
 static bool initSpi2() {
     TT_LOG_I(TAG, LOG_MESSAGE_SPI_INIT_START_FMT, SPI2_HOST);
@@ -41,23 +50,20 @@ static bool initSpi2() {
 }
 
 bool initAxp() {
-    if (!i2c.begin(I2C_NUM_0, GPIO_NUM_21, GPIO_NUM_22)) {
-        TT_LOG_E(TAG, "I2C init failed");
-        return false;
-    }
+    axpDevice.read = axpI2cRead;
+    axpDevice.write = axpI2cWrite;
 
-    if (!axpDevice.begin()) {
-        TT_LOG_E(TAG, "AXP init failed");
-        return false;
-    }
+    axp192_ioctl(&axpDevice, AXP192_LDO2_SET_VOLTAGE, 3300); // LCD + SD
+    axp192_ioctl(&axpDevice, AXP192_LDO3_SET_VOLTAGE, 0); // VIB_MOTOR STOP
+    axp192_ioctl(&axpDevice, AXP192_DCDC3_SET_VOLTAGE, 3300);
 
-    axpDevice.setLDO2(3300); // LCD + SD peripheral power supply
-    axpDevice.setLDO3(0); // VIB_MOTOR STOP
-    axpDevice.setGPIO2(false); // SPEAKER STOP
-    axpDevice.writeRegister8(0x9A, 255); // PWM 255 (LED OFF)
-    axpDevice.writeRegister8(0x92, 0x02); // GPIO1 PWM
-    axpDevice.setChargeCurrent(390); // Core2 battery = 390mAh
-    axpDevice.setDCDC3(3300);
+    axp192_ioctl(&axpDevice, AXP192_LDO2_ENABLE);
+    axp192_ioctl(&axpDevice, AXP192_LDO3_DISABLE);
+    axp192_ioctl(&axpDevice, AXP192_DCDC3_ENABLE);
+
+    axp192_write(&axpDevice, AXP192_PWM1_DUTY_CYCLE_2, 255); // PWM 255 (LED OFF)
+    axp192_write(&axpDevice, AXP192_GPIO1_CONTROL, 0x02); // GPIO1 PWM
+    // TODO: We could charge at 390mA according to the M5Unified code, but the AXP driver in M5Unified limits to 132mA, so it's unclear what the AXP supports.
 
     return true;
 }
diff --git a/Boards/M5stackCore2/Source/axp192/LICENSE b/Boards/M5stackCore2/Source/axp192/LICENSE
new file mode 100644
index 00000000..bf31dcf4
--- /dev/null
+++ b/Boards/M5stackCore2/Source/axp192/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2019-2021 Mika Tuupola
+
+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.
\ No newline at end of file
diff --git a/Boards/M5stackCore2/Source/axp192/README.md b/Boards/M5stackCore2/Source/axp192/README.md
new file mode 100644
index 00000000..c2110a24
--- /dev/null
+++ b/Boards/M5stackCore2/Source/axp192/README.md
@@ -0,0 +1,4 @@
+From https://github.com/tuupola/axp192
+
+Changed some code for Tactility:
+- `axp192_write()` now accepts a byte value instead of a byte pointer
\ No newline at end of file
diff --git a/Boards/M5stackCore2/Source/axp192/axp192.c b/Boards/M5stackCore2/Source/axp192/axp192.c
new file mode 100644
index 00000000..04a3f0d8
--- /dev/null
+++ b/Boards/M5stackCore2/Source/axp192/axp192.c
@@ -0,0 +1,440 @@
+/*
+
+MIT License
+
+Copyright (c) 2019-2021 Mika Tuupola
+
+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.
+
+-cut-
+
+This file is part of hardware agnostic I2C driver for AXP192:
+https://github.com/tuupola/axp192
+
+SPDX-License-Identifier: MIT
+Version: 0.6.0
+
+*/
+
+#include <stdarg.h>
+#include <stdint.h>
+
+#include "axp192_config.h"
+#include "axp192.h"
+
+static axp192_err_t read_coloumb_counter(const axp192_t *axp, float *buffer);
+static axp192_err_t read_battery_power(const axp192_t *axp, float *buffer);
+
+static const axp192_init_command_t init_commands[] = {
+#ifdef AXP192_INCLUDE_SDKCONFIG_H
+    /* Currently you have to use menuconfig to be able to use axp192_init() */
+    {AXP192_DCDC1_VOLTAGE, {CONFIG_AXP192_DCDC1_VOLTAGE}, 1},
+    {AXP192_DCDC3_VOLTAGE, {CONFIG_AXP192_DCDC3_VOLTAGE}, 1},
+    {AXP192_LDO23_VOLTAGE, {CONFIG_AXP192_LDO23_VOLTAGE}, 1},
+    {AXP192_GPIO0_LDOIO0_VOLTAGE, {CONFIG_AXP192_GPIO0_LDOIO0_VOLTAGE}, 1},
+    {AXP192_DCDC13_LDO23_CONTROL, {CONFIG_AXP192_DCDC13_LDO23_CONTROL}, 1},
+    {AXP192_EXTEN_DCDC2_CONTROL, {CONFIG_AXP192_EXTEN_DCDC2_CONTROL}, 1},
+    {AXP192_GPIO0_CONTROL, {CONFIG_AXP192_GPIO0_CONTROL}, 1},
+    {AXP192_GPIO1_CONTROL, {CONFIG_AXP192_GPIO1_CONTROL}, 1},
+    {AXP192_GPIO2_CONTROL, {CONFIG_AXP192_GPIO2_CONTROL}, 1},
+    {AXP192_GPIO43_FUNCTION_CONTROL, {CONFIG_AXP192_GPIO43_FUNCTION_CONTROL}, 1},
+    {AXP192_ADC_ENABLE_1, {CONFIG_AXP192_ADC_ENABLE_1}, 1},
+    {AXP192_CHARGE_CONTROL_1, {CONFIG_AXP192_CHARGE_CONTROL_1}, 1},
+    {AXP192_BATTERY_CHARGE_CONTROL, {CONFIG_AXP192_BATTERY_CHARGE_CONTROL}, 1},
+#endif /* AXP192_INCLUDE_SDKCONFIG_H */
+    /* End of commands. */
+    {0, {0}, 0xff},
+};
+
+axp192_err_t axp192_init(const axp192_t *axp)
+{
+    uint8_t cmd = 0;
+    axp192_err_t status;
+
+    /* Send all the commands. */
+    while (init_commands[cmd].count != 0xff) {
+        status = axp->write(
+            axp->handle,
+            AXP192_ADDRESS,
+            init_commands[cmd].command,
+            init_commands[cmd].data,
+            init_commands[cmd].count & 0x1f
+        );
+        if (AXP192_OK != status) {
+            return status;
+        }
+        cmd++;
+    }
+
+    return AXP192_OK;
+}
+
+static axp192_err_t axp192_read_adc(const axp192_t *axp, uint8_t reg, float *buffer)
+{
+    uint8_t tmp[4];
+    float sensitivity = 1.0;
+    float offset = 0.0;
+    axp192_err_t status;
+
+    switch (reg) {
+    case AXP192_ACIN_VOLTAGE:
+    case AXP192_VBUS_VOLTAGE:
+        /* 1.7mV per LSB */
+        sensitivity = 1.7 / 1000;
+        break;
+    case AXP192_ACIN_CURRENT:
+        /* 0.375mA per LSB */
+        sensitivity = 0.625 / 1000;
+        break;
+    case AXP192_VBUS_CURRENT:
+        /* 0.375mA per LSB */
+        sensitivity = 0.375 / 1000;
+        break;
+    case AXP192_TEMP:
+        /* 0.1C per LSB, 0x00 = -144.7C */
+        sensitivity = 0.1;
+        offset = -144.7;
+        break;
+    case AXP192_TS_INPUT:
+        /* 0.8mV per LSB */
+        sensitivity = 0.8 / 1000;
+        break;
+    case AXP192_BATTERY_POWER:
+        /* 1.1mV * 0.5mA per LSB */
+        return read_battery_power(axp, buffer);
+        break;
+    case AXP192_BATTERY_VOLTAGE:
+        /* 1.1mV per LSB */
+        sensitivity = 1.1 / 1000;
+        break;
+    case AXP192_CHARGE_CURRENT:
+    case AXP192_DISCHARGE_CURRENT:
+        /* 0.5mV per LSB */
+        sensitivity = 0.5 / 1000;
+        break;
+    case AXP192_APS_VOLTAGE:
+        /* 1.4mV per LSB */
+        sensitivity = 1.4 / 1000;
+        break;
+    case AXP192_COULOMB_COUNTER:
+        /* This is currently untested. */
+        return read_coloumb_counter(axp, buffer);
+        break;
+    }
+
+    status = axp->read(axp->handle, AXP192_ADDRESS, reg, tmp, 2);
+    if (AXP192_OK != status) {
+        return status;
+    }
+    *buffer = (((tmp[0] << 4) + tmp[1]) * sensitivity) + offset;
+
+    return AXP192_OK;
+}
+
+axp192_err_t axp192_read(const axp192_t *axp, uint8_t reg, void *buffer) {
+    switch (reg) {
+    case AXP192_ACIN_VOLTAGE:
+    case AXP192_VBUS_VOLTAGE:
+    case AXP192_ACIN_CURRENT:
+    case AXP192_VBUS_CURRENT:
+    case AXP192_TEMP:
+    case AXP192_TS_INPUT:
+    case AXP192_BATTERY_POWER:
+    case AXP192_BATTERY_VOLTAGE:
+    case AXP192_CHARGE_CURRENT:
+    case AXP192_DISCHARGE_CURRENT:
+    case AXP192_APS_VOLTAGE:
+    case AXP192_COULOMB_COUNTER:
+        /* Return ADC value. */
+        return axp192_read_adc(axp, reg, buffer);
+        break;
+    default:
+        /* Return raw register value. */
+        return axp->read(axp->handle, AXP192_ADDRESS, reg, buffer, 1);
+    }
+}
+
+axp192_err_t axp192_write(const axp192_t *axp, uint8_t reg, uint8_t value) {
+    switch (reg) {
+    case AXP192_ACIN_VOLTAGE:
+    case AXP192_VBUS_VOLTAGE:
+    case AXP192_ACIN_CURRENT:
+    case AXP192_VBUS_CURRENT:
+    case AXP192_TEMP:
+    case AXP192_TS_INPUT:
+    case AXP192_BATTERY_POWER:
+    case AXP192_BATTERY_VOLTAGE:
+    case AXP192_CHARGE_CURRENT:
+    case AXP192_DISCHARGE_CURRENT:
+    case AXP192_APS_VOLTAGE:
+    case AXP192_COULOMB_COUNTER:
+        /* Read only register. */
+        return AXP192_ERROR_ENOTSUP;
+        break;
+    default:
+        /* Write raw register value. */
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &value, 1);
+    }
+}
+
+axp192_err_t axp192_ioctl(const axp192_t *axp, int command, ...)
+{
+    uint8_t reg = command >> 8;
+    uint8_t tmp;
+    uint16_t argument;
+    va_list ap;
+
+    switch (command) {
+    case AXP192_COULOMB_COUNTER_ENABLE:
+        tmp = 0b10000000;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_COULOMB_COUNTER_DISABLE:
+        tmp = 0b00000000;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_COULOMB_COUNTER_SUSPEND:
+        tmp = 0b11000000;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_COULOMB_COUNTER_CLEAR:
+        tmp = 0b10100000;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_LDOIO0_ENABLE:
+        /* 0x02 = LDO */
+        tmp = 0b00000010;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_LDOIO0_DISABLE:
+        /* 0x07 = float */
+        tmp = 0b00000111;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_LDO2_ENABLE:
+    /* This is currently untested. */
+    case AXP192_EXTEN_ENABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp |= 0b00000100;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_LDO2_DISABLE:
+    /* This is currently untested. */
+    case AXP192_EXTEN_DISABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp &= ~0b00000100;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_GPIO2_SET_LEVEL:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        va_start(ap, command);
+        argument = (uint8_t) va_arg(ap, int);
+        va_end(ap);
+        if (argument) {
+            tmp |= 0b00000100;
+        } else {
+            tmp &= ~0b00000100;
+        }
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_LDO3_ENABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp |= 0b00001000;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_LDO3_DISABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp &= ~0b00001000;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_DCDC1_ENABLE:
+    /* This is currently untested. */
+    case AXP192_DCDC2_ENABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp |= 0b00000001;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_DCDC1_DISABLE:
+    /* This is currently untested. */
+    case AXP192_DCDC2_DISABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp &= ~0b00000001;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_DCDC3_ENABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp |= 0b00000010;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_DCDC3_DISABLE:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        tmp &= ~0b00000010;
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_GPIO1_SET_LEVEL:
+    case AXP192_GPIO4_SET_LEVEL:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        va_start(ap, command);
+        argument = (uint8_t) va_arg(ap, int);
+        va_end(ap);
+        if (argument) {
+            tmp |= 0b00000010;
+        } else {
+            tmp &= ~0b00000010;
+        }
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_GPIO0_SET_LEVEL:
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        va_start(ap, command);
+        argument = (uint8_t) va_arg(ap, int);
+        va_end(ap);
+        if (argument) {
+            tmp |= 0b00000001;
+        } else {
+            tmp &= ~0b00000001;
+        }
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    case AXP192_DCDC1_SET_VOLTAGE:
+    case AXP192_DCDC3_SET_VOLTAGE:
+        va_start(ap, command);
+        argument = (uint16_t) va_arg(ap, int);
+        va_end(ap);
+
+        /*  700-3500mv 25mV per step */
+        if ((argument < 700) || (argument > 3500)) {
+            return AXP192_ERROR_EINVAL;
+        }
+        tmp = (argument - 700) / 25;
+
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_DCDC2_SET_VOLTAGE:
+        va_start(ap, command);
+        argument = (uint16_t) va_arg(ap, int);
+        va_end(ap);
+
+        /*  700-2275mV 25mV per step */
+        if ((argument < 700) || (argument > 2275)) {
+            return AXP192_ERROR_EINVAL;
+        }
+        tmp = (argument - 700) / 25;
+
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_LDO2_SET_VOLTAGE:
+        va_start(ap, command);
+        argument = (uint16_t) va_arg(ap, int);
+        va_end(ap);
+
+        /*  1800-3300mV 100mV per step */
+        if ((argument < 1800) || (argument > 3300)) {
+            return AXP192_ERROR_EINVAL;
+        }
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+
+        tmp &= ~0xf0;
+        tmp |= (((argument - 1800) / 100) << 4);
+
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_LDO3_SET_VOLTAGE:
+        va_start(ap, command);
+        argument = (uint16_t) va_arg(ap, int);
+        va_end(ap);
+
+        /*  1800-3300mV 100mV per step */
+        if ((argument < 1800) || (argument > 3300)) {
+            return AXP192_ERROR_EINVAL;
+        }
+        axp->read(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+
+        tmp &= ~0x0f;
+        tmp |= ((argument - 1800) / 100);
+
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    /* This is currently untested. */
+    case AXP192_LDOIO0_SET_VOLTAGE:
+        va_start(ap, command);
+        argument = (uint16_t) va_arg(ap, int);
+        va_end(ap);
+
+        /*  1800-3300mV 100mV per step, 2800mV default. */
+        if ((argument < 1800) || (argument > 3300)) {
+            return AXP192_ERROR_EINVAL;
+        }
+        tmp = (((argument - 1800) / 100) << 4);
+
+        return axp->write(axp->handle, AXP192_ADDRESS, reg, &tmp, 1);
+        break;
+    }
+
+    return AXP192_ERROR_NOTTY;
+}
+
+static axp192_err_t read_coloumb_counter(const axp192_t *axp, float *buffer)
+{
+    uint8_t tmp[4];
+    int32_t coin, coout;
+    axp192_err_t status;
+
+    status = axp->read(axp->handle, AXP192_ADDRESS, AXP192_CHARGE_COULOMB, tmp, sizeof(coin));
+    if (AXP192_OK != status) {
+        return status;
+    }
+    coin = (tmp[0] << 24) + (tmp[1] << 16) + (tmp[2] << 8) + tmp[3];
+
+    status = axp->read(axp->handle, AXP192_ADDRESS, AXP192_DISCHARGE_COULOMB, tmp, sizeof(coout));
+    if (AXP192_OK != status) {
+        return status;
+    }
+    coout = (tmp[0] << 24) + (tmp[1] << 16) + (tmp[2] << 8) + tmp[3];
+
+    /* CmAh = 65536 * 0.5mA *（coin - cout) / 3600 / ADC sample rate */
+    *buffer = 32768 * (coin - coout) / 3600 / 25;
+
+    return AXP192_OK;
+}
+
+static axp192_err_t read_battery_power(const axp192_t *axp, float *buffer)
+{
+    uint8_t tmp[4];
+    float sensitivity;
+    axp192_err_t status;
+
+    /* 1.1mV * 0.5mA per LSB */
+    sensitivity = 1.1 * 0.5 / 1000;
+    status = axp->read(axp->handle, AXP192_ADDRESS, AXP192_BATTERY_POWER, tmp, 3);
+    if (AXP192_OK != status) {
+        return status;
+    }
+    *buffer = (((tmp[0] << 16) + (tmp[1] << 8) + tmp[2]) * sensitivity);
+    return AXP192_OK;
+}
\ No newline at end of file
diff --git a/Boards/M5stackCore2/Source/axp192/axp192.h b/Boards/M5stackCore2/Source/axp192/axp192.h
new file mode 100644
index 00000000..ff022117
--- /dev/null
+++ b/Boards/M5stackCore2/Source/axp192/axp192.h
@@ -0,0 +1,213 @@
+/*
+
+MIT License
+
+Copyright (c) 2019-2021 Mika Tuupola
+
+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.
+
+-cut-
+
+This file is part of hardware agnostic I2C driver for AXP192:
+https://github.com/tuupola/axp192
+
+SPDX-License-Identifier: MIT
+Version: 0.6.0
+
+*/
+
+#ifndef _AXP192_H
+#define _AXP192_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#define	AXP192_ADDRESS	                (0x34)
+
+/* Power control registers */
+#define AXP192_POWER_STATUS             (0x00)
+#define AXP192_CHARGE_STATUS            (0x01)
+#define AXP192_OTG_VBUS_STATUS          (0x04)
+#define AXP192_DATA_BUFFER0             (0x06)
+#define AXP192_DATA_BUFFER1             (0x07)
+#define AXP192_DATA_BUFFER2             (0x08)
+#define AXP192_DATA_BUFFER3             (0x09)
+#define AXP192_DATA_BUFFER4             (0x0a)
+#define AXP192_DATA_BUFFER5             (0x0b)
+/* Output control: 2 EXTEN, 0 DCDC2 */
+#define AXP192_EXTEN_DCDC2_CONTROL      (0x10)
+/* Power output control: 6 EXTEN, 4 DCDC2, 3 LDO3, 2 LDO2, 1 DCDC3, 0 DCDC1 */
+#define AXP192_DCDC13_LDO23_CONTROL     (0x12)
+#define AXP192_DCDC2_VOLTAGE            (0x23)
+#define AXP192_DCDC2_SLOPE              (0x25)
+#define AXP192_DCDC1_VOLTAGE            (0x26)
+#define AXP192_DCDC3_VOLTAGE            (0x27)
+/* Output voltage control: 7-4 LDO2, 3-0 LDO3 */
+#define AXP192_LDO23_VOLTAGE            (0x28)
+#define AXP192_VBUS_IPSOUT_CHANNEL      (0x30)
+#define AXP192_SHUTDOWN_VOLTAGE         (0x31)
+#define AXP192_SHUTDOWN_BATTERY_CHGLED_CONTROL (0x32)
+#define AXP192_CHARGE_CONTROL_1         (0x33)
+#define AXP192_CHARGE_CONTROL_2         (0x34)
+#define AXP192_BATTERY_CHARGE_CONTROL   (0x35)
+#define AXP192_PEK                      (0x36)
+#define AXP192_DCDC_FREQUENCY           (0x37)
+#define AXP192_BATTERY_CHARGE_LOW_TEMP  (0x38)
+#define AXP192_BATTERY_CHARGE_HIGH_TEMP (0x39)
+#define AXP192_APS_LOW_POWER1           (0x3A)
+#define AXP192_APS_LOW_POWER2           (0x3B)
+#define AXP192_BATTERY_DISCHARGE_LOW_TEMP  (0x3c)
+#define AXP192_BATTERY_DISCHARGE_HIGH_TEMP (0x3d)
+#define AXP192_DCDC_MODE                (0x80)
+#define AXP192_ADC_ENABLE_1             (0x82)
+#define AXP192_ADC_ENABLE_2             (0x83)
+#define AXP192_ADC_RATE_TS_PIN          (0x84)
+#define AXP192_GPIO30_INPUT_RANGE       (0x85)
+#define AXP192_GPIO0_ADC_IRQ_RISING     (0x86)
+#define AXP192_GPIO0_ADC_IRQ_FALLING    (0x87)
+#define AXP192_TIMER_CONTROL            (0x8a)
+#define AXP192_VBUS_MONITOR             (0x8b)
+#define AXP192_TEMP_SHUTDOWN_CONTROL    (0x8f)
+
+/* GPIO control registers */
+#define AXP192_GPIO0_CONTROL            (0x90)
+#define AXP192_GPIO0_LDOIO0_VOLTAGE     (0x91)
+#define AXP192_GPIO1_CONTROL            (0x92)
+#define AXP192_GPIO2_CONTROL            (0x93)
+#define AXP192_GPIO20_SIGNAL_STATUS     (0x94)
+#define AXP192_GPIO43_FUNCTION_CONTROL  (0x95)
+#define AXP192_GPIO43_SIGNAL_STATUS     (0x96)
+#define AXP192_GPIO20_PULLDOWN_CONTROL  (0x97)
+#define AXP192_PWM1_FREQUENCY           (0x98)
+#define AXP192_PWM1_DUTY_CYCLE_1        (0x99)
+#define AXP192_PWM1_DUTY_CYCLE_2        (0x9a)
+#define AXP192_PWM2_FREQUENCY           (0x9b)
+#define AXP192_PWM2_DUTY_CYCLE_1        (0x9c)
+#define AXP192_PWM2_DUTY_CYCLE_2        (0x9d)
+#define AXP192_N_RSTO_GPIO5_CONTROL     (0x9e)
+
+/* Interrupt control registers */
+#define AXP192_ENABLE_CONTROL_1         (0x40)
+#define AXP192_ENABLE_CONTROL_2         (0x41)
+#define AXP192_ENABLE_CONTROL_3         (0x42)
+#define AXP192_ENABLE_CONTROL_4         (0x43)
+#define AXP192_ENABLE_CONTROL_5         (0x4a)
+#define AXP192_IRQ_STATUS_1             (0x44)
+#define AXP192_IRQ_STATUS_2             (0x45)
+#define AXP192_IRQ_STATUS_3             (0x46)
+#define AXP192_IRQ_STATUS_4             (0x47)
+#define AXP192_IRQ_STATUS_5             (0x4d)
+
+/* ADC data registers */
+#define AXP192_ACIN_VOLTAGE             (0x56)
+#define AXP192_ACIN_CURRENT             (0x58)
+#define AXP192_VBUS_VOLTAGE             (0x5a)
+#define AXP192_VBUS_CURRENT             (0x5c)
+#define AXP192_TEMP                     (0x5e)
+#define AXP192_TS_INPUT                 (0x62)
+#define AXP192_GPIO0_VOLTAGE            (0x64)
+#define AXP192_GPIO1_VOLTAGE            (0x66)
+#define AXP192_GPIO2_VOLTAGE            (0x68)
+#define AXP192_GPIO3_VOLTAGE            (0x6a)
+#define AXP192_BATTERY_POWER            (0x70)
+#define AXP192_BATTERY_VOLTAGE          (0x78)
+#define AXP192_CHARGE_CURRENT           (0x7a)
+#define AXP192_DISCHARGE_CURRENT        (0x7c)
+#define AXP192_APS_VOLTAGE              (0x7e)
+#define AXP192_CHARGE_COULOMB           (0xb0)
+#define AXP192_DISCHARGE_COULOMB        (0xb4)
+#define AXP192_COULOMB_COUNTER_CONTROL  (0xb8)
+
+/* Computed ADC */
+#define AXP192_COULOMB_COUNTER          (0xff)
+
+/* IOCTL commands */
+#define	AXP192_READ_POWER_STATUS        (0x0001)
+#define	AXP192_READ_CHARGE_STATUS       (0x0101)
+
+#define AXP192_COULOMB_COUNTER_ENABLE   (0xb801)
+#define AXP192_COULOMB_COUNTER_DISABLE  (0xb802)
+#define AXP192_COULOMB_COUNTER_SUSPEND  (0xb803)
+#define AXP192_COULOMB_COUNTER_CLEAR    (0xb804)
+
+#define AXP192_LDOIO0_ENABLE            (0x9000)
+#define AXP192_LDOIO0_DISABLE           (0x9001)
+
+#define AXP192_DCDC2_ENABLE             (0x1000)
+#define AXP192_DCDC2_DISABLE            (0x1001)
+#define AXP192_EXTEN_ENABLE             (0x1002)
+#define AXP192_EXTEN_DISABLE            (0x1003)
+
+#define AXP192_LDO2_ENABLE              (0x1200)
+#define AXP192_LDO2_DISABLE             (0x1201)
+#define AXP192_LDO3_ENABLE              (0x1202)
+#define AXP192_LDO3_DISABLE             (0x1203)
+#define AXP192_DCDC1_ENABLE             (0x1204)
+#define AXP192_DCDC1_DISABLE            (0x1205)
+#define AXP192_DCDC3_ENABLE             (0x1206)
+#define AXP192_DCDC3_DISABLE            (0x1207)
+
+#define AXP192_DCDC1_SET_VOLTAGE        (0x2600)
+#define AXP192_DCDC2_SET_VOLTAGE        (0x2300)
+#define AXP192_DCDC3_SET_VOLTAGE        (0x2700)
+#define AXP192_LDO2_SET_VOLTAGE         (0x2800)
+#define AXP192_LDO3_SET_VOLTAGE         (0x2801)
+#define AXP192_LDOIO0_SET_VOLTAGE       (0x9100)
+
+#define AXP192_LOW                      (0)
+#define AXP192_HIGH                     (1)
+
+#define AXP192_GPIO0_SET_LEVEL          (0x9400)
+#define AXP192_GPIO1_SET_LEVEL          (0x9401)
+#define AXP192_GPIO2_SET_LEVEL          (0x9402)
+#define AXP192_GPIO4_SET_LEVEL          (0x9601)
+
+/* Error codes */
+#define AXP192_OK                       (0)
+#define AXP192_ERROR_NOTTY              (-1)
+#define AXP192_ERROR_EINVAL             (-22)
+#define AXP192_ERROR_ENOTSUP            (-95)
+
+typedef struct {
+    uint8_t command;
+    uint8_t data[2];
+    uint8_t count;
+} axp192_init_command_t;
+
+/* These should be provided by the HAL. */
+typedef struct {
+    int32_t (* read)(void *handle, uint8_t address, uint8_t reg, uint8_t *buffer, uint16_t size);
+    int32_t (* write)(void *handle, uint8_t address, uint8_t reg, const uint8_t *buffer, uint16_t size);
+    void *handle;
+} axp192_t;
+
+typedef int32_t axp192_err_t;
+
+axp192_err_t axp192_init(const axp192_t *axp);
+axp192_err_t axp192_read(const axp192_t *axp, uint8_t reg, void *buffer);
+axp192_err_t axp192_write(const axp192_t *axp, uint8_t reg, uint8_t value);
+axp192_err_t axp192_ioctl(const axp192_t *axp, int command, ...);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/Boards/M5stackCore2/Source/axp192/axp192_config.h b/Boards/M5stackCore2/Source/axp192/axp192_config.h
new file mode 100644
index 00000000..1a87e24a
--- /dev/null
+++ b/Boards/M5stackCore2/Source/axp192/axp192_config.h
@@ -0,0 +1,125 @@
+/*
+
+MIT License
+
+Copyright (c) 2019-2021 Mika Tuupola
+
+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.
+
+-cut-
+
+This file is part of hardware agnostic I2C driver for AXP192:
+https://github.com/tuupola/axp192
+
+SPDX-License-Identifier: MIT
+Version: 0.6.0
+
+*/
+
+#ifndef _AXP192_CONFIG_H
+#define _AXP192_CONFIG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef AXP192_INCLUDE_SDKCONFIG_H
+#include "sdkconfig.h"
+
+/* This requires you to run menuconfig first. */
+
+#define CONFIG_AXP192_EXTEN_DCDC2_CONTROL ( \
+    CONFIG_AXP192_EXTEN_DCDC2_CONTROL_BIT2 | \
+    CONFIG_AXP192_EXTEN_DCDC2_CONTROL_BIT0 \
+)
+
+#define CONFIG_AXP192_DCDC13_LDO23_CONTROL ( \
+    CONFIG_AXP192_DCDC13_LDO23_CONTROL_BIT6 | \
+    CONFIG_AXP192_DCDC13_LDO23_CONTROL_BIT4 | \
+    CONFIG_AXP192_DCDC13_LDO23_CONTROL_BIT3 | \
+    CONFIG_AXP192_DCDC13_LDO23_CONTROL_BIT2 | \
+    CONFIG_AXP192_DCDC13_LDO23_CONTROL_BIT1 | \
+    CONFIG_AXP192_DCDC13_LDO23_CONTROL_BIT0 \
+)
+
+#define CONFIG_AXP192_LDO23_VOLTAGE ( \
+    CONFIG_AXP192_LDO23_VOLTAGE_BIT74 | \
+    CONFIG_AXP192_LDO23_VOLTAGE_BIT30 \
+)
+
+#define CONFIG_AXP192_DCDC1_VOLTAGE ( \
+    CONFIG_AXP192_DCDC1_VOLTAGE_BIT60 \
+)
+
+#define CONFIG_AXP192_DCDC3_VOLTAGE ( \
+    CONFIG_AXP192_DCDC3_VOLTAGE_BIT60 \
+)
+
+#define CONFIG_AXP192_ADC_ENABLE_1 ( \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT7 | \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT6 | \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT5 | \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT4 | \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT3 | \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT2 | \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT1 | \
+    CONFIG_AXP192_ADC_ENABLE_1_BIT0 \
+)
+
+#define CONFIG_AXP192_CHARGE_CONTROL_1 ( \
+    CONFIG_AXP192_CHARGE_CONTROL_1_BIT7 | \
+    CONFIG_AXP192_CHARGE_CONTROL_1_BIT65 | \
+    CONFIG_AXP192_CHARGE_CONTROL_1_BIT4 | \
+    CONFIG_AXP192_CHARGE_CONTROL_1_BIT30 \
+)
+
+#define CONFIG_AXP192_BATTERY_CHARGE_CONTROL ( \
+    CONFIG_AXP192_BATTERY_CHARGE_CONTROL_BIT7 | \
+    CONFIG_AXP192_BATTERY_CHARGE_CONTROL_BIT65 | \
+    CONFIG_AXP192_BATTERY_CHARGE_CONTROL_BIT10 \
+)
+
+#define CONFIG_AXP192_GPIO0_CONTROL ( \
+    CONFIG_AXP192_GPIO0_CONTROL_BIT20 \
+)
+
+#define CONFIG_AXP192_GPIO1_CONTROL ( \
+    CONFIG_AXP192_GPIO1_CONTROL_BIT20 \
+)
+
+#define CONFIG_AXP192_GPIO2_CONTROL ( \
+    CONFIG_AXP192_GPIO2_CONTROL_BIT20 \
+)
+
+#define CONFIG_AXP192_GPIO43_FUNCTION_CONTROL ( \
+    CONFIG_AXP192_GPIO43_FUNCTION_CONTROL_BIT7 | \
+    CONFIG_AXP192_GPIO43_FUNCTION_CONTROL_BIT32 | \
+    CONFIG_AXP192_GPIO43_FUNCTION_CONTROL_BIT10 \
+)
+
+#define CONFIG_AXP192_GPIO0_LDOIO0_VOLTAGE ( \
+    CONFIG_AXP192_GPIO0_LDOIO0_VOLTAGE_BIT74 \
+)
+
+#endif /* AXP192_INCLUDE_SDKCONFIG_H */
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/Boards/M5stackCore2/Source/hal/Core2Power.cpp b/Boards/M5stackCore2/Source/hal/Core2Power.cpp
index 5e84bb92..7c68c0b6 100644
--- a/Boards/M5stackCore2/Source/hal/Core2Power.cpp
+++ b/Boards/M5stackCore2/Source/hal/Core2Power.cpp
@@ -1,11 +1,10 @@
 #include "Core2Power.h"
-
-#include "utility/AXP192_Class.hpp"
 #include "TactilityCore.h"
+#include "axp192/axp192.h"
 
 #define TAG "core2_power"
 
-extern m5::AXP192_Class axpDevice;
+extern axp192_t axpDevice;
 
 bool Core2Power::supportsMetric(MetricType type) const {
     switch (type) {
@@ -22,35 +21,81 @@ bool Core2Power::supportsMetric(MetricType type) const {
 
 bool Core2Power::getMetric(Power::MetricType type, Power::MetricData& data) {
     switch (type) {
-        case BATTERY_VOLTAGE:
-            data.valueAsUint32 = (uint32_t)TT_MAX((axpDevice.getBatteryVoltage() * 1000.f), 0.0f);
-            return true;
-        case CHARGE_LEVEL: {
-            auto level = axpDevice.getBatteryLevel();
-            if (level > 0) {
-                data.valueAsUint8 = (uint8_t)level;
+        case BATTERY_VOLTAGE: {
+            float voltage;
+            if (axp192_read(&axpDevice, AXP192_BATTERY_VOLTAGE, &voltage) == ESP_OK) {
+                data.valueAsUint32 = (uint32_t)TT_MAX((voltage * 1000.f), 0.0f);
+                return true;
+            } else {
+                return false;
+            }
+        }
+        case CHARGE_LEVEL: {
+            float vbat, charge_current;
+            if (
+                axp192_read(&axpDevice, AXP192_BATTERY_VOLTAGE, &vbat) == ESP_OK &&
+                axp192_read(&axpDevice, AXP192_CHARGE_CURRENT, &charge_current) == ESP_OK
+            ) {
+                float max_voltage = 4.20f;
+                float min_voltage = 2.69f; // From M5Unified
+                float voltage_correction = (charge_current > 0.01f) ? -0.1f : 0.f; // Roughly 0.1V drop when ccharging
+                float corrected_voltage = vbat + voltage_correction;
+                if (corrected_voltage > 2.69f) {
+                    float charge_factor = (corrected_voltage - min_voltage) / (max_voltage - min_voltage);
+                    data.valueAsUint8 = (uint8_t)(charge_factor * 100.f);
+                } else {
+                    data.valueAsUint8 = 0;
+                }
+                return true;
+            } else {
+                return false;
+            }
+        }
+        case IS_CHARGING: {
+            float charge_current;
+            if (axp192_read(&axpDevice, AXP192_CHARGE_CURRENT, &charge_current) == ESP_OK) {
+                data.valueAsBool = charge_current > 0.001f;
+                return true;
+            } else {
+                return false;
+            }
+        }
+        case CURRENT: {
+            float charge_current, discharge_current;
+            if (
+                axp192_read(&axpDevice, AXP192_CHARGE_CURRENT, &charge_current) == ESP_OK &&
+                axp192_read(&axpDevice, AXP192_DISCHARGE_CURRENT, &discharge_current) == ESP_OK
+            ) {
+                if (charge_current > 0.0f) {
+                    data.valueAsInt32 = (int32_t) (charge_current * 1000.0f);
+                } else {
+                    data.valueAsInt32 = -(int32_t) (discharge_current * 1000.0f);
+                }
                 return true;
             } else {
                 return false;
             }
         }
-        case IS_CHARGING:
-            data.valueAsBool = axpDevice.isCharging();
-            return true;
-        case CURRENT:
-            return false;
     }
 
     return false; // Safety guard for when new enum values are introduced
 }
 
 bool Core2Power::isAllowedToCharge() const {
-    return allowedToCharge;
+    uint8_t buffer;
+    if (axp192_read(&axpDevice, AXP192_CHARGE_CONTROL_1, &buffer) == ESP_OK) {
+        return buffer & 0x80;
+    } else {
+        return false;
+    }
 }
 
 void Core2Power::setAllowedToCharge(bool canCharge) {
-    allowedToCharge = canCharge;
-    axpDevice.setBatteryCharge(canCharge);
+    uint8_t buffer;
+    if (axp192_read(&axpDevice, AXP192_CHARGE_CONTROL_1, &buffer) == ESP_OK) {
+        buffer = (buffer & 0x7F) + (canCharge ? 0x80 : 0x00);
+        axp192_write(&axpDevice, AXP192_CHARGE_CONTROL_1, buffer);
+    }
 }
 
 static std::shared_ptr<Power> power;
diff --git a/Boards/M5stackCore2/Source/hal/Core2Power.h b/Boards/M5stackCore2/Source/hal/Core2Power.h
index 1494da4e..0f5a7938 100644
--- a/Boards/M5stackCore2/Source/hal/Core2Power.h
+++ b/Boards/M5stackCore2/Source/hal/Core2Power.h
@@ -7,10 +7,6 @@ using namespace tt::hal;
 
 class Core2Power : public Power {
 
-private:
-
-    bool allowedToCharge = true;
-
 public:
 
     Core2Power() = default;
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f609c8fd..4a89986c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,9 +49,6 @@ if (DEFINED ENV{ESP_IDF_VERSION})
         set(EXCLUDE_COMPONENTS "M5stackCoreS3")
     endif()
 
-    # TEMP - DO NOT COMMIT
-    set(EXCLUDE_COMPONENTS "YellowBoard" "M5stackCore2" "WaveshareS3Touch")
-
     # LVGL
     get_filename_component(
         LVGL_CONFIG_FULL_PATH Libraries/lvgl_conf ABSOLUTE
diff --git a/Tactility/Source/app/power/Power.cpp b/Tactility/Source/app/power/Power.cpp
index 3b135152..3006e9d6 100644
--- a/Tactility/Source/app/power/Power.cpp
+++ b/Tactility/Source/app/power/Power.cpp
@@ -53,7 +53,7 @@ static void updateUi(std::shared_ptr<Data> data) {
     }
 
     bool charging_enabled_set = data->power->supportsChargeControl();
-    bool charging_enabled = data->power->supportsChargeControl() && data->power->isAllowedToCharge();
+    bool charging_enabled_and_allowed = data->power->supportsChargeControl() && data->power->isAllowedToCharge();
 
     int32_t current;
     bool current_set = false;
@@ -72,7 +72,7 @@ static void updateUi(std::shared_ptr<Data> data) {
     lvgl::lock(kernel::millisToTicks(1000));
 
     if (charging_enabled_set) {
-        lv_obj_set_state(data->enable_switch, LV_STATE_CHECKED, charging_enabled);
+        lv_obj_set_state(data->enable_switch, LV_STATE_CHECKED, charging_enabled_and_allowed);
         lv_obj_remove_flag(data->enable_switch, LV_OBJ_FLAG_HIDDEN);
         lv_obj_remove_flag(data->enable_label, LV_OBJ_FLAG_HIDDEN);
     } else {
diff --git a/TactilityHeadless/Source/hal/Hal.cpp b/TactilityHeadless/Source/hal/Hal.cpp
index 14f42185..16b430d7 100644
--- a/TactilityHeadless/Source/hal/Hal.cpp
+++ b/TactilityHeadless/Source/hal/Hal.cpp
@@ -6,17 +6,17 @@
 namespace tt::hal {
 
 void init(const Configuration& configuration) {
-    if (configuration.initBoot != nullptr) {
-        TT_LOG_I(TAG, "Init power");
-        tt_check(configuration.initBoot(), "Init power failed");
-    }
-
     tt_check(i2c::init(configuration.i2c), "I2C init failed");
     if (configuration.initHardware != nullptr) {
         TT_LOG_I(TAG, "Init hardware");
         tt_check(configuration.initHardware(), "Hardware init failed");
     }
 
+    if (configuration.initBoot != nullptr) {
+        TT_LOG_I(TAG, "Init power");
+        tt_check(configuration.initBoot(), "Init power failed");
+    }
+
     if (configuration.sdcard != nullptr) {
         TT_LOG_I(TAG, "Mounting sdcard");
         if (!configuration.sdcard->mount(TT_SDCARD_MOUNT_POINT )) {
diff --git a/TactilityHeadless/Source/hal/Power.h b/TactilityHeadless/Source/hal/Power.h
index 4a834bec..082e8035 100644
--- a/TactilityHeadless/Source/hal/Power.h
+++ b/TactilityHeadless/Source/hal/Power.h
@@ -13,7 +13,7 @@ public:
 
     enum MetricType {
         IS_CHARGING, // bool
-        CURRENT, // int32_t, mAh
+        CURRENT, // int32_t, mAh - battery current: either during charging (positive value) or discharging (negative value)
         BATTERY_VOLTAGE, // uint32_t, mV
         CHARGE_LEVEL, // uint8_t [0, 100]
     };
diff --git a/TactilityHeadless/Source/hal/i2c/I2c.cpp b/TactilityHeadless/Source/hal/i2c/I2c.cpp
index fd8fee7f..1eebf8cf 100644
--- a/TactilityHeadless/Source/hal/i2c/I2c.cpp
+++ b/TactilityHeadless/Source/hal/i2c/I2c.cpp
@@ -8,6 +8,8 @@
 
 namespace tt::hal::i2c {
 
+static const uint8_t ACK_CHECK_EN = 1;
+
 typedef struct Data {
     Mutex mutex;
     bool isConfigured = false;
@@ -164,6 +166,34 @@ bool masterRead(i2c_port_t port, uint8_t address, uint8_t* data, size_t dataSize
     return result == ESP_OK;
 }
 
+esp_err_t masterRead(i2c_port_t port, uint8_t address, uint8_t reg, uint8_t* data, size_t dataSize, TickType_t timeout) {
+    tt_check(reg != 0);
+
+    lock(port);
+
+    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
+    // Set address pointer
+    i2c_master_start(cmd);
+    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
+    i2c_master_write(cmd, &reg, 1, ACK_CHECK_EN);
+    // Read length of response from current pointer
+    i2c_master_start(cmd);
+    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_READ, ACK_CHECK_EN);
+    if (dataSize > 1) {
+        i2c_master_read(cmd, data, dataSize - 1, I2C_MASTER_ACK);
+    }
+    i2c_master_read_byte(cmd, data + dataSize - 1, I2C_MASTER_NACK);
+    i2c_master_stop(cmd);
+    esp_err_t result = i2c_master_cmd_begin(port, cmd, timeout);
+    i2c_cmd_link_delete(cmd);
+
+    ESP_LOG_BUFFER_HEX_LEVEL(TAG, data, dataSize, ESP_LOG_DEBUG);
+    ESP_ERROR_CHECK_WITHOUT_ABORT(result);
+
+    unlock(port);
+    return result;
+}
+
 bool masterWrite(i2c_port_t port, uint16_t address, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
     lock(port);
     esp_err_t result = i2c_master_write_to_device(port, address, data, dataSize, timeout);
@@ -171,6 +201,27 @@ bool masterWrite(i2c_port_t port, uint16_t address, const uint8_t* data, uint16_
     return result == ESP_OK;
 }
 
+esp_err_t masterWrite(i2c_port_t port, uint16_t address, uint8_t reg, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
+    tt_check(reg != 0);
+
+    lock(port);
+
+    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
+    i2c_master_start(cmd);
+    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
+    i2c_master_write_byte(cmd, reg, ACK_CHECK_EN);
+    i2c_master_write(cmd, (uint8_t*) data, dataSize, ACK_CHECK_EN);
+    i2c_master_stop(cmd);
+    esp_err_t result = i2c_master_cmd_begin(port, cmd, timeout);
+    i2c_cmd_link_delete(cmd);
+
+    ESP_ERROR_CHECK_WITHOUT_ABORT(result);
+
+    unlock(port);
+
+    return result;
+}
+
 bool masterWriteRead(i2c_port_t port, uint8_t address, const uint8_t* writeData, size_t writeDataSize, uint8_t* readData, size_t readDataSize, TickType_t timeout) {
     lock(port);
     esp_err_t result = i2c_master_write_read_device(port, address, writeData, writeDataSize, readData, readDataSize, timeout);
diff --git a/TactilityHeadless/Source/hal/i2c/I2c.h b/TactilityHeadless/Source/hal/i2c/I2c.h
index 7e821b3b..a04e43e4 100644
--- a/TactilityHeadless/Source/hal/i2c/I2c.h
+++ b/TactilityHeadless/Source/hal/i2c/I2c.h
@@ -35,7 +35,9 @@ bool start(i2c_port_t port);
 bool stop(i2c_port_t port);
 bool isStarted(i2c_port_t port);
 bool masterRead(i2c_port_t port, uint8_t address, uint8_t* data, size_t dataSize, TickType_t timeout);
+esp_err_t masterRead(i2c_port_t port, uint8_t address, uint8_t reg, uint8_t* data, size_t dataSize, TickType_t timeout);
 bool masterWrite(i2c_port_t port, uint16_t address, const uint8_t* data, uint16_t dataSize, TickType_t timeout);
+esp_err_t masterWrite(i2c_port_t port, uint16_t address, uint8_t reg, const uint8_t* data, uint16_t dataSize, TickType_t timeout);
 bool masterWriteRead(i2c_port_t port, uint8_t address, const uint8_t* writeData, size_t writeDataSize, uint8_t* readData, size_t readDataSize, TickType_t timeout);
 bool masterCheckAddressForDevice(i2c_port_t port, uint8_t address, TickType_t timeout);
 TtStatus lock(i2c_port_t port, TickType_t timeout = UINT_MAX);