From 0cb15b1210f6077113b6c7e592e4505a64166a51 Mon Sep 17 00:00:00 2001
From: Pirata <bmorcelli@gmail.com>
Date: Sat, 24 Aug 2024 00:31:07 -0300
Subject: [PATCH 1/2] Clearer port to Core and Core devices
Now we are not using M5Unified for Core2 and Core (basic, Gray and Fire) devices. This change was needed because M5Unified fills the IRAM_tex0 table with functions that limits bruce development.
The libraries M5Core2 and M5Stack were partially copied to the project, and modified to avoid conflicts with TFT_eSPI.
All credits to M5Stack!
---
lib/M5Core2/readme.md | 8 +
lib/M5Core2/src/AXP.cpp | 555 +++++++++
lib/M5Core2/src/AXP.h | 142 +++
lib/M5Core2/src/AXP192.cpp | 473 ++++++++
lib/M5Core2/src/AXP192.h | 92 ++
lib/M5Core2/src/AXP2101.cpp | 482 ++++++++
lib/M5Core2/src/AXP2101.h | 105 ++
lib/M5Core2/src/Free_Fonts.h | 383 +++++++
lib/M5Core2/src/INA3221.cpp | 536 +++++++++
lib/M5Core2/src/INA3221.h | 313 +++++
lib/M5Core2/src/M5Core2.cpp | 97 ++
lib/M5Core2/src/M5Core2.h | 88 ++
lib/M5Core2/src/M5Display.cpp | 707 ++++++++++++
lib/M5Core2/src/M5Display.h | 155 +++
lib/M5Core2/src/M5Touch.cpp | 178 +++
lib/M5Core2/src/M5Touch.h | 288 +++++
lib/M5Core2/src/RTC.cpp | 301 +++++
lib/M5Core2/src/RTC.h | 73 ++
lib/M5Core2/src/Speaker.cpp | 106 ++
lib/M5Core2/src/Speaker.h | 37 +
lib/M5Core2/src/utility/CommUtil.cpp | 164 +++
lib/M5Core2/src/utility/CommUtil.h | 32 +
lib/M5Core2/src/utility/Config.h | 30 +
lib/M5Core2/src/utility/M5Button.cpp | 779 +++++++++++++
lib/M5Core2/src/utility/M5Button.h | 994 ++++++++++++++++
lib/M5Core2/src/utility/M5Timer.cpp | 224 ++++
lib/M5Core2/src/utility/M5Timer.h | 125 ++
lib/M5Core2/src/utility/MPU6886.cpp | 265 +++++
lib/M5Core2/src/utility/MPU6886.h | 120 ++
lib/M5Core2/src/utility/MahonyAHRS.cpp | 267 +++++
lib/M5Core2/src/utility/MahonyAHRS.h | 39 +
lib/M5Core2/src/utility/PointAndZone.cpp | 201 ++++
lib/M5Core2/src/utility/PointAndZone.h | 192 ++++
lib/M5Core2/src/utility/pngle.c | 983 ++++++++++++++++
lib/M5Core2/src/utility/pngle.h | 94 ++
lib/M5Core2/src/utility/qrcode.c | 970 ++++++++++++++++
lib/M5Core2/src/utility/qrcode.h | 88 ++
lib/M5Core2/src/utility/quaternionFilters.cpp | 275 +++++
lib/M5Core2/src/utility/quaternionFilters.h | 16 +
lib/M5Stack/readme.md | 8 +
lib/M5Stack/src/Free_Fonts.h | 381 +++++++
lib/M5Stack/src/IMU.cpp | 157 +++
lib/M5Stack/src/IMU.h | 42 +
lib/M5Stack/src/LoRaWan.cpp | 1006 +++++++++++++++++
lib/M5Stack/src/LoRaWan.h | 562 +++++++++
lib/M5Stack/src/M5Display.cpp | 637 +++++++++++
lib/M5Stack/src/M5Display.h | 118 ++
lib/M5Stack/src/M5Faces.cpp | 42 +
lib/M5Stack/src/M5Faces.h | 21 +
lib/M5Stack/src/M5LoRa.cpp | 508 +++++++++
lib/M5Stack/src/M5LoRa.h | 108 ++
lib/M5Stack/src/M5Stack.cpp | 96 ++
lib/M5Stack/src/M5Stack.h | 186 +++
lib/M5Stack/src/gitTagVersion.h | 1 +
lib/M5Stack/src/utility/Button.cpp | 143 +++
lib/M5Stack/src/utility/Button.h | 50 +
lib/M5Stack/src/utility/CommUtil.cpp | 165 +++
lib/M5Stack/src/utility/CommUtil.h | 32 +
lib/M5Stack/src/utility/Config.h | 41 +
lib/M5Stack/src/utility/M5Timer.cpp | 224 ++++
lib/M5Stack/src/utility/M5Timer.h | 126 +++
lib/M5Stack/src/utility/MPU6886.cpp | 284 +++++
lib/M5Stack/src/utility/MPU6886.h | 101 ++
lib/M5Stack/src/utility/MPU9250.cpp | 576 ++++++++++
lib/M5Stack/src/utility/MPU9250.h | 251 ++++
lib/M5Stack/src/utility/MahonyAHRS.cpp | 269 +++++
lib/M5Stack/src/utility/MahonyAHRS.h | 39 +
lib/M5Stack/src/utility/Power.cpp | 452 ++++++++
lib/M5Stack/src/utility/Power.h | 78 ++
lib/M5Stack/src/utility/SH200Q.cpp | 268 +++++
lib/M5Stack/src/utility/SH200Q.h | 78 ++
lib/M5Stack/src/utility/Speaker.cpp | 83 ++
lib/M5Stack/src/utility/Speaker.h | 42 +
lib/M5Stack/src/utility/pngle.c | 983 ++++++++++++++++
lib/M5Stack/src/utility/pngle.h | 94 ++
lib/M5Stack/src/utility/quaternionFilters.cpp | 275 +++++
lib/M5Stack/src/utility/quaternionFilters.h | 16 +
lib/TFT_eSPI/TFT_Drivers/ILI9341_Init.h | 81 +-
lib/TFT_eSPI/TFT_eSPI.h | 7 +-
platformio.ini | 23 +-
src/core/display.cpp | 12 +-
src/core/globals.h | 10 +-
src/core/main_menu.cpp | 6 +-
src/core/mykeyboard.cpp | 56 +-
src/core/sd_functions.cpp | 2 +
src/core/settings.cpp | 15 +
src/main.cpp | 13 +-
src/modules/others/qrcode_menu.cpp | 4 +-
88 files changed, 19713 insertions(+), 36 deletions(-)
create mode 100644 lib/M5Core2/readme.md
create mode 100644 lib/M5Core2/src/AXP.cpp
create mode 100644 lib/M5Core2/src/AXP.h
create mode 100644 lib/M5Core2/src/AXP192.cpp
create mode 100644 lib/M5Core2/src/AXP192.h
create mode 100644 lib/M5Core2/src/AXP2101.cpp
create mode 100644 lib/M5Core2/src/AXP2101.h
create mode 100644 lib/M5Core2/src/Free_Fonts.h
create mode 100644 lib/M5Core2/src/INA3221.cpp
create mode 100644 lib/M5Core2/src/INA3221.h
create mode 100644 lib/M5Core2/src/M5Core2.cpp
create mode 100644 lib/M5Core2/src/M5Core2.h
create mode 100644 lib/M5Core2/src/M5Display.cpp
create mode 100644 lib/M5Core2/src/M5Display.h
create mode 100644 lib/M5Core2/src/M5Touch.cpp
create mode 100644 lib/M5Core2/src/M5Touch.h
create mode 100644 lib/M5Core2/src/RTC.cpp
create mode 100644 lib/M5Core2/src/RTC.h
create mode 100644 lib/M5Core2/src/Speaker.cpp
create mode 100644 lib/M5Core2/src/Speaker.h
create mode 100644 lib/M5Core2/src/utility/CommUtil.cpp
create mode 100644 lib/M5Core2/src/utility/CommUtil.h
create mode 100644 lib/M5Core2/src/utility/Config.h
create mode 100644 lib/M5Core2/src/utility/M5Button.cpp
create mode 100644 lib/M5Core2/src/utility/M5Button.h
create mode 100644 lib/M5Core2/src/utility/M5Timer.cpp
create mode 100644 lib/M5Core2/src/utility/M5Timer.h
create mode 100644 lib/M5Core2/src/utility/MPU6886.cpp
create mode 100644 lib/M5Core2/src/utility/MPU6886.h
create mode 100644 lib/M5Core2/src/utility/MahonyAHRS.cpp
create mode 100644 lib/M5Core2/src/utility/MahonyAHRS.h
create mode 100644 lib/M5Core2/src/utility/PointAndZone.cpp
create mode 100644 lib/M5Core2/src/utility/PointAndZone.h
create mode 100644 lib/M5Core2/src/utility/pngle.c
create mode 100644 lib/M5Core2/src/utility/pngle.h
create mode 100644 lib/M5Core2/src/utility/qrcode.c
create mode 100644 lib/M5Core2/src/utility/qrcode.h
create mode 100644 lib/M5Core2/src/utility/quaternionFilters.cpp
create mode 100644 lib/M5Core2/src/utility/quaternionFilters.h
create mode 100644 lib/M5Stack/readme.md
create mode 100644 lib/M5Stack/src/Free_Fonts.h
create mode 100644 lib/M5Stack/src/IMU.cpp
create mode 100644 lib/M5Stack/src/IMU.h
create mode 100644 lib/M5Stack/src/LoRaWan.cpp
create mode 100644 lib/M5Stack/src/LoRaWan.h
create mode 100644 lib/M5Stack/src/M5Display.cpp
create mode 100644 lib/M5Stack/src/M5Display.h
create mode 100644 lib/M5Stack/src/M5Faces.cpp
create mode 100644 lib/M5Stack/src/M5Faces.h
create mode 100644 lib/M5Stack/src/M5LoRa.cpp
create mode 100644 lib/M5Stack/src/M5LoRa.h
create mode 100644 lib/M5Stack/src/M5Stack.cpp
create mode 100644 lib/M5Stack/src/M5Stack.h
create mode 100644 lib/M5Stack/src/gitTagVersion.h
create mode 100644 lib/M5Stack/src/utility/Button.cpp
create mode 100644 lib/M5Stack/src/utility/Button.h
create mode 100644 lib/M5Stack/src/utility/CommUtil.cpp
create mode 100644 lib/M5Stack/src/utility/CommUtil.h
create mode 100644 lib/M5Stack/src/utility/Config.h
create mode 100644 lib/M5Stack/src/utility/M5Timer.cpp
create mode 100644 lib/M5Stack/src/utility/M5Timer.h
create mode 100644 lib/M5Stack/src/utility/MPU6886.cpp
create mode 100644 lib/M5Stack/src/utility/MPU6886.h
create mode 100644 lib/M5Stack/src/utility/MPU9250.cpp
create mode 100644 lib/M5Stack/src/utility/MPU9250.h
create mode 100644 lib/M5Stack/src/utility/MahonyAHRS.cpp
create mode 100644 lib/M5Stack/src/utility/MahonyAHRS.h
create mode 100644 lib/M5Stack/src/utility/Power.cpp
create mode 100644 lib/M5Stack/src/utility/Power.h
create mode 100644 lib/M5Stack/src/utility/SH200Q.cpp
create mode 100644 lib/M5Stack/src/utility/SH200Q.h
create mode 100644 lib/M5Stack/src/utility/Speaker.cpp
create mode 100644 lib/M5Stack/src/utility/Speaker.h
create mode 100644 lib/M5Stack/src/utility/pngle.c
create mode 100644 lib/M5Stack/src/utility/pngle.h
create mode 100644 lib/M5Stack/src/utility/quaternionFilters.cpp
create mode 100644 lib/M5Stack/src/utility/quaternionFilters.h
diff --git a/lib/M5Core2/readme.md b/lib/M5Core2/readme.md
new file mode 100644
index 000000000..0696b5d27
--- /dev/null
+++ b/lib/M5Core2/readme.md
@@ -0,0 +1,8 @@
+Library from https://github.com/m5stack/M5Core2
+
+Changed to use TFT_eSPI from this project
+
+Delete all other files to save space and avoid Class redeclarations.
+
+
+
diff --git a/lib/M5Core2/src/AXP.cpp b/lib/M5Core2/src/AXP.cpp
new file mode 100644
index 000000000..3ba72cf50
--- /dev/null
+++ b/lib/M5Core2/src/AXP.cpp
@@ -0,0 +1,555 @@
+#if defined (CORE2)
+#include "AXP.h"
+
+void WriteBitOn(uint8_t Addr, uint8_t bit) {
+ Write1Byte(Addr, (Read8bit(Addr)) | bit);
+}
+
+void WriteBitOff(uint8_t Addr, uint8_t bit) {
+ Write1Byte(Addr, (Read8bit(Addr)) & ~bit);
+}
+
+void Write1Byte(uint8_t Addr, uint8_t Data) {
+ Wire1.beginTransmission(0x34);
+ Wire1.write(Addr);
+ Wire1.write(Data);
+ Wire1.endTransmission();
+}
+
+uint8_t Read8bit(uint8_t Addr) {
+ Wire1.beginTransmission(0x34);
+ Wire1.write(Addr);
+ Wire1.endTransmission();
+ Wire1.requestFrom(0x34, 1);
+ return Wire1.read();
+}
+
+uint16_t Read12Bit(uint8_t Addr) {
+ uint16_t Data = 0;
+ uint8_t buf[2];
+ ReadBuff(Addr, 2, buf);
+ Data = ((buf[0] << 4) + buf[1]); //
+ return Data;
+}
+
+uint16_t Read13Bit(uint8_t Addr) {
+ uint16_t Data = 0;
+ uint8_t buf[2];
+ ReadBuff(Addr, 2, buf);
+ Data = ((buf[0] << 5) + buf[1]); //
+ return Data;
+}
+
+uint16_t Read16bit(uint8_t Addr) {
+ uint16_t ReData = 0;
+ Wire1.beginTransmission(0x34);
+ Wire1.write(Addr);
+ Wire1.endTransmission();
+ Wire1.requestFrom(0x34, 2);
+ for (int i = 0; i < 2; i++) {
+ ReData <<= 8;
+ ReData |= Wire1.read();
+ }
+ return ReData;
+}
+
+uint32_t Read24bit(uint8_t Addr) {
+ uint32_t ReData = 0;
+ Wire1.beginTransmission(0x34);
+ Wire1.write(Addr);
+ Wire1.endTransmission();
+ Wire1.requestFrom(0x34, 3);
+ for (int i = 0; i < 3; i++) {
+ ReData <<= 8;
+ ReData |= Wire1.read();
+ }
+ return ReData;
+}
+
+uint32_t Read32bit(uint8_t Addr) {
+ uint32_t ReData = 0;
+ Wire1.beginTransmission(0x34);
+ Wire1.write(Addr);
+ Wire1.endTransmission();
+ Wire1.requestFrom(0x34, 4);
+ for (int i = 0; i < 4; i++) {
+ ReData <<= 8;
+ ReData |= Wire1.read();
+ }
+ return ReData;
+}
+
+void ReadBuff(uint8_t Addr, uint8_t Size, uint8_t* Buff) {
+ Wire1.beginTransmission(0x34);
+ Wire1.write(Addr);
+ Wire1.endTransmission();
+ Wire1.requestFrom(0x34, (int)Size);
+ for (int i = 0; i < Size; i++) {
+ *(Buff + i) = Wire1.read();
+ }
+}
+
+bool writeRegister8Array(const std::uint8_t* reg_data_array,
+ std::size_t length) {
+ for (size_t i = 0; i < length; i += 2) {
+ Write1Byte(reg_data_array[i], reg_data_array[i + 1]);
+ }
+ return true;
+}
+
+AXP::AXP() {
+}
+
+// Will be deprecated
+void AXP::begin(mbus_mode_t mode) {
+ begin();
+}
+
+void AXP::begin() {
+ Wire1.begin(21, 22);
+ Wire1.setClock(100000);
+ uint8_t val = Read8bit(0x03);
+ if (val == 0x03) {
+ _pmic = pmic_axp192;
+ axp192.begin();
+ } else if (val == 0x4A) {
+ _pmic = pmic_axp2101;
+ axp2101.begin(&Wire1, AXP2101_ADDR, 21, 22);
+ axp2101.set_bus_3v3(3300);
+ ina3221.begin(&Wire1);
+ axp2101.set_lcd_back_light_voltage(3000);
+ axp2101.set_lcd_and_tf_voltage(3300);
+ if (ina3221.getVoltage(INA3221_CH2) > 4.5f)
+ axp2101.set_bus_5v(false);
+ else {
+ axp2101.set_bus_5v(true);
+ }
+ axp2101.set_sys_led(true);
+ } else {
+ _pmic = pmic_unknown;
+ }
+}
+
+void AXP::ScreenBreath(int brightness) {
+ if (_pmic == pmic_axp192) {
+ axp192.ScreenBreath(brightness);
+ }
+ if (_pmic == pmic_axp2101) {
+ if (brightness >= 100)
+ brightness = 100;
+ else if (brightness < 0)
+ brightness = 0;
+ int vol = map(brightness, 0, 100, 2400, 3300);
+ axp2101.set_lcd_back_light_voltage(vol);
+ }
+}
+
+bool AXP::GetBatState() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatState();
+ }
+ if (_pmic == pmic_axp2101) {
+ }
+ return false;
+}
+//---------coulombcounter_from_here---------
+// enable: void EnableCoulombcounter(void);
+// disable: void DisableCOulombcounter(void);
+// stop: void StopCoulombcounter(void);
+// clear: void ClearCoulombcounter(void);
+// get charge data: uint32_t GetCoulombchargeData(void);
+// get discharge data: uint32_t GetCoulombdischargeData(void);
+// get coulomb val affter calculation: float GetCoulombData(void);
+//------------------------------------------
+void AXP::EnableCoulombcounter(void) {
+ if (_pmic == pmic_axp192) {
+ axp192.EnableCoulombcounter();
+ }
+}
+
+void AXP::DisableCoulombcounter(void) {
+ if (_pmic == pmic_axp192) {
+ axp192.DisableCoulombcounter();
+ }
+}
+
+void AXP::StopCoulombcounter(void) {
+ if (_pmic == pmic_axp192) {
+ axp192.StopCoulombcounter();
+ }
+}
+
+void AXP::ClearCoulombcounter(void) {
+ if (_pmic == pmic_axp192) {
+ axp192.ClearCoulombcounter();
+ }
+}
+
+uint32_t AXP::GetCoulombchargeData(void) {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetCoulombchargeData();
+ }
+ return 0;
+}
+
+uint32_t AXP::GetCoulombdischargeData(void) {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetCoulombdischargeData();
+ }
+ return 0;
+}
+
+float AXP::GetCoulombData(void) {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetCoulombData();
+ }
+ return -1;
+}
+
+// Cut all power, except for LDO1 (RTC)
+void AXP::PowerOff(void) {
+ if (_pmic == pmic_axp192) {
+ axp192.PowerOff();
+ }
+
+ if (_pmic == pmic_axp2101) {
+ axp2101.power_off();
+ }
+}
+
+void AXP::SetAdcState(bool state) {
+ // Enable / Disable all ADCs
+
+ if (_pmic == pmic_axp192) {
+ axp192.SetAdcState(state);
+ }
+}
+
+void AXP::PrepareToSleep(void) {
+ if (_pmic == pmic_axp192) {
+ axp192.PrepareToSleep();
+ }
+
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_sys_led(false);
+ }
+}
+
+// Get current battery level
+float AXP::GetBatteryLevel(void) {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatteryLevel();
+ }
+ if (_pmic == pmic_axp2101) {
+ const float batVoltage = ina3221.getVoltage(INA3221_CH1);
+ const float batPercentage =
+ (batVoltage < 3.248088) ? (0) : (batVoltage - 3.120712) * 100;
+ return (batPercentage <= 100) ? batPercentage : 100;
+ }
+ return -1;
+}
+
+void AXP::RestoreFromLightSleep(void) {
+ if (_pmic == pmic_axp192) {
+ axp192.RestoreFromLightSleep();
+ }
+}
+
+uint8_t AXP::GetWarningLeve(void) {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetWarningLeve();
+ }
+ return -1;
+}
+
+// -- sleep
+void AXP::DeepSleep(uint64_t time_in_us) {
+ if (_pmic == pmic_axp192) {
+ return axp192.DeepSleep(time_in_us);
+ }
+}
+
+void AXP::LightSleep(uint64_t time_in_us) {
+ if (_pmic == pmic_axp192) {
+ return axp192.LightSleep(time_in_us);
+ }
+}
+
+uint8_t AXP::GetWarningLevel(void) {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetWarningLevel();
+ }
+ return -1;
+}
+
+float AXP::GetBatVoltage() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatVoltage();
+ }
+ if (_pmic == pmic_axp2101) {
+ return ina3221.getVoltage(INA3221_CH1);
+ }
+ return -1;
+}
+
+float AXP::GetBatCurrent() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatCurrent();
+ }
+ if (_pmic == pmic_axp2101) {
+ return ina3221.getCurrent(INA3221_CH1);
+ }
+ return -1;
+}
+
+float AXP::GetVinVoltage() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetVinVoltage();
+ }
+ if (_pmic == pmic_axp2101) {
+ return ina3221.getVoltage(INA3221_CH3);
+ }
+ return -1;
+}
+
+float AXP::GetVinCurrent() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetVinCurrent();
+ }
+ if (_pmic == pmic_axp2101) {
+ return ina3221.getCurrent(INA3221_CH3);
+ }
+ return -1;
+}
+
+float AXP::GetVBusVoltage() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetVBusVoltage();
+ }
+ if (_pmic == pmic_axp2101) {
+ return ina3221.getVoltage(INA3221_CH2);
+ }
+ return -1;
+}
+
+float AXP::GetVBusCurrent() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetVBusCurrent();
+ }
+ if (_pmic == pmic_axp2101) {
+ return ina3221.getCurrent(INA3221_CH2);
+ }
+ return -1;
+}
+
+float AXP::GetTempInAXP192() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetTempInAXP192();
+ }
+ return -1;
+}
+
+float AXP::GetBatPower() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatPower();
+ }
+ return -1;
+}
+
+float AXP::GetBatChargeCurrent() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatChargeCurrent();
+ }
+ return -1;
+}
+float AXP::GetAPSVoltage() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetAPSVoltage();
+ }
+ return -1;
+}
+
+float AXP::GetBatCoulombInput() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatCoulombInput();
+ }
+ return -1;
+}
+
+float AXP::GetBatCoulombOut() {
+ if (_pmic == pmic_axp192) {
+ return axp192.GetBatCoulombOut();
+ }
+ return -1;
+}
+
+void AXP::SetCoulombClear() {
+ if (_pmic == pmic_axp192) {
+ axp192.SetCoulombClear();
+ }
+}
+
+void AXP::SetLDO2(bool State) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetLDO2(State);
+ }
+}
+
+void AXP::SetDCDC3(bool State) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetDCDC3(State);
+ }
+}
+
+uint8_t AXP::AXPInState() {
+ if (_pmic == pmic_axp192) {
+ return axp192.AXPInState();
+ }
+ return -1;
+}
+bool AXP::isACIN() {
+ if (_pmic == pmic_axp192) {
+ return axp192.isACIN();
+ }
+ if (_pmic == pmic_axp2101) {
+ return (ina3221.getVoltage(INA3221_CH3) > 1);
+ }
+
+ return true;
+}
+bool AXP::isCharging() {
+ if (_pmic == pmic_axp192) {
+ return axp192.isCharging();
+ }
+ return true;
+}
+bool AXP::isVBUS() {
+ if (_pmic == pmic_axp192) {
+ return axp192.isVBUS();
+ }
+ if (_pmic == pmic_axp2101) {
+ return (ina3221.getVoltage(INA3221_CH2) > 1);
+ }
+ return true;
+}
+
+uint8_t calcVoltageData(uint16_t value, uint16_t maxv, uint16_t minv,
+ uint16_t step) {
+ uint8_t data = 0;
+ if (value > maxv) value = maxv;
+ if (value > minv) data = (value - minv) / step;
+ return data;
+}
+
+void AXP::SetLDOVoltage(uint8_t number, uint16_t voltage) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetLDOVoltage(number, voltage);
+ }
+}
+
+/// @param number 0=DCDC1 / 1=DCDC2 / 2=DCDC3
+void AXP::SetDCVoltage(uint8_t number, uint16_t voltage) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetDCVoltage(number, voltage);
+ }
+ if (_pmic == pmic_axp2101) {
+ }
+}
+
+void AXP::SetESPVoltage(uint16_t voltage) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetESPVoltage(voltage);
+ }
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_bus_3v3(voltage);
+ }
+}
+
+void AXP::SetLcdVoltage(uint16_t voltage) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetLcdVoltage(voltage);
+ }
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_lcd_back_light_voltage(voltage);
+ }
+}
+
+void AXP::SetLDOEnable(uint8_t number, bool state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetLDOEnable(number, state);
+ }
+
+ if (_pmic == pmic_axp2101) {
+ }
+}
+
+void AXP::SetLCDRSet(bool state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetLCDRSet(state);
+ }
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_lcd_rst(state);
+ }
+}
+
+// Select source for BUS_5V
+// 0 : use internal boost
+// 1 : powered externally
+void AXP::SetBusPowerMode(uint8_t state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetBusPowerMode(state);
+ }
+ if (_pmic == pmic_axp2101) {
+ }
+}
+
+void AXP::SetLed(uint8_t state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetLed(state);
+ }
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_sys_led(state);
+ }
+}
+
+// set led state(GPIO high active,set 1 to enable amplifier)
+void AXP::SetSpkEnable(uint8_t state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetSpkEnable(state);
+ }
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_spk(state);
+ }
+}
+
+void AXP::SetCHGCurrent(uint8_t state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetCHGCurrent(state);
+ }
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_bat_charge(1);
+ }
+}
+
+void AXP::SetPeripherialsPower(uint8_t state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetPeripherialsPower(state);
+ }
+ if (_pmic == pmic_axp2101) {
+ axp2101.set_bus_5v(state);
+ }
+}
+
+void AXP::SetVibration(uint8_t state) {
+ if (_pmic == pmic_axp192) {
+ axp192.SetLDOEnable(3, state);
+ }
+ if (_pmic == pmic_axp2101) {
+ if (state) {
+ axp2101.set_vib_motor_voltage(3000);
+ } else {
+ axp2101.set_vib_motor_voltage(0);
+ }
+ }
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/AXP.h b/lib/M5Core2/src/AXP.h
new file mode 100644
index 000000000..d9dfb63e0
--- /dev/null
+++ b/lib/M5Core2/src/AXP.h
@@ -0,0 +1,142 @@
+#if defined (CORE2)
+#ifndef __AXP_PMIC_H__
+#define __AXP_PMIC_H__
+
+#include <Wire.h>
+#include <Arduino.h>
+
+#include "AXP192.h"
+#include "AXP2101.h"
+#include "INA3221.h"
+
+#define SLEEP_MSEC(us) (((uint64_t)us) * 1000L)
+#define SLEEP_SEC(us) (((uint64_t)us) * 1000000L)
+#define SLEEP_MIN(us) (((uint64_t)us) * 60L * 1000000L)
+#define SLEEP_HR(us) (((uint64_t)us) * 60L * 60L * 1000000L)
+
+typedef enum {
+ kMBusModeOutput = 0, // powered by USB or Battery
+ kMBusModeInput = 1 // powered by outside input
+} mbus_mode_t;
+
+#define AXP_ADDR 0X34
+
+void WriteBitOn(uint8_t Addr, uint8_t bit);
+void WriteBitOff(uint8_t Addr, uint8_t bit);
+
+void Write1Byte(uint8_t Addr, uint8_t Data);
+uint8_t Read8bit(uint8_t Addr);
+uint16_t Read12Bit(uint8_t Addr);
+uint16_t Read13Bit(uint8_t Addr);
+uint16_t Read16bit(uint8_t Addr);
+uint32_t Read24bit(uint8_t Addr);
+uint32_t Read32bit(uint8_t Addr);
+void ReadBuff(uint8_t Addr, uint8_t Size, uint8_t *Buff);
+bool writeRegister8Array(const std::uint8_t *reg_data_array,
+ std::size_t length);
+
+uint8_t calcVoltageData(uint16_t value, uint16_t maxv, uint16_t minv,
+ uint16_t step);
+
+class AXP {
+ public:
+ enum pmic_t { pmic_unknown = 0, pmic_axp192, pmic_axp2101 };
+
+ AXP192 axp192;
+ AXP2101 axp2101;
+ INA3221 ina3221;
+
+ enum CHGCurrent {
+ kCHG_100mA = 0,
+ kCHG_190mA,
+ kCHG_280mA,
+ kCHG_360mA,
+ kCHG_450mA,
+ kCHG_550mA,
+ kCHG_630mA,
+ kCHG_700mA,
+ kCHG_780mA,
+ kCHG_880mA,
+ kCHG_960mA,
+ kCHG_1000mA,
+ kCHG_1080mA,
+ kCHG_1160mA,
+ kCHG_1240mA,
+ kCHG_1320mA,
+ };
+
+ AXP();
+ void begin();
+ // Will be deprecated
+ void begin(mbus_mode_t mode);
+ void ScreenBreath(int brightness);
+ bool GetBatState();
+
+ void EnableCoulombcounter(void);
+ void DisableCoulombcounter(void);
+ void StopCoulombcounter(void);
+ void ClearCoulombcounter(void);
+ uint32_t GetCoulombchargeData(void);
+ uint32_t GetCoulombdischargeData(void);
+ float GetCoulombData(void);
+ float GetBatteryLevel(void);
+ void PowerOff(void);
+ void SetSleep(void) {
+ PowerOff();
+ };
+
+ void SetAdcState(bool state);
+ // -- sleep
+ void PrepareToSleep(void);
+ void RestoreFromLightSleep(void);
+ void DeepSleep(uint64_t time_in_us = 0);
+ void LightSleep(uint64_t time_in_us = 0);
+ uint8_t GetWarningLeve(void);
+
+ // void SetChargeVoltage( uint8_t );
+ // void SetChargeCurrent( uint8_t );
+ float GetBatVoltage();
+ float GetBatCurrent();
+ float GetVinVoltage();
+ float GetVinCurrent();
+ float GetVBusVoltage();
+ float GetVBusCurrent();
+ float GetTempInAXP192();
+ float GetBatPower();
+ float GetBatChargeCurrent();
+ float GetAPSVoltage();
+ float GetBatCoulombInput();
+ float GetBatCoulombOut();
+ uint8_t GetWarningLevel(void);
+ void SetCoulombClear();
+ void SetLDO2(bool State);
+ void SetDCDC3(bool State);
+
+ uint8_t AXPInState();
+ bool isACIN();
+ bool isCharging();
+ bool isVBUS();
+
+ void SetLDOVoltage(uint8_t number, uint16_t voltage);
+ void SetDCVoltage(uint8_t number, uint16_t voltage);
+ void SetESPVoltage(uint16_t voltage);
+ void SetLcdVoltage(uint16_t voltage);
+ void SetLDOEnable(uint8_t number, bool state);
+ void SetLCDRSet(bool state);
+ void SetBusPowerMode(uint8_t state);
+ void SetLed(uint8_t state);
+ void SetSpkEnable(uint8_t state);
+ void SetCHGCurrent(uint8_t state);
+ void SetPeripherialsPower(uint8_t state);
+ void SetVibration(uint8_t state);
+
+ pmic_t getPmicType() const {
+ return _pmic;
+ }
+
+ private:
+ pmic_t _pmic;
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/AXP192.cpp b/lib/M5Core2/src/AXP192.cpp
new file mode 100644
index 000000000..683f48ef5
--- /dev/null
+++ b/lib/M5Core2/src/AXP192.cpp
@@ -0,0 +1,473 @@
+#if defined (CORE2)
+#include "AXP192.h"
+#include "AXP.h"
+
+AXP192::AXP192() {
+}
+
+void AXP192::begin() {
+ // Wire1.begin(21, 22);
+ // Wire1.setClock(400000);
+
+ // AXP192 30H
+ Write1Byte(0x30, (Read8bit(0x30) & 0x04) | 0X02);
+ Serial.printf("axp: vbus limit off\n");
+
+ // AXP192 GPIO1:OD OUTPUT
+ Write1Byte(0x92, Read8bit(0x92) & 0xf8);
+ Serial.printf("axp: gpio1 init\n");
+
+ // AXP192 GPIO2:OD OUTPUT
+ Write1Byte(0x93, Read8bit(0x93) & 0xf8);
+ Serial.printf("axp: gpio2 init\n");
+
+ // AXP192 RTC CHG
+ Write1Byte(0x35, (Read8bit(0x35) & 0x1c) | 0xa2);
+ Serial.printf("axp: rtc battery charging enabled\n");
+
+ SetESPVoltage(3350);
+ Serial.printf("axp: esp32 power voltage was set to 3.35v\n");
+
+ SetLcdVoltage(2800);
+ Serial.printf("axp: lcd backlight voltage was set to 2.80v\n");
+
+ SetLDOVoltage(2, 3300); // Periph power voltage preset (LCD_logic, SD card)
+ Serial.printf("axp: lcd logic and sdcard voltage preset to 3.3v\n");
+
+ SetLDOVoltage(3, 2000); // Vibrator power voltage preset
+ Serial.printf("axp: vibrator voltage preset to 2v\n");
+
+ SetLDOEnable(2, true);
+ SetDCDC3(true); // LCD backlight
+ SetLed(true);
+
+ SetCHGCurrent(kCHG_100mA);
+ // SetAxpPriphPower(1);
+ // Serial.printf("axp: lcd_logic and sdcard power enabled\n\n");
+
+ // pinMode(39, INPUT_PULLUP);
+
+ // AXP192 GPIO4
+ Write1Byte(0X95, (Read8bit(0x95) & 0x72) | 0X84);
+
+ Write1Byte(0X36, 0X4C);
+
+ Write1Byte(0x82, 0xff);
+
+ SetLCDRSet(0);
+ delay(100);
+ SetLCDRSet(1);
+ delay(100);
+ // I2C_WriteByteDataAt(0X15,0XFE,0XFF);
+
+ // axp: check v-bus status
+ if (Read8bit(0x00) & 0x08) {
+ Write1Byte(0x30, Read8bit(0x30) | 0x80);
+ // if v-bus can use, disable M-Bus 5V output to input
+ SetBusPowerMode(kMBusModeInput);
+ } else {
+ // if not, enable M-Bus 5V output
+ SetBusPowerMode(kMBusModeOutput);
+ }
+}
+
+void AXP192::ScreenBreath(int brightness) {
+ if (brightness >= 100)
+ brightness = 100;
+ else if (brightness < 0)
+ brightness = 0;
+ int vol = map(brightness, 0, 100, 2400, 3300);
+ SetLcdVoltage((uint16_t)vol);
+}
+
+bool AXP192::GetBatState() {
+ if (Read8bit(0x01) | 0x20)
+ return true;
+ else
+ return false;
+}
+//---------coulombcounter_from_here---------
+// enable: void EnableCoulombcounter(void);
+// disable: void DisableCOulombcounter(void);
+// stop: void StopCoulombcounter(void);
+// clear: void ClearCoulombcounter(void);
+// get charge data: uint32_t GetCoulombchargeData(void);
+// get discharge data: uint32_t GetCoulombdischargeData(void);
+// get coulomb val affter calculation: float GetCoulombData(void);
+//------------------------------------------
+void AXP192::EnableCoulombcounter(void) {
+ Write1Byte(0xB8, 0x80);
+}
+
+void AXP192::DisableCoulombcounter(void) {
+ Write1Byte(0xB8, 0x00);
+}
+
+void AXP192::StopCoulombcounter(void) {
+ Write1Byte(0xB8, 0xC0);
+}
+
+void AXP192::ClearCoulombcounter(void) {
+ Write1Byte(0xB8, 0xA0);
+}
+
+uint32_t AXP192::GetCoulombchargeData(void) {
+ return Read32bit(0xB0);
+}
+
+uint32_t AXP192::GetCoulombdischargeData(void) {
+ return Read32bit(0xB4);
+}
+
+float AXP192::GetCoulombData(void) {
+ uint32_t coin = 0;
+ uint32_t coout = 0;
+
+ coin = GetCoulombchargeData();
+ coout = GetCoulombdischargeData();
+
+ // c = 65536 * current_LSB * (coin - coout) / 3600 / ADC rate
+ // Adc rate can be read from 84H ,change this variable if you change the ADC
+ // reate
+ float ccc = 65536 * 0.5 * (int32_t)(coin - coout) / 3600.0 / 25.0;
+ return ccc;
+}
+
+// Cut all power, except for LDO1 (RTC)
+void AXP192::PowerOff(void) {
+ Write1Byte(0x32, Read8bit(0x32) | 0b10000000);
+}
+
+void AXP192::SetAdcState(bool state) {
+ // Enable / Disable all ADCs
+ Write1Byte(0x82, state ? 0xff : 0x00);
+}
+
+void AXP192::PrepareToSleep(void) {
+ // Disable ADCs
+ SetAdcState(false);
+
+ // Turn LED off
+ SetLed(false);
+
+ // Turn LCD backlight off
+ SetDCDC3(false);
+}
+
+// Get current battery level
+float AXP192::GetBatteryLevel(void) {
+ const float batVoltage = GetBatVoltage();
+ const float batPercentage =
+ (batVoltage < 3.248088) ? (0) : (batVoltage - 3.120712) * 100;
+ return (batPercentage <= 100) ? batPercentage : 100;
+}
+
+void AXP192::RestoreFromLightSleep(void) {
+ // Turn LCD backlight on
+ SetDCDC3(true);
+
+ // Turn LED on
+ SetLed(true);
+
+ // Enable ADCs
+ SetAdcState(true);
+}
+
+uint8_t AXP192::GetWarningLeve(void) {
+ Wire1.beginTransmission(AXP192_ADDR);
+ Wire1.write(0x47);
+ Wire1.endTransmission();
+ Wire1.requestFrom(AXP192_ADDR, 1);
+ uint8_t buf = Wire1.read();
+ return (buf & 0x01);
+}
+
+// -- sleep
+void AXP192::DeepSleep(uint64_t time_in_us) {
+ PrepareToSleep();
+
+ if (time_in_us > 0) {
+ esp_sleep_enable_timer_wakeup(time_in_us);
+ } else {
+ esp_sleep_disable_wakeup_source(ESP_SLEEP_WAKEUP_TIMER);
+ }
+ (time_in_us == 0) ? esp_deep_sleep_start() : esp_deep_sleep(time_in_us);
+
+ // Never reached - after deep sleep ESP32 restarts
+}
+
+void AXP192::LightSleep(uint64_t time_in_us) {
+ PrepareToSleep();
+
+ if (time_in_us > 0) {
+ esp_sleep_enable_timer_wakeup(time_in_us);
+ } else {
+ esp_sleep_disable_wakeup_source(ESP_SLEEP_WAKEUP_TIMER);
+ }
+ esp_light_sleep_start();
+
+ RestoreFromLightSleep();
+}
+
+uint8_t AXP192::GetWarningLevel(void) {
+ return Read8bit(0x47) & 0x01;
+}
+
+float AXP192::GetBatVoltage() {
+ float ADCLSB = 1.1 / 1000.0;
+ uint16_t ReData = Read12Bit(0x78);
+ return ReData * ADCLSB;
+}
+
+float AXP192::GetBatCurrent() {
+ float ADCLSB = 0.5;
+ uint16_t CurrentIn = Read13Bit(0x7A);
+ uint16_t CurrentOut = Read13Bit(0x7C);
+ return (CurrentIn - CurrentOut) * ADCLSB;
+}
+
+float AXP192::GetVinVoltage() {
+ float ADCLSB = 1.7 / 1000.0;
+ uint16_t ReData = Read12Bit(0x56);
+ return ReData * ADCLSB;
+}
+
+float AXP192::GetVinCurrent() {
+ float ADCLSB = 0.625;
+ uint16_t ReData = Read12Bit(0x58);
+ return ReData * ADCLSB;
+}
+
+float AXP192::GetVBusVoltage() {
+ float ADCLSB = 1.7 / 1000.0;
+ uint16_t ReData = Read12Bit(0x5A);
+ return ReData * ADCLSB;
+}
+
+float AXP192::GetVBusCurrent() {
+ float ADCLSB = 0.375;
+ uint16_t ReData = Read12Bit(0x5C);
+ return ReData * ADCLSB;
+}
+
+float AXP192::GetTempInAXP192() {
+ float ADCLSB = 0.1;
+ const float OFFSET_DEG_C = -144.7;
+ uint16_t ReData = Read12Bit(0x5E);
+ return OFFSET_DEG_C + ReData * ADCLSB;
+}
+
+float AXP192::GetBatPower() {
+ float VoltageLSB = 1.1;
+ float CurrentLCS = 0.5;
+ uint32_t ReData = Read24bit(0x70);
+ return VoltageLSB * CurrentLCS * ReData / 1000.0;
+}
+
+float AXP192::GetBatChargeCurrent() {
+ float ADCLSB = 0.5;
+ uint16_t ReData = Read12Bit(0x7A);
+ return ReData * ADCLSB;
+}
+float AXP192::GetAPSVoltage() {
+ float ADCLSB = 1.4 / 1000.0;
+ uint16_t ReData = Read12Bit(0x7E);
+ return ReData * ADCLSB;
+}
+
+float AXP192::GetBatCoulombInput() {
+ uint32_t ReData = Read32bit(0xB0);
+ return ReData * 65536 * 0.5 / 3600 / 25.0;
+}
+
+float AXP192::GetBatCoulombOut() {
+ uint32_t ReData = Read32bit(0xB4);
+ return ReData * 65536 * 0.5 / 3600 / 25.0;
+}
+
+void AXP192::SetCoulombClear() {
+ Write1Byte(0xB8, 0x20);
+}
+
+void AXP192::SetLDO2(bool State) {
+ uint8_t buf = Read8bit(0x12);
+ if (State == true)
+ buf = (1 << 2) | buf;
+ else
+ buf = ~(1 << 2) & buf;
+ Write1Byte(0x12, buf);
+}
+
+void AXP192::SetDCDC3(bool State) {
+ uint8_t buf = Read8bit(0x12);
+ if (State == true)
+ buf = (1 << 1) | buf;
+ else
+ buf = ~(1 << 1) & buf;
+ Write1Byte(0x12, buf);
+}
+
+uint8_t AXP192::AXPInState() {
+ return Read8bit(0x00);
+}
+bool AXP192::isACIN() {
+ return (Read8bit(0x00) & 0x80) ? true : false;
+}
+bool AXP192::isCharging() {
+ return (Read8bit(0x00) & 0x04) ? true : false;
+}
+bool AXP192::isVBUS() {
+ return (Read8bit(0x00) & 0x20) ? true : false;
+}
+
+void AXP192::SetLDOVoltage(uint8_t number, uint16_t voltage) {
+ uint8_t vdata = calcVoltageData(voltage, 3300, 1800, 100) & 0x0F;
+ switch (number) {
+ // uint8_t reg, data;
+ case 2:
+ Write1Byte(0x28, (Read8bit(0x28) & 0x0F) | (vdata << 4));
+ break;
+ case 3:
+ Write1Byte(0x28, (Read8bit(0x28) & 0xF0) | vdata);
+ break;
+ }
+}
+
+/// @param number 0=DCDC1 / 1=DCDC2 / 2=DCDC3
+void AXP192::SetDCVoltage(uint8_t number, uint16_t voltage) {
+ uint8_t addr;
+ uint8_t vdata;
+ if (number > 2) return;
+ switch (number) {
+ case 0:
+ addr = 0x26;
+ vdata = calcVoltageData(voltage, 3500, 700, 25) & 0x7F;
+ break;
+ case 1:
+ addr = 0x25;
+ vdata = calcVoltageData(voltage, 2275, 700, 25) & 0x3F;
+ break;
+ case 2:
+ addr = 0x27;
+ vdata = calcVoltageData(voltage, 3500, 700, 25) & 0x7F;
+ break;
+ }
+ // Serial.printf("result:%hhu\n", (Read8bit(addr) & 0X80) | (voltage &
+ // 0X7F)); Serial.printf("result:%d\n", (Read8bit(addr) & 0X80) | (voltage &
+ // 0X7F)); Serial.printf("result:%x\n", (Read8bit(addr) & 0X80) | (voltage &
+ // 0X7F));
+ Write1Byte(addr, (Read8bit(addr) & 0x80) | vdata);
+}
+
+void AXP192::SetESPVoltage(uint16_t voltage) {
+ if (voltage >= 3000 && voltage <= 3400) {
+ SetDCVoltage(0, voltage);
+ }
+}
+
+void AXP192::SetLcdVoltage(uint16_t voltage) {
+ if (voltage >= 2500 && voltage <= 3300) {
+ SetDCVoltage(2, voltage);
+ }
+}
+
+void AXP192::SetLDOEnable(uint8_t number, bool state) {
+ uint8_t mark = 0x01;
+ if ((number < 2) || (number > 3)) return;
+
+ mark <<= number;
+ if (state) {
+ Write1Byte(0x12, (Read8bit(0x12) | mark));
+ } else {
+ Write1Byte(0x12, (Read8bit(0x12) & (~mark)));
+ }
+}
+
+void AXP192::SetLCDRSet(bool state) {
+ uint8_t reg_addr = 0x96;
+ uint8_t gpio_bit = 0x02;
+ uint8_t data;
+ data = Read8bit(reg_addr);
+
+ if (state) {
+ data |= gpio_bit;
+ } else {
+ data &= ~gpio_bit;
+ }
+
+ Write1Byte(reg_addr, data);
+}
+
+// Select source for BUS_5V
+// 0 : use internal boost
+// 1 : powered externally
+void AXP192::SetBusPowerMode(uint8_t state) {
+ uint8_t data;
+ if (state == 0) {
+ // Set GPIO to 3.3V (LDO OUTPUT mode)
+ data = Read8bit(0x91);
+ Write1Byte(0x91, (data & 0x0F) | 0xF0);
+ // Set GPIO0 to LDO OUTPUT, pullup N_VBUSEN to disable VBUS supply from
+ // BUS_5V
+ data = Read8bit(0x90);
+ Write1Byte(0x90, (data & 0xF8) | 0x02);
+ // Set EXTEN to enable 5v boost
+ data = Read8bit(0x10);
+ Write1Byte(0x10, data | 0x04);
+ } else {
+ // Set EXTEN to disable 5v boost
+ data = Read8bit(0x10);
+ Write1Byte(0x10, data & ~0x04);
+ // Set GPIO0 to float, using enternal pulldown resistor to enable VBUS
+ // supply from BUS_5V
+ data = Read8bit(0x90);
+ Write1Byte(0x90, (data & 0xF8) | 0x07);
+ }
+}
+
+void AXP192::SetLed(uint8_t state) {
+ uint8_t reg_addr = 0x94;
+ uint8_t data;
+ data = Read8bit(reg_addr);
+
+ if (state) {
+ data = data & 0XFD;
+ } else {
+ data |= 0X02;
+ }
+
+ Write1Byte(reg_addr, data);
+}
+
+// set led state(GPIO high active,set 1 to enable amplifier)
+void AXP192::SetSpkEnable(uint8_t state) {
+ uint8_t reg_addr = 0x94;
+ uint8_t gpio_bit = 0x04;
+ uint8_t data;
+ data = Read8bit(reg_addr);
+
+ if (state) {
+ data |= gpio_bit;
+ } else {
+ data &= ~gpio_bit;
+ }
+
+ Write1Byte(reg_addr, data);
+}
+
+void AXP192::SetCHGCurrent(uint8_t state) {
+ uint8_t data = Read8bit(0x33);
+ data &= 0xf0;
+ data = data | (state & 0x0f);
+ Write1Byte(0x33, data);
+}
+
+void AXP192::SetPeripherialsPower(uint8_t state) {
+ if (!state)
+ Write1Byte(0x10, Read8bit(0x10) & 0XFB);
+ else if (state)
+ Write1Byte(0x10, Read8bit(0x10) | 0X04);
+ // uint8_t data;
+ // Set EXTEN to enable 5v boost
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/AXP192.h b/lib/M5Core2/src/AXP192.h
new file mode 100644
index 000000000..93ab88363
--- /dev/null
+++ b/lib/M5Core2/src/AXP192.h
@@ -0,0 +1,92 @@
+#if defined (CORE2)
+#ifndef __AXP192_H__
+#define __AXP192_H__
+
+#include <Wire.h>
+#include <Arduino.h>
+
+#define AXP192_ADDR 0X34
+
+class AXP192 {
+ public:
+ enum CHGCurrent {
+ kCHG_100mA = 0,
+ kCHG_190mA,
+ kCHG_280mA,
+ kCHG_360mA,
+ kCHG_450mA,
+ kCHG_550mA,
+ kCHG_630mA,
+ kCHG_700mA,
+ kCHG_780mA,
+ kCHG_880mA,
+ kCHG_960mA,
+ kCHG_1000mA,
+ kCHG_1080mA,
+ kCHG_1160mA,
+ kCHG_1240mA,
+ kCHG_1320mA,
+ };
+
+ AXP192();
+ void begin();
+ // Will be deprecated
+ void ScreenBreath(int brightness);
+ bool GetBatState();
+
+ void EnableCoulombcounter(void);
+ void DisableCoulombcounter(void);
+ void StopCoulombcounter(void);
+ void ClearCoulombcounter(void);
+ uint32_t GetCoulombchargeData(void);
+ uint32_t GetCoulombdischargeData(void);
+ float GetCoulombData(void);
+ float GetBatteryLevel(void);
+ void PowerOff(void);
+ void SetAdcState(bool state);
+ // -- sleep
+ void PrepareToSleep(void);
+ void RestoreFromLightSleep(void);
+ void DeepSleep(uint64_t time_in_us = 0);
+ void LightSleep(uint64_t time_in_us = 0);
+ uint8_t GetWarningLeve(void);
+
+ // void SetChargeVoltage( uint8_t );
+ // void SetChargeCurrent( uint8_t );
+ float GetBatVoltage();
+ float GetBatCurrent();
+ float GetVinVoltage();
+ float GetVinCurrent();
+ float GetVBusVoltage();
+ float GetVBusCurrent();
+ float GetTempInAXP192();
+ float GetBatPower();
+ float GetBatChargeCurrent();
+ float GetAPSVoltage();
+ float GetBatCoulombInput();
+ float GetBatCoulombOut();
+ uint8_t GetWarningLevel(void);
+ void SetCoulombClear();
+ void SetLDO2(bool State);
+ void SetDCDC3(bool State);
+
+ uint8_t AXPInState();
+ bool isACIN();
+ bool isCharging();
+ bool isVBUS();
+
+ void SetLDOVoltage(uint8_t number, uint16_t voltage);
+ void SetDCVoltage(uint8_t number, uint16_t voltage);
+ void SetESPVoltage(uint16_t voltage);
+ void SetLcdVoltage(uint16_t voltage);
+ void SetLDOEnable(uint8_t number, bool state);
+ void SetLCDRSet(bool state);
+ void SetBusPowerMode(uint8_t state);
+ void SetLed(uint8_t state);
+ void SetSpkEnable(uint8_t state);
+ void SetCHGCurrent(uint8_t state);
+ void SetPeripherialsPower(uint8_t state);
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/AXP2101.cpp b/lib/M5Core2/src/AXP2101.cpp
new file mode 100644
index 000000000..cfaa4a12c
--- /dev/null
+++ b/lib/M5Core2/src/AXP2101.cpp
@@ -0,0 +1,482 @@
+#if defined (CORE2)
+#include "AXP2101.h"
+
+bool AXP2101::begin(TwoWire* wire, uint8_t addr, uint8_t sda, uint8_t scl,
+ uint32_t speed) {
+ _wire = wire;
+ _addr = addr;
+ _sda = sda;
+ _scl = scl;
+ _speed = speed;
+ _wire->begin(_sda, _scl, _speed);
+ delay(10);
+ _wire->beginTransmission(_addr);
+ uint8_t error = _wire->endTransmission();
+ if (error == 0) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+void AXP2101::set_bus_3v3(uint16_t voltage) {
+ if (!voltage) {
+ set_dcdc1_on_off(false);
+ set_dcdc3_on_off(false);
+ } else {
+ set_dcdc1_on_off(true);
+ set_dcdc3_on_off(true);
+ axp2101_set_dcdc1_voltage(voltage);
+ axp2101_set_dcdc3_voltage(voltage);
+ }
+}
+
+void AXP2101::set_lcd_back_light_voltage(uint16_t voltage) {
+ if (!voltage) {
+ set_bldo1_on_off(false);
+ } else {
+ set_bldo1_on_off(true);
+ axp2101_set_bldo1_voltage(voltage);
+ }
+}
+
+void AXP2101::set_bus_5v(uint8_t sw) {
+ if (sw) {
+ set_blod2_on_off(true);
+ axp2101_set_bldo2_voltage(3300);
+ } else {
+ set_blod2_on_off(false);
+ }
+}
+
+bool AXP2101::set_sys_led(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result = bitOn(AXP2101_ADDR, AXP2101_CHGLED_REG, 0b00110000, _speed);
+ } else {
+ result = bitOff(AXP2101_ADDR, AXP2101_CHGLED_REG, 0b00110000, _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_charger_term_current_to_zero(void) {
+ bool result = false;
+
+ result =
+ bitOff(AXP2101_ADDR, AXP2101_CHARGER_SETTING_REG, 0b00001111, _speed);
+
+ return result;
+}
+
+bool AXP2101::set_charger_constant_current_to_50mA(void) {
+ bool result = false;
+
+ result = writeRegister8(AXP2101_ADDR, AXP2101_ICC_CHARGER_SETTING_REG, 2,
+ _speed);
+
+ return result;
+}
+
+void AXP2101::set_spk(bool sw) {
+ if (sw) {
+ set_aldo3_on_off(true);
+ axp2101_set_aldo3_voltage(3300);
+ } else {
+ set_aldo3_on_off(false);
+ }
+}
+
+void AXP2101::set_lcd_rst(bool sw) {
+ if (sw) {
+ set_aldo2_on_off(true);
+ axp2101_set_aldo2_voltage(3300);
+ } else {
+ set_aldo2_on_off(false);
+ }
+}
+
+void AXP2101::set_lcd_and_tf_voltage(uint16_t voltage) {
+ if (!voltage) {
+ set_aldo4_on_off(false);
+ } else {
+ set_aldo4_on_off(true);
+ axp2101_set_aldo4_voltage(voltage);
+ }
+}
+
+void AXP2101::set_vib_motor_voltage(uint16_t voltage) {
+ if (!voltage) {
+ set_dldo1_on_off(false);
+ } else {
+ set_dldo1_on_off(true);
+ axp2101_set_dldo1_voltage(voltage);
+ }
+}
+
+bool AXP2101::axp2101_set_dldo1_voltage(uint16_t voltage) {
+ uint16_t temp;
+
+ temp = voltage;
+ if (temp < 500) temp = 500;
+ if (temp > 3400) temp = 3400;
+ if (writeRegister8(AXP2101_ADDR, AXP2101_DLDO1_VOLTAGE_REG,
+ (temp - 500) / 100, _speed)) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+bool AXP2101::axp2101_set_aldo3_voltage(uint16_t voltage) {
+ uint16_t temp;
+
+ temp = voltage;
+ if (temp < 500) temp = 500;
+ // AXP2101 BLDO1 max voltage value is 3500mV
+ if (temp > 3500) temp = 3500;
+ if (writeRegister8(AXP2101_ADDR, AXP2101_ALDO3_VOLTAGE_REG,
+ (temp - 500) / 100, _speed)) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+void AXP2101::set_bat_charge(bool enable) {
+ uint8_t val = 0;
+ if (readRegister(AXP2101_ADDR, 0x18, &val, 1, _speed)) {
+ writeRegister8(AXP2101_ADDR, 0x18, (val & 0xFD) + (enable << 1),
+ _speed);
+ }
+}
+
+bool AXP2101::axp2101_enable_pwrok_resets(void) {
+ return (bitOn(AXP2101_ADDR, 0x10, 1 << 3, _speed));
+}
+
+void AXP2101::power_off(void) {
+ // 1. AXP2101 Power off
+ bitOn(AXP2101_ADDR, 0x41, 1 << 1,
+ 400000); // POWERON Negative Edge IRQ(ponne_irq_en) enable
+ writeRegister8(AXP2101_ADDR, 0x25, 0b00011011,
+ 400000); // sleep and wait for wakeup
+ delay(100);
+ writeRegister8(AXP2101_ADDR, 0x10, 0b00110001, 400000); // power off
+}
+
+bool AXP2101::readRegister(uint8_t addr, uint8_t reg, uint8_t* result,
+ uint16_t length, uint32_t freq) {
+ uint8_t index = 0;
+ uint8_t err = 0;
+
+ _wire->beginTransmission(addr);
+ _wire->write(reg);
+ err = _wire->endTransmission();
+ _wire->requestFrom(addr, length);
+ for (int i = 0; i < length; i++) {
+ result[index++] = _wire->read();
+ }
+ if (err == 0) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+uint8_t AXP2101::readRegister8(uint8_t addr, uint8_t reg, uint32_t freq) {
+ uint8_t result;
+
+ _wire->beginTransmission(addr);
+ _wire->write(reg);
+ _wire->endTransmission();
+ _wire->requestFrom(addr, 1);
+ result = _wire->read();
+ return result;
+}
+
+bool AXP2101::writeRegister8(uint8_t addr, uint8_t reg, uint8_t data,
+ uint32_t freq) {
+ uint8_t result;
+
+ _wire->beginTransmission(addr);
+ _wire->write(reg);
+ _wire->write(data);
+ result = _wire->endTransmission();
+ if (result == 0) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+bool AXP2101::bitOn(uint8_t addr, uint8_t reg, uint8_t data, uint32_t freq) {
+ uint8_t temp;
+ uint8_t write_back;
+
+ temp = readRegister8(addr, reg, freq);
+ write_back = (temp | data);
+ // Serial.printf("biton read 0x%X, data = 0x%X, write back 0x%X\r\n", temp,
+ // data, write_back);
+ return (writeRegister8(addr, reg, write_back, freq));
+}
+
+bool AXP2101::bitOff(uint8_t addr, uint8_t reg, uint8_t data, uint32_t freq) {
+ uint8_t temp;
+ uint8_t write_back;
+
+ temp = readRegister8(addr, reg, freq);
+ write_back = (temp & (~data));
+ return (writeRegister8(addr, reg, write_back, freq));
+}
+
+uint8_t AXP2101::axp2101_get_dcdc_status(void) {
+ return readRegister8(_addr, AXP2101_DCDC_CTRL_REG, _speed);
+}
+
+bool AXP2101::axp2101_set_bldo1_voltage(uint16_t voltage) {
+ uint16_t temp;
+
+ temp = voltage;
+ if (temp < 500) temp = 500;
+ // AXP2101 BLDO1 max voltage value is 3500mV
+ if (temp > 3500) temp = 3500;
+ if (writeRegister8(AXP2101_ADDR, AXP2101_BLDO1_VOLTAGE_REG,
+ (temp - 500) / 100, _speed)) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+uint8_t AXP2101::axp2101_get_bldo1_voltage(void) {
+ return readRegister8(_addr, AXP2101_BLDO1_VOLTAGE_REG, _speed);
+}
+
+bool AXP2101::axp2101_set_bldo2_voltage(uint16_t voltage) {
+ uint16_t temp;
+
+ temp = voltage;
+ if (temp < 500) temp = 500;
+ // AXP2101 BLDO1 max voltage value is 3500mV
+ if (temp > 3500) temp = 3500;
+ if (writeRegister8(AXP2101_ADDR, AXP2101_BLDO2_VOLTAGE_REG,
+ (temp - 500) / 100, _speed)) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+uint8_t AXP2101::axp2101_get_bldo2_voltage(void) {
+ return readRegister8(_addr, AXP2101_BLDO2_VOLTAGE_REG, _speed);
+}
+
+bool AXP2101::set_dldo1_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result =
+ bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_DLDO1_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_DLDO1_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_cpusldo_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result = bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_CPUSLDO_CTRL_MASK,
+ _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_CPUSLDO_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_blod2_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result =
+ bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_BLDO2_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_BLDO2_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_bldo1_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result =
+ bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_BLDO1_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_BLDO1_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_aldo4_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result =
+ bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO4_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO4_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_aldo3_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result =
+ bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO3_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO3_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_aldo2_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result =
+ bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO2_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO2_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_aldo1_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result =
+ bitOn(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO1_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(_addr, AXP2101_LDO_CTRL_REG, AXP2101_ALDO1_CTRL_MASK,
+ _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_dcdc1_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result = bitOn(AXP2101_ADDR, AXP2101_DCDC_CTRL_REG,
+ AXP2101_DCDC1_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(AXP2101_ADDR, AXP2101_DCDC_CTRL_REG,
+ AXP2101_DCDC1_CTRL_MASK, _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_dcdc2_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result = bitOn(AXP2101_ADDR, AXP2101_DCDC_CTRL_REG,
+ AXP2101_DCDC2_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(AXP2101_ADDR, AXP2101_DCDC_CTRL_REG,
+ AXP2101_DCDC2_CTRL_MASK, _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::set_dcdc3_on_off(bool sw) {
+ bool result = false;
+
+ if (sw) {
+ result = bitOn(AXP2101_ADDR, AXP2101_DCDC_CTRL_REG,
+ AXP2101_DCDC3_CTRL_MASK, _speed);
+ } else {
+ result = bitOff(AXP2101_ADDR, AXP2101_DCDC_CTRL_REG,
+ AXP2101_DCDC3_CTRL_MASK, _speed);
+ }
+
+ return result;
+}
+
+bool AXP2101::axp2101_set_aldo4_voltage(uint16_t voltage) {
+ uint16_t temp;
+
+ temp = voltage;
+ if (temp < 500) temp = 500;
+ // AXP2101 BLDO1 max voltage value is 3500mV
+ if (temp > 3500) temp = 3500;
+ if (writeRegister8(AXP2101_ADDR, AXP2101_ALDO4_VOLTAGE_REG,
+ (temp - 500) / 100, _speed)) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+bool AXP2101::axp2101_set_dcdc1_voltage(uint16_t voltage) {
+ uint16_t temp;
+
+ temp = voltage;
+ if (temp < 1500) temp = 1500;
+ // AXP2101 DCDC1 max voltage value is 3400mV
+ if (temp > 3400) temp = 3400;
+ if (writeRegister8(AXP2101_ADDR, AXP2101_DCDC1_VOLTAGE_REG,
+ (temp - 1500) / 100, _speed)) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+bool AXP2101::axp2101_set_dcdc3_voltage(uint16_t voltage) {
+ uint16_t temp;
+ uint8_t data;
+
+ temp = voltage;
+ if (temp < 1500)
+ temp = 1500;
+ else if (temp > 1540 && temp < 1600)
+ temp = 1540;
+ else if (temp > 3400)
+ temp = 3400;
+
+ if (temp <= 1540)
+ data = ((temp - 1220) / 20 + (uint8_t)0b01000111);
+ else
+ data = ((temp - 1600) / 100 + (uint8_t)0b01011000);
+ if (writeRegister8(AXP2101_ADDR, AXP2101_DCDC3_VOLTAGE_REG, data, _speed)) {
+ return true;
+ } else {
+ return false;
+ }
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/AXP2101.h b/lib/M5Core2/src/AXP2101.h
new file mode 100644
index 000000000..6f461704d
--- /dev/null
+++ b/lib/M5Core2/src/AXP2101.h
@@ -0,0 +1,105 @@
+#if defined (CORE2)
+#ifndef __AXP2101_H
+#define __AXP2101_H
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#define AXP2101_ADDR 0x34
+
+#define AXP2101_SLAVE_ADDRESS 0x34
+#define AXP2101_DCDC1_VOLTAGE_REG 0x82
+#define AXP2101_DCDC3_VOLTAGE_REG 0x84
+#define AXP2101_DCDC_CTRL_REG 0x80
+#define AXP2101_LDO_CTRL_REG 0x90
+#define AXP2101_BLDO1_VOLTAGE_REG 0x96
+#define AXP2101_BLDO2_VOLTAGE_REG 0x97
+#define AXP2101_CHGLED_REG 0x69
+#define AXP2101_CHARGER_SETTING_REG 0x63
+#define AXP2101_ICC_CHARGER_SETTING_REG 0x62
+#define AXP2101_ALDO3_VOLTAGE_REG 0x94
+#define AXP2101_ALDO2_VOLTAGE_REG 0x93
+#define AXP2101_ALDO4_VOLTAGE_REG 0x95
+#define AXP2101_DLDO1_VOLTAGE_REG 0x99
+#define AXP2101_PMU_CONFIG_REG 0x10
+#define AXP2101_ADC_ENABLE_REG 0x30
+
+#define AXP2101_DLDO1_CTRL_MASK 1 << 7
+#define AXP2101_CPUSLDO_CTRL_MASK 1 << 6
+#define AXP2101_BLDO2_CTRL_MASK 1 << 5
+#define AXP2101_BLDO1_CTRL_MASK 1 << 4
+#define AXP2101_ALDO4_CTRL_MASK 1 << 3
+#define AXP2101_ALDO3_CTRL_MASK 1 << 2
+#define AXP2101_ALDO2_CTRL_MASK 1 << 1
+#define AXP2101_ALDO1_CTRL_MASK 1 << 0
+
+#define AXP2101_DCDC5_CTRL_MASK 1 << 4
+#define AXP2101_DCDC4_CTRL_MASK 1 << 3
+#define AXP2101_DCDC3_CTRL_MASK 1 << 2
+#define AXP2101_DCDC2_CTRL_MASK 1 << 1
+#define AXP2101_DCDC1_CTRL_MASK 1 << 0
+
+class AXP2101 {
+ private:
+ uint8_t _addr;
+ TwoWire* _wire;
+ uint8_t _scl;
+ uint8_t _sda;
+ uint8_t _speed;
+
+ public:
+ bool begin(TwoWire* wire = &Wire, uint8_t addr = AXP2101_ADDR,
+ uint8_t sda = 21, uint8_t scl = 22, uint32_t speed = 400000L);
+
+ bool readRegister(uint8_t addr, uint8_t reg, uint8_t* result,
+ uint16_t length, uint32_t freq);
+ uint8_t readRegister8(uint8_t addr, uint8_t reg, uint32_t freq);
+ bool writeRegister8(uint8_t addr, uint8_t reg, uint8_t data, uint32_t freq);
+ bool bitOn(uint8_t addr, uint8_t reg, uint8_t data, uint32_t freq);
+ bool bitOff(uint8_t addr, uint8_t reg, uint8_t data, uint32_t freq);
+ void set_bus_3v3(uint16_t voltage);
+ void set_lcd_back_light_voltage(uint16_t voltage);
+ void set_bus_5v(uint8_t sw);
+ bool set_sys_led(bool sw);
+ void set_spk(bool sw);
+ void set_lcd_rst(bool sw);
+ void set_lcd_and_tf_voltage(uint16_t voltage);
+ void set_vib_motor_voltage(uint16_t voltage);
+ void set_bat_charge(bool enable);
+ void power_off(void);
+ bool set_charger_term_current_to_zero(void);
+ bool set_charger_constant_current_to_50mA(void);
+
+ bool axp2101_enable_pwrok_resets(void);
+ uint8_t axp2101_get_dcdc_status(void);
+ bool axp2101_set_bldo1_voltage(uint16_t voltage);
+ uint8_t axp2101_get_bldo1_voltage(void);
+ bool axp2101_set_bldo2_voltage(uint16_t voltage);
+ uint8_t axp2101_get_bldo2_voltage(void);
+ bool axp2101_set_dcdc1_voltage(uint16_t voltage);
+ uint8_t axp2101_get_dcdc1_voltage(void);
+ bool axp2101_set_dcdc3_voltage(uint16_t voltage);
+ uint8_t axp2101_get_dcdc3_voltage(void);
+ bool axp2101_set_aldo3_voltage(uint16_t voltage);
+ uint8_t axp2101_get_aldo3_voltage(void);
+ bool axp2101_set_aldo2_voltage(uint16_t voltage);
+ uint8_t axp2101_get_aldo2_voltage(void);
+ bool axp2101_set_aldo4_voltage(uint16_t voltage);
+ uint8_t axp2101_get_aldo4_voltage(void);
+ bool axp2101_set_dldo1_voltage(uint16_t voltage);
+ uint8_t axp2101_get_dldo1_voltage(void);
+ bool set_dldo1_on_off(bool sw);
+ bool set_cpusldo_on_off(bool sw);
+ bool set_blod2_on_off(bool sw);
+ bool set_bldo1_on_off(bool sw);
+ bool set_aldo4_on_off(bool sw);
+ bool set_aldo3_on_off(bool sw);
+ bool set_aldo2_on_off(bool sw);
+ bool set_aldo1_on_off(bool sw);
+ bool set_dcdc1_on_off(bool sw);
+ bool set_dcdc2_on_off(bool sw);
+ bool set_dcdc3_on_off(bool sw);
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/Free_Fonts.h b/lib/M5Core2/src/Free_Fonts.h
new file mode 100644
index 000000000..b3fae0ae9
--- /dev/null
+++ b/lib/M5Core2/src/Free_Fonts.h
@@ -0,0 +1,383 @@
+#if defined (CORE2)
+// Attach this header file to your sketch to use the GFX Free Fonts. You can
+// write sketches without it, but it makes referencing them easier.
+
+// This calls up ALL the fonts but they only get loaded if you actually
+// use them in your sketch.
+//
+// No changes are needed to this header file unless new fonts are added to the
+// library "Fonts/GFXFF" folder.
+//
+// To save a lot of typing long names, each font can easily be referenced in the
+// sketch in three ways, either with:
+//
+// 1. Font file name with the & in front such as &FreeSansBoldOblique24pt7b
+// an example being:
+//
+// tft.setFreeFont(&FreeSansBoldOblique24pt7b);
+//
+// 2. FF# where # is a number determined by looking at the list below
+// an example being:
+//
+// tft.setFreeFont(FF32);
+//
+// 3. An abbreviation of the file name. Look at the list below to see
+// the abbreviations used, for example:
+//
+// tft.setFreeFont(FSSBO24)
+//
+// Where the letters mean:
+// F = Free font
+// M = Mono
+// SS = Sans Serif (double S to distinguish is form serif fonts)
+// S = Serif
+// B = Bold
+// O = Oblique (letter O not zero)
+// I = Italic
+// # = point size, either 9, 12, 18 or 24
+//
+// Setting the font to NULL will select the GLCD font:
+//
+// tft.setFreeFont(NULL); // Set font to GLCD
+
+#define LOAD_GFXFF
+
+#ifdef LOAD_GFXFF // Only include the fonts if LOAD_GFXFF is defined in
+ // User_Setup.h
+
+// Use these when printing or drawing text in GLCD and high rendering speed
+// fonts
+#define GFXFF 1
+#define GLCD 0
+#define FONT2 2
+#define FONT4 4
+#define FONT6 6
+#define FONT7 7
+#define FONT8 8
+
+// Use the following when calling setFont()
+//
+// Reserved for GLCD font // FF0
+//
+
+#define TT1 &TomThumb
+
+#define FM9 &FreeMono9pt7b
+#define FM12 &FreeMono12pt7b
+#define FM18 &FreeMono18pt7b
+#define FM24 &FreeMono24pt7b
+
+#define FMB9 &FreeMonoBold9pt7b
+#define FMB12 &FreeMonoBold12pt7b
+#define FMB18 &FreeMonoBold18pt7b
+#define FMB24 &FreeMonoBold24pt7b
+
+#define FMO9 &FreeMonoOblique9pt7b
+#define FMO12 &FreeMonoOblique12pt7b
+#define FMO18 &FreeMonoOblique18pt7b
+#define FMO24 &FreeMonoOblique24pt7b
+
+#define FMBO9 &FreeMonoBoldOblique9pt7b
+#define FMBO12 &FreeMonoBoldOblique12pt7b
+#define FMBO18 &FreeMonoBoldOblique18pt7b
+#define FMBO24 &FreeMonoBoldOblique24pt7b
+
+#define FSS9 &FreeSans9pt7b
+#define FSS12 &FreeSans12pt7b
+#define FSS18 &FreeSans18pt7b
+#define FSS24 &FreeSans24pt7b
+
+#define FSSB9 &FreeSansBold9pt7b
+#define FSSB12 &FreeSansBold12pt7b
+#define FSSB18 &FreeSansBold18pt7b
+#define FSSB24 &FreeSansBold24pt7b
+
+#define FSSO9 &FreeSansOblique9pt7b
+#define FSSO12 &FreeSansOblique12pt7b
+#define FSSO18 &FreeSansOblique18pt7b
+#define FSSO24 &FreeSansOblique24pt7b
+
+#define FSSBO9 &FreeSansBoldOblique9pt7b
+#define FSSBO12 &FreeSansBoldOblique12pt7b
+#define FSSBO18 &FreeSansBoldOblique18pt7b
+#define FSSBO24 &FreeSansBoldOblique24pt7b
+
+#define FS9 &FreeSerif9pt7b
+#define FS12 &FreeSerif12pt7b
+#define FS18 &FreeSerif18pt7b
+#define FS24 &FreeSerif24pt7b
+
+#define FSI9 &FreeSerifItalic9pt7b
+#define FSI12 &FreeSerifItalic12pt7b
+#define FSI19 &FreeSerifItalic18pt7b
+#define FSI24 &FreeSerifItalic24pt7b
+
+#define FSB9 &FreeSerifBold9pt7b
+#define FSB12 &FreeSerifBold12pt7b
+#define FSB18 &FreeSerifBold18pt7b
+#define FSB24 &FreeSerifBold24pt7b
+
+#define FSBI9 &FreeSerifBoldItalic9pt7b
+#define FSBI12 &FreeSerifBoldItalic12pt7b
+#define FSBI18 &FreeSerifBoldItalic18pt7b
+#define FSBI24 &FreeSerifBoldItalic24pt7b
+
+#define FF0 NULL // ff0 reserved for GLCD
+#define FF1 &FreeMono9pt7b
+#define FF2 &FreeMono12pt7b
+#define FF3 &FreeMono18pt7b
+#define FF4 &FreeMono24pt7b
+
+#define FF5 &FreeMonoBold9pt7b
+#define FF6 &FreeMonoBold12pt7b
+#define FF7 &FreeMonoBold18pt7b
+#define FF8 &FreeMonoBold24pt7b
+
+#define FF9 &FreeMonoOblique9pt7b
+#define FF10 &FreeMonoOblique12pt7b
+#define FF11 &FreeMonoOblique18pt7b
+#define FF12 &FreeMonoOblique24pt7b
+
+#define FF13 &FreeMonoBoldOblique9pt7b
+#define FF14 &FreeMonoBoldOblique12pt7b
+#define FF15 &FreeMonoBoldOblique18pt7b
+#define FF16 &FreeMonoBoldOblique24pt7b
+
+#define FF17 &FreeSans9pt7b
+#define FF18 &FreeSans12pt7b
+#define FF19 &FreeSans18pt7b
+#define FF20 &FreeSans24pt7b
+
+#define FF21 &FreeSansBold9pt7b
+#define FF22 &FreeSansBold12pt7b
+#define FF23 &FreeSansBold18pt7b
+#define FF24 &FreeSansBold24pt7b
+
+#define FF25 &FreeSansOblique9pt7b
+#define FF26 &FreeSansOblique12pt7b
+#define FF27 &FreeSansOblique18pt7b
+#define FF28 &FreeSansOblique24pt7b
+
+#define FF29 &FreeSansBoldOblique9pt7b
+#define FF30 &FreeSansBoldOblique12pt7b
+#define FF31 &FreeSansBoldOblique18pt7b
+#define FF32 &FreeSansBoldOblique24pt7b
+
+#define FF33 &FreeSerif9pt7b
+#define FF34 &FreeSerif12pt7b
+#define FF35 &FreeSerif18pt7b
+#define FF36 &FreeSerif24pt7b
+
+#define FF37 &FreeSerifItalic9pt7b
+#define FF38 &FreeSerifItalic12pt7b
+#define FF39 &FreeSerifItalic18pt7b
+#define FF40 &FreeSerifItalic24pt7b
+
+#define FF41 &FreeSerifBold9pt7b
+#define FF42 &FreeSerifBold12pt7b
+#define FF43 &FreeSerifBold18pt7b
+#define FF44 &FreeSerifBold24pt7b
+
+#define FF45 &FreeSerifBoldItalic9pt7b
+#define FF46 &FreeSerifBoldItalic12pt7b
+#define FF47 &FreeSerifBoldItalic18pt7b
+#define FF48 &FreeSerifBoldItalic24pt7b
+
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+// Now we define "s"tring versions for easy printing of the font name so:
+// tft.println(sFF5);
+// will print
+// Mono bold 9
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+
+#define sFF0 "GLCD"
+#define sTT1 "Tom Thumb"
+#define sFF1 "Mono 9"
+#define sFF2 "Mono 12"
+#define sFF3 "Mono 18"
+#define sFF4 "Mono 24"
+
+#define sFF5 "Mono bold 9"
+#define sFF6 "Mono bold 12"
+#define sFF7 "Mono bold 18"
+#define sFF8 "Mono bold 24"
+
+#define sFF9 "Mono oblique 9"
+#define sFF10 "Mono oblique 12"
+#define sFF11 "Mono oblique 18"
+#define sFF12 "Mono oblique 24"
+
+#define sFF13 "Mono bold oblique 9"
+#define sFF14 "Mono bold oblique 12"
+#define sFF15 "Mono bold oblique 18"
+#define sFF16 \
+ "Mono bold oblique 24" // Full text line is too big for 480 pixel wide
+ // screen
+
+#define sFF17 "Sans 9"
+#define sFF18 "Sans 12"
+#define sFF19 "Sans 18"
+#define sFF20 "Sans 24"
+
+#define sFF21 "Sans bold 9"
+#define sFF22 "Sans bold 12"
+#define sFF23 "Sans bold 18"
+#define sFF24 "Sans bold 24"
+
+#define sFF25 "Sans oblique 9"
+#define sFF26 "Sans oblique 12"
+#define sFF27 "Sans oblique 18"
+#define sFF28 "Sans oblique 24"
+
+#define sFF29 "Sans bold oblique 9"
+#define sFF30 "Sans bold oblique 12"
+#define sFF31 "Sans bold oblique 18"
+#define sFF32 "Sans bold oblique 24"
+
+#define sFF33 "Serif 9"
+#define sFF34 "Serif 12"
+#define sFF35 "Serif 18"
+#define sFF36 "Serif 24"
+
+#define sFF37 "Serif italic 9"
+#define sFF38 "Serif italic 12"
+#define sFF39 "Serif italic 18"
+#define sFF40 "Serif italic 24"
+
+#define sFF41 "Serif bold 9"
+#define sFF42 "Serif bold 12"
+#define sFF43 "Serif bold 18"
+#define sFF44 "Serif bold 24"
+
+#define sFF45 "Serif bold italic 9"
+#define sFF46 "Serif bold italic 12"
+#define sFF47 "Serif bold italic 18"
+#define sFF48 "Serif bold italic 24"
+
+#else // LOAD_GFXFF not defined so setup defaults to prevent error messages
+
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+// Free fonts are not loaded in User_Setup.h so we must define all as font 1
+// to prevent compile error messages
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+
+#define GFXFF 1
+#define GLCD 1
+#define FONT2 2
+#define FONT4 4
+#define FONT6 6
+#define FONT7 7
+#define FONT8 8
+
+#define FF0 1
+#define FF1 1
+#define FF2 1
+#define FF3 1
+#define FF4 1
+#define FF5 1
+#define FF6 1
+#define FF7 1
+#define FF8 1
+#define FF9 1
+#define FF10 1
+#define FF11 1
+#define FF12 1
+#define FF13 1
+#define FF14 1
+#define FF15 1
+#define FF16 1
+#define FF17 1
+#define FF18 1
+#define FF19 1
+#define FF20 1
+#define FF21 1
+#define FF22 1
+#define FF23 1
+#define FF24 1
+#define FF25 1
+#define FF26 1
+#define FF27 1
+#define FF28 1
+#define FF29 1
+#define FF30 1
+#define FF31 1
+#define FF32 1
+#define FF33 1
+#define FF34 1
+#define FF35 1
+#define FF36 1
+#define FF37 1
+#define FF38 1
+#define FF39 1
+#define FF40 1
+#define FF41 1
+#define FF42 1
+#define FF43 1
+#define FF44 1
+#define FF45 1
+#define FF46 1
+#define FF47 1
+#define FF48 1
+
+#define FM9 1
+#define FM12 1
+#define FM18 1
+#define FM24 1
+
+#define FMB9 1
+#define FMB12 1
+#define FMB18 1
+#define FMB24 1
+
+#define FMO9 1
+#define FMO12 1
+#define FMO18 1
+#define FMO24 1
+
+#define FMBO9 1
+#define FMBO12 1
+#define FMBO18 1
+#define FMBO24 1
+
+#define FSS9 1
+#define FSS12 1
+#define FSS18 1
+#define FSS24 1
+
+#define FSSB9 1
+#define FSSB12 1
+#define FSSB18 1
+#define FSSB24 1
+
+#define FSSO9 1
+#define FSSO12 1
+#define FSSO18 1
+#define FSSO24 1
+
+#define FSSBO9 1
+#define FSSBO12 1
+#define FSSBO18 1
+#define FSSBO24 1
+
+#define FS9 1
+#define FS12 1
+#define FS18 1
+#define FS24 1
+
+#define FSI9 1
+#define FSI12 1
+#define FSI19 1
+#define FSI24 1
+
+#define FSB9 1
+#define FSB12 1
+#define FSB18 1
+#define FSB24 1
+
+#define FSBI9 1
+#define FSBI12 1
+#define FSBI18 1
+#define FSBI24 1
+
+#endif // LOAD_GFXFF
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/INA3221.cpp b/lib/M5Core2/src/INA3221.cpp
new file mode 100644
index 000000000..45c2b3ca3
--- /dev/null
+++ b/lib/M5Core2/src/INA3221.cpp
@@ -0,0 +1,536 @@
+#if defined (CORE2)
+/*
+
+ Arduino library for INA3221 current and voltage sensor.
+
+ MIT License
+
+ Copyright (c) 2020 Beast Devices, Andrejs Bondarevs
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy
+ of this software and associated documentation files (the "Software"), to
+ deal in the Software without restriction, including without limitation the
+ rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ sell copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in
+ all copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ IN THE SOFTWARE.
+
+*/
+
+#include "INA3221.h"
+
+void INA3221::_read(ina3221_reg_t reg, uint16_t *val) {
+ _i2c->beginTransmission(_i2c_addr);
+ _i2c->write(reg); // Register
+ _i2c->endTransmission(false);
+
+ _i2c->requestFrom((uint8_t)_i2c_addr, (uint8_t)2);
+
+ if (_i2c->available()) {
+ *val = ((_i2c->read() << 8) | _i2c->read());
+ }
+}
+
+void INA3221::_write(ina3221_reg_t reg, uint16_t *val) {
+ _i2c->beginTransmission(_i2c_addr);
+ _i2c->write(reg); // Register
+ _i2c->write((*val >> 8) & 0xFF); // Upper 8-bits
+ _i2c->write(*val & 0xFF); // Lower 8-bits
+ _i2c->endTransmission();
+}
+
+void INA3221::begin(TwoWire *theWire) {
+ _i2c = theWire;
+
+ _shuntRes[0] = 10;
+ _shuntRes[1] = 10;
+ _shuntRes[2] = 10;
+
+ _filterRes[0] = 0;
+ _filterRes[1] = 0;
+ _filterRes[2] = 0;
+
+ _i2c->begin();
+}
+
+void INA3221::setShuntRes(uint32_t res_ch1, uint32_t res_ch2,
+ uint32_t res_ch3) {
+ _shuntRes[0] = res_ch1;
+ _shuntRes[1] = res_ch2;
+ _shuntRes[2] = res_ch3;
+}
+
+void INA3221::setFilterRes(uint32_t res_ch1, uint32_t res_ch2,
+ uint32_t res_ch3) {
+ _filterRes[0] = res_ch1;
+ _filterRes[1] = res_ch2;
+ _filterRes[2] = res_ch3;
+}
+
+uint16_t INA3221::getReg(ina3221_reg_t reg) {
+ uint16_t val = 0;
+ _read(reg, &val);
+ return val;
+}
+
+void INA3221::reset() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.reset = 1;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setModePowerDown() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.mode_bus_en = 0;
+ conf_reg.mode_continious_en = 0;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setModeContinious() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.mode_continious_en = 1;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setModeTriggered() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.mode_continious_en = 0;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setShuntMeasEnable() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.mode_shunt_en = 1;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setShuntMeasDisable() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.mode_shunt_en = 0;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setBusMeasEnable() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.mode_bus_en = 1;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setBusMeasDisable() {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.mode_bus_en = 0;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setAveragingMode(ina3221_avg_mode_t mode) {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.avg_mode = mode;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setBusConversionTime(ina3221_conv_time_t convTime) {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.bus_conv_time = convTime;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setShuntConversionTime(ina3221_conv_time_t convTime) {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+ conf_reg.shunt_conv_time = convTime;
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setPwrValidUpLimit(int16_t voltagemV) {
+ _write(INA3221_REG_PWR_VALID_HI_LIM, (uint16_t *)&voltagemV);
+}
+
+void INA3221::setPwrValidLowLimit(int16_t voltagemV) {
+ _write(INA3221_REG_PWR_VALID_LO_LIM, (uint16_t *)&voltagemV);
+}
+
+void INA3221::setShuntSumAlertLimit(int32_t voltageuV) {
+ int16_t val = 0;
+ val = voltageuV / 20;
+ _write(INA3221_REG_SHUNTV_SUM_LIM, (uint16_t *)&val);
+}
+
+void INA3221::setCurrentSumAlertLimit(int32_t currentmA) {
+ int16_t shuntuV = 0;
+ shuntuV = currentmA * (int32_t)_shuntRes[INA3221_CH1];
+ setShuntSumAlertLimit(shuntuV);
+}
+
+void INA3221::setWarnAlertLatchEnable() {
+ masken_reg_t masken_reg;
+
+ _read(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ masken_reg.warn_alert_latch_en = 1;
+ _write(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ _masken_reg = masken_reg;
+}
+
+void INA3221::setWarnAlertLatchDisable() {
+ masken_reg_t masken_reg;
+
+ _read(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ masken_reg.warn_alert_latch_en = 1;
+ _write(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ _masken_reg = masken_reg;
+}
+
+void INA3221::setCritAlertLatchEnable() {
+ masken_reg_t masken_reg;
+
+ _read(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ masken_reg.crit_alert_latch_en = 1;
+ _write(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ _masken_reg = masken_reg;
+}
+
+void INA3221::setCritAlertLatchDisable() {
+ masken_reg_t masken_reg;
+
+ _read(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ masken_reg.crit_alert_latch_en = 1;
+ _write(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ _masken_reg = masken_reg;
+}
+
+void INA3221::readFlags() {
+ _read(INA3221_REG_MASK_ENABLE, (uint16_t *)&_masken_reg);
+}
+
+bool INA3221::getTimingCtrlAlertFlag() {
+ return _masken_reg.timing_ctrl_alert;
+}
+
+bool INA3221::getPwrValidAlertFlag() {
+ return _masken_reg.pwr_valid_alert;
+}
+
+bool INA3221::getCurrentSumAlertFlag() {
+ return _masken_reg.shunt_sum_alert;
+}
+
+bool INA3221::getConversionReadyFlag() {
+ return _masken_reg.conv_ready;
+}
+
+uint16_t INA3221::getManufID() {
+ uint16_t id = 0;
+ _read(INA3221_REG_MANUF_ID, &id);
+ return id;
+}
+
+uint16_t INA3221::getDieID() {
+ uint16_t id = 0;
+ _read(INA3221_REG_DIE_ID, &id);
+ return id;
+}
+
+void INA3221::setChannelEnable(ina3221_ch_t channel) {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+
+ switch (channel) {
+ case INA3221_CH1:
+ conf_reg.ch1_en = 1;
+ break;
+ case INA3221_CH2:
+ conf_reg.ch2_en = 1;
+ break;
+ case INA3221_CH3:
+ conf_reg.ch3_en = 1;
+ break;
+ default:
+ break;
+ }
+
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setChannelDisable(ina3221_ch_t channel) {
+ conf_reg_t conf_reg;
+
+ _read(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+
+ switch (channel) {
+ case INA3221_CH1:
+ conf_reg.ch1_en = 0;
+ break;
+ case INA3221_CH2:
+ conf_reg.ch2_en = 0;
+ break;
+ case INA3221_CH3:
+ conf_reg.ch3_en = 0;
+ break;
+ default:
+ break;
+ }
+
+ _write(INA3221_REG_CONF, (uint16_t *)&conf_reg);
+}
+
+void INA3221::setWarnAlertShuntLimit(ina3221_ch_t channel, int32_t voltageuV) {
+ ina3221_reg_t reg = INA3221_REG_CONF;
+ int16_t val = 0;
+
+ switch (channel) {
+ case INA3221_CH1:
+ reg = INA3221_REG_CH1_WARNING_ALERT_LIM;
+ break;
+ case INA3221_CH2:
+ reg = INA3221_REG_CH2_WARNING_ALERT_LIM;
+ break;
+ case INA3221_CH3:
+ reg = INA3221_REG_CH3_WARNING_ALERT_LIM;
+ break;
+ default:
+ break;
+ }
+
+ val = voltageuV / 5;
+ _write(reg, (uint16_t *)&val);
+}
+
+void INA3221::setCritAlertShuntLimit(ina3221_ch_t channel, int32_t voltageuV) {
+ ina3221_reg_t reg = INA3221_REG_CONF;
+ int16_t val = 0;
+
+ switch (channel) {
+ case INA3221_CH1:
+ reg = INA3221_REG_CH1_CRIT_ALERT_LIM;
+ break;
+ case INA3221_CH2:
+ reg = INA3221_REG_CH2_CRIT_ALERT_LIM;
+ break;
+ case INA3221_CH3:
+ reg = INA3221_REG_CH3_CRIT_ALERT_LIM;
+ break;
+ default:
+ break;
+ }
+
+ val = voltageuV / 5;
+ _write(reg, (uint16_t *)&val);
+}
+
+void INA3221::setWarnAlertCurrentLimit(ina3221_ch_t channel,
+ int32_t currentmA) {
+ int32_t shuntuV = 0;
+ shuntuV = currentmA * (int32_t)_shuntRes[channel];
+ setWarnAlertShuntLimit(channel, shuntuV);
+}
+
+void INA3221::setCritAlertCurrentLimit(ina3221_ch_t channel,
+ int32_t currentmA) {
+ int32_t shuntuV = 0;
+ shuntuV = currentmA * (int32_t)_shuntRes[channel];
+ setCritAlertShuntLimit(channel, shuntuV);
+}
+
+void INA3221::setCurrentSumEnable(ina3221_ch_t channel) {
+ masken_reg_t masken_reg;
+
+ _read(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+
+ switch (channel) {
+ case INA3221_CH1:
+ masken_reg.shunt_sum_en_ch1 = 1;
+ break;
+ case INA3221_CH2:
+ masken_reg.shunt_sum_en_ch2 = 1;
+ break;
+ case INA3221_CH3:
+ masken_reg.shunt_sum_en_ch3 = 1;
+ break;
+ default:
+ break;
+ }
+
+ _write(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ _masken_reg = masken_reg;
+}
+
+void INA3221::setCurrentSumDisable(ina3221_ch_t channel) {
+ masken_reg_t masken_reg;
+
+ _read(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+
+ switch (channel) {
+ case INA3221_CH1:
+ masken_reg.shunt_sum_en_ch1 = 0;
+ break;
+ case INA3221_CH2:
+ masken_reg.shunt_sum_en_ch2 = 0;
+ break;
+ case INA3221_CH3:
+ masken_reg.shunt_sum_en_ch3 = 0;
+ break;
+ default:
+ break;
+ }
+
+ _write(INA3221_REG_MASK_ENABLE, (uint16_t *)&masken_reg);
+ _masken_reg = masken_reg;
+}
+
+int32_t INA3221::getShuntVoltage(ina3221_ch_t channel) {
+ int32_t res;
+ ina3221_reg_t reg = INA3221_REG_CONF;
+ uint16_t val_raw = 0;
+
+ switch (channel) {
+ case INA3221_CH1:
+ reg = INA3221_REG_CH1_SHUNTV;
+ break;
+ case INA3221_CH2:
+ reg = INA3221_REG_CH2_SHUNTV;
+ break;
+ case INA3221_CH3:
+ reg = INA3221_REG_CH3_SHUNTV;
+ break;
+ default:
+ break;
+ }
+
+ _read(reg, &val_raw);
+
+ // 1 LSB = 5uV
+ res = (int16_t)val_raw * 5;
+
+ return res;
+}
+
+bool INA3221::getWarnAlertFlag(ina3221_ch_t channel) {
+ switch (channel) {
+ case INA3221_CH1:
+ return _masken_reg.warn_alert_ch1;
+ case INA3221_CH2:
+ return _masken_reg.warn_alert_ch2;
+ case INA3221_CH3:
+ return _masken_reg.warn_alert_ch3;
+ default:
+ return false;
+ }
+}
+
+bool INA3221::getCritAlertFlag(ina3221_ch_t channel) {
+ switch (channel) {
+ case INA3221_CH1:
+ return _masken_reg.crit_alert_ch1;
+ case INA3221_CH2:
+ return _masken_reg.crit_alert_ch2;
+ case INA3221_CH3:
+ return _masken_reg.crit_alert_ch3;
+ default:
+ return false;
+ }
+}
+
+int32_t INA3221::estimateOffsetVoltage(ina3221_ch_t channel, uint32_t busV) {
+ float bias_in = 10.0; // Input bias current at IN– in uA
+ float r_in = 0.670; // Input resistance at IN– in MOhm
+ uint32_t adc_step = 40; // smallest shunt ADC step in uV
+ float shunt_res = _shuntRes[channel] / 1000.0; // convert to Ohm
+ float filter_res = _filterRes[channel];
+ int32_t offset = 0.0;
+ float reminder;
+
+ offset = (shunt_res + filter_res) * (busV / r_in + bias_in) -
+ bias_in * filter_res;
+
+ // Round the offset to the closest shunt ADC value
+ reminder = offset % adc_step;
+ if (reminder < adc_step / 2) {
+ offset -= reminder;
+ } else {
+ offset += adc_step - reminder;
+ }
+
+ return offset;
+}
+
+float INA3221::getCurrent(ina3221_ch_t channel) {
+ int32_t shunt_uV = 0;
+ float current_A = 0;
+
+ shunt_uV = getShuntVoltage(channel);
+ current_A = shunt_uV / (int32_t)_shuntRes[channel] / 1000.0;
+ return current_A;
+}
+
+float INA3221::getCurrentCompensated(ina3221_ch_t channel) {
+ int32_t shunt_uV = 0;
+ int32_t bus_V = 0;
+ float current_A = 0.0;
+ int32_t offset_uV = 0;
+
+ shunt_uV = getShuntVoltage(channel);
+ bus_V = getVoltage(channel);
+ offset_uV = estimateOffsetVoltage(channel, bus_V);
+
+ current_A = (shunt_uV - offset_uV) / (int32_t)_shuntRes[channel] / 1000.0;
+
+ return current_A;
+}
+
+float INA3221::getVoltage(ina3221_ch_t channel) {
+ float voltage_V = 0.0;
+ ina3221_reg_t reg = INA3221_REG_CONF;
+ uint16_t val_raw = 0;
+
+ switch (channel) {
+ case INA3221_CH1:
+ reg = INA3221_REG_CH1_BUSV;
+ break;
+ case INA3221_CH2:
+ reg = INA3221_REG_CH2_BUSV;
+ break;
+ case INA3221_CH3:
+ reg = INA3221_REG_CH3_BUSV;
+ break;
+ default:
+ break;
+ }
+
+ _read(reg, &val_raw);
+
+ voltage_V = val_raw / 1000.0;
+
+ return voltage_V;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/INA3221.h b/lib/M5Core2/src/INA3221.h
new file mode 100644
index 000000000..047b4ca4b
--- /dev/null
+++ b/lib/M5Core2/src/INA3221.h
@@ -0,0 +1,313 @@
+#if defined (CORE2)
+/*
+
+ Arduino library for INA3221 current and voltage sensor.
+
+ MIT License
+
+ Copyright (c) 2020 Beast Devices, Andrejs Bondarevs
+
+ 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.
+
+*/
+
+#ifndef _INA3221_H_
+#define _INA3221_H_
+
+#include "Arduino.h"
+#include "Wire.h"
+
+typedef enum {
+ INA3221_ADDR40_GND = 0b1000000, // A0 pin -> GND
+ INA3221_ADDR41_VCC = 0b1000001, // A0 pin -> VCC
+ INA3221_ADDR42_SDA = 0b1000010, // A0 pin -> SDA
+ INA3221_ADDR43_SCL = 0b1000011 // A0 pin -> SCL
+} ina3221_addr_t;
+
+// Channels
+typedef enum {
+ INA3221_CH1 = 0,
+ INA3221_CH2,
+ INA3221_CH3,
+ INA3221_CH_NUM
+} ina3221_ch_t;
+
+// Registers
+typedef enum {
+ INA3221_REG_CONF = 0,
+ INA3221_REG_CH1_SHUNTV,
+ INA3221_REG_CH1_BUSV,
+ INA3221_REG_CH2_SHUNTV,
+ INA3221_REG_CH2_BUSV,
+ INA3221_REG_CH3_SHUNTV,
+ INA3221_REG_CH3_BUSV,
+ INA3221_REG_CH1_CRIT_ALERT_LIM,
+ INA3221_REG_CH1_WARNING_ALERT_LIM,
+ INA3221_REG_CH2_CRIT_ALERT_LIM,
+ INA3221_REG_CH2_WARNING_ALERT_LIM,
+ INA3221_REG_CH3_CRIT_ALERT_LIM,
+ INA3221_REG_CH3_WARNING_ALERT_LIM,
+ INA3221_REG_SHUNTV_SUM,
+ INA3221_REG_SHUNTV_SUM_LIM,
+ INA3221_REG_MASK_ENABLE,
+ INA3221_REG_PWR_VALID_HI_LIM,
+ INA3221_REG_PWR_VALID_LO_LIM,
+ INA3221_REG_MANUF_ID = 0xFE,
+ INA3221_REG_DIE_ID = 0xFF
+} ina3221_reg_t;
+
+// Conversion times
+typedef enum {
+ INA3221_REG_CONF_CT_140US = 0,
+ INA3221_REG_CONF_CT_204US,
+ INA3221_REG_CONF_CT_332US,
+ INA3221_REG_CONF_CT_588US,
+ INA3221_REG_CONF_CT_1100US,
+ INA3221_REG_CONF_CT_2116US,
+ INA3221_REG_CONF_CT_4156US,
+ INA3221_REG_CONF_CT_8244US
+} ina3221_conv_time_t;
+
+// Averaging modes
+typedef enum {
+ INA3221_REG_CONF_AVG_1 = 0,
+ INA3221_REG_CONF_AVG_4,
+ INA3221_REG_CONF_AVG_16,
+ INA3221_REG_CONF_AVG_64,
+ INA3221_REG_CONF_AVG_128,
+ INA3221_REG_CONF_AVG_256,
+ INA3221_REG_CONF_AVG_512,
+ INA3221_REG_CONF_AVG_1024
+} ina3221_avg_mode_t;
+
+class INA3221 {
+ // Configuration register
+ typedef struct {
+ uint16_t mode_shunt_en : 1;
+ uint16_t mode_bus_en : 1;
+ uint16_t mode_continious_en : 1;
+ uint16_t shunt_conv_time : 3;
+ uint16_t bus_conv_time : 3;
+ uint16_t avg_mode : 3;
+ uint16_t ch3_en : 1;
+ uint16_t ch2_en : 1;
+ uint16_t ch1_en : 1;
+ uint16_t reset : 1;
+ } __attribute__((packed)) conf_reg_t;
+
+ // Mask/Enable register
+ typedef struct {
+ uint16_t conv_ready : 1;
+ uint16_t timing_ctrl_alert : 1;
+ uint16_t pwr_valid_alert : 1;
+ uint16_t warn_alert_ch3 : 1;
+ uint16_t warn_alert_ch2 : 1;
+ uint16_t warn_alert_ch1 : 1;
+ uint16_t shunt_sum_alert : 1;
+ uint16_t crit_alert_ch3 : 1;
+ uint16_t crit_alert_ch2 : 1;
+ uint16_t crit_alert_ch1 : 1;
+ uint16_t crit_alert_latch_en : 1;
+ uint16_t warn_alert_latch_en : 1;
+ uint16_t shunt_sum_en_ch3 : 1;
+ uint16_t shunt_sum_en_ch2 : 1;
+ uint16_t shunt_sum_en_ch1 : 1;
+ uint16_t reserved : 1;
+ } __attribute__((packed)) masken_reg_t;
+
+ // Arduino's I2C library
+ TwoWire *_i2c;
+
+ // I2C address
+ ina3221_addr_t _i2c_addr;
+
+ // Shunt resistance in mOhm
+ uint32_t _shuntRes[INA3221_CH_NUM];
+
+ // Series filter resistance in Ohm
+ uint32_t _filterRes[INA3221_CH_NUM];
+
+ // Value of Mask/Enable register.
+ masken_reg_t _masken_reg;
+
+ // Reads 16 bytes from a register.
+ void _read(ina3221_reg_t reg, uint16_t *val);
+
+ // Writes 16 bytes to a register.
+ void _write(ina3221_reg_t reg, uint16_t *val);
+
+ public:
+ INA3221(ina3221_addr_t addr = INA3221_ADDR40_GND) : _i2c_addr(addr){};
+ // Initializes INA3221
+ void begin(TwoWire *theWire = &Wire);
+
+ // Sets shunt resistor value in mOhm
+ void setShuntRes(uint32_t res_ch1, uint32_t res_ch2, uint32_t res_ch3);
+
+ // Sets filter resistors value in Ohm
+ void setFilterRes(uint32_t res_ch1, uint32_t res_ch2, uint32_t res_ch3);
+
+ // Sets I2C address of INA3221
+ void setAddr(ina3221_addr_t addr) {
+ _i2c_addr = addr;
+ }
+
+ // Gets a register value.
+ uint16_t getReg(ina3221_reg_t reg);
+
+ // Resets INA3221
+ void reset();
+
+ // Sets operating mode to power-down
+ void setModePowerDown();
+
+ // Sets operating mode to continious
+ void setModeContinious();
+
+ // Sets operating mode to triggered (single-shot)
+ void setModeTriggered();
+
+ // Enables shunt-voltage measurement
+ void setShuntMeasEnable();
+
+ // Disables shunt-voltage mesurement
+ void setShuntMeasDisable();
+
+ // Enables bus-voltage measurement
+ void setBusMeasEnable();
+
+ // Disables bus-voltage measureement
+ void setBusMeasDisable();
+
+ // Sets averaging mode. Sets number of samples that are collected
+ // and averaged togehter.
+ void setAveragingMode(ina3221_avg_mode_t mode);
+
+ // Sets bus-voltage conversion time.
+ void setBusConversionTime(ina3221_conv_time_t convTime);
+
+ // Sets shunt-voltage conversion time.
+ void setShuntConversionTime(ina3221_conv_time_t convTime);
+
+ // Sets power-valid upper-limit voltage. The power-valid condition
+ // is reached when all bus-voltage channels exceed the value set.
+ // When the powervalid condition is met, the PV alert pin asserts high.
+ void setPwrValidUpLimit(int16_t voltagemV);
+
+ // Sets power-valid lower-limit voltage. If any bus-voltage channel drops
+ // below the power-valid lower-limit, the PV alert pin pulls low.
+ void setPwrValidLowLimit(int16_t voltagemV);
+
+ // Sets the value that is compared to the Shunt-Voltage Sum register value
+ // following each completed cycle of all selected channels to detect
+ // for system overcurrent events.
+ void setShuntSumAlertLimit(int32_t voltagemV);
+
+ // Sets the current value that is compared to the sum all currents.
+ // This function is a helper for setShuntSumAlertLim(). It onverts current
+ // value to shunt voltage value.
+ void setCurrentSumAlertLimit(int32_t currentmA);
+
+ // Enables warning alert latch.
+ void setWarnAlertLatchEnable();
+
+ // Disables warning alert latch.
+ void setWarnAlertLatchDisable();
+
+ // Enables critical alert latch.
+ void setCritAlertLatchEnable();
+
+ // Disables critical alert latch.
+ void setCritAlertLatchDisable();
+
+ // Reads flags from Mask/Enable register.
+ // When Mask/Enable register is read, flags are cleared.
+ // Use getTimingCtrlAlertFlag(), getPwrValidAlertFlag(),
+ // getCurrentSumAlertFlag() and getConvReadyFlag() to get flags after
+ // readFlags() is called.
+ void readFlags();
+
+ // Gets timing-control-alert flag indicator.
+ bool getTimingCtrlAlertFlag();
+
+ // Gets power-valid-alert flag indicator.
+ bool getPwrValidAlertFlag();
+
+ // Gets summation-alert flag indicator.
+ bool getCurrentSumAlertFlag();
+
+ // Gets Conversion-ready flag.
+ bool getConversionReadyFlag();
+
+ // Gets manufacturer ID.
+ // Should read 0x5449.
+ uint16_t getManufID();
+
+ // Gets die ID.
+ // Should read 0x3220.
+ uint16_t getDieID();
+
+ // Enables channel measurements
+ void setChannelEnable(ina3221_ch_t channel);
+
+ // Disables channel measurements
+ void setChannelDisable(ina3221_ch_t channel);
+
+ // Sets warning alert shunt voltage limit
+ void setWarnAlertShuntLimit(ina3221_ch_t channel, int32_t voltageuV);
+
+ // Sets critical alert shunt voltage limit
+ void setCritAlertShuntLimit(ina3221_ch_t channel, int32_t voltageuV);
+
+ // Sets warning alert current limit
+ void setWarnAlertCurrentLimit(ina3221_ch_t channel, int32_t currentmA);
+
+ // Sets critical alert current limit
+ void setCritAlertCurrentLimit(ina3221_ch_t channel, int32_t currentmA);
+
+ // Includes channel to fill Shunt-Voltage Sum register.
+ void setCurrentSumEnable(ina3221_ch_t channel);
+
+ // Excludes channel from filling Shunt-Voltage Sum register.
+ void setCurrentSumDisable(ina3221_ch_t channel);
+
+ // Gets shunt voltage in uV.
+ int32_t getShuntVoltage(ina3221_ch_t channel);
+
+ // Gets warning alert flag.
+ bool getWarnAlertFlag(ina3221_ch_t channel);
+
+ // Gets critical alert flag.
+ bool getCritAlertFlag(ina3221_ch_t channel);
+
+ // Estimates offset voltage added by the series filter resitors
+ int32_t estimateOffsetVoltage(ina3221_ch_t channel, uint32_t busVoltage);
+
+ // Gets current in A.
+ float getCurrent(ina3221_ch_t channel);
+
+ // Gets current compensated with calculated offset voltage.
+ float getCurrentCompensated(ina3221_ch_t channel);
+
+ // Gets bus voltage in V.
+ float getVoltage(ina3221_ch_t channel);
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/M5Core2.cpp b/lib/M5Core2/src/M5Core2.cpp
new file mode 100644
index 000000000..e7dc693cd
--- /dev/null
+++ b/lib/M5Core2/src/M5Core2.cpp
@@ -0,0 +1,97 @@
+#if defined (CORE2)
+// Copyright (c) M5Core2. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full
+// license information.
+
+#include "M5Core2.h"
+
+M5Core2::M5Core2() : isInited(0) {
+}
+
+void M5Core2::begin(bool LCDEnable, bool SDEnable, bool SerialEnable,
+ bool I2CEnable, mbus_mode_t mode, bool SpeakerEnable) {
+ // Correct init once
+ if (isInited == true) {
+ return;
+ } else {
+ isInited = true;
+ }
+
+ // UART
+ if (SerialEnable == true) {
+ Serial.begin(115200);
+ Serial.flush();
+ delay(50);
+ Serial.print("M5Core2 initializing...");
+ }
+
+ // I2C init
+ if (I2CEnable == true) {
+ Wire.begin(32, 33);
+ }
+
+ Axp.begin(mode);
+
+ // LCD INIT
+ if (LCDEnable == true) {
+ Lcd.begin();
+ }
+
+ // Touch init
+ Touch.begin(); // Touch begin after AXP begin. (Reset at the start of AXP)
+
+ // TF Card
+ if (SDEnable == true) {
+ SD.begin(TFCARD_CS_PIN, SPI, 40000000);
+ }
+
+ // TONE
+
+// if (SpeakerEnable == true) {
+// Spk.begin();
+// }
+
+ if (SerialEnable == true) {
+ Serial.println("OK");
+ }
+
+// Rtc.begin();
+}
+
+void M5Core2::update() {
+ Touch.update();
+ Buttons.update();
+ yield();
+}
+
+void M5Core2::shutdown() {
+ Axp.PowerOff();
+}
+/*
+int M5Core2::shutdown(int seconds) {
+ Rtc.clearIRQ();
+ Rtc.SetAlarmIRQ(seconds);
+ delay(10);
+ Axp.PowerOff();
+ return 0;
+}
+
+int M5Core2::shutdown(const RTC_TimeTypeDef &RTC_TimeStruct) {
+ Rtc.clearIRQ();
+ Rtc.SetAlarmIRQ(RTC_TimeStruct);
+ delay(10);
+ Axp.PowerOff();
+ return 0;
+}
+
+int M5Core2::shutdown(const RTC_DateTypeDef &RTC_DateStruct,
+ const RTC_TimeTypeDef &RTC_TimeStruct) {
+ Rtc.clearIRQ();
+ Rtc.SetAlarmIRQ(RTC_DateStruct, RTC_TimeStruct);
+ delay(10);
+ Axp.PowerOff();
+ return 0;
+}
+*/
+M5Core2 M5;
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/M5Core2.h b/lib/M5Core2/src/M5Core2.h
new file mode 100644
index 000000000..efec48fd4
--- /dev/null
+++ b/lib/M5Core2/src/M5Core2.h
@@ -0,0 +1,88 @@
+#pragma once
+#if defined (CORE2)
+#ifndef _M5Core2_H_
+#define _M5Core2_H_
+
+#if defined(ESP32)
+
+#include <Arduino.h>
+#include <Wire.h>
+#include <SPI.h>
+#include "FS.h"
+#include "SD.h"
+
+#include "M5Display.h"
+#include "M5Touch.h" // M5Touch
+#include "utility/M5Button.h" // M5Buttons, M5Events, Button, Gesture
+#include "utility/Config.h"
+#include "utility/CommUtil.h"
+//#include "utility/MPU6886.h"
+//#include "Speaker.h"
+#include "AXP.h"
+//#include "RTC.h"
+
+class M5Core2 {
+ public:
+ M5Core2();
+ //[[deprecated("It is recommended to use M5Unified Lib")]]
+
+ void begin(bool LCDEnable = true, bool SDEnable = true,
+ bool SerialEnable = true, bool I2CEnable = false,
+ mbus_mode_t mode = kMBusModeOutput, bool SpeakerEnable = false);
+ void update();
+
+ void shutdown();
+ int shutdown(int seconds);
+ //int shutdown(const RTC_TimeTypeDef &RTC_TimeStruct);
+ //int shutdown(const RTC_DateTypeDef &RTC_DateStruct,
+ // const RTC_TimeTypeDef &RTC_TimeStruct);
+
+ // LCD
+ M5Display Lcd;
+
+ // Power
+ AXP Axp;
+
+ // Touch
+ M5Touch Touch;
+
+ // Buttons (global button and gesture functions)
+ M5Buttons Buttons;
+
+ // Default "button" that gets events where there is no button.
+ Button background = Button(0, 0, TOUCH_W, TOUCH_H, true, "background");
+
+ // Touch version of the buttons on older M5stack cores, below screen
+ Button BtnA = Button(10, 220, 110, 60, true, "BtnA"); // from 240 to 220 and from 40 to 60
+ Button BtnB = Button(130, 220, 70, 60, true, "BtnB"); // from 240 to 220 and from 40 to 60
+ Button BtnC = Button(230, 220, 80, 60, true, "BtnC"); // from 240 to 220 and from 40 to 60
+
+ //MPU6886 IMU;
+
+ // I2C
+ CommUtil I2C;
+
+ //RTC Rtc;
+
+ //Speaker Spk;
+
+ /**
+ * Functions have been moved to Power class for compatibility.
+ * These will be removed in a future release.
+ */
+ void setPowerBoostKeepOn(bool en) __attribute__((deprecated));
+ void setWakeupButton(uint8_t button) __attribute__((deprecated));
+ void powerOFF() __attribute__((deprecated));
+
+ private:
+ bool isInited;
+};
+
+extern M5Core2 M5;
+#define m5 M5
+#define lcd Lcd
+#else
+#error "This library only supports boards with ESP32 processor."
+#endif
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/M5Display.cpp b/lib/M5Core2/src/M5Display.cpp
new file mode 100644
index 000000000..1750f9110
--- /dev/null
+++ b/lib/M5Core2/src/M5Display.cpp
@@ -0,0 +1,707 @@
+#if defined (CORE2)
+#include "M5Display.h"
+
+#ifdef M5Stack_M5Core2
+#include <M5Touch.h>
+#endif /* M5Stack_M5Core2 */
+
+#define BLK_PWM_CHANNEL 7 // LEDC_CHANNEL_7
+
+// So we can use this instance without including all of M5Core2 / M5Stack
+M5Display *M5Display::instance;
+
+M5Display::M5Display() : TFT_eSPI() {
+ if (!instance) instance = this;
+}
+
+void M5Display::begin() {
+ TFT_eSPI::begin();
+ setRotation(1);
+ fillScreen(0);
+
+ // Init the back-light LED PWM
+ ledcSetup(BLK_PWM_CHANNEL, 44100, 8);
+ // ledcAttachPin(TFT_BL, BLK_PWM_CHANNEL);
+ ledcWrite(BLK_PWM_CHANNEL, 80);
+}
+
+void M5Display::sleep() {
+ startWrite();
+ writecommand(ILI9341_SLPIN); // Software reset
+ endWrite();
+}
+
+void M5Display::wakeup() {
+ startWrite();
+ writecommand(ILI9341_SLPOUT);
+ endWrite();
+}
+
+void M5Display::setBrightness(uint8_t brightness) {
+ ledcWrite(BLK_PWM_CHANNEL, brightness);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h, data);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint16_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h, data);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data, uint16_t transparent) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h, data,
+ transparent);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint8_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h,
+ (const uint16_t *)data);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint8_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h,
+ (uint16_t *)data);
+ setSwapBytes(swap);
+}
+
+void M5Display::progressBar(int x, int y, int w, int h, uint8_t val) {
+ drawRect(x, y, w, h, 0x09F1);
+ fillRect(x + 1, y + 1, w * (((float)val) / 100.0), h - 1, 0x09F1);
+}
+
+#include "utility/qrcode.h"
+void M5Display::qrcode(const char *string, uint16_t x, uint16_t y,
+ uint8_t width, uint8_t version) {
+ // Create the QR code
+ QRCode qrcode;
+ uint8_t qrcodeData[qrcode_getBufferSize(version)];
+ qrcode_initText(&qrcode, qrcodeData, version, 0, string);
+
+ // Top quiet zone
+ uint8_t thickness = width / qrcode.size;
+ uint16_t lineLength = qrcode.size * thickness;
+ uint8_t xOffset = x + (width - lineLength) / 2;
+ uint8_t yOffset = y + (width - lineLength) / 2;
+ fillRect(x, y, width, width, TFT_WHITE);
+
+ for (uint8_t y = 0; y < qrcode.size; y++) {
+ for (uint8_t x = 0; x < qrcode.size; x++) {
+ uint8_t q = qrcode_getModule(&qrcode, x, y);
+ if (q)
+ fillRect(x * thickness + xOffset, y * thickness + yOffset,
+ thickness, thickness, TFT_BLACK);
+ }
+ }
+}
+
+void M5Display::qrcode(const String &string, uint16_t x, uint16_t y,
+ uint8_t width, uint8_t version) {
+ int16_t len = string.length() + 2;
+ char buffer[len];
+ string.toCharArray(buffer, len);
+ qrcode(buffer, x, y, width, version);
+}
+
+// These read 16- and 32-bit types from the SD card file.
+// BMP data is stored little-endian, Arduino is little-endian too.
+// May need to reverse subscript order if porting elsewhere.
+
+uint16_t read16(fs::File &f) {
+ uint16_t result;
+ ((uint8_t *)&result)[0] = f.read(); // LSB
+ ((uint8_t *)&result)[1] = f.read(); // MSB
+ return result;
+}
+
+uint32_t read32(fs::File &f) {
+ uint32_t result;
+ ((uint8_t *)&result)[0] = f.read(); // LSB
+ ((uint8_t *)&result)[1] = f.read();
+ ((uint8_t *)&result)[2] = f.read();
+ ((uint8_t *)&result)[3] = f.read(); // MSB
+ return result;
+}
+
+// Bodmers BMP image rendering function
+void M5Display::drawBmpFile(fs::FS &fs, const char *path, uint16_t x,
+ uint16_t y) {
+ if ((x >= width()) || (y >= height())) return;
+
+ // Open requested file on SD card
+ File bmpFS = fs.open(path, "r");
+
+ if (!bmpFS) {
+ Serial.print("File not found");
+ return;
+ }
+
+ uint32_t seekOffset;
+ uint16_t w, h, row, col;
+ uint8_t r, g, b;
+
+ uint32_t startTime = millis();
+
+ if (read16(bmpFS) == 0x4D42) {
+ read32(bmpFS);
+ read32(bmpFS);
+ seekOffset = read32(bmpFS);
+ read32(bmpFS);
+ w = read32(bmpFS);
+ h = read32(bmpFS);
+
+ if ((read16(bmpFS) == 1) && (read16(bmpFS) == 24) &&
+ (read32(bmpFS) == 0)) {
+ y += h - 1;
+
+ setSwapBytes(true);
+ bmpFS.seek(seekOffset);
+
+ uint16_t padding = (4 - ((w * 3) & 3)) & 3;
+ uint8_t lineBuffer[w * 3 + padding];
+
+ for (row = 0; row < h; row++) {
+ bmpFS.read(lineBuffer, sizeof(lineBuffer));
+ uint8_t *bptr = lineBuffer;
+ uint16_t *tptr = (uint16_t *)lineBuffer;
+ // Convert 24 to 16 bit colours
+ for (col = 0; col < w; col++) {
+ b = *bptr++;
+ g = *bptr++;
+ r = *bptr++;
+ *tptr++ = ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+ }
+
+ // Push the pixel row to screen, pushImage will crop the line if
+ // needed y is decremented as the BMP image is drawn bottom up
+ pushImage(x, y--, w, 1, (uint16_t *)lineBuffer);
+ }
+ Serial.print("Loaded in ");
+ Serial.print(millis() - startTime);
+ Serial.println(" ms");
+ } else
+ Serial.println("BMP format not recognized.");
+ }
+ bmpFS.close();
+}
+
+// void M5Display::drawBmp(fs::FS &fs, const char *path, uint16_t x, uint16_t y)
+// {
+// drawBmpFile(fs, path, x, y);
+// }
+/***************************************************
+ This library is written to be compatible with Adafruit's ILI9341
+ library and automatically detects the display type on ESP_WROVER_KITs
+ Earlier WROVERs had ILI9341, while newer releases have ST7789V
+
+ MIT license, all text above must be included in any redistribution
+ ****************************************************/
+
+/*
+ * JPEG
+ */
+
+#include "rom/tjpgd.h"
+
+#define jpgColor(c) \
+ (((uint16_t)(((uint8_t *)(c))[0] & 0xF8) << 8) | \
+ ((uint16_t)(((uint8_t *)(c))[1] & 0xFC) << 3) | \
+ ((((uint8_t *)(c))[2] & 0xF8) >> 3))
+
+#if ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_ERROR
+const char *jd_errors[] = {"Succeeded",
+ "Interrupted by output function",
+ "Device error or wrong termination of input stream",
+ "Insufficient memory pool for the image",
+ "Insufficient stream input buffer",
+ "Parameter error",
+ "Data format error",
+ "Right format but not supported",
+ "Not supported JPEG standard"};
+#endif
+
+typedef struct {
+ uint16_t x;
+ uint16_t y;
+ uint16_t maxWidth;
+ uint16_t maxHeight;
+ uint16_t offX;
+ uint16_t offY;
+ jpeg_div_t scale;
+ const void *src;
+ size_t len;
+ size_t index;
+ M5Display *tft;
+ uint16_t outWidth;
+ uint16_t outHeight;
+} jpg_file_decoder_t;
+
+static uint32_t jpgReadFile(JDEC *decoder, uint8_t *buf, uint32_t len) {
+ jpg_file_decoder_t *jpeg = (jpg_file_decoder_t *)decoder->device;
+ File *file = (File *)jpeg->src;
+ if (buf) {
+ return file->read(buf, len);
+ } else {
+ file->seek(len, SeekCur);
+ }
+ return len;
+}
+
+static uint32_t jpgRead(JDEC *decoder, uint8_t *buf, uint32_t len) {
+ jpg_file_decoder_t *jpeg = (jpg_file_decoder_t *)decoder->device;
+ if (buf) {
+ memcpy(buf, (const uint8_t *)jpeg->src + jpeg->index, len);
+ }
+ jpeg->index += len;
+ return len;
+}
+
+static uint32_t jpgWrite(JDEC *decoder, void *bitmap, JRECT *rect) {
+ jpg_file_decoder_t *jpeg = (jpg_file_decoder_t *)decoder->device;
+ uint16_t x = rect->left;
+ uint16_t y = rect->top;
+ uint16_t w = rect->right + 1 - x;
+ uint16_t h = rect->bottom + 1 - y;
+ uint16_t oL = 0, oR = 0;
+ uint8_t *data = (uint8_t *)bitmap;
+
+ if (rect->right < jpeg->offX) {
+ return 1;
+ }
+ if (rect->left >= (jpeg->offX + jpeg->outWidth)) {
+ return 1;
+ }
+ if (rect->bottom < jpeg->offY) {
+ return 1;
+ }
+ if (rect->top >= (jpeg->offY + jpeg->outHeight)) {
+ return 1;
+ }
+ if (rect->top < jpeg->offY) {
+ uint16_t linesToSkip = jpeg->offY - rect->top;
+ data += linesToSkip * w * 3;
+ h -= linesToSkip;
+ y += linesToSkip;
+ }
+ if (rect->bottom >= (jpeg->offY + jpeg->outHeight)) {
+ uint16_t linesToSkip =
+ (rect->bottom + 1) - (jpeg->offY + jpeg->outHeight);
+ h -= linesToSkip;
+ }
+ if (rect->left < jpeg->offX) {
+ oL = jpeg->offX - rect->left;
+ }
+ if (rect->right >= (jpeg->offX + jpeg->outWidth)) {
+ oR = (rect->right + 1) - (jpeg->offX + jpeg->outWidth);
+ }
+
+ uint16_t pixBuf[32];
+ uint8_t pixIndex = 0;
+ uint16_t line;
+
+ jpeg->tft->startWrite();
+ // jpeg->tft->setAddrWindow(x - jpeg->offX + jpeg->x + oL, y - jpeg->offY +
+ // jpeg->y, w - (oL + oR), h);
+ jpeg->tft->setWindow(x - jpeg->offX + jpeg->x + oL,
+ y - jpeg->offY + jpeg->y,
+ x - jpeg->offX + jpeg->x + oL + w - (oL + oR) - 1,
+ y - jpeg->offY + jpeg->y + h - 1);
+
+ while (h--) {
+ data += 3 * oL;
+ line = w - (oL + oR);
+ while (line--) {
+ pixBuf[pixIndex++] = jpgColor(data);
+ data += 3;
+ if (pixIndex == 32) {
+ jpeg->tft->writePixels(pixBuf, 32);
+ // SPI.writePixels((uint8_t *)pixBuf, 64);
+ pixIndex = 0;
+ }
+ }
+ data += 3 * oR;
+ }
+ if (pixIndex) {
+ jpeg->tft->writePixels(pixBuf, pixIndex);
+ // SPI.writePixels((uint8_t *)pixBuf, pixIndex * 2);
+ }
+ jpeg->tft->endWrite();
+ return 1;
+}
+
+static bool jpgDecode(jpg_file_decoder_t *jpeg,
+ uint32_t (*reader)(JDEC *, uint8_t *, uint32_t)) {
+ static uint8_t work[3100];
+ JDEC decoder;
+
+ JRESULT jres = jd_prepare(&decoder, reader, work, 3100, jpeg);
+ if (jres != JDR_OK) {
+ log_e("jd_prepare failed! %s", jd_errors[jres]);
+ return false;
+ }
+
+ uint16_t jpgWidth = decoder.width / (1 << (uint8_t)(jpeg->scale));
+ uint16_t jpgHeight = decoder.height / (1 << (uint8_t)(jpeg->scale));
+
+ if (jpeg->offX >= jpgWidth || jpeg->offY >= jpgHeight) {
+ log_e("Offset Outside of JPEG size");
+ return false;
+ }
+
+ size_t jpgMaxWidth = jpgWidth - jpeg->offX;
+ size_t jpgMaxHeight = jpgHeight - jpeg->offY;
+
+ jpeg->outWidth =
+ (jpgMaxWidth > jpeg->maxWidth) ? jpeg->maxWidth : jpgMaxWidth;
+ jpeg->outHeight =
+ (jpgMaxHeight > jpeg->maxHeight) ? jpeg->maxHeight : jpgMaxHeight;
+
+ jres = jd_decomp(&decoder, jpgWrite, (uint8_t)jpeg->scale);
+ if (jres != JDR_OK) {
+ log_e("jd_decomp failed! %s", jd_errors[jres]);
+ return false;
+ }
+
+ return true;
+}
+
+void M5Display::drawJpg(const uint8_t *jpg_data, size_t jpg_len, uint16_t x,
+ uint16_t y, uint16_t maxWidth, uint16_t maxHeight,
+ uint16_t offX, uint16_t offY, jpeg_div_t scale) {
+ if ((x + maxWidth) > width() || (y + maxHeight) > height()) {
+ log_e("Bad dimensions given");
+ return;
+ }
+
+ jpg_file_decoder_t jpeg;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ jpeg.src = jpg_data;
+ jpeg.len = jpg_len;
+ jpeg.index = 0;
+ jpeg.x = x;
+ jpeg.y = y;
+ jpeg.maxWidth = maxWidth;
+ jpeg.maxHeight = maxHeight;
+ jpeg.offX = offX;
+ jpeg.offY = offY;
+ jpeg.scale = scale;
+ jpeg.tft = this;
+
+ jpgDecode(&jpeg, jpgRead);
+}
+
+void M5Display::drawJpgFile(fs::FS &fs, const char *path, uint16_t x,
+ uint16_t y, uint16_t maxWidth, uint16_t maxHeight,
+ uint16_t offX, uint16_t offY, jpeg_div_t scale) {
+ if ((x + maxWidth) > width() || (y + maxHeight) > height()) {
+ log_e("Bad dimensions given");
+ return;
+ }
+
+ File file = fs.open(path);
+ if (!file) {
+ log_e("Failed to open file for reading");
+ return;
+ }
+
+ jpg_file_decoder_t jpeg;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ jpeg.src = &file;
+ jpeg.len = file.size();
+ jpeg.index = 0;
+ jpeg.x = x;
+ jpeg.y = y;
+ jpeg.maxWidth = maxWidth;
+ jpeg.maxHeight = maxHeight;
+ jpeg.offX = offX;
+ jpeg.offY = offY;
+ jpeg.scale = scale;
+ jpeg.tft = this;
+
+ jpgDecode(&jpeg, jpgReadFile);
+
+ file.close();
+}
+
+/*
+ * PNG
+ */
+
+#include "utility/pngle.h"
+#include <HTTPClient.h>
+
+typedef struct _png_draw_params {
+ uint16_t x;
+ uint16_t y;
+ uint16_t maxWidth;
+ uint16_t maxHeight;
+ uint16_t offX;
+ uint16_t offY;
+ double scale;
+ uint8_t alphaThreshold;
+
+ M5Display *tft;
+} png_file_decoder_t;
+
+static void pngle_draw_callback(pngle_t *pngle, uint32_t x, uint32_t y,
+ uint32_t w, uint32_t h, uint8_t rgba[4]) {
+ png_file_decoder_t *p = (png_file_decoder_t *)pngle_get_user_data(pngle);
+ uint16_t color = jpgColor(rgba); // XXX: It's PNG ;)
+
+ if (x < p->offX || y < p->offY) return;
+ x -= p->offX;
+ y -= p->offY;
+
+ // An interlaced file with alpha channel causes disaster, so use 1 here for
+ // simplicity
+ w = 1;
+ h = 1;
+
+ if (p->scale != 1.0) {
+ x = (uint32_t)ceil(x * p->scale);
+ y = (uint32_t)ceil(y * p->scale);
+ w = (uint32_t)ceil(w * p->scale);
+ h = (uint32_t)ceil(h * p->scale);
+ }
+
+ if (x >= p->maxWidth || y >= p->maxHeight) return;
+ if (x + w >= p->maxWidth) w = p->maxWidth - x;
+ if (y + h >= p->maxHeight) h = p->maxHeight - y;
+
+ x += p->x;
+ y += p->y;
+
+ if (rgba[3] >= p->alphaThreshold) {
+ p->tft->fillRect(x, y, w, h, color);
+ }
+}
+
+void M5Display::drawPngFile(fs::FS &fs, const char *path, uint16_t x,
+ uint16_t y, uint16_t maxWidth, uint16_t maxHeight,
+ uint16_t offX, uint16_t offY, double scale,
+ uint8_t alphaThreshold) {
+ File file = fs.open(path);
+ if (!file) {
+ log_e("Failed to open file for reading");
+ return;
+ }
+
+ pngle_t *pngle = pngle_new();
+
+ png_file_decoder_t png;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ png.x = x;
+ png.y = y;
+ png.maxWidth = maxWidth;
+ png.maxHeight = maxHeight;
+ png.offX = offX;
+ png.offY = offY;
+ png.scale = scale;
+ png.alphaThreshold = alphaThreshold;
+ png.tft = this;
+
+ pngle_set_user_data(pngle, &png);
+ pngle_set_draw_callback(pngle, pngle_draw_callback);
+
+ // Feed data to pngle
+ uint8_t buf[1024];
+ int remain = 0;
+ int len;
+ while ((len = file.read(buf + remain, sizeof(buf) - remain)) > 0) {
+ int fed = pngle_feed(pngle, buf, remain + len);
+ if (fed < 0) {
+ log_e("[pngle error] %s", pngle_error(pngle));
+ break;
+ }
+
+ remain = remain + len - fed;
+ if (remain > 0) memmove(buf, buf + fed, remain);
+ }
+
+ pngle_destroy(pngle);
+ file.close();
+}
+
+void M5Display::drawPngUrl(const char *url, uint16_t x, uint16_t y,
+ uint16_t maxWidth, uint16_t maxHeight, uint16_t offX,
+ uint16_t offY, double scale,
+ uint8_t alphaThreshold) {
+ HTTPClient http;
+
+ if (WiFi.status() != WL_CONNECTED) {
+ log_e("Not connected");
+ return;
+ }
+
+ http.begin(url);
+
+ int httpCode = http.GET();
+ if (httpCode != HTTP_CODE_OK) {
+ log_e("HTTP ERROR: %d\n", httpCode);
+ http.end();
+ return;
+ }
+
+ WiFiClient *stream = http.getStreamPtr();
+
+ pngle_t *pngle = pngle_new();
+
+ png_file_decoder_t png;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ png.x = x;
+ png.y = y;
+ png.maxWidth = maxWidth;
+ png.maxHeight = maxHeight;
+ png.offX = offX;
+ png.offY = offY;
+ png.scale = scale;
+ png.alphaThreshold = alphaThreshold;
+ png.tft = this;
+
+ pngle_set_user_data(pngle, &png);
+ pngle_set_draw_callback(pngle, pngle_draw_callback);
+
+ // Feed data to pngle
+ uint8_t buf[1024];
+ int remain = 0;
+ int len;
+ while (http.connected()) {
+ size_t size = stream->available();
+ if (!size) {
+ delay(1);
+ continue;
+ }
+
+ if (size > sizeof(buf) - remain) size = sizeof(buf) - remain;
+ if ((len = stream->readBytes(buf + remain, size)) > 0) {
+ int fed = pngle_feed(pngle, buf, remain + len);
+ if (fed < 0) {
+ log_e("[pngle error] %s", pngle_error(pngle));
+ break;
+ }
+
+ remain = remain + len - fed;
+ if (remain > 0) memmove(buf, buf + fed, remain);
+ }
+ }
+
+ pngle_destroy(pngle);
+ http.end();
+}
+
+// Saves and restores font properties, datum, cursor, colors
+
+void M5Display::pushState() {
+ DisplayState s;
+ s.textfont = textfont;
+ s.textsize = textsize;
+ s.textcolor = textcolor;
+ s.textbgcolor = textbgcolor;
+ s.cursor_x = cursor_x;
+ s.cursor_y = cursor_y;
+ s.padX = padX;
+ _displayStateStack.push_back(s);
+}
+
+void M5Display::popState() {
+ if (_displayStateStack.empty()) return;
+ DisplayState s = _displayStateStack.back();
+ _displayStateStack.pop_back();
+ textfont = s.textfont;
+ textsize = s.textsize;
+ textcolor = s.textcolor;
+ textbgcolor = s.textbgcolor;
+ cursor_x = s.cursor_x;
+ cursor_y = s.cursor_y;
+ padX = s.padX;
+}
+
+#ifdef M5Stack_M5Core2
+
+#ifdef TFT_eSPI_TOUCH_EMULATION
+
+// Emulates the native (resistive) TFT_eSPI touch interface using M5.Touch
+
+uint8_t M5Display::getTouchRaw(uint16_t *x, uint16_t *y) {
+ return getTouch(x, y);
+}
+
+uint16_t M5Display::getTouchRawZ(void) {
+ return (TOUCH->ispressed()) ? 1000 : 0;
+}
+
+void M5Display::convertRawXY(uint16_t *x, uint16_t *y) {
+ return;
+}
+
+uint8_t M5Display::getTouch(uint16_t *x, uint16_t *y,
+ uint16_t threshold /* = 600 */) {
+ TOUCH->read();
+ if (TOUCH->points) {
+ *x = TOUCH->point[0].x;
+ *y = TOUCH->point[0].y;
+ return true;
+ }
+ return false;
+}
+
+void M5Display::calibrateTouch(uint16_t *data, uint32_t color_fg,
+ uint32_t color_bg, uint8_t size) {
+ return;
+}
+
+void M5Display::setTouch(uint16_t *data) {
+ return;
+}
+
+#endif /* TFT_eSPI_TOUCH_EMULATION */
+
+#endif /* M5Stack_M5Core2 */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/M5Display.h b/lib/M5Core2/src/M5Display.h
new file mode 100644
index 000000000..563f8d4ed
--- /dev/null
+++ b/lib/M5Core2/src/M5Display.h
@@ -0,0 +1,155 @@
+#pragma once
+#if defined (CORE2)
+#ifndef _M5DISPLAY_H_
+#define _M5DISPLAY_H_
+
+#include <Arduino.h>
+#include <vector>
+#include <FS.h>
+#include <SPI.h>
+
+#include "utility/Config.h" // This is where Core2 defines would be
+//#include "utility/In_eSPI.h"
+//#include "utility/Sprite.h"
+#include <TFT_eSPI.h>
+typedef enum {
+ JPEG_DIV_NONE,
+ JPEG_DIV_2,
+ JPEG_DIV_4,
+ JPEG_DIV_8,
+ JPEG_DIV_MAX
+} jpeg_div_t;
+
+struct DisplayState {
+ uint8_t textfont, textsize, datum;
+ uint32_t textcolor, textbgcolor;
+ int32_t cursor_x, cursor_y, padX;
+};
+
+class M5Display : public TFT_eSPI {
+ public:
+ static M5Display *instance;
+ M5Display();
+ void begin();
+ void sleep();
+ void wakeup();
+ void setBrightness(uint8_t brightness);
+ void clearDisplay(uint32_t color = ILI9341_BLACK) {
+ fillScreen(color);
+ }
+ void clear(uint32_t color = ILI9341_BLACK) {
+ fillScreen(color);
+ }
+ void display() {
+ }
+
+ inline void startWrite(void) {
+#if defined(SPI_HAS_TRANSACTION) && defined(SUPPORT_TRANSACTIONS) && \
+ !defined(ESP32_PARALLEL)
+ if (locked) {
+ locked = false;
+ SPI.beginTransaction(
+ SPISettings(SPI_FREQUENCY, MSBFIRST, SPI_MODE0));
+ }
+#endif
+ CS_L;
+ }
+ inline void endWrite(void) {
+#if defined(SPI_HAS_TRANSACTION) && defined(SUPPORT_TRANSACTIONS) && \
+ !defined(ESP32_PARALLEL)
+ if (!inTransaction) {
+ if (!locked) {
+ locked = true;
+ SPI.endTransaction();
+ }
+ }
+#endif
+ CS_H;
+ }
+ inline void writePixel(uint16_t color) {
+ SPI.write16(color);
+ }
+ inline void writePixels(uint16_t *colors, uint32_t len) {
+ SPI.writePixels((uint8_t *)colors, len * 2);
+ }
+ void progressBar(int x, int y, int w, int h, uint8_t val);
+
+#define setFont setFreeFont
+
+ void qrcode(const char *string, uint16_t x = 50, uint16_t y = 10,
+ uint8_t width = 220, uint8_t version = 6);
+ void qrcode(const String &string, uint16_t x = 50, uint16_t y = 10,
+ uint8_t width = 220, uint8_t version = 6);
+
+ void drawBmp(fs::FS &fs, const char *path, uint16_t x, uint16_t y);
+ void drawBmpFile(fs::FS &fs, const char *path, uint16_t x, uint16_t y);
+
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint8_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint16_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint8_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data, uint16_t transparent);
+
+ void drawJpg(const uint8_t *jpg_data, size_t jpg_len, uint16_t x = 0,
+ uint16_t y = 0, uint16_t maxWidth = 0, uint16_t maxHeight = 0,
+ uint16_t offX = 0, uint16_t offY = 0,
+ jpeg_div_t scale = JPEG_DIV_NONE);
+
+ void drawJpg(fs::FS &fs, const char *path, uint16_t x = 0, uint16_t y = 0,
+ uint16_t maxWidth = 0, uint16_t maxHeight = 0,
+ uint16_t offX = 0, uint16_t offY = 0,
+ jpeg_div_t scale = JPEG_DIV_NONE);
+
+ void drawJpgFile(fs::FS &fs, const char *path, uint16_t x = 0,
+ uint16_t y = 0, uint16_t maxWidth = 0,
+ uint16_t maxHeight = 0, uint16_t offX = 0,
+ uint16_t offY = 0, jpeg_div_t scale = JPEG_DIV_NONE);
+
+ void drawPngFile(fs::FS &fs, const char *path, uint16_t x = 0,
+ uint16_t y = 0, uint16_t maxWidth = 0,
+ uint16_t maxHeight = 0, uint16_t offX = 0,
+ uint16_t offY = 0, double scale = 1.0,
+ uint8_t alphaThreshold = 127);
+
+ void drawPngUrl(const char *url, uint16_t x = 0, uint16_t y = 0,
+ uint16_t maxWidth = 0, uint16_t maxHeight = 0,
+ uint16_t offX = 0, uint16_t offY = 0, double scale = 1.0,
+ uint8_t alphaThreshold = 127);
+
+// Saves and restores font properties, datum, cursor and colors so
+// code can be non-invasive. Just make sure that every push is also
+// popped when you're done to prevent stack from growing.
+//
+// (User code can never do this completely because the gfxFont
+// class variable of TFT_eSPI is protected.)
+#define M5DISPLAY_HAS_PUSH_POP
+ public:
+ void pushState();
+ void popState();
+
+ private:
+ std::vector<DisplayState> _displayStateStack;
+
+#ifdef M5Stack_M5Core2
+
+#ifdef TFT_eSPI_TOUCH_EMULATION
+ // Emulates the TFT_eSPI touch interface using M5.Touch
+ public:
+ uint8_t getTouchRaw(uint16_t *x, uint16_t *y);
+ uint16_t getTouchRawZ(void);
+ void convertRawXY(uint16_t *x, uint16_t *y);
+ uint8_t getTouch(uint16_t *x, uint16_t *y, uint16_t threshold = 600);
+ void calibrateTouch(uint16_t *data, uint32_t color_fg, uint32_t color_bg,
+ uint8_t size);
+ void setTouch(uint16_t *data);
+#endif /* TFT_eSPI_TOUCH_EMULATION */
+
+#endif /* M5Stack_M5Core2 */
+};
+#endif /* _M5DISPLAY_H_ */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/M5Touch.cpp b/lib/M5Core2/src/M5Touch.cpp
new file mode 100644
index 000000000..27e52da36
--- /dev/null
+++ b/lib/M5Core2/src/M5Touch.cpp
@@ -0,0 +1,178 @@
+#if defined (CORE2)
+#include <M5Core2.h>
+
+// M5Touch class
+
+/* static */ M5Touch* M5Touch::instance;
+
+M5Touch::M5Touch() {
+ if (!instance) instance = this;
+}
+
+void M5Touch::begin() {
+ Wire1.begin(21, 22);
+ pinMode(CST_INT, INPUT);
+
+ // By default, the FT6336 will pulse the INT line for every touch
+ // event. But because it shares the Wire1 TwoWire/I2C with other
+ // devices, we cannot easily create an interrupt service routine to
+ // handle these events. So instead, we set the INT wire to polled mode,
+ // so it simply goes low as long as there is at least one valid touch.
+ ft6336(0xA4, 0x00);
+
+ Serial.print("touch: ");
+ if (interval(DEFAULT_INTERVAL) == DEFAULT_INTERVAL) {
+ Serial.printf("FT6336 ready (fw id 0x%02X rel %d, lib 0x%02X%02X)\n",
+ ft6336(0xA6), ft6336(0xAF), ft6336(0xA1), ft6336(0xA2));
+ } else {
+ Serial.println("ERROR - FT6336 not responding");
+ }
+}
+
+bool M5Touch::ispressed() {
+ return (digitalRead(CST_INT) == LOW);
+}
+
+// Single register read and write
+
+uint8_t M5Touch::ft6336(uint8_t reg) {
+ Wire1.beginTransmission((uint8_t)CST_DEVICE_ADDR);
+ Wire1.write(reg);
+ Wire1.endTransmission();
+ Wire1.requestFrom((uint8_t)CST_DEVICE_ADDR, uint8_t(1));
+ return Wire1.read();
+}
+
+void M5Touch::ft6336(uint8_t reg, uint8_t value) {
+ Wire1.beginTransmission(CST_DEVICE_ADDR);
+ Wire1.write(reg);
+ Wire1.write((uint8_t)value);
+ Wire1.endTransmission();
+}
+
+// Reading size bytes into data
+void M5Touch::ft6336(uint8_t reg, uint8_t size, uint8_t* data) {
+ Wire1.beginTransmission((uint8_t)CST_DEVICE_ADDR);
+ Wire1.write(reg);
+ Wire1.endTransmission();
+ Wire1.requestFrom((uint8_t)CST_DEVICE_ADDR, size);
+ for (uint8_t i = 0; i < size; i++) data[i] = Wire1.read();
+}
+
+uint8_t M5Touch::interval(uint8_t ivl) {
+ ft6336(0x88, ivl);
+ return interval();
+}
+
+uint8_t M5Touch::interval() {
+ _interval = ft6336(0x88);
+ return _interval;
+}
+
+// This is normally called from update()
+bool M5Touch::read() {
+ // true if real read, not a "come back later"
+ wasRead = false;
+
+ // true is something actually changed on the touchpad
+ changed = false;
+
+ // Return immediately if read() is called more frequently than the
+ // touch sensor updates. This prevents unnecessary I2C reads, and the
+ // data can also get corrupted if reads are too close together.
+ if (millis() - _lastRead < _interval) return false;
+ _lastRead = millis();
+
+ Point p[2];
+ uint8_t pts = 0;
+ uint8_t p0f = 0;
+ if (ispressed()) {
+ uint8_t data[11];
+ ft6336(0x02, 11, data);
+ pts = data[0];
+ if (pts > 2) return false;
+ if (pts) {
+ // Read the data. Never mind trying to read the "weight" and
+ // "size" properties or using the built-in gestures: they
+ // are always set to zero.
+ p0f = (data[3] >> 4) ? 1 : 0;
+ p[0].x = ((data[1] << 8) | data[2]) & 0x0fff;
+ p[0].y = ((data[3] << 8) | data[4]) & 0x0fff;
+ if (p[0].x >= TOUCH_W || p[0].y >= TOUCH_H) return false;
+ if (pts == 2) {
+ p[1].x = ((data[7] << 8) | data[8]) & 0x0fff;
+ p[1].y = ((data[9] << 8) | data[10]) & 0x0fff;
+ if (p[1].x >= TOUCH_W || p[1].y >= TOUCH_H) return false;
+ }
+ }
+ }
+
+#ifdef TFT
+ p[0].rotate(TFT->rotation);
+ p[1].rotate(TFT->rotation);
+#endif /* TFT */
+
+ if (p[0] != point[0] || p[1] != point[1]) {
+ changed = true;
+ point[0] = p[0];
+ point[1] = p[1];
+ points = pts;
+ point0finger = p0f;
+ }
+ wasRead = true;
+ return true;
+}
+
+Point M5Touch::getPressPoint() {
+ read();
+ if (point[0]) return point[0];
+ return Point(-1, -1); // -1, -1 is old API's definition of invalid
+}
+
+void M5Touch::update() {
+ read();
+}
+
+void M5Touch::dump() {
+ uint8_t data[256] = {0};
+ ft6336(0x00, 0x80, data);
+ ft6336(0x80, 0x80, data + 0x80);
+ Serial.printf("\n ");
+ for (uint8_t i = 0; i < 16; i++) Serial.printf(".%1X ", i);
+ Serial.printf("\n");
+ for (uint16_t i = 0; i < 0x100; i++) {
+ if (!(i % 16)) Serial.printf("\n%1X. ", i / 16);
+ Serial.printf("%02X ", data[i]);
+ }
+ Serial.printf("\n\n\n");
+}
+
+// HotZone class (for compatibility with older M5Core2 code)
+
+HotZone::HotZone(int16_t x0_, int16_t y0_, int16_t x1_, int16_t y1_,
+ void (*fun_)() /* = nullptr */
+ )
+ : Zone(x0_, y0_, x1_ - x0_, y1_ - y0_) {
+ fun = fun_;
+}
+
+void HotZone::setZone(int16_t x0_, int16_t y0_, int16_t x1_, int16_t y1_,
+ void (*fun_)() /*= nullptr */
+) {
+ set(x0_, y0_, x1_ - x0_, y1_ - y0_);
+ fun = fun_;
+}
+
+bool HotZone::inHotZone(Point& p) {
+ return contains(p);
+}
+
+bool HotZone::inHotZoneDoFun(Point& p) {
+ if (contains(p)) {
+ if (fun) fun();
+ return true;
+ } else {
+ return false;
+ }
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/M5Touch.h b/lib/M5Core2/src/M5Touch.h
new file mode 100644
index 000000000..6a65617a8
--- /dev/null
+++ b/lib/M5Core2/src/M5Touch.h
@@ -0,0 +1,288 @@
+#if defined (CORE2)
+/*
+
+== M5Touch - The M5Stack Core2 Touch Library ==
+
+ This is the library behind the M5.Touch object that you can use to read
+ from the touch sensor on the M5Stack Core2. It was made to be an input
+ source for the M5Button library that provides higher level buttons,
+ gestures and events, but both libraries can be also be used alone.
+
+
+== About the Touch Sensor in the M5Stack Core2 ==
+
+ Touchpanel interfacing is done by a FocalTech FT6336 chip, which supports
+ two simultaneous touches. However, the M5Stack Core2 touch display is only
+ multi-touch in one dimension. What that means is that it can detect two
+ separate touches only if they occur on different vertical positions. This
+ has to do with the way the touch screen is wired, it's not something that
+ can be changed in software. So you will only ever see two points if they do
+ not occur side-by-side. Touches that do happen side-by-side blend into one
+ touch that is detected somewhere between the actual touches.
+
+ While this limits multi-touch somewhat, you can still create multiple
+ buttons and see two that are not on the same row simultaneously. You could
+ also use one of the buttons below the screen as a modifier for something
+ touched on the screen.
+
+ The touch sensor extends to below the screen of the Core2: the sensor maps
+ to 320x280 pixels, the screen is 320x240. The missing 40 pixels are placed
+ below the screen, where the printed circles are. This is meant to simulate
+ the three hardware buttons on the original M5Stack units. Note that on some
+ units the touch sensor in this area only operates properly if the USB cable
+ is plugged in or if the unit is placed firmly in your hand on a metal
+ surface.
+
+ For a quick view of how the sensor sees the world, try this sketch:
+
+ #include <M5Core2.h>
+
+ void setup() {
+ M5.begin();
+ M5.Lcd.fillScreen(WHITE);
+ }
+
+ void loop() {
+ M5.update();
+ Event& e = M5.Buttons.event;
+ if (e & (E_MOVE | E_RELEASE)) circle(e & E_MOVE ? e.from : e.to, WHITE);
+ if (e & (E_TOUCH | E_MOVE)) circle(e.to, e.finger ? BLUE : RED);
+ }
+
+ void circle(Point p, uint16_t c) {
+ M5.Lcd.drawCircle(p.x, p.y, 50, c);
+ M5.Lcd.drawCircle(p.x, p.y, 52, c);
+ }
+
+ (Don't worry if this all seems abracadabra now, we'll get to all of
+ this is due time.)
+
+
+== Point and Zone: Describing Points and Areas on the Screen ==
+
+ The Point and Zone classes allow you to create variables that hold a point
+ or an area on the screen. You can
+
+ Point(x, y)
+
+ Holds a point on the screen. Has members x and y that hold the coordinates
+ of a touch. Values INVALID_VALUE for x and y indicate an invalid value,
+ and that's what a point starts out with if you declare it without
+ parameters. The 'valid()' method tests if a point is valid. If you
+ explicitly evaluate a Point as a boolean ("if (p) ..."), you also get
+ whether the point is valid, so this is equivalent to writing "if
+ (p.valid()) ...".
+
+ Zone(x, y, w, h)
+
+ Holds a rectangular area on the screen. Members x, y, w and h are for the
+ x and y coordinate of the top-left corner and the width and height of the
+ rectangle.
+
+ The 'set' method allows you to change the properties of an existing Point
+ or Zone. Using the 'in' or 'contains' method you can test if a point lies
+ in a zone.
+
+ The PointAndZone library also provides the low-level support for direction
+ from one point to another and for screen rotation translations.
+
+ The documentation in src/utility/PointAndZone.h provides more details and
+ examples.
+
+
+== Basic Touch API ==
+
+ The basic touch API provides a way to read the data from the touch sensor.
+
+
+ M5.update()
+ In the loop() part of your sketch, call "M5.update()". This will in turn
+ call M5.Touch.update(), which is the only part that talks to the touch
+ interface. It updates the data used by the rest of the API.
+
+ M5.Touch.changed
+ Is true if anything changed on the touchpad since the last time
+ M5.update() was called.
+
+ M5.Touch.points
+ Contains the number of touches detected: 0, 1 or 2.
+
+ M5.Touch.point[0], M5.Touch.point[1]
+ M5.Touch.point[0] and M5.Touch.point[1] are Points that hold the detected
+ touches.
+
+
+ A very simple sketch to print the location where the screen is touched:
+
+ #include <M5Core2.h>
+
+ void setup() {
+ M5.begin();
+ }
+
+ void loop() {
+ M5.update();
+ if ( M5.Touch.changed ) Serial.println( M5.Touch.point[0] );
+ }
+
+
+== Buttons, Gestures, Events ==
+
+ Note that you may not want to use any of the above directly. The M5Buttons
+ library provides button, gestures and events that allow you to quickly
+ create reactive visual buttons on the screen and react differently based on
+ whether a button was clicked, tapped, or double-tapped. Have a look at the
+ documentation for that, which is in the M5Button.h file in the src/utility
+ directory of this repository. The examples under "File / Examples
+ / M5Core2 / Touch" in your Arduino environment should give you an
+ idea of what's possible.
+
+
+== Screen Rotation ==
+
+ If you rotate the screen with M5.Lcd.setRotation, the touch coordinates
+ will rotate along with it.
+
+ * What that means is that either x or y for the area below the screen may
+ go negative. Say you use the screen upside-down with
+ M5.Lcd.setRotation(3). In that case the off-screen touch area (Y
+ coordinates 240 through 279) that was below the screen now becomes above
+ the screen and has Y coordinates -40 through -1.
+
+ * See the M5Button library for a feature that allows you to keep some Zone
+ and Button objects in the same place on the physical screen, regardless
+ of rotation.
+
+
+== TFT_eSPI Resistive Touch API emulation ==
+
+ While technically not part of this library itself, we added an emulation
+ for the TFT_eSPI touch API to the M5Display object that merely passes
+ informaton on to the M5.Touch object. So M5.Lcd can be addressed as if it's
+ a touch screen using that older resistive touch interface. Do note that
+ this interface is not nearly as powerful as M5.Touch's native API. But
+ together with M5Button's TFT_eSPI_Button emulation, this should allow you
+ to compile lots of ESP32 software written for touch screens.
+
+
+== Advanced Uses of the Touch Library ==
+
+ You should never need any of the below features in everyday use. But
+ they're there just in case...
+
+ M5.Touch.wasRead
+ True if the sensor was actually read. The sensor can only provide updates
+ every 13 milliseconds or so. M5.update() can loop as quick as once every
+ 20 MICROseconds, meaning it would continually read the sensor when there
+ was nothing to read. So M5.Touch.read() only really reads when it's time
+ to do so, and returns with M5.Touch.wasRead false otherwise.
+
+ M5.point0finger
+ The FT6336 chip keeps track of fingers, each touch has a finger ID of 0
+ or 1. So when there are two touches in point[0] and point[1] and then one
+ goes away, point0finger allows you to see which touch is left in
+ point[0].
+
+ M5.Touch.interval()
+ Without arguments returns the current interval between sensor updates in
+ milliseconds. If you supply a number that's the new interval. The default
+ of 13 seems to give the most updates per second.
+
+ M5.Touch.ft6336(reg)
+ M5.Touch.ft6336(reg, value)
+ M5.Touch.ft6336(reg, size, *data)
+ Allows you to read and write registers on the ft6336 touch interface chip
+ directly. The first form reads one byte, the second form writes one and
+ the third form reads a block of 'size' bytes starting at 'reg' into a
+ buffer at '*data'.
+
+ M5.Touch.dump()
+ M5.Touch.dump() dumps the entire register space on the FT6336 chip as a
+ formatted hexdump to the serial port.
+
+
+== Legacy API ==
+
+ There was a previous version of this library, and it provided a number of
+ functions that were single touch only. The older version did not have
+ M5.update(). Instead it used ispressed() and getPressedPoint() functions as
+ well as HotZones, which provided something that worked a little bit like
+ Buttons. This older API is still supported (the M5Core2 Factory Test sketch
+ still works), but you should not use it for new programs. The ispressed()
+ function specifically does not mix well with code that uses M5.update().
+
+
+== Example ==
+
+ It may sound complicated when you read it all in this document, but it's
+ all made to be easy to use.
+
+ Under File / Examples / M5Core2 / Touch in your Arduino environment is an
+ example sketch called "events_buttons_gestures_rotation" that shows both
+ this library and M5Button in action. Please have a look at it to understand
+ how this all works and run the sketch to see all the events printed to the
+ serial port. It shows buttons, gestures and events and should be pretty
+ self-explanatory.
+
+*/
+
+#ifndef _M5TOUCH_H_
+#define _M5TOUCH_H_
+
+#include <M5Display.h>
+
+#include "utility/Config.h" // Defines 'TFT', a pointer to the display
+#include "utility/PointAndZone.h"
+
+#define TOUCH_W 320
+#define TOUCH_H 280
+#define CST_DEVICE_ADDR 0x38
+#define CST_INT 39
+
+// Strangely, the value 13 leads to slightly more frequent updates than 10
+// (Still not every 13 ms, often more like 15 to 20)
+#define DEFAULT_INTERVAL 13
+
+class M5Touch {
+ public:
+ static M5Touch* instance;
+ M5Touch();
+ void begin();
+ uint8_t ft6336(uint8_t reg);
+ void ft6336(uint8_t reg, uint8_t value);
+ void ft6336(uint8_t reg, uint8_t size, uint8_t* data);
+ uint8_t interval(uint8_t ivl);
+ uint8_t interval();
+ void update();
+ bool read();
+ bool ispressed();
+ void dump();
+ Point getPressPoint();
+ uint8_t points;
+ bool changed, wasRead;
+ Point point[2];
+ uint8_t point0finger;
+
+ protected:
+ uint8_t _interval;
+ uint32_t _lastRead;
+};
+
+// For compatibility with older M5Core2 code
+class HotZone : public Zone {
+ public:
+ HotZone(int16_t x0_, int16_t y0_, int16_t x1_, int16_t y1_,
+ void (*fun_)() = nullptr);
+ void setZone(int16_t x0_, int16_t y0_, int16_t x1_, int16_t y1_,
+ void (*fun_)() = nullptr);
+ bool inHotZone(Point& p);
+ bool inHotZoneDoFun(Point& p);
+ void (*fun)();
+};
+
+#define HotZone_t HotZone
+#define TouchPoint Point
+#define TouchPoint_t Point
+
+#endif /* _M5TOUCH_H_ */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/RTC.cpp b/lib/M5Core2/src/RTC.cpp
new file mode 100644
index 000000000..d30a3c79b
--- /dev/null
+++ b/lib/M5Core2/src/RTC.cpp
@@ -0,0 +1,301 @@
+#if defined (CORE2)
+#include "RTC.h"
+
+RTC::RTC() {
+}
+
+void RTC::begin(void) {
+ Wire1.begin(21, 22);
+ WriteReg(0x00, 0x00);
+ WriteReg(0x01, 0x00);
+ WriteReg(0x0D, 0x00);
+}
+
+void RTC::WriteReg(uint8_t reg, uint8_t data) {
+ Wire1.beginTransmission(0x51);
+ Wire1.write(reg);
+ Wire1.write(data);
+ Wire1.endTransmission();
+}
+
+uint8_t RTC::ReadReg(uint8_t reg) {
+ Wire1.beginTransmission(0x51);
+ Wire1.write(reg);
+ Wire1.endTransmission(false);
+ Wire1.requestFrom(0x51, 1);
+ return Wire1.read();
+}
+
+bool RTC::getVoltLow(void) {
+ return (ReadReg(0x02) & 0x80) >> 7; // RTCC_VLSEC_MASK
+}
+
+void RTC::GetBm8563Time(void) {
+ Wire1.beginTransmission(0x51);
+ Wire1.write(0x02);
+ Wire1.endTransmission(false);
+ Wire1.requestFrom(0x51, 7);
+ while (Wire1.available()) {
+ trdata[0] = Wire1.read();
+ trdata[1] = Wire1.read();
+ trdata[2] = Wire1.read();
+ trdata[3] = Wire1.read();
+ trdata[4] = Wire1.read();
+ trdata[5] = Wire1.read();
+ trdata[6] = Wire1.read();
+ }
+
+ DataMask();
+ Bcd2asc();
+ Str2Time();
+}
+
+void RTC::Str2Time(void) {
+ Second = (asc[0] - 0x30) * 10 + asc[1] - 0x30;
+ Minute = (asc[2] - 0x30) * 10 + asc[3] - 0x30;
+ Hour = (asc[4] - 0x30) * 10 + asc[5] - 0x30;
+ /*
+ uint8_t Hour;
+ uint8_t Week;
+ uint8_t Day;
+ uint8_t Month;
+ uint8_t Year;
+ */
+}
+
+void RTC::DataMask() {
+ trdata[0] = trdata[0] & 0x7f; // 秒
+ trdata[1] = trdata[1] & 0x7f; // 分
+ trdata[2] = trdata[2] & 0x3f; // 时
+
+ trdata[3] = trdata[3] & 0x3f; // 日
+ trdata[4] = trdata[4] & 0x07; // 星期
+ trdata[5] = trdata[5] & 0x1f; // 月
+
+ trdata[6] = trdata[6] & 0xff; // 年
+}
+/********************************************************************
+函 数 名： void Bcd2asc(void)
+功 能： bcd 码转换成 asc 码，供Lcd显示用
+说 明：
+调 用：
+入口参数：
+返 回 值：无
+***********************************************************************/
+void RTC::Bcd2asc(void) {
+ uint8_t i, j;
+ for (j = 0, i = 0; i < 7; i++) {
+ asc[j++] =
+ (trdata[i] & 0xf0) >> 4 | 0x30; /*格式为: 秒 分 时 日 月 星期 年 */
+ asc[j++] = (trdata[i] & 0x0f) | 0x30;
+ }
+}
+
+uint8_t RTC::Bcd2ToByte(uint8_t Value) {
+ uint8_t tmp = 0;
+ tmp = ((uint8_t)(Value & (uint8_t)0xF0) >> (uint8_t)0x4) * 10;
+ return (tmp + (Value & (uint8_t)0x0F));
+}
+
+uint8_t RTC::ByteToBcd2(uint8_t Value) {
+ uint8_t bcdhigh = Value / 10;
+ return (bcdhigh << 4) | (Value - (bcdhigh * 10));
+}
+
+void RTC::GetTime(RTC_TimeTypeDef *RTC_TimeStruct) {
+ // if()
+ uint8_t buf[3] = {0};
+
+ Wire1.beginTransmission(0x51);
+ Wire1.write(0x02);
+ Wire1.endTransmission(false);
+ Wire1.requestFrom(0x51, 3);
+
+ while (Wire1.available()) {
+ buf[0] = Wire1.read();
+ buf[1] = Wire1.read();
+ buf[2] = Wire1.read();
+ }
+
+ RTC_TimeStruct->Seconds = Bcd2ToByte(buf[0] & 0x7f); // 秒
+ RTC_TimeStruct->Minutes = Bcd2ToByte(buf[1] & 0x7f); // 分
+ RTC_TimeStruct->Hours = Bcd2ToByte(buf[2] & 0x3f); // 时
+}
+
+int RTC::SetTime(RTC_TimeTypeDef *RTC_TimeStruct) {
+ if (RTC_TimeStruct == NULL || RTC_TimeStruct->Hours > 24 ||
+ RTC_TimeStruct->Minutes > 60 || RTC_TimeStruct->Seconds > 60)
+ return 0;
+ Wire1.beginTransmission(0x51);
+ Wire1.write(0x02);
+ Wire1.write(ByteToBcd2(RTC_TimeStruct->Seconds));
+ Wire1.write(ByteToBcd2(RTC_TimeStruct->Minutes));
+ Wire1.write(ByteToBcd2(RTC_TimeStruct->Hours));
+ Wire1.endTransmission();
+ return 1;
+}
+
+void RTC::GetDate(RTC_DateTypeDef *RTC_DateStruct) {
+ uint8_t buf[4] = {0};
+
+ Wire1.beginTransmission(0x51);
+ Wire1.write(0x05);
+ Wire1.endTransmission(false);
+ Wire1.requestFrom(0x51, 4);
+
+ while (Wire1.available()) {
+ buf[0] = Wire1.read();
+ buf[1] = Wire1.read();
+ buf[2] = Wire1.read();
+ buf[3] = Wire1.read();
+ }
+
+ RTC_DateStruct->Date = Bcd2ToByte(buf[0] & 0x3f);
+ RTC_DateStruct->WeekDay = Bcd2ToByte(buf[1] & 0x07);
+ RTC_DateStruct->Month = Bcd2ToByte(buf[2] & 0x1f);
+
+ if (buf[2] & 0x80) {
+ RTC_DateStruct->Year = 1900 + Bcd2ToByte(buf[3] & 0xff);
+ } else {
+ RTC_DateStruct->Year = 2000 + Bcd2ToByte(buf[3] & 0xff);
+ }
+}
+
+int RTC::SetDate(RTC_DateTypeDef *RTC_DateStruct) {
+ if (RTC_DateStruct == NULL || RTC_DateStruct->WeekDay > 7 ||
+ RTC_DateStruct->Date > 31 || RTC_DateStruct->Month > 12)
+ return 0;
+ Wire1.beginTransmission(0x51);
+ Wire1.write(0x05);
+ Wire1.write(ByteToBcd2(RTC_DateStruct->Date));
+ Wire1.write(ByteToBcd2(RTC_DateStruct->WeekDay));
+
+ if (RTC_DateStruct->Year < 2000) {
+ Wire1.write(ByteToBcd2(RTC_DateStruct->Month) | 0x80);
+ Wire1.write(ByteToBcd2((uint8_t)(RTC_DateStruct->Year % 100)));
+ } else {
+ /* code */
+ Wire1.write(ByteToBcd2(RTC_DateStruct->Month) | 0x00);
+ Wire1.write(ByteToBcd2((uint8_t)(RTC_DateStruct->Year % 100)));
+ }
+
+ Wire1.endTransmission();
+ return 1;
+}
+
+int RTC::SetAlarmIRQ(int afterSeconds) {
+ uint8_t reg_value = 0;
+ reg_value = ReadReg(0x01);
+
+ if (afterSeconds < 0) {
+ reg_value &= ~(1 << 0);
+ WriteReg(0x01, reg_value);
+ reg_value = 0x03;
+ WriteReg(0x0E, reg_value);
+ return -1;
+ }
+
+ uint8_t type_value = 2;
+ uint8_t div = 1;
+ if (afterSeconds > 255) {
+ div = 60;
+ type_value = 0x83;
+ } else {
+ type_value = 0x82;
+ }
+
+ afterSeconds = (afterSeconds / div) & 0xFF;
+ WriteReg(0x0F, afterSeconds);
+ WriteReg(0x0E, type_value);
+
+ reg_value |= (1 << 0);
+ reg_value &= ~(1 << 7);
+ WriteReg(0x01, reg_value);
+ return afterSeconds * div;
+}
+
+int RTC::SetAlarmIRQ(const RTC_TimeTypeDef &RTC_TimeStruct) {
+ uint8_t irq_enable = false;
+ uint8_t out_buf[4] = {0x80, 0x80, 0x80, 0x80};
+
+ if (RTC_TimeStruct.Minutes >= 0) {
+ irq_enable = true;
+ out_buf[0] = ByteToBcd2(RTC_TimeStruct.Minutes) &
+ 0x7f; // 将第7位置0,其他表示分钟
+ }
+
+ if (RTC_TimeStruct.Hours >= 0) {
+ irq_enable = true;
+ out_buf[1] = ByteToBcd2(RTC_TimeStruct.Hours) &
+ 0x3f; // 将第7,6位置0,其他表示小时
+ }
+
+ for (int i = 0; i < 4; i++) {
+ WriteReg(0x09 + i, out_buf[i]);
+ delay(2);
+ }
+
+ uint8_t reg_value = ReadReg(0x01);
+
+ if (irq_enable) {
+ reg_value |= (1 << 1); // 第 2 位设置为 1
+ } else {
+ reg_value &= ~(1 << 1);
+ }
+
+ WriteReg(0x01, reg_value);
+
+ return irq_enable ? 1 : 0;
+}
+
+int RTC::SetAlarmIRQ(const RTC_DateTypeDef &RTC_DateStruct,
+ const RTC_TimeTypeDef &RTC_TimeStruct) {
+ uint8_t irq_enable = false;
+ uint8_t out_buf[4] = {0x80, 0x80, 0x80, 0x80};
+
+ if (RTC_TimeStruct.Minutes >= 0) {
+ irq_enable = true;
+ out_buf[0] = ByteToBcd2(RTC_TimeStruct.Minutes) & 0x7f;
+ }
+
+ if (RTC_TimeStruct.Hours >= 0) {
+ irq_enable = true;
+ out_buf[1] = ByteToBcd2(RTC_TimeStruct.Hours) & 0x3f;
+ }
+
+ if (RTC_DateStruct.Date >= 0) {
+ irq_enable = true;
+ out_buf[2] = ByteToBcd2(RTC_DateStruct.Date) & 0x3f;
+ }
+
+ if (RTC_DateStruct.WeekDay >= 0) {
+ irq_enable = true;
+ out_buf[3] = ByteToBcd2(RTC_DateStruct.WeekDay) & 0x07;
+ }
+
+ uint8_t reg_value = ReadReg(0x01);
+
+ if (irq_enable) {
+ reg_value |= (1 << 1);
+ } else {
+ reg_value &= ~(1 << 1);
+ }
+
+ for (int i = 0; i < 4; i++) {
+ WriteReg(0x09 + i, out_buf[i]);
+ }
+ WriteReg(0x01, reg_value);
+
+ return irq_enable ? 1 : 0;
+}
+
+void RTC::clearIRQ() {
+ uint8_t data = ReadReg(0x01);
+ WriteReg(0x01, data & 0xf3);
+}
+void RTC::disableIRQ() {
+ clearIRQ();
+ uint8_t data = ReadReg(0x01);
+ WriteReg(0x01, data & 0xfC);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/RTC.h b/lib/M5Core2/src/RTC.h
new file mode 100644
index 000000000..4b7e64c7f
--- /dev/null
+++ b/lib/M5Core2/src/RTC.h
@@ -0,0 +1,73 @@
+#if defined (CORE2)
+#ifndef __RTC_H__
+#define __RTC_H__
+
+#include <Wire.h>
+
+typedef struct {
+ int8_t Hours;
+ int8_t Minutes;
+ int8_t Seconds;
+} RTC_TimeTypeDef;
+
+typedef struct {
+ int8_t WeekDay;
+ int8_t Month;
+ int8_t Date;
+ int16_t Year;
+} RTC_DateTypeDef;
+
+class RTC {
+ public:
+ RTC();
+
+ void begin(void);
+ void GetBm8563Time(void);
+
+ int SetTime(RTC_TimeTypeDef* RTC_TimeStruct);
+ int SetDate(RTC_DateTypeDef* RTC_DateStruct);
+
+ void GetTime(RTC_TimeTypeDef* RTC_TimeStruct);
+ void GetDate(RTC_DateTypeDef* RTC_DateStruct);
+
+ int SetAlarmIRQ(int afterSeconds);
+ int SetAlarmIRQ(const RTC_TimeTypeDef& RTC_TimeStruct);
+ int SetAlarmIRQ(const RTC_DateTypeDef& RTC_DateStruct,
+ const RTC_TimeTypeDef& RTC_TimeStruct);
+
+ void clearIRQ();
+ void disableIRQ();
+
+ bool getVoltLow(void);
+
+ public:
+ uint8_t Second;
+ uint8_t Minute;
+ uint8_t Hour;
+ uint8_t Week;
+ uint8_t Day;
+ uint8_t Month;
+ uint8_t Year;
+ uint8_t DateString[9];
+ uint8_t TimeString[9];
+
+ uint8_t asc[14];
+
+ private:
+ void Bcd2asc(void);
+ void DataMask();
+ void Str2Time(void);
+ void WriteReg(uint8_t reg, uint8_t data);
+ uint8_t ReadReg(uint8_t reg);
+ uint8_t Bcd2ToByte(uint8_t Value);
+ uint8_t ByteToBcd2(uint8_t Value);
+
+ private:
+ /*定义数组用来存储读取的时间数据 */
+ uint8_t trdata[7];
+ /*定义数组用来存储转换的 asc 码时间数据*/
+ // uint8_t asc[14];
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/Speaker.cpp b/lib/M5Core2/src/Speaker.cpp
new file mode 100644
index 000000000..3e873d31f
--- /dev/null
+++ b/lib/M5Core2/src/Speaker.cpp
@@ -0,0 +1,106 @@
+#if defined (CORE2)
+#include "Speaker.h"
+
+bool Speaker::InitI2SSpeakOrMic(int mode) { // Init I2S. 初始化I2S
+ esp_err_t err = ESP_OK;
+
+ i2s_driver_uninstall(
+ Speak_I2S_NUMBER); // Uninstall the I2S driver. 卸载I2S驱动
+ i2s_config_t i2s_config = {
+ .mode = (i2s_mode_t)(I2S_MODE_MASTER), // Set the I2S operating mode.
+ // 设置I2S工作模式
+ .sample_rate = 44100, // Set the I2S sampling rate. 设置I2S采样率
+ .bits_per_sample =
+ I2S_BITS_PER_SAMPLE_16BIT, // Fixed 12-bit stereo MSB.
+ // 固定为12位立体声MSB
+ .channel_format =
+ I2S_CHANNEL_FMT_ONLY_RIGHT, // Set the channel format. 设置频道格式
+#if ESP_IDF_VERSION > ESP_IDF_VERSION_VAL(4, 1, 0)
+ .communication_format =
+ I2S_COMM_FORMAT_STAND_I2S, // Set the format of the communication.
+ // 设置通讯格式
+#else
+ .communication_format = I2S_COMM_FORMAT_I2S,
+#endif
+ .intr_alloc_flags =
+ ESP_INTR_FLAG_LEVEL1, // Set the interrupt flag. 设置中断的标志
+ .dma_buf_count = 2, // DMA buffer count. DMA缓冲区计数
+ .dma_buf_len = 128, // DMA buffer length. DMA缓冲区长度
+ .use_apll = false,
+ .tx_desc_auto_clear = true,
+ .fixed_mclk = -1,
+ .mclk_multiple = I2S_MCLK_MULTIPLE_DEFAULT,
+ .bits_per_chan = I2S_BITS_PER_CHAN_DEFAULT,
+ };
+ if (mode == MODE_MIC) {
+ i2s_config.mode =
+ (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX | I2S_MODE_PDM);
+ } else {
+ i2s_config.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX);
+ i2s_config.use_apll = false; // I2S clock setup. I2S时钟设置
+ i2s_config.tx_desc_auto_clear =
+ true; // Enables auto-cleanup descriptors for understreams.
+ // 开启欠流自动清除描述符
+ }
+ // Install and drive I2S. 安装并驱动I2S
+ err += i2s_driver_install(Speak_I2S_NUMBER, &i2s_config, 0, NULL);
+
+ i2s_pin_config_t tx_pin_config;
+
+#if (ESP_IDF_VERSION > ESP_IDF_VERSION_VAL(4, 3, 0))
+ tx_pin_config.mck_io_num = I2S_PIN_NO_CHANGE;
+#endif
+ tx_pin_config.bck_io_num =
+ CONFIG_I2S_BCK_PIN; // Link the BCK to the CONFIG_I2S_BCK_PIN pin.
+ // 将BCK链接至CONFIG_I2S_BCK_PIN引脚
+ tx_pin_config.ws_io_num = CONFIG_I2S_LRCK_PIN; // ...
+ tx_pin_config.data_out_num = CONFIG_I2S_DATA_PIN; // ...
+ tx_pin_config.data_in_num = CONFIG_I2S_DATA_IN_PIN; // ...
+ err +=
+ i2s_set_pin(Speak_I2S_NUMBER,
+ &tx_pin_config); // Set the I2S pin number. 设置I2S引脚编号
+ err += i2s_set_clk(
+ Speak_I2S_NUMBER, 44100, I2S_BITS_PER_SAMPLE_16BIT,
+ I2S_CHANNEL_MONO); // Set the clock and bitwidth used by I2S Rx and Tx.
+ // 设置I2S RX、Tx使用的时钟和位宽
+ return true;
+}
+
+void Speaker::begin(void) { // 初始化扬声器
+ uint8_t val = Read8bit(0x03);
+ bool axp192 = false;
+ if (val == 0x03) {
+ _pmic = pmic_axp192;
+ } else if (val == 0x4A) {
+ _pmic = pmic_axp2101;
+ } else {
+ _pmic = pmic_unknown;
+ }
+ Serial.printf("\n_pmic:%d", _pmic);
+ if (_pmic) {
+ uint8_t reg_addr = 0x94;
+ uint8_t gpio_bit = 0x04;
+ uint8_t data;
+ data = Read8bit(reg_addr);
+ data |= gpio_bit;
+ Write1Byte(reg_addr, data);
+ } else if (!_pmic) {
+ uint8_t reg_addr = 0x94;
+ uint8_t data = 0x1C;
+ Write1Byte(reg_addr, data);
+ }
+
+ InitI2SSpeakOrMic(MODE_SPK);
+}
+
+size_t Speaker::PlaySound(const unsigned char* data,
+ const size_t& amount_of_bytes) {
+ size_t bytes_written = 0;
+ if (data == nullptr) {
+ return bytes_written;
+ }
+ i2s_write(Speak_I2S_NUMBER, data, amount_of_bytes, &bytes_written,
+ portMAX_DELAY);
+ return bytes_written;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/Speaker.h b/lib/M5Core2/src/Speaker.h
new file mode 100644
index 000000000..7581b26f7
--- /dev/null
+++ b/lib/M5Core2/src/Speaker.h
@@ -0,0 +1,37 @@
+#if defined (CORE2)
+#ifndef _SPEAKER_H_
+#define _SPEAKER_H_
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+#include <driver/i2s.h>
+#include <esp_err.h>
+#include "AXP.h"
+#include "Arduino.h"
+
+#define CONFIG_I2S_BCK_PIN 12 // 定义I2S相关端口
+#define CONFIG_I2S_LRCK_PIN 0
+#define CONFIG_I2S_DATA_PIN 2
+#define CONFIG_I2S_DATA_IN_PIN 34
+
+#define Speak_I2S_NUMBER I2S_NUM_0 // 定义扬声器端口
+
+#define MODE_MIC 0 // 定义工作模式
+#define MODE_SPK 1
+#define DATA_SIZE 1024
+
+enum pmic_t { pmic_unknown = 0, pmic_axp192, pmic_axp2101 };
+
+class Speaker {
+ public:
+ void begin(void);
+ bool InitI2SSpeakOrMic(int mode);
+ // Plays the given amount of bytes from the given data array and returns the
+ // amount of bytes, that were actually played by the speaker.
+ size_t PlaySound(const unsigned char* data, const size_t& amount_of_bytes);
+
+ private:
+ pmic_t _pmic;
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/CommUtil.cpp b/lib/M5Core2/src/utility/CommUtil.cpp
new file mode 100644
index 000000000..f50f21bb9
--- /dev/null
+++ b/lib/M5Core2/src/utility/CommUtil.cpp
@@ -0,0 +1,164 @@
+#if defined (CORE2)
+/*----------------------------------------------------------------------*
+ * M5Stack I2C Common Library v1.0 *
+ * *
+ * This work is licensed under the GNU Lesser General Public *
+ * License v2.1 *
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.en.html *
+ *----------------------------------------------------------------------*/
+#include "CommUtil.h"
+#include "../M5Core2.h"
+
+extern M5Core2 M5;
+
+// debug for message of I2C ( bypass message to serial)
+// #define I2C_DEBUG_TO_SERIAL
+
+CommUtil::CommUtil() {
+}
+
+// Wire.h read and write protocols
+bool CommUtil::writeCommand(uint8_t address, uint8_t subAddress) {
+ bool function_result = false;
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ function_result = (Wire.endTransmission() == 0); // Send the Tx buffer
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("writeCommand:send to 0x%02x [0x%02x] result:%s\n", address,
+ subAddress, function_result ? "OK" : "NG");
+#endif
+
+ return (function_result);
+}
+
+// Wire.h read and write protocols
+bool CommUtil::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
+ bool function_result = false;
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ Wire.write(data); // Put data in Tx buffer
+ function_result = (Wire.endTransmission() == 0); // Send the Tx buffer
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("writeByte:send to 0x%02x [0x%2x] data=0x%02x result:%s\n",
+ address, subAddress, data, function_result ? "OK" : "NG");
+#endif
+
+ return (function_result);
+}
+
+// Wire.h read and write protocols
+bool CommUtil::writeBytes(uint8_t address, uint8_t subAddress, uint8_t *data,
+ uint8_t length) {
+ bool function_result = false;
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("writeBytes:send to 0x%02x [0x%02x] data=", address,
+ subAddress);
+#endif
+
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ for (int i = 0; i < length; i++) {
+ Wire.write(*(data + i)); // Put data in Tx buffer
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x ", *(data + i));
+#endif
+ }
+ function_result = (Wire.endTransmission() == 0); // Send the Tx buffer
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("result:%s\n", function_result ? "OK" : "NG");
+#endif
+
+ return function_result; // Send the Tx buffer
+}
+
+bool CommUtil::readByte(uint8_t address, uint8_t *result) {
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("readByte :read from 0x%02x requestByte=1 receive=", address);
+#endif
+
+ if (Wire.requestFrom(address, (uint8_t)1)) {
+ *result = Wire.read(); // Fill Rx buffer with result
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x\n", result);
+#endif
+ return true;
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("none\n");
+#endif
+ return false;
+}
+
+bool CommUtil::readByte(uint8_t address, uint8_t subAddress, uint8_t *result) {
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("readByte :read from 0x%02x [0x%02x] requestByte=1 receive=",
+ address, subAddress);
+#endif
+
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ if (Wire.endTransmission(false) == 0 &&
+ Wire.requestFrom(address, (uint8_t)1)) {
+ *result = Wire.read(); // Fill Rx buffer with result
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x\n", *result);
+#endif
+ return true;
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("none\n");
+#endif
+ return false;
+}
+
+bool CommUtil::readBytes(uint8_t address, uint8_t subAddress, uint8_t count,
+ uint8_t *dest) {
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("readBytes:read from 0x%02x [0x%02x] requestByte=%d receive=",
+ address, subAddress, count);
+#endif
+
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ uint8_t i = 0;
+ if (Wire.endTransmission(false) == 0 &&
+ Wire.requestFrom(address, (uint8_t)count)) {
+ while (Wire.available()) {
+ dest[i++] = Wire.read(); // Put read results in the Rx buffer
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x ", dest[i - 1]);
+#endif
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf(" (len:%d)\n", i);
+#endif
+ return true;
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("none\n");
+#endif
+ return false;
+}
+
+bool CommUtil::readBytes(uint8_t address, uint8_t count, uint8_t *dest) {
+ uint8_t i = 0;
+ if (Wire.requestFrom(address, (uint8_t)count)) {
+ while (Wire.available()) {
+ // Put read results in the Rx buffer
+ dest[i++] = Wire.read();
+ }
+ return true;
+ }
+ return false;
+}
+
+void CommUtil::scanID(bool *result) {
+ for (int i = 0x00; i <= 0x7f; i++) {
+ *(result + i) = writeCommand(i, 0x00);
+ }
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/CommUtil.h b/lib/M5Core2/src/utility/CommUtil.h
new file mode 100644
index 000000000..cbbd87867
--- /dev/null
+++ b/lib/M5Core2/src/utility/CommUtil.h
@@ -0,0 +1,32 @@
+#if defined (CORE2)
+/*----------------------------------------------------------------------*
+ * M5Stack I2C Common Library v1.0 *
+ * *
+ * This work is licensed under the GNU Lesser General Public *
+ * License v2.1 *
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.en.html *
+ *----------------------------------------------------------------------*/
+#ifndef CommUtil_h
+#define CommUtil_h
+
+#include <Arduino.h>
+#include <Wire.h>
+
+class CommUtil {
+ public:
+ CommUtil();
+ bool writeCommand(uint8_t address, uint8_t subAddress);
+ bool writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+ bool writeBytes(uint8_t address, uint8_t subAddress, uint8_t *data,
+ uint8_t length);
+ bool readByte(uint8_t address, uint8_t *result);
+ bool readByte(uint8_t address, uint8_t subAddress, uint8_t *result);
+ bool readBytes(uint8_t address, uint8_t count, uint8_t *dest);
+ bool readBytes(uint8_t address, uint8_t subAddress, uint8_t count,
+ uint8_t *dest);
+ void scanID(bool *result);
+
+ private:
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/Config.h b/lib/M5Core2/src/utility/Config.h
new file mode 100644
index 000000000..5ce3a0d5e
--- /dev/null
+++ b/lib/M5Core2/src/utility/Config.h
@@ -0,0 +1,30 @@
+#include "../M5Display.h"
+#if defined (CORE2)
+#ifndef _CONFIG_H_
+#define _CONFIG_H_
+
+#define TFT M5Display::instance
+#define BUTTONS M5Buttons::instance
+
+// Screen
+#define TFT_LED_PIN 32
+#define TFT_DC_PIN 27
+#define TFT_CS_PIN 14
+#define TFT_MOSI_PIN 23
+#define TFT_CLK_PIN 18
+#define TFT_RST_PIN 33
+#define TFT_MISO_PIN 19
+
+// SD card
+#define TFCARD_CS_PIN 4
+
+// UART
+#define USE_SERIAL Serial
+
+// Core2 defines
+#define M5Stack_M5Core2
+#define TFT_eSPI_TOUCH_EMULATION
+#define TOUCH M5Touch::instance
+
+#endif /* CONFIG_H */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/M5Button.cpp b/lib/M5Core2/src/utility/M5Button.cpp
new file mode 100644
index 000000000..2c6d54f00
--- /dev/null
+++ b/lib/M5Core2/src/utility/M5Button.cpp
@@ -0,0 +1,779 @@
+#if defined (CORE2)
+#include "M5Button.h"
+
+// Button class
+
+/* static */ std::vector<Button*> Button::instances;
+
+Button::Button(int16_t x_, int16_t y_, int16_t w_, int16_t h_,
+ bool rot1_ /* = false */, const char* name_ /* = "" */,
+ ButtonColors off_ /*= {NODRAW, NODRAW, NODRAW} */,
+ ButtonColors on_ /* = {NODRAW, NODRAW, NODRAW} */,
+ uint8_t datum_ /* = BUTTON_DATUM */, int16_t dx_ /* = 0 */,
+ int16_t dy_ /* = 0 */, uint8_t r_ /* = 0xFF */
+ )
+ : Zone(x_, y_, w_, h_, rot1_) {
+ _pin = 0xFF;
+ _invert = false;
+ _dbTime = 0;
+ strncpy(_name, name_, 15);
+ off = off_;
+ on = on_;
+ datum = datum_;
+ dx = dx_;
+ dy = dy_;
+ r = r_;
+ init();
+}
+
+Button::Button(uint8_t pin_, uint8_t invert_, uint32_t dbTime_,
+ String hw_ /* = "hw" */, int16_t x_ /* = 0 */,
+ int16_t y_ /* = 0 */, int16_t w_ /* = 0 */, int16_t h_ /* = 0 */,
+ bool rot1_ /* = false */, const char* name_ /* = "" */,
+ ButtonColors off_ /*= {NODRAW, NODRAW, NODRAW} */,
+ ButtonColors on_ /* = {NODRAW, NODRAW, NODRAW} */,
+ uint8_t datum_ /* = BUTTON_DATUM */, int16_t dx_ /* = 0 */,
+ int16_t dy_ /* = 0 */, uint8_t r_ /* = 0xFF */
+ )
+ : Zone(x_, y_, w_, h_, rot1_) {
+ _pin = pin_;
+ _invert = invert_;
+ _dbTime = dbTime_;
+ strncpy(_name, name_, 15);
+ off = off_;
+ on = on_;
+ datum = datum_;
+ dx = dx_;
+ dy = dy_;
+ r = r_;
+ init();
+}
+
+Button::~Button() {
+ for (int i = 0; i < instances.size(); ++i) {
+ if (instances[i] == this) {
+ BUTTONS->delHandlers(nullptr, this, nullptr);
+ instances.erase(instances.begin() + i);
+ return;
+ }
+ }
+}
+
+Button::operator bool() {
+ return _state;
+}
+
+bool Button::operator==(const Button& b) {
+ return (this == &b);
+}
+bool Button::operator!=(const Button& b) {
+ return (this != &b);
+}
+bool Button::operator==(Button* b) {
+ return (this == b);
+}
+bool Button::operator!=(Button* b) {
+ return (this != b);
+}
+
+void Button::init() {
+ _state = _tapWait = _pressing = _manuallyRead = false;
+ _time = _lastChange = _pressTime = millis();
+ _hold_time = -1;
+ _textFont = _textSize = 0;
+ //_freeFont = nullptr;
+ drawFn = nullptr;
+ _compat = 0;
+ drawZone = Zone();
+ tapTime = TAP_TIME;
+ dbltapTime = DBLTAP_TIME;
+ longPressTime = LONGPRESS_TIME;
+ repeatDelay = REPEAT_DELAY;
+ repeatInterval = REPEAT_INTERVAL;
+ strncpy(_label, _name, 16);
+ if (_pin != 0xFF) pinMode(_pin, INPUT_PULLUP);
+ instances.push_back(this);
+ draw();
+}
+
+int16_t Button::instanceIndex() {
+ for (int16_t i = 0; i < instances.size(); ++i) {
+ if (instances[i] == this) return i;
+ }
+ return -1;
+}
+
+bool Button::read(bool manualRead /* = true */) {
+ if (manualRead) _manuallyRead = true;
+ event = Event();
+ if (_changed) {
+ _changed = false;
+ _lastChange = _time;
+ if (!_state && !_cancelled && postReleaseEvents()) return _state;
+ } else {
+ if (!_cancelled && timeoutEvents()) return _state;
+ if (!_state) _cancelled = false;
+ }
+ // Do actual read from the pin if this is a hardware button
+ _time = millis();
+ uint8_t newState = false;
+ if (_pin != 0xFF) {
+ newState = (digitalRead(_pin));
+ newState = _invert ? !newState : newState;
+ if (newState != _state && _time - _lastChange >= _dbTime) {
+ if (newState) fingerDown();
+ if (!newState) fingerUp();
+ }
+ }
+ return _state;
+}
+
+void Button::fingerDown(Point pos /* = Point() */, uint8_t finger /* = 0 */) {
+ _finger = finger;
+ _currentPt[finger] = _fromPt[finger] = pos;
+ if (!_state && !_currentPt[1 - finger]) {
+ // other finger not here
+ _state = true;
+ _changed = true;
+ _pressTime = _time;
+ _lastChange = _time;
+ draw();
+ }
+ BUTTONS->fireEvent(finger, E_TOUCH, pos, pos, 0, this, nullptr);
+}
+
+void Button::fingerUp(uint8_t finger /* = 0 */) {
+ uint32_t duration = _time - _pressTime;
+ _finger = finger;
+ _toPt[finger] = _currentPt[finger];
+ _currentPt[finger] = Point();
+ if (_state && !_currentPt[1 - finger]) {
+ // other finger not here
+ _state = false;
+ _changed = true;
+ _lastChange = _time;
+ draw();
+ }
+ BUTTONS->fireEvent(finger, E_RELEASE, _fromPt[finger], _toPt[finger],
+ duration, this, nullptr);
+}
+
+void Button::fingerMove(Point pos, uint8_t finger) {
+ BUTTONS->fireEvent(finger, E_MOVE, _currentPt[finger], pos,
+ _time - _lastChange, this, nullptr);
+ _currentPt[finger] = pos;
+}
+
+bool Button::postReleaseEvents() {
+ uint32_t duration = _time - _pressTime;
+ if (_toPt[_finger] && !contains(_toPt[_finger])) {
+ BUTTONS->fireEvent(_finger, E_DRAGGED, _fromPt[_finger], _toPt[_finger],
+ duration, this, nullptr);
+ _tapWait = false;
+ _pressing = false;
+ _longPressing = false;
+ return true;
+ }
+ if (duration <= tapTime) {
+ if (_tapWait) {
+ BUTTONS->fireEvent(_finger, E_DBLTAP, _fromPt[_finger],
+ _toPt[_finger], duration, this, nullptr);
+ _tapWait = false;
+ _pressing = false;
+ _longPressing = false;
+ return true;
+ }
+ _tapWait = true;
+ } else if (_pressing) {
+ BUTTONS->fireEvent(_finger, _longPressing ? E_LONGPRESSED : E_PRESSED,
+ _fromPt[_finger], _toPt[_finger], duration, this,
+ nullptr);
+ _pressing = false;
+ _longPressing = false;
+ return true;
+ }
+ return false;
+}
+
+bool Button::timeoutEvents() {
+ uint32_t duration = _time - _pressTime;
+ if (_tapWait && duration >= dbltapTime) {
+ BUTTONS->fireEvent(_finger, E_TAP, _fromPt[_finger], _toPt[_finger],
+ duration, this, nullptr);
+ _tapWait = false;
+ _pressing = false;
+ return true;
+ }
+ if (!_state) return false;
+ if ((!_pressing && duration > tapTime) ||
+ (repeatDelay && duration > repeatDelay &&
+ _time - _lastRepeat > repeatInterval)) {
+ BUTTONS->fireEvent(_finger, E_PRESSING, _fromPt[_finger],
+ _currentPt[_finger], duration, this, nullptr);
+ _lastRepeat = _time;
+ _pressing = true;
+ return true;
+ }
+ if (longPressTime && !_longPressing && duration > longPressTime) {
+ BUTTONS->fireEvent(_finger, E_LONGPRESSING, _fromPt[_finger],
+ _currentPt[_finger], duration, this, nullptr);
+ _longPressing = true;
+ return true;
+ }
+ return false;
+}
+
+void Button::cancel() {
+ _cancelled = true;
+ _tapWait = false;
+ draw(off);
+}
+
+char* Button::getName() {
+ return _name;
+}
+
+bool Button::isPressed() {
+ return _state;
+}
+
+bool Button::isReleased() {
+ return !_state;
+}
+
+bool Button::wasPressed() {
+ return _state && _changed;
+}
+
+bool Button::wasReleased() {
+ return (!_state && _changed && millis() - _pressTime < _hold_time);
+}
+
+bool Button::wasReleasefor(uint32_t ms) {
+ _hold_time = ms;
+ return (!_state && _changed && millis() - _pressTime >= ms);
+}
+
+bool Button::pressedFor(uint32_t ms) {
+ return (_state && _time - _lastChange >= ms) ? 1 : 0;
+}
+
+bool Button::pressedFor(uint32_t ms, uint32_t continuous_time) {
+ if (_state && _time - _lastChange >= ms &&
+ _time - _lastLongPress >= continuous_time) {
+ _lastLongPress = _time;
+ return true;
+ }
+ return false;
+}
+
+bool Button::releasedFor(uint32_t ms) {
+ return (!_state && _time - _lastChange >= ms);
+}
+
+uint32_t Button::lastChange() {
+ return (_lastChange);
+}
+
+void Button::addHandler(EventHandlerCallback fn,
+ uint16_t eventMask /* = E_ALL */) {
+ BUTTONS->addHandler(fn, eventMask, this, nullptr);
+}
+
+void Button::delHandlers(EventHandlerCallback fn /* = nullptr */) {
+ BUTTONS->delHandlers(fn, this, nullptr);
+}
+
+// visual things for Button
+
+void Button::draw() {
+ if (_state)
+ draw(on);
+ else
+ draw(off);
+}
+
+void Button::erase(uint16_t color /* = BLACK */) {
+ draw({color, NODRAW, NODRAW});
+}
+
+void Button::draw(ButtonColors bc) {
+ _hidden = false;
+ // use locally set draw function if aplicable, global one otherwise
+ if (drawFn) {
+ drawFn(*this, bc);
+ } else if (BUTTONS->drawFn) {
+ BUTTONS->drawFn(*this, bc);
+ }
+}
+
+void Button::hide(uint16_t color /* = NODRAW */) {
+ _hidden = true;
+ if (color != NODRAW) erase(color);
+}
+
+char* Button::label() {
+ return _label;
+}
+
+void Button::setLabel(const char* label_) {
+ strncpy(_label, label_, 50);
+}
+/*
+void Button::setFont(const GFXfont* freeFont_) {
+ //_freeFont = freeFont_;
+ _textFont = 1;
+}
+*/
+void Button::setFont(uint8_t textFont_ /* = 0 */) {
+ //_freeFont = nullptr;
+ _textFont = textFont_;
+}
+
+void Button::setTextSize(uint8_t textSize_ /* = 0 */) {
+ _textSize = textSize_;
+}
+
+// M5Buttons class
+
+/* static */ M5Buttons* M5Buttons::instance;
+
+/* static */ void M5Buttons::drawFunction(Button& b, ButtonColors bc) {
+ if (bc.bg == NODRAW && bc.outline == NODRAW && bc.text == NODRAW) return;
+ Zone z = (b.drawZone) ? b.drawZone : b;
+ if (z.rot1) z.rotate(TFT->rotation);
+
+ uint8_t r = (b.r == 0xFF) ? min(z.w, z.h) / 4 : b.r;
+
+ if (bc.bg != NODRAW) {
+ if (r >= 2) {
+ TFT->fillRoundRect(z.x, z.y, z.w, z.h, r, bc.bg);
+ } else {
+ TFT->fillRect(z.x, z.y, z.w, z.h, bc.bg);
+ }
+ }
+
+ if (bc.outline != NODRAW) {
+ if (r >= 2) {
+ TFT->drawRoundRect(z.x, z.y, z.w, z.h, r, bc.outline);
+ } else {
+ TFT->drawRect(z.x, z.y, z.w, z.h, bc.outline);
+ }
+ }
+
+ if (bc.text != NODRAW && bc.text != bc.bg && strlen(b._label)) {
+ // figure out where to put the text
+ uint16_t tx, ty;
+ tx = z.x + (z.w / 2);
+ ty = z.y + (z.h / 2);
+
+ if (!b._compat) {
+ uint8_t margin = max(r / 2, 6);
+ switch (b.datum) {
+ case TL_DATUM:
+ case ML_DATUM:
+ case BL_DATUM:
+ tx = z.x + margin;
+ break;
+ case TR_DATUM:
+ case MR_DATUM:
+ case BR_DATUM:
+ tx = z.x + z.w - margin;
+ break;
+ }
+ switch (b.datum) {
+ case TL_DATUM:
+ case TC_DATUM:
+ case TR_DATUM:
+ ty = z.y + margin;
+ break;
+ case BL_DATUM:
+ case BC_DATUM:
+ case BR_DATUM:
+ ty = z.y + z.h - margin;
+ break;
+ }
+ }
+
+ // Save state
+ uint8_t tempdatum = TFT->getTextDatum();
+ uint16_t tempPadding = TFT->padX;
+ if (!b._compat) TFT->pushState();
+
+ // Actual drawing of text
+ TFT->setTextColor(bc.text);
+ if (b._textSize)
+ TFT->setTextSize(b._textSize);
+ else
+ TFT->setTextSize(BUTTONS->_textSize);
+ /*
+ if (b._textFont) {
+ if (b._freeFont)
+ TFT->setFreeFont(b._freeFont);
+ else
+ TFT->setTextFont(b._textFont);
+ } else {
+ if (BUTTONS->_freeFont)
+ TFT->setFreeFont(BUTTONS->_freeFont);
+ else
+ TFT->setTextFont(BUTTONS->_textFont);
+ }
+ */
+ TFT->setTextDatum(b.datum);
+ TFT->setTextPadding(0);
+ TFT->drawString(b._label, tx + b.dx, ty + b.dy);
+ // Set state back
+ if (!b._compat) {
+ TFT->popState();
+ } else {
+ TFT->setTextDatum(tempdatum);
+ TFT->setTextPadding(tempPadding);
+ }
+ }
+}
+
+M5Buttons::M5Buttons() {
+ if (!instance) instance = this;
+ drawFn = drawFunction;
+ //_freeFont = BUTTON_FREEFONT;
+ _textFont = BUTTON_TEXTFONT;
+ _textSize = BUTTON_TEXTSIZE;
+}
+
+Button* M5Buttons::which(Point& p) {
+ if (!Button::instances.size()) return nullptr;
+ for (int i = Button::instances.size() - 1; i >= 0; --i) {
+ Button* b = Button::instances[i];
+ // Always return button when i == 0 --> background
+ if (!i || (b->_pin == 0xFF && !b->_hidden && b->contains(p))) return b;
+ }
+ return nullptr;
+}
+
+void M5Buttons::draw() {
+ for (auto button : Button::instances) button->draw();
+}
+
+void M5Buttons::update() {
+#ifdef _M5TOUCH_H_
+ for (auto gesture : Gesture::instances) gesture->_detected = false;
+ BUTTONS->event = Event();
+ if (TOUCH->wasRead || _leftovers) {
+ _finger[TOUCH->point0finger].current = TOUCH->point[0];
+ _finger[1 - TOUCH->point0finger].current = TOUCH->point[1];
+ _leftovers = true;
+ for (uint8_t i = 0; i < 2; i++) {
+ if (i == 1) _leftovers = false;
+ Finger& fi = _finger[i];
+ Point& curr = fi.current;
+ Point prev = fi.previous;
+ fi.previous = fi.current;
+ if (curr == prev) continue;
+ if (!prev && curr) {
+ // A new touch happened
+ fi.startTime = millis();
+ fi.startPoint = curr;
+ fi.button = BUTTONS->which(curr);
+ if (fi.button) {
+ fi.button->fingerDown(curr, i);
+ return;
+ }
+ } else if (prev && !curr) {
+ // Finger removed
+ uint16_t duration = millis() - fi.startTime;
+ for (auto gesture : Gesture::instances) {
+ if (gesture->test(fi.startPoint, prev, duration)) {
+ BUTTONS->fireEvent(i, E_GESTURE, fi.startPoint, prev,
+ duration, nullptr, gesture);
+ if (fi.button) fi.button->cancel();
+ break;
+ }
+ }
+ if (fi.button) {
+ fi.button->fingerUp(i);
+ return;
+ }
+ } else {
+ // Finger moved
+ if (fi.button) {
+ fi.button->fingerMove(curr, i);
+ return;
+ }
+ }
+ }
+ }
+#endif /* _M5TOUCH_H_ */
+
+ for (auto button : Button::instances) {
+ if (!button->_manuallyRead) button->read(false);
+ }
+}
+/*
+void M5Buttons::setFont(const GFXfont* freeFont_) {
+ //_freeFont = freeFont_;
+ _textFont = 1;
+}
+*/
+void M5Buttons::setFont(uint8_t textFont_) {
+ //_freeFont = nullptr;
+ _textFont = textFont_;
+}
+
+void M5Buttons::setTextSize(uint8_t textSize_) {
+ _textSize = textSize_;
+}
+
+void M5Buttons::fireEvent(uint8_t finger, uint16_t type, Point& from, Point& to,
+ uint16_t duration, Button* button, Gesture* gesture) {
+ Event e;
+ e.finger = finger;
+ e.type = type;
+ e.from = from;
+ e.to = to;
+ e.duration = duration;
+ e.button = button;
+ e.gesture = gesture;
+ if (button) button->event = e;
+ event = e;
+ for (auto h : _eventHandlers) {
+ if (!(h.eventMask & e.type)) continue;
+ if (h.button && h.button != e.button) continue;
+ if (h.gesture && h.gesture != e.gesture) continue;
+ h.fn(e);
+ }
+}
+
+void M5Buttons::addHandler(EventHandlerCallback fn,
+ uint16_t eventMask /* = E_ALL */,
+ Button* button /* = nullptr */,
+ Gesture* gesture /* = nullptr */
+) {
+ EventHandler handler;
+ handler.fn = fn;
+ handler.eventMask = eventMask;
+ handler.button = button;
+ handler.gesture = gesture;
+ _eventHandlers.push_back(handler);
+}
+
+void M5Buttons::delHandlers(EventHandlerCallback fn /* = nullptr */,
+ Button* button /* = nullptr */,
+ Gesture* gesture /* = nullptr */
+) {
+ for (int i = _eventHandlers.size() - 1; i >= 0; --i) {
+ // this doesn't compile anymore
+ // if (fn && fn != _eventHandlers[i].fn) continue;
+ if (button && _eventHandlers[i].button != button) continue;
+ if (gesture && _eventHandlers[i].gesture != gesture) continue;
+ _eventHandlers.erase(_eventHandlers.begin() + i);
+ }
+}
+
+// Gesture class
+
+std::vector<Gesture*> Gesture::instances;
+
+Gesture::Gesture(Zone fromZone_, Zone toZone_, const char* name_ /* = "" */,
+ uint16_t minDistance_ /* = GESTURE_MINDIST */,
+ int16_t direction_ /* = INVALID_VALUE */,
+ uint8_t plusminus_ /* = PLUSMINUS */, bool rot1_ /* = false */,
+ uint16_t maxTime_ /* = GESTURE_MAXTIME */
+) {
+ fromZone = fromZone_;
+ toZone = toZone_;
+ strncpy(_name, name_, 15);
+ minDistance = minDistance_;
+ direction = direction_;
+ plusminus = plusminus_;
+ rot1 = rot1_;
+ maxTime = maxTime_;
+ _detected = false;
+ instances.push_back(this);
+}
+
+Gesture::Gesture(const char* name_ /* = "" */,
+ uint16_t minDistance_ /* = GESTURE_MINDIST */,
+ int16_t direction_ /* = INVALID_VALUE */,
+ uint8_t plusminus_ /* = PLUSMINUS */, bool rot1_ /* = false */,
+ uint16_t maxTime_ /* = GESTURE_MAXTIME */
+) {
+ fromZone = ANYWHERE;
+ toZone = ANYWHERE;
+ strncpy(_name, name_, 15);
+ minDistance = minDistance_;
+ direction = direction_;
+ plusminus = plusminus_;
+ rot1 = rot1_;
+ maxTime = maxTime_;
+ _detected = false;
+ instances.push_back(this);
+}
+
+Gesture::~Gesture() {
+ for (int i = 0; i < instances.size(); ++i) {
+ if (instances[i] == this) {
+ instances.erase(instances.begin() + i);
+ BUTTONS->delHandlers(nullptr, nullptr, this);
+ return;
+ }
+ }
+}
+
+Gesture::operator bool() {
+ return _detected;
+}
+
+int16_t Gesture::instanceIndex() {
+ for (int16_t i = 0; i < instances.size(); ++i) {
+ if (instances[i] == this) return i;
+ }
+ return -1;
+}
+
+char* Gesture::getName() {
+ return _name;
+}
+
+bool Gesture::test(Point& from, Point& to, uint16_t duration) {
+ if (from.distanceTo(to) < minDistance) return false;
+ if (fromZone && !fromZone.contains(from)) return false;
+ if (toZone && !toZone.contains(to)) return false;
+ if (direction != INVALID_VALUE &&
+ !from.isDirectionTo(to, direction, plusminus, rot1))
+ return false;
+ if (duration > maxTime) return false;
+ _detected = true;
+ return true;
+}
+
+bool Gesture::wasDetected() {
+ return _detected;
+}
+
+void Gesture::addHandler(EventHandlerCallback fn,
+ uint16_t eventMask /* = E_ALL */) {
+ BUTTONS->addHandler(fn, eventMask, nullptr, this);
+}
+
+void Gesture::delHandlers(EventHandlerCallback fn /* = nullptr */) {
+ BUTTONS->delHandlers(fn, nullptr, this);
+}
+
+// Event class
+
+Event::Event() {
+ finger = type = duration = 0;
+ from = to = Point();
+ button = nullptr;
+ gesture = nullptr;
+}
+
+Event::operator uint16_t() {
+ return type;
+}
+
+const char* Event::typeName() {
+ const char* unknown = "E_UNKNOWN";
+ const char* none = "E_NONE";
+ const char* eventNames[NUM_EVENTS] = {
+ "E_TOUCH", "E_RELEASE", "E_MOVE", "E_GESTURE",
+ "E_TAP", "E_DBLTAP", "E_DRAGGED", "E_PRESSED",
+ "E_PRESSING", "E_LONGPRESSED", "E_LONGPRESSING"};
+ if (!type) return none;
+ for (uint8_t i = 0; i < NUM_EVENTS; i++) {
+ if ((type >> i) & 1) return eventNames[i];
+ }
+ return unknown;
+}
+
+const char* Event::objName() {
+ const char* empty = "";
+ if (gesture) return gesture->getName();
+ if (button) return button->getName();
+ return empty;
+};
+
+uint16_t Event::direction(bool rot1 /* = false */) {
+ return from.directionTo(to, rot1);
+}
+
+bool Event::isDirection(int16_t wanted, uint8_t plusminus /* = PLUSMINUS */,
+ bool rot1 /* = false */) {
+ return from.isDirectionTo(to, wanted, plusminus, rot1);
+}
+
+uint16_t Event::distance() {
+ return from.distanceTo(to);
+}
+
+// TFT_eSPI_Button2 compatibility mode
+
+TFT_eSPI_Button2::TFT_eSPI_Button2() : Button(0, 0, 0, 0) {
+ _compat = true;
+}
+
+void TFT_eSPI_Button2::initButton(TFT_eSPI* gfx, int16_t x, int16_t y,
+ uint16_t w, uint16_t h, uint16_t outline,
+ uint16_t fill, uint16_t textcolor,
+ char* label_, uint8_t textsize) {
+ initButtonUL(gfx, x - (w / 2), y - (h / 2), w, h, outline, fill, textcolor,
+ label_, textsize);
+}
+
+void TFT_eSPI_Button2::initButtonUL(TFT_eSPI* gfx, int16_t x_, int16_t y_,
+ uint16_t w_, uint16_t h_, uint16_t outline,
+ uint16_t fill, uint16_t textcolor,
+ char* label_, uint8_t textsize_) {
+ x = x_;
+ y = y_;
+ w = w_;
+ h = h_;
+ off = {fill, textcolor, outline};
+ on = {textcolor, fill, outline};
+ setTextSize(textsize_);
+ strncpy(_label, label_, 9);
+}
+
+void TFT_eSPI_Button2::setLabelDatum(int16_t dx_, int16_t dy_,
+ uint8_t datum_ /* = MC_DATUM */) {
+ dx = dx_;
+ dy = dy_;
+ datum = datum_;
+}
+
+void TFT_eSPI_Button2::drawButton(bool inverted /* = false */,
+ String long_name /* = "" */) {
+ char oldLabel[51];
+ if (long_name != "") {
+ strncpy(oldLabel, _label, 50);
+ strncpy(_label, long_name.c_str(), 50);
+ }
+ draw(inverted ? on : off);
+ if (long_name != "") strncpy(_label, oldLabel, 50);
+}
+
+bool TFT_eSPI_Button2::isPressed() {
+ return currstate;
+}
+
+bool TFT_eSPI_Button2::contains(int16_t _x, int16_t _y) {
+ return ((_x >= x) && (_x < (x + w)) && (_y >= y) && (_y < (y + h)));
+}
+
+void TFT_eSPI_Button2::press(bool p) {
+ if (p)
+ fingerDown();
+ else
+ fingerUp();
+ laststate = currstate;
+ currstate = p;
+}
+
+bool TFT_eSPI_Button2::justPressed() {
+ return (currstate && !laststate);
+}
+
+bool TFT_eSPI_Button2::justReleased() {
+ return (!currstate && laststate);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/M5Button.h b/lib/M5Core2/src/utility/M5Button.h
new file mode 100644
index 000000000..61f172ac3
--- /dev/null
+++ b/lib/M5Core2/src/utility/M5Button.h
@@ -0,0 +1,994 @@
+#if defined (CORE2)
+/*
+
+== M5Button: Buttons, Gestures and Events ==
+
+ * Hardware button support that is 100% Arduino Button Library compatible.
+
+ * Buttons on the screen, either as labels above the original M5Stack's
+ hardware buttons or anywhere on the touch screen of the Core2.
+
+ * Zone and Point objects to work with screen locations and areas. Functions
+ for distance, direction and more.
+
+ * Touch gestures that are processed before the buttons, so you can still
+ use gestures when the screen is full of buttons.
+
+ * Buttons and gestures send events that you can attach handler routines to,
+ or poll in a loop. Events include tap, doubletap, pressed, dragged and
+ more. Support for key repeat.
+
+ * Extensive screen rotation support, including support for buttons and
+ gestures that stay referenced to the physical screen regardless of rotation.
+
+ * Intuitive, consistent and well-documented API.
+
+ * Emulation of the (much less feature-rich) TFT_eSPI_Button class. This
+ goes together well with M5Touch's emulation of the TFT_eSPI resistive
+ touch screen interface to run a lot of existing programs without
+ modification.
+
+ This library was written for the M5Stack series of devices, but was made to
+ be general enough to be produce pretty visual buttons with any TFT_eSPI
+ display. Its more advanced features need the M5Touch interface, although
+ other input methods could be implemented.
+
+
+== Point and Zone: Describing Points and Areas on the Screen ==
+
+ The Point and Zone classes allow you to create variables that hold a point
+ or an area on the screen.
+
+ Point(x, y)
+
+ Holds a point on the screen. Has members x and y that hold the
+ coordinates of a touch. Values -1 for x and y indicate an invalid value,
+ and that's what a point starts out with if you declare it without
+ parameters. The 'valid()' method tests if a point is valid. If you
+ explicitly evaluate a Point as a boolean ("if (p) ..."), you also get
+ whether the point is valid, so this is equivalent to writing "if
+ (p.valid()) ...".
+
+ Zone(x, y, w, h)
+
+ Holds a rectangular area on the screen. Members x, y, w and h are for the
+ x and y coordinate of the top-left corner and the width and height of the
+ rectangle.
+
+ The 'set' method allows you to change the properties of an existing Point
+ or Zone. Using the 'in' or 'contains' method you can test if a point lies
+ in a zone.
+
+ The PointAndZone library also provides the low-level support for direction
+ from one point to another and for screen rotation translations.
+
+ The documentation in src/utility/PointAndZone.h provides more details about
+ rotation and examples covering most of the above.
+
+
+== Buttons ==
+
+ You can create classic Arduino buttons that act on the voltage on a pin of
+ the controller. On the M5Stack Core2, you can also create buttons that act
+ on touch within a given rectangle on the screen. If you want, that same
+ rectangle will also be used for a graphical representation of the button
+ that will show a text label in a colored background with a colored outline.
+ The colors of background, text, and outline can be configured, both for the
+ 'off' and the 'on' state of the button.
+
+ Whether on the M5Stack with hardware buttons or on the Core2 with a touch
+ screen, buttons are special forms of the 'Zone' object, meaning that all
+ functions that apply to 'Zone' objects also work on buttons. On the M5Stack
+ with buttons, while this zone cannot be used for touch input, it can still
+ be used to display a button that responds to the button state.
+
+
+== Hardware Buttons ==
+
+ For hardware buttons, use the classic Arduino way of setting up the button,
+ by providing the pin, whether the pin is inverted and the debounce time.
+
+
+ #include <M5Stack.h>
+
+ Button myButton(39, true, 10);
+
+ void setup() {
+ M5.begin();
+ }
+
+ void loop() {
+ M5.update();
+ if (myButton.wasPressed()) Serial.print("* ");
+ }
+
+
+ This would set up 'myButton' with the inverse state of pin 39, and a
+ debounce time of 10 milliseconds. Because pin 39 is the left button on an
+ M5Stack with buttons, this sketch will output a star to the serial port
+ every time you release the button. (And because pin 39 is the interrupt
+ wire for the touch screen on the Core2, it also happens to output a star on
+ that device every time you touch the screen.)
+
+ Note that the sketch uses 'M5.update()' instead of 'myButton.read()'. You
+ don't need to read() your buttons explicitly anymore. All buttons created
+ with M5Button are automatically read by 'M5.update()'. (Unless you read
+ them with 'myButton.read()', in which case 'M5.update()' stops doing that
+ to avoid you missing things.)
+
+ The next sections will describe buttons and gestures on the touch screen,
+ but if you have an M5Stack device without a touch screen: keep reading
+ because many events work on hardware buttons too. Hardware buttons can have
+ responsive representation on the screen, we'll get to that also.
+
+
+== Buttons Using the Touch Screen ==
+
+ Note: It may make sense to also read the documentation in the M5Touch.h
+ file, as tells you about the touch sensor and the lower-level touch
+ interface that is underneath the M5Button library.
+
+ To have a button that reacts to the touch sensor, all you need to do is
+ create a variable for the Button and provide the coordinates (x, y, width
+ and height). These buttons can be used in two ways. You can either use them
+ the way you would a normal Arduino button, or you can provide handler
+ functions to process various events for the button. We'll talk about the
+ events later, but here's the same simple sketch from above again but now it
+ defines a 100x50 pixel touch button near the top-right of the screen. Note
+ that this button does not show anything on the sreen just yet.
+
+
+ #include <M5Core2.h>
+
+ Button myButton(10, 10, 200, 100);
+
+ void setup() {
+ M5.begin();
+ }
+
+ void loop() {
+ M5.update();
+ if (myButton.wasPressed()) Serial.print("* ");
+ }
+
+
+ 'wasPressed()' will only be true once when you press the button. You can
+ also use the other Arduino button functions such as 'isPressed()' that is
+ true as soon and as long as the button is touched. Note that the buttons
+ only become pressed if the touch starts within the button, not if you swipe
+ over it, and that they will stay pressed as long as the finger touches,
+ even if it leaves the button area. You may want read about the events
+ further down to distinguish between different kinds of button-presses.
+
+ On the Core2 the three buttons M5.BtnA, M5.BtnB and M5.BtnC from the older
+ M5Stack units come already implemented as touch buttons that lie just below
+ the screen where the three circles are.
+
+
+== Buttons with visual appearance ==
+
+ If you want you button to show on the screen, all you need to do is provide
+ a set of three colors for the background of the button, the text printed on
+ it and the outline of the button. Using yet the same skech again:
+
+
+ #include <M5Core2.h>
+
+ Button myButton(10, 10, 200, 100, false, "I'm a button !",
+ {BLACK, WHITE, WHITE});
+
+ void setup() {
+ M5.begin();
+ }
+
+ void loop() {
+ M5.update();
+ if (myButton.wasPressed()) Serial.print("* ");
+ }
+
+
+ As you can see the colors are provided in {curly braces}, that's because
+ they are one variable, of the 'ButtonColors' type. Especialy if you're
+ going to define a bunch of buttons, you're better off replacing the button
+ line by:
+
+
+ ButtonColors col = {BLACK, WHITE, WHITE};
+ Button myButton(10, 10, 200, 100, false, "I'm a button !", col);
+
+
+ The order there is background, text, outline. If you do not want any of
+ these components drawn, simply put NODRAW in that position. The thing we are
+ defining here is what the button draws in its 'off' state. Since we haven
+ specified anything to draw in the 'on' state, the button just stays like it
+ is, regardless of whether it's pressed. Thus, if we say
+
+
+ ButtonColors onCol = {BLACK, WHITE, WHITE};
+ ButtonColors offCol = {RED, WHITE, WHITE};
+ Button myButton(10, 10, 200, 100, false, "I'm a button !", onCol, offCol);
+
+
+ the button background would turn red if the button was pressed. The button
+ colors can also be addressed directly. "myButton.on.bg = BLUE;" will turn
+ the background blue in the on state. The other two properties of the
+ ButtonColors variable are predicatably called 'text' and 'outline'.
+
+ If you run the sketches you will see the text is placed in the center of
+ the button and the buttons have visually please round edges. The corner
+ radius defaults to a quarter of the shortest side of the button. You can
+ change all this with the remaining parameters when setting up the button:
+
+
+ Button myButton(10, 10, 200, 100, false, "I'm a button !", onCol, offCol,
+ TL_DATUM, 0, 10, 0);
+
+
+ These last parameters indicate where to put the label in the TFT_eSPI
+ standard datum values, top-left in this case. The values after that are the
+ dx and dy, meaning the offsets from the default position. In this case
+ that's no left-right offset and 10 pixels down. Negative values move the
+ other way. The last value is the corner radius. In this case it would draw
+ an ugly 1980's rectangular button.
+
+ You can make a button draw its current state with "myButton.draw()", or all
+ buttons do that with "M5.Buttons.draw()". You can also call draw with a
+ ButtonColors variable so "myButton.draw({BLUE, WHITE, WHITE})" draws it
+ with those colors. Until the next state-change comes along that is, if you
+ have colors for the new state defined.
+
+ Note that the text provided here is the name of the buttton. A button
+ always keeps the same name, but the label (that which is shown) can change,
+ but initialises to the name. Use 'myButton.setLabel("New text!")' to change
+ it.
+
+ With "myButton.hide()" you can make a button temporarily invisible to the
+ touch sensor. You can specify an optional color value to draw over the
+ button if you want to make it visually disappear also. myButton.draw() makes
+ it visible to the touch sensor again, even if you have no colors defined, so
+ nothing shows on the screen. "MyButton.erase()" only paints over the button,
+ in a color you can specify (default black).
+
+
+== Visual Buttons (Labels) with Hardware Buttons ==
+
+ You can have a visual representation of the state of a hardware button on
+ the screen, for example right above the hardware buttons of the original
+ M5Stack. We'll call these buttons "labels", but they're regular buttons
+ that just respond to a physical button insetad of the touch sensor. If you
+ want to display a label on the screen that responds to the state of a
+ hardware button, just set up a hardware button up as usual, but then follow
+ the parameter list with "hw" (in quotes), followed by the parameters of the
+ touch button below.
+
+ The hardware buttons in the older M5Stack devices are already set up to
+ display labels: all you need is supply colors. Their initialization (in
+ M5Stack.h in this library) looks like this:
+
+
+ Button BtnA = Button(BUTTON_A_PIN, true, DEBOUNCE_MS,
+ "hw", 3, 218, 102, 21, true, "BtnA");
+ Button BtnB = Button(BUTTON_B_PIN, true, DEBOUNCE_MS,
+ "hw", 109, 218, 102, 21, true, "BtnB");
+ Button BtnC = Button(BUTTON_C_PIN, true, DEBOUNCE_MS,
+ "hw", 215, 218, 102, 21, true, "BtnC");
+
+
+ As you can see: its just a hardware button that has a zone to display the
+ label. So the sketch below is all that is needed to show repsonsive labels
+ on the M5Stack:
+
+
+ #include <M5Stack.h>
+
+ void setup() {
+ M5.BtnA.off = M5.BtnB.off = M5.BtnC.off = {BLUE, WHITE, NODRAW};
+ M5.BtnA.on = M5.BtnB.on = M5.BtnC.on = {RED, WHITE, NODRAW};
+ M5.begin();
+ M5.Buttons.draw();
+ }
+
+ void loop() {
+ M5.update();
+ }
+
+
+ If you looked closely you might have noticed that the mysterious fifth
+ argument has changed from 'false' to 'true'. This argument is called
+ 'rot1', and it determines that the location of this Zone or Button is
+ specified in rotation one, i.e. the normal default screen rotation. What
+ that means is that no matter what rotation you set the display to, these
+ button will always stay in the same place. The documentation in
+ src/utility/PointAndZone.h has more details if you want to know more about
+ this. You will only ever need rot1 if you need multiple screen rotations
+ AND you want objects to stay in the same physical place regardless.
+
+
+== M5.Buttons ==
+
+ Apart from the class "Button" that you use to create buttons of your own,
+ there is an instance called "M5.Buttons" (plural), that is used to talk to
+ the M5Button library for things that involve all buttons. For instance:
+ "M5.Buttons.setFont" sets a font for all buttons, and you can use
+ "M5.Buttons.addHandler" to add a handler that gets events for all buttons
+ (and gestures).
+
+
+== Events ==
+
+ Buttons (and gestures, but we'll get to those later) have a set of simple
+ functions to see if they are pressed or not. These Arduino-compatible
+ functions work fine for that purpose. But if you want to detect whether a
+ button received a short tap, or even a double-tap on many buttons
+ simultaneously, you find yourself writing quite a bit of code for that.
+
+ Events are M5Button's way of making it easier to react to events on
+ hardware buttons or the touch screen. For this you need to define one or
+ more event handler functions. This is done like this:
+
+ void myHandler(Event& e) { ... }
+
+ It's important to do it exactly this way, only changing the name of the
+ function. You can then set things up so that this function receives events.
+
+ Here's an events-based sketch for the Core2. We'll base it on the same
+ buton we've seen before.
+
+
+ #include <M5Core2.h>
+
+ ButtonColors onCol = {BLACK, WHITE, WHITE};
+ ButtonColors offCol = {RED, WHITE, WHITE};
+ Button myButton(10, 10, 200, 100, false, "I'm a button !", onCol, offCol);
+
+ void setup() {
+ M5.begin();
+ myButton.addHandler(touched, E_TOUCH);
+ myButton.addHandler(released, E_RELEASE);
+ }
+
+ void loop() {
+ M5.update();
+ }
+
+ void touched(Event& e) {
+ Serial.println("Touched!");
+ }
+
+ void released(Event& e) {
+ Serial.println("Released!");
+ }
+
+
+ Note that the function names "touched" and "released" are provided to
+ addHandler without the parenthesis. Here's two ways you can set up a handler
+ function to receive events:
+
+ M5.Buttons.addHandler(myHandler);
+
+ - or -
+
+ myButton.addHandler(myHandler);
+
+ The first form receives all the events relating to all buttons and gestures,
+ the second form only receives the events for that specific button. After the
+ name of the function, without the brackets, you can specify which events the
+ function needs to receive. You can add together (or "bitwise or") the names
+ of the events if you want a handler function to reive multiple events.
+
+ The Event object that is passed to the handler function contains all sorts
+ of information about the event: where on the screen it started, where it
+ ended, the duration, etc. etc.
+
+ Let's first look at all the possible events and when they are fired. The
+ first three events always happen when a finger touches the display.
+
+ E_TOUCH, E_MOVE and E_RELEASE
+
+ The E_TOUCH and E_RELEASE events fire when a button is pressed and
+ released. On a touch sensor, E_MOVE will fire every time it detects the
+ finger has moved. These events cannot be prevented from firing, like most
+ of the other ones. So every time your finger touches the display it will
+ fire E_TOUCH and then E_MOVEs until finally, when you release your
+ finger, an E_RELEASE.
+
+ E_PRESSING and E_LONGPRESSING
+
+ There are also events that happen while the button is still pressed.
+ These are E_PRESSING and E_LONGPRESSING. E_PRESSING happens as soon as
+ M5Button is sure that's not just a short tap (more later). The maximum
+ time for a tap is settable, but defaults to 150 ms. So if the button is
+ still held 150 ms after E_TOUCH, E_PRESSING fires. Just once, unless you
+ have set up a key repeat, more about that later too. Then at some point
+ you might get a E_LONGPRESSING, if you have set up a duration for that to
+ happen.
+
+ E_TAP, E_DBLTAP, E_PRESSED, E_LONGPRESSED and E_DRAGGED
+
+ Unless the keypress is cancelled (more later), exactly one of these events
+ will fire after the button has been released, after E_RELEASE has fired.
+ Think of these as final decisions on what kind of keypress this was.
+ (E_TAP takes a tiny bit longer before it fires because M5Button needs to
+ make sure it wasn't a doubletap, in that case E_DBLTAP wil fire instead.)
+
+ So tap and doubletap are sort of obvious, E_LONGPRESSED fires if the key
+ was pressed more that the set time in ms. E_DRAGGED fires if the finger
+ has moved outside of the button area when it was released. E_PRESSED is
+ fires in all other cases.
+
+ E_GESTURE
+
+ Doesn't really fit in with the others, but is the event that gets fired
+ when a gesture is detected.
+
+
+ If at any point after the pressing of a button, "myButton.cancel()" is
+ called, no further high-level events for that button will fire. What that
+ means is nothing other than possible E_MOVEs and one E_RELEASE event will
+ fire for that button until it is released and then pressed again. This is
+ used internally when a gesture is detected, so that when a touch gesture
+ starts on a button, there won't be an E_PRESSED, or any of the others.
+
+ The second thing to look at more closely is the 'Event' object itself. When
+ you set up a handler function like this
+
+ void myhandler(Event& e) {
+
+ what that means is you're creating a function that recives a (reference to)
+ an event. That event has all sorts of properties that we can look at.
+
+
+ e.type
+
+ The type of event, such as E_TOUCH or E_TAP from above. The event
+ itself, when you evaluate it, also returns the type. What that means is
+ that "if (e.type == E_TAP) .." is equivalent with "if (e == E_TAP) .."
+
+ e.finger
+
+ 0 or 1, whether this is the first or second finger detected on the
+ touch screen. Left at zero on the M5Stack with buttons.
+
+ e.from and e.to
+
+ Points that say from where to where this event happened. Left at
+ invalid for the M5Stack with buttons.
+
+ e.duration
+
+ Duration of the event in milliseconds.
+
+ e.button
+
+ Pointer to the button attached to the event. What that means is that you
+ can use all the methods for button as long as you precede them with
+ "e.button->". Note the '->' there because this is a pointer to an
+ object.
+
+ e.gesture
+
+ e.gesture is a pointer to the gesture attached to the event, and may be
+ null if the event is not a gesture. So unless you know for sure this
+ event is a gesture (because handler attached to that gesture or because
+ you asked for E_GESTURE events only), this pointer needs to be tested
+ using "if (e.gesture)" before using -> methods on it, oterwise your
+ program will crash.
+
+ other methods
+
+ Additionally, you can ask for the name of the event as text by using
+ "e.typeName()" and get the name of the gesture or button with
+ "e.objName()". "e.direction()" gives the direction, for instance of a
+ gesture or of an E_RELEASE event, where it gives direction between
+ E_TOUCH and E_RELEASE. "e.isDirectionTo(0,30)" will output true if the
+ swipe was upwards, plus or minus 30 degrees.
+
+
+ When you add a handler function you can also specify what events it should
+ receive by supplying it as the second argument after the handler function.
+ If you want to register multiple events for the same function, don't
+ register the handler twice, but simply add (or bitwise or) the event
+ values. The default value there is the pseudo-event E_ALL, which is simply
+ a value with all the event bits turned on. You can also subtract event type
+ values from E_ALL to exclude them.
+
+ Here are some examples of ways to add a handler function:
+
+
+ button1.addHandler(b1Function, E_TOUCH + E_RELEASE);
+
+ b1Function only get these two events for button1.
+
+
+ M5.Buttons.addHandler(btnHandle, E_ALL - E_MOVE);
+
+ btnHandle gets all events, except E_MOVE.
+
+
+ swipeUp.addHandler(nextPage);
+
+ Handler nextPage is called when swipeUp gesture detected.
+
+
+ Note that all handler functions must be of the "void someName(Event& e)"
+ type, even if they plan to completely ignore the event that is passed to
+ them.
+
+
+ If your event reads data or calls functions in e.button or e.gesture,
+ remember that these are pointers. Without going into too much detail, it
+ means it must do so with the -> notation, so to read the button x position,
+ you would say "e.button->x".
+
+ Please have a look at the example sketch (see below) to understand how this
+ all works and run the sketch to see all the events printed to the serial
+ port.
+
+
+== Taps, Doubletaps, Longpresses and Key Repeat ==
+
+ Some features are best explained with some examples:
+
+ myButton.tapTime = 0;
+
+ Turns off detection of taps and doubletaps, the button will fire
+ E_PRESSING immediately when pressed. Any other value makes that the
+ maximum time a tap can take in milliseconds, and thus the wait tme
+ before "E_PRESSING" fires.
+
+ mybutton.tapWait = 0;
+
+ Turns off detection of doubletaps only. Any other value makes that the
+ wait before an E_TAP fires, because M5Button is still waiting to see if
+ it's maybe a doubletap.
+
+ mybutton.longPressTime = 700;
+
+ Sets up the button to fire an E_LONGPRESSING after 700 ms, and then fire
+ E_LONGPRESSED instead of E_PRESSED when the button is released. By
+ default this is set to zero, meaning longpress detection is off.
+
+ myButton.repeatDelay = 500;
+ myButton.repeatInterval = 250;
+
+ Makes the button repeat the sending of its E_PRESSING event every 250
+ milliseconds if key is held for 500 ms.
+
+
+== In Loop vs. Event Handlers ==
+
+ Button and Gesture objects have an 'event' method that returns the event
+ that was detected this time around by 'M5.update()'. Each event comes in
+ it's own rotation of 'M5.update()', so if you prefer to detect events this
+ way and not with handler routines that's fine too.
+
+ If nothing was detected, the event type will be set to E_NONE with a value
+ of 0, so you can do "if (myButton.event) ...". 'M5.Buttons.event' has the
+ event detected this time around, regardless of what button or gesture it was
+ attached to. This example prints a star to serial if it is doubletapped.
+
+ #include <M5Core2.h>
+
+ Button myButton(50,70,220, 100, false, "Button",
+ {YELLOW, BLACK, NODRAW},
+ {RED, BLACK, NODRAW} );
+
+ void setup() {
+ M5.begin();
+ M5.Buttons.setFont(FSS18);
+ M5.Buttons.draw();
+ }
+
+ void loop() {
+ M5.update();
+ if (myButton.event == E_DBLTAP) Serial.print("* ");
+ }
+
+
+== M5.background ==
+
+ Only one button can become pressed for any spot on the touch screen. If you
+ define overlapping buttons, the first defined button for the overlap become
+ pressed and gets all subsequent events.
+
+ One special button, "M5.background", was defined before any others, and it
+ has the size of the entire touch sensor. This means it gets all events
+ where the first touch was not within any of the defined buttons.
+
+
+== Gestures on the Touch Screen ==
+
+ Whenever a finger is released from the touch screen and before any
+ higher-level button events are fired, the library first checks whether this
+ was perhaps a gesture. When you define gestures, you can optionally specify
+ the zone in which the gesture must start, the zone in which it must end, the
+ minimum distance the finger must have travelled, the direction it has
+ travelled in and the maximum time the gesture may take.
+
+ Gesture exampleGesture(fromZone, toZone, "exampleName", minimumDistance,
+ direction, plusminus, ro1, maxTime)
+
+ Where fromZone and toZone can be valid zones or the word "ANYWHERE". If you
+ want to specify neither fromZone nor toZone, you can also leave them off
+ completely. The minimum distance defaults to 75 pixels. The direction
+ (default: don't care) is in compass degrees (so 180 is down), but the
+ compiler defines DIR_UP, DIR_DOWN, DIR_LEFT and DIR_RIGHT are provided for
+ convenience. The plusminus deines how many degress off-course the gesture
+ may be, and the rot1 flag defines whether this direction is relative to the
+ current rotation, or as seen in rotation 1. maxTime is in milliseconds as
+ usual and defaults to 500 ms. DIR_ANY can be used for direction if you need
+ to specify it in order provide a rot1 or maximum time value.
+
+ here are a few examples of valid gesture definitions:
+
+
+ Gesture swipeDown("swipe down", 100, DIR_DOWN, 30);
+
+ Down (plus or minus 30 degrees) for at least 100 pixels within 500 ms
+
+
+ Gesture fromTop(Zone(0, 0, 360, 30), ANYWHERE, "from top", 100, DIR_DOWN,
+30);
+
+ The same but starting from within the top 30 pixels. (If you make that
+ too narrow you may miss the swipe because the sensor 'sees' only once
+ every 13 ms or so.
+
+
+ (Note that if you defined both these gestures in this order the second one
+ would never fire because any swipe that matched number two would first match
+ number one and fire that one instead.)
+
+ Gestures have a 'wasDetected()' method if you want to detect them in the
+ main loop, or you attach a handler the same way you would for a button,
+ with "myGesture.addhandler(myHandler)"
+
+
+ #include <M5Core2.h>
+
+ Gesture swipeDown("swipe down", DIR_DOWN, 30);
+
+ void setup() {
+ M5.begin();
+ }
+
+ void loop() {
+ M5.update();
+ if (swipeDown.wasDetected()) Serial.println("Swiped down!");
+ }
+
+
+== Advanced Hints and Tricks
+
+ ## drawFn
+
+ If you look at the source code for the library you will see that the
+ drawing of the button is done by a static function in the M5Buttons
+ object. It's defined as
+
+ void M5Buttons::drawFunction(Button& b, ButtonColors bc)
+
+ If you make your own function that takes the same arguments but that does
+ something different, you can make the library use it by saying
+ "M5.Buttons.drawFn = myFunction". You can even do that on a per-button
+ basis with "myButton.drawFn = myFunction".
+
+
+ ## drawZone
+
+ A Button instance _is_ also a Zone object, in that it descends from it.
+ Which means a Button has all the methods of a Zone object, as well as its
+ own. But it contains another zone, called drawZone. This allows you to
+ have the visual representation happen somewhere else than where the
+ button is on the touch sensor. Normally this is set to "invalid zone",
+ but if you set it to a valid screen area, the button will be drawn there.
+ This is used internally to put the optional labels for the off-screen
+ buttons on the Core2 on the screen just above their touch areas.
+
+
+ ## Drawing is Non-Invasive
+
+ This library uses a brand-new feature of the M5Display object --
+ M5.Lcd.popState() and M5.lcd.pushState() -- that allows it to save and
+ reload the complete display driver state before and after drawing a
+ button. What that means is that you can draw to the display without
+ worrying that the button drawing will mess with your font setting, cursor
+ position or anything else that is display-related.
+
+
+ ## TFT_ePI_Button Emulation
+
+ This libary also defines an object called TFT_eSPI_Button, which is the
+ old way of doing buttons that comes as an optional extra with the display
+ library. Together with M5Touch's emulation of the TFT_eSPI touch
+ interface (written for the older resistive touch-screens), you can use it
+ to run software made for those APIs. Do not use either for new code: the
+ native interfaces are much more powerful.
+
+
+ ## Buttons and Variable Scope
+
+ Buttons come into existence and are drawn in their initial state when
+ their variables are defined and are not detected anymore when their
+ variables are removed from memory when the function they were defined in
+ returns. Except for global buttons - defined outside any functions: their
+ variables always exist. The programmer has to take responsability for
+ erasing expired buttons off the screen because Button doesnt know what is
+ supposed to be in the background. If you're not clearing the entire
+ screen anyway, this can be done with "myButton.erase(BLACK)" if the
+ background is to be black.
+
+*/
+
+#ifndef _M5BUTTON_H_
+#define _M5BUTTON_H_
+
+class Gesture;
+
+#include <Arduino.h>
+#include <functional>
+#include <Free_Fonts.h>
+#include <M5Display.h>
+#include <TFT_eSPI.h>
+#include <vector>
+
+#include "PointAndZone.h"
+#include "utility/Config.h"
+
+//#ifdef M5Stack_M5Core2
+#include <M5Touch.h>
+//#endif /* M5Stack_M5Core2 */
+
+#define BUTTON_FREEFONT FSS9
+#define BUTTON_TEXTFONT 1
+#define BUTTON_TEXTSIZE 1
+#define BUTTON_DATUM MC_DATUM
+
+#define TAP_TIME 150
+#define DBLTAP_TIME 300
+#define LONGPRESS_TIME 0
+#define REPEAT_DELAY 0
+#define REPEAT_INTERVAL 200
+
+#define GESTURE_MAXTIME 500
+#define GESTURE_MINDIST 75
+#define ANYWHERE Zone()
+
+#define NUM_EVENTS 11
+#define E_TOUCH 0x0001
+#define E_RELEASE 0x0002
+#define E_MOVE 0x0004
+#define E_GESTURE 0x0008
+#define E_TAP 0x0010
+#define E_DBLTAP 0x0020
+#define E_DRAGGED 0x0040
+#define E_PRESSED 0x0080
+#define E_PRESSING 0x0100
+#define E_LONGPRESSED 0x0200
+#define E_LONGPRESSING 0x0400
+
+#define E_ALL 0x0FFF
+
+#define NODRAW 0x0120 // Special color value: transparent
+
+struct ButtonColors {
+ uint16_t bg;
+ uint16_t text;
+ uint16_t outline;
+};
+
+class Button;
+class Event;
+
+#ifdef _M5TOUCH_H_
+struct Finger {
+ Point current, previous, startPoint, tapPoint;
+ uint32_t startTime, tapTime;
+ Button* button;
+};
+#endif
+
+class Event {
+ public:
+ Event();
+ operator uint16_t();
+ const char* typeName();
+ const char* objName();
+ uint16_t direction(bool rot1 = false);
+ bool isDirection(int16_t wanted, uint8_t plusminus = PLUSMINUS,
+ bool rot1 = false);
+ uint16_t distance();
+ uint8_t finger;
+ uint16_t type;
+ Point from, to;
+ uint16_t duration;
+ Button* button;
+ Gesture* gesture;
+};
+
+typedef std::function<void(Event&)> EventHandlerCallback;
+
+class Button : public Zone {
+ public:
+ static std::vector<Button*> instances;
+ Button(int16_t x_, int16_t y_, int16_t w_, int16_t h_, bool rot1_ = false,
+ const char* name_ = "", ButtonColors off_ = {NODRAW, NODRAW, NODRAW},
+ ButtonColors on_ = {NODRAW, NODRAW, NODRAW},
+ uint8_t datum_ = BUTTON_DATUM, int16_t dx_ = 0, int16_t dy_ = 0,
+ uint8_t r_ = 0xFF);
+ Button(uint8_t pin_, uint8_t invert_, uint32_t dbTime_, String hw = "hw",
+ int16_t x_ = 0, int16_t y_ = 0, int16_t w_ = 0, int16_t h_ = 0,
+ bool rot1_ = false, const char* name_ = "",
+ ButtonColors off_ = {NODRAW, NODRAW, NODRAW},
+ ButtonColors on_ = {NODRAW, NODRAW, NODRAW},
+ uint8_t datum_ = BUTTON_DATUM, int16_t dx_ = 0, int16_t dy_ = 0,
+ uint8_t r_ = 0xFF);
+ ~Button();
+ operator bool();
+ bool operator==(const Button& b);
+ bool operator!=(const Button& b);
+ bool operator==(Button* b);
+ bool operator!=(Button* b);
+ int16_t instanceIndex();
+ bool read(bool manualRead = true);
+ void fingerDown(Point pos = Point(), uint8_t finger = 0);
+ void fingerUp(uint8_t finger = 0);
+ void fingerMove(Point pos, uint8_t finger);
+ void cancel();
+ bool isPressed();
+ bool isReleased();
+ bool wasPressed();
+ bool wasReleased();
+ bool pressedFor(uint32_t ms);
+ bool pressedFor(uint32_t ms, uint32_t continuous_time);
+ bool releasedFor(uint32_t ms);
+ bool wasReleasefor(uint32_t ms);
+ void addHandler(EventHandlerCallback fn, uint16_t eventMask = E_ALL);
+ void delHandlers(EventHandlerCallback fn = nullptr);
+ char* getName();
+ uint32_t lastChange();
+ Event event;
+ uint16_t userData;
+ uint16_t tapTime, dbltapTime, longPressTime;
+ uint16_t repeatDelay, repeatInterval;
+
+ protected:
+ void init();
+ bool postReleaseEvents();
+ bool timeoutEvents();
+ friend class M5Buttons;
+ char _name[16];
+ uint8_t _pin;
+ uint16_t _dbTime;
+ bool _invert;
+ bool _changed, _state, _tapWait, _pressing;
+ bool _longPressing, _cancelled, _manuallyRead;
+ uint8_t _setState;
+ uint32_t _time, _lastRepeat;
+ uint32_t _lastChange, _lastLongPress, _pressTime, _hold_time;
+ uint8_t _finger;
+ Point _fromPt[2], _toPt[2], _currentPt[2];
+
+ // visual stuff
+ public:
+ void draw(ButtonColors bc);
+ void draw();
+ void hide(uint16_t color = NODRAW);
+ void erase(uint16_t color = TFT_BLACK);
+ void setLabel(const char* label_);
+ //void setFont(const GFXfont* freeFont_);
+ void setFont(uint8_t textFont_ = 0);
+ void setTextSize(uint8_t textSize_ = 0);
+ char* label();
+ ButtonColors off, on;
+ Zone drawZone;
+ uint8_t datum, r;
+ int16_t dx, dy;
+ void (*drawFn)(Button& b, ButtonColors bc);
+
+ protected:
+ bool _hidden;
+ bool _compat; // For TFT_eSPI_Button emulation
+ char _label[51];
+ uint8_t _textFont;
+ //const GFXfont* _freeFont;
+ uint8_t _textSize;
+};
+
+class Gesture {
+ public:
+ static std::vector<Gesture*> instances;
+ Gesture(Zone fromZone_, Zone toZone_, const char* name_ = "",
+ uint16_t minDistance_ = GESTURE_MINDIST,
+ int16_t direction_ = INVALID_VALUE, uint8_t plusminus_ = PLUSMINUS,
+ bool rot1_ = false, uint16_t maxTime_ = GESTURE_MAXTIME);
+ Gesture(const char* name_ = "", uint16_t minDistance_ = GESTURE_MINDIST,
+ int16_t direction_ = INVALID_VALUE, uint8_t plusminus_ = PLUSMINUS,
+ bool rot1_ = false, uint16_t maxTime_ = GESTURE_MAXTIME);
+ ~Gesture();
+ operator bool();
+ int16_t instanceIndex();
+ bool test(Point& from, Point& to, uint16_t duration);
+ bool wasDetected();
+ void addHandler(EventHandlerCallback fn, uint16_t eventMask = E_ALL);
+ void delHandlers(EventHandlerCallback fn = nullptr);
+ char* getName();
+ Zone fromZone;
+ Zone toZone;
+ Event event;
+ int16_t direction;
+ uint8_t plusminus;
+ bool rot1;
+ uint16_t maxTime, minDistance;
+
+ protected:
+ friend class M5Buttons;
+ bool _detected;
+ char _name[16];
+};
+
+struct EventHandler {
+ uint16_t eventMask;
+ Button* button;
+ Gesture* gesture;
+ EventHandlerCallback fn;
+};
+
+class M5Buttons {
+ public:
+ static M5Buttons* instance;
+ static void drawFunction(Button& b, ButtonColors bc);
+ M5Buttons();
+ Button* which(Point& p);
+ void draw();
+ void update();
+ //void setFont(const GFXfont* freeFont_);
+ void setFont(uint8_t textFont_);
+ void setTextSize(uint8_t textSize_);
+ void (*drawFn)(Button& b, ButtonColors bc);
+ void fireEvent(uint8_t finger, uint16_t type, Point& from, Point& to,
+ uint16_t duration, Button* button, Gesture* gesture);
+ void addHandler(EventHandlerCallback fn, uint16_t eventMask = E_ALL,
+ Button* button = nullptr, Gesture* gesture = nullptr);
+ void delHandlers(EventHandlerCallback fn, Button* button, Gesture* gesture);
+ Event event;
+
+ protected:
+ std::vector<EventHandler> _eventHandlers;
+ uint8_t _textFont;
+ //const GFXfont* _freeFont;
+ uint8_t _textSize;
+ bool _leftovers;
+
+#ifdef _M5TOUCH_H_
+ protected:
+ Finger _finger[2];
+#endif
+};
+
+// TFT_eSPI_Button compatibility emulation
+
+class TFT_eSPI_Button2 : public Button {
+ public:
+ TFT_eSPI_Button2();
+ void initButton(TFT_eSPI* gfx, int16_t x, int16_t y, uint16_t w, uint16_t h,
+ uint16_t outline, uint16_t fill, uint16_t textcolor,
+ char* label_, uint8_t textsize_);
+ void initButtonUL(TFT_eSPI* gfx, int16_t x_, int16_t y_, uint16_t w_,
+ uint16_t h_, uint16_t outline, uint16_t fill,
+ uint16_t textcolor, char* label_, uint8_t textsize_);
+ void setLabelDatum(int16_t x_delta, int16_t y_delta,
+ uint8_t datum = MC_DATUM);
+ void drawButton(bool inverted = false, String long_name = "");
+ bool contains(int16_t x, int16_t y);
+ void press(bool p);
+ bool isPressed();
+ bool justPressed();
+ bool justReleased();
+
+ private:
+ bool currstate, laststate;
+};
+
+#endif /* _M5BUTTON_H_ */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/M5Timer.cpp b/lib/M5Core2/src/utility/M5Timer.cpp
new file mode 100644
index 000000000..664c0b6ca
--- /dev/null
+++ b/lib/M5Core2/src/utility/M5Timer.cpp
@@ -0,0 +1,224 @@
+#if defined (CORE2)
+/*
+ * M5Timer.cpp
+ *
+ * M5Timer - A timer library for Arduino.
+ * Author: mromani@ottotecnica.com
+ * Copyright (c) 2010 OTTOTECNICA Italy
+ *
+ * This library is free software; you can redistribute it
+ * and/or modify it under the terms of the GNU Lesser
+ * General Public License as published by the Free Software
+ * Foundation; either version 2.1 of the License, or (at
+ * your option) any later version.
+ *
+ * This library is distributed in the hope that it will
+ * be useful, but WITHOUT ANY WARRANTY; without even the
+ * implied warranty of MERCHANTABILITY or FITNESS FOR A
+ * PARTICULAR PURPOSE. See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser
+ * General Public License along with this library; if not,
+ * write to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include "M5Timer.h"
+
+// Select time function:
+// static inline unsigned long elapsed() { return micros(); }
+static inline unsigned long elapsed() {
+ return millis();
+}
+
+M5Timer::M5Timer() {
+ unsigned long current_millis = elapsed();
+
+ for (int i = 0; i < MAX_TIMERS; i++) {
+ enabled[i] = false;
+ callbacks[i] = 0; // if the callback pointer is zero, the slot is free,
+ // i.e. doesn't "contain" any timer
+ prev_millis[i] = current_millis;
+ numRuns[i] = 0;
+ }
+ numTimers = 0;
+}
+
+void M5Timer::run() {
+ int i;
+ unsigned long current_millis;
+
+ // get current time
+ current_millis = elapsed();
+
+ for (i = 0; i < MAX_TIMERS; i++) {
+ toBeCalled[i] = DEFCALL_DONTRUN;
+
+ // no callback == no timer, i.e. jump over empty slots
+ if (callbacks[i] != 0) {
+ // is it time to process this timer ?
+ // see
+ // http://arduino.cc/forum/index.php/topic,124048.msg932592.html#msg932592
+
+ if (current_millis - prev_millis[i] >= delays[i]) {
+ // update time
+ // prev_millis[i] = current_millis;
+ prev_millis[i] += delays[i];
+
+ // check if the timer callback has to be executed
+ if (enabled[i] == true) {
+ // "run forever" timers must always be executed
+ if (maxNumRuns[i] == RUN_FOREVER) {
+ toBeCalled[i] = DEFCALL_RUNONLY;
+ } else if (numRuns[i] < maxNumRuns[i]) {
+ // other timers get executed the specified number of
+ // times
+ toBeCalled[i] = DEFCALL_RUNONLY;
+ numRuns[i]++;
+ // after the last run, delete the timer
+ if (numRuns[i] >= maxNumRuns[i]) {
+ toBeCalled[i] = DEFCALL_RUNANDDEL;
+ }
+ }
+ }
+ }
+ }
+ }
+ for (i = 0; i < MAX_TIMERS; i++) {
+ switch (toBeCalled[i]) {
+ case DEFCALL_DONTRUN:
+ break;
+
+ case DEFCALL_RUNONLY:
+ callbacks[i]();
+ break;
+
+ case DEFCALL_RUNANDDEL:
+ callbacks[i]();
+ deleteTimer(i);
+ break;
+ }
+ }
+}
+
+// find the first available slot
+// return -1 if none found
+int M5Timer::findFirstFreeSlot() {
+ int i;
+
+ // all slots are used
+ if (numTimers >= MAX_TIMERS) {
+ return -1;
+ }
+
+ // return the first slot with no callback (i.e. free)
+ for (i = 0; i < MAX_TIMERS; i++) {
+ if (callbacks[i] == 0) {
+ return i;
+ }
+ }
+ // no free slots found
+ return -1;
+}
+
+int M5Timer::setTimer(long d, timer_callback f, int n) {
+ int freeTimer;
+
+ freeTimer = findFirstFreeSlot();
+ if (freeTimer < 0) {
+ return -1;
+ }
+
+ if (f == NULL) {
+ return -1;
+ }
+
+ delays[freeTimer] = d;
+ callbacks[freeTimer] = f;
+ maxNumRuns[freeTimer] = n;
+ enabled[freeTimer] = true;
+ prev_millis[freeTimer] = elapsed();
+
+ numTimers++;
+
+ return freeTimer;
+}
+
+int M5Timer::setInterval(long d, timer_callback f) {
+ return setTimer(d, f, RUN_FOREVER);
+}
+
+int M5Timer::setTimeout(long d, timer_callback f) {
+ return setTimer(d, f, RUN_ONCE);
+}
+
+void M5Timer::deleteTimer(int timerId) {
+ if (timerId >= MAX_TIMERS) {
+ return;
+ }
+
+ // nothing to delete if no timers are in use
+ if (numTimers == 0) {
+ return;
+ }
+
+ // don't decrease the number of timers if the
+ // specified slot is already empty
+ if (callbacks[timerId] != NULL) {
+ callbacks[timerId] = 0;
+ enabled[timerId] = false;
+ toBeCalled[timerId] = DEFCALL_DONTRUN;
+ delays[timerId] = 0;
+ numRuns[timerId] = 0;
+
+ // update number of timers
+ numTimers--;
+ }
+}
+
+// function contributed by code@rowansimms.com
+void M5Timer::restartTimer(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ prev_millis[numTimer] = elapsed();
+}
+
+boolean M5Timer::isEnabled(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return false;
+ }
+
+ return enabled[numTimer];
+}
+
+void M5Timer::enable(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ enabled[numTimer] = true;
+}
+
+void M5Timer::disable(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ enabled[numTimer] = false;
+}
+
+void M5Timer::toggle(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ enabled[numTimer] = !enabled[numTimer];
+}
+
+int M5Timer::getNumTimers() {
+ return numTimers;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/M5Timer.h b/lib/M5Core2/src/utility/M5Timer.h
new file mode 100644
index 000000000..3854fa132
--- /dev/null
+++ b/lib/M5Core2/src/utility/M5Timer.h
@@ -0,0 +1,125 @@
+#if defined (CORE2)
+/*
+ * M5Timer.h
+ *
+ * M5Timer - A timer library for Arduino.
+ * Author: mromani@ottotecnica.com
+ * Copyright (c) 2010 OTTOTECNICA Italy
+ *
+ * This library is free software; you can redistribute it
+ * and/or modify it under the terms of the GNU Lesser
+ * General Public License as published by the Free Software
+ * Foundation; either version 2.1 of the License, or (at
+ * your option) any later version.
+ *
+ * This library is distributed in the hope that it will
+ * be useful, but WITHOUT ANY WARRANTY; without even the
+ * implied warranty of MERCHANTABILITY or FITNESS FOR A
+ * PARTICULAR PURPOSE. See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser
+ * General Public License along with this library; if not,
+ * write to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifndef M5Timer_H
+#define M5Timer_H
+
+#include <functional>
+#include <Arduino.h>
+
+typedef std::function<void(void)> timer_callback;
+
+class M5Timer {
+ public:
+ // maximum number of timers
+ const static int MAX_TIMERS = 10;
+
+ // setTimer() constants
+ const static int RUN_FOREVER = 0;
+ const static int RUN_ONCE = 1;
+
+ // constructor
+ M5Timer();
+
+ // this function must be called inside loop()
+ void run();
+
+ // call function f every d milliseconds
+ int setInterval(long d, timer_callback f);
+
+ // call function f once after d milliseconds
+ int setTimeout(long d, timer_callback f);
+
+ // call function f every d milliseconds for n times
+ int setTimer(long d, timer_callback f, int n);
+
+ // destroy the specified timer
+ void deleteTimer(int numTimer);
+
+ // restart the specified timer
+ void restartTimer(int numTimer);
+
+ // returns true if the specified timer is enabled
+ boolean isEnabled(int numTimer);
+
+ // enables the specified timer
+ void enable(int numTimer);
+
+ // disables the specified timer
+ void disable(int numTimer);
+
+ // enables the specified timer if it's currently disabled,
+ // and vice-versa
+ void toggle(int numTimer);
+
+ // returns the number of used timers
+ int getNumTimers();
+
+ // returns the number of available timers
+ int getNumAvailableTimers() {
+ return MAX_TIMERS - numTimers;
+ };
+
+ private:
+ // deferred call constants
+ const static int DEFCALL_DONTRUN = 0; // don't call the callback function
+ const static int DEFCALL_RUNONLY =
+ 1; // call the callback function but don't delete the timer
+ const static int DEFCALL_RUNANDDEL =
+ 2; // call the callback function and delete the timer
+
+ // find the first available slot
+ int findFirstFreeSlot();
+
+ // value returned by the millis() function
+ // in the previous run() call
+ unsigned long prev_millis[MAX_TIMERS];
+
+ // pointers to the callback functions
+ timer_callback callbacks[MAX_TIMERS];
+
+ // delay values
+ long delays[MAX_TIMERS];
+
+ // number of runs to be executed for each timer
+ int maxNumRuns[MAX_TIMERS];
+
+ // number of executed runs for each timer
+ int numRuns[MAX_TIMERS];
+
+ // which timers are enabled
+ boolean enabled[MAX_TIMERS];
+
+ // deferred function call (sort of) - N.B.: this array is only used in run()
+ int toBeCalled[MAX_TIMERS];
+
+ // actual number of timers in use
+ int numTimers;
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/MPU6886.cpp b/lib/M5Core2/src/utility/MPU6886.cpp
new file mode 100644
index 000000000..4563118bf
--- /dev/null
+++ b/lib/M5Core2/src/utility/MPU6886.cpp
@@ -0,0 +1,265 @@
+#if defined (CORE2)
+#include "MPU6886.h"
+#include <math.h>
+#include <Arduino.h>
+
+MPU6886::MPU6886() {
+}
+
+void MPU6886::I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* read_Buffer) {
+ Wire1.beginTransmission(driver_Addr);
+ Wire1.write(start_Addr);
+ Wire1.endTransmission(false);
+ uint8_t i = 0;
+ Wire1.requestFrom(driver_Addr, number_Bytes);
+
+ //! Put read results in the Rx buffer
+ while (Wire1.available()) {
+ read_Buffer[i++] = Wire1.read();
+ }
+}
+
+void MPU6886::I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* write_Buffer) {
+ Wire1.beginTransmission(driver_Addr);
+ Wire1.write(start_Addr);
+ Wire1.write(*write_Buffer);
+ Wire1.endTransmission();
+}
+
+int MPU6886::Init(void) {
+ unsigned char tempdata[1];
+ unsigned char regdata;
+
+ Wire1.begin(21, 22);
+
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_WHOAMI, 1, tempdata);
+ if (tempdata[0] != 0x19) return -1;
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ delay(10);
+
+ regdata = (0x01 << 7);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ delay(10);
+
+ regdata = (0x01 << 0);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ delay(10);
+
+ regdata = 0x10;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, ®data);
+ delay(1);
+
+ regdata = 0x18;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data);
+ delay(1);
+
+ regdata = 0x01;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, ®data);
+ delay(1);
+
+ regdata = 0x05;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_SMPLRT_DIV, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_EN, 1, ®data);
+ delay(1);
+
+ regdata = 0x22;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_PIN_CFG, 1, ®data);
+ delay(1);
+
+ regdata = 0x01;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, ®data);
+
+ delay(100);
+ getGres();
+ getAres();
+ return 0;
+}
+
+void MPU6886::getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_XOUT_H, 6, buf);
+
+ *ax = ((int16_t)buf[0] << 8) | buf[1];
+ *ay = ((int16_t)buf[2] << 8) | buf[3];
+ *az = ((int16_t)buf[4] << 8) | buf[5];
+}
+void MPU6886::getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_XOUT_H, 6, buf);
+
+ *gx = ((uint16_t)buf[0] << 8) | buf[1];
+ *gy = ((uint16_t)buf[2] << 8) | buf[3];
+ *gz = ((uint16_t)buf[4] << 8) | buf[5];
+}
+
+void MPU6886::getTempAdc(int16_t* t) {
+ uint8_t buf[2];
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_TEMP_OUT_H, 2, buf);
+
+ *t = ((uint16_t)buf[0] << 8) | buf[1];
+}
+
+//! 俯仰，航向，横滚：pitch，yaw，roll，指三维空间中飞行器的旋转状态。
+void MPU6886::getAhrsData(float* pitch, float* roll, float* yaw) {
+ float accX = 0;
+ float accY = 0;
+ float accZ = 0;
+
+ float gyroX = 0;
+ float gyroY = 0;
+ float gyroZ = 0;
+
+ getGyroData(&gyroX, &gyroY, &gyroZ);
+ getAccelData(&accX, &accY, &accZ);
+
+ MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD,
+ gyroZ * DEG_TO_RAD, accX, accY, accZ, pitch, roll, yaw);
+}
+
+void MPU6886::getGres() {
+ switch (Gyscale) {
+ // Possible gyro scales (and their register bit settings) are:
+ case GFS_250DPS:
+ gRes = 250.0 / 32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0 / 32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0 / 32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0 / 32768.0;
+ break;
+ }
+}
+
+void MPU6886::getAres() {
+ switch (Acscale) {
+ // Possible accelerometer scales (and their register bit settings)
+ // are: 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). Here's a
+ // bit of an algorith to calculate DPS/(ADC tick) based on that
+ // 2-bit value:
+ case AFS_2G:
+ aRes = 2.0 / 32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0 / 32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0 / 32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0 / 32768.0;
+ break;
+ }
+}
+
+void MPU6886::SetGyroFsr(Gscale scale) {
+ // return IIC_Write_Byte(MPU_GYRO_CFG_REG,scale<<3);//设置陀螺仪满量程范围
+ unsigned char regdata;
+ regdata = (scale << 3);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data);
+ delay(10);
+
+ Gyscale = scale;
+ getGres();
+}
+
+void MPU6886::SetAccelFsr(Ascale scale) {
+ unsigned char regdata;
+ regdata = (scale << 3);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, ®data);
+ delay(10);
+
+ Acscale = scale;
+ getAres();
+}
+
+void MPU6886::getAccelData(float* ax, float* ay, float* az) {
+ int16_t accX = 0;
+ int16_t accY = 0;
+ int16_t accZ = 0;
+ getAccelAdc(&accX, &accY, &accZ);
+
+ *ax = (float)accX * aRes;
+ *ay = (float)accY * aRes;
+ *az = (float)accZ * aRes;
+}
+
+void MPU6886::getGyroData(float* gx, float* gy, float* gz) {
+ int16_t gyroX = 0;
+ int16_t gyroY = 0;
+ int16_t gyroZ = 0;
+ getGyroAdc(&gyroX, &gyroY, &gyroZ);
+
+ *gx = (float)gyroX * gRes;
+ *gy = (float)gyroY * gRes;
+ *gz = (float)gyroZ * gRes;
+}
+
+void MPU6886::getTempData(float* t) {
+ int16_t temp = 0;
+ getTempAdc(&temp);
+
+ *t = (float)temp / 326.8 + 25.0;
+}
+
+void MPU6886::sleep() {
+ unsigned char regdata;
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ regdata |= (0x01 << 6);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+}
+
+void MPU6886::wakeup() {
+ unsigned char regdata;
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ regdata &= ~(0x01 << 6);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+}
+
+void MPU6886::setAccelLPF(accel_lpf_t config)
+{
+ unsigned char regdata;
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, ®data);
+ regdata &= 0b11111000;
+ regdata |= config;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, ®data);
+}
+
+void MPU6886::setGyroLPF(gyro_lpf_t config)
+{
+ unsigned char regdata;
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, ®data);
+ regdata &= 0b11111000;
+ regdata |= config;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, ®data);
+
+
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data);
+ regdata &= 0b11111100;
+ regdata |= (config >> 3);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/MPU6886.h b/lib/M5Core2/src/utility/MPU6886.h
new file mode 100644
index 000000000..e5d18869a
--- /dev/null
+++ b/lib/M5Core2/src/utility/MPU6886.h
@@ -0,0 +1,120 @@
+#if defined (CORE2)
+/*
+ Note: The MPU6886 is an I2C sensor and uses the Arduino Wire library.
+ Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or
+ a 3.3 V Teensy 3.1. We have disabled the internal pull-ups used by the Wire
+ library in the Wire.h/twi.c utility file. We are also using the 400 kHz fast
+ I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
+ */
+#ifndef _MPU6886_H_
+#define _MPU6886_H_
+
+#include <Wire.h>
+#include <Arduino.h>
+#include "MahonyAHRS.h"
+
+#define MPU6886_ADDRESS 0x68
+#define MPU6886_WHOAMI 0x75
+#define MPU6886_ACCEL_INTEL_CTRL 0x69
+#define MPU6886_SMPLRT_DIV 0x19
+#define MPU6886_INT_PIN_CFG 0x37
+#define MPU6886_INT_ENABLE 0x38
+#define MPU6886_ACCEL_XOUT_H 0x3B
+#define MPU6886_ACCEL_XOUT_L 0x3C
+#define MPU6886_ACCEL_YOUT_H 0x3D
+#define MPU6886_ACCEL_YOUT_L 0x3E
+#define MPU6886_ACCEL_ZOUT_H 0x3F
+#define MPU6886_ACCEL_ZOUT_L 0x40
+
+#define MPU6886_TEMP_OUT_H 0x41
+#define MPU6886_TEMP_OUT_L 0x42
+
+#define MPU6886_GYRO_XOUT_H 0x43
+#define MPU6886_GYRO_XOUT_L 0x44
+#define MPU6886_GYRO_YOUT_H 0x45
+#define MPU6886_GYRO_YOUT_L 0x46
+#define MPU6886_GYRO_ZOUT_H 0x47
+#define MPU6886_GYRO_ZOUT_L 0x48
+
+#define MPU6886_USER_CTRL 0x6A
+#define MPU6886_PWR_MGMT_1 0x6B
+#define MPU6886_PWR_MGMT_2 0x6C
+#define MPU6886_CONFIG 0x1A
+#define MPU6886_GYRO_CONFIG 0x1B
+#define MPU6886_ACCEL_CONFIG 0x1C
+#define MPU6886_ACCEL_CONFIG2 0x1D
+#define MPU6886_FIFO_EN 0x23
+
+// #define G (9.8)
+#define RtA 57.324841
+#define AtR 0.0174533
+#define Gyro_Gr 0.0010653
+
+class MPU6886 {
+ public:
+ enum Ascale { AFS_2G = 0, AFS_4G, AFS_8G, AFS_16G };
+
+ enum Gscale { GFS_250DPS = 0, GFS_500DPS, GFS_1000DPS, GFS_2000DPS };
+
+ typedef enum {
+ ACCEL_LPF_BYPASS = 0b1000,
+ ACCEL_LPF_218HZ = 0b0001,
+ ACCEL_LPF_99HZ = 0b0010,
+ ACCEL_LPF_45HZ = 0b0011,
+ ACCEL_LPF_21HZ = 0b0100,
+ ACCEL_LPF_10HZ = 0b0101,
+ ACCEL_LPF_5HZ = 0b0110,
+ ACCEL_LPF_420HZ = 0b0111
+ } accel_lpf_t;
+
+ typedef enum {
+ GYRO_LPF_BYPASS_1 = 0b11000,
+ GYRO_LPF_BYPASS_2 = 0b10000,
+ GYRO_LPF_250HZ = 0b00000,
+ GYRO_LPF_176HZ = 0b00001,
+ GYRO_LPF_92HZ = 0b00010,
+ GYRO_LPF_41HZ = 0b00011,
+ GYRO_LPF_20HZ = 0b00100,
+ GYRO_LPF_10HZ = 0b00101,
+ GYRO_LPF_5HZ = 0b00110,
+ GYRO_LPF_3281HZ = 0b00111
+ } gyro_lpf_t;
+
+ Gscale Gyscale = GFS_2000DPS;
+ Ascale Acscale = AFS_8G;
+
+ public:
+ MPU6886();
+ int Init(void);
+ void getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az);
+ void getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz);
+ void getTempAdc(int16_t* t);
+
+ void getAccelData(float* ax, float* ay, float* az);
+ void getGyroData(float* gx, float* gy, float* gz);
+ void getTempData(float* t);
+
+ void SetGyroFsr(Gscale scale);
+ void SetAccelFsr(Ascale scale);
+
+ void getAhrsData(float* pitch, float* roll, float* yaw);
+
+ void sleep();
+ void wakeup();
+ void setAccelLPF(accel_lpf_t config);
+ void setGyroLPF(gyro_lpf_t config);
+
+ public:
+ float aRes, gRes;
+
+ private:
+ protected:
+ void I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* read_Buffer);
+ void I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* write_Buffer);
+ void getGres();
+ void getAres();
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/MahonyAHRS.cpp b/lib/M5Core2/src/utility/MahonyAHRS.cpp
new file mode 100644
index 000000000..d75656abe
--- /dev/null
+++ b/lib/M5Core2/src/utility/MahonyAHRS.cpp
@@ -0,0 +1,267 @@
+#if defined (CORE2)
+//=====================================================================================================
+// MahonyAHRS.c
+//=====================================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date Author Notes
+// 29/09/2011 SOH Madgwick Initial release
+// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
+//
+//=====================================================================================================
+
+//---------------------------------------------------------------------------------------------------
+// Header files
+
+#include "MahonyAHRS.h"
+#include "Arduino.h"
+#include <math.h>
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreq 25.0f // sample frequency in Hz
+#define twoKpDef (2.0f * 1.0f) // 2 * proportional gain
+#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
+
+// #define twoKiDef (0.0f * 0.0f)
+
+//---------------------------------------------------------------------------------------------------
+// Variable definitions
+
+volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp)
+volatile float twoKi = twoKiDef; // 2 * integral gain (Ki)
+volatile float
+ q0 = 1.0,
+ q1 = 0.0, q2 = 0.0,
+ q3 = 0.0; // quaternion of sensor frame relative to auxiliary frame
+volatile float integralFBx = 0.0f, integralFBy = 0.0f,
+ integralFBz = 0.0f; // integral error terms scaled by Ki
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+// float invSqrt(float x);
+
+//====================================================================================================
+// Functions
+
+//---------------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay,
+ float az, float mx, float my, float mz) {
+ float recipNorm;
+ float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+ float hx, hy, bx, bz;
+ float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
+ float halfex, halfey, halfez;
+ float qa, qb, qc;
+
+ // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
+ // magnetometer normalisation)
+ if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+ // MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+ return;
+ }
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in
+ // accelerometer normalisation)
+ if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+ // Normalise accelerometer measurement
+ recipNorm = sqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Normalise magnetometer measurement
+ recipNorm = sqrt(mx * mx + my * my + mz * mz);
+ mx *= recipNorm;
+ my *= recipNorm;
+ mz *= recipNorm;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ q0q0 = q0 * q0;
+ q0q1 = q0 * q1;
+ q0q2 = q0 * q2;
+ q0q3 = q0 * q3;
+ q1q1 = q1 * q1;
+ q1q2 = q1 * q2;
+ q1q3 = q1 * q3;
+ q2q2 = q2 * q2;
+ q2q3 = q2 * q3;
+ q3q3 = q3 * q3;
+
+ // Reference direction of Earth's magnetic field
+ hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) +
+ mz * (q1q3 + q0q2));
+ hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) +
+ mz * (q2q3 - q0q1));
+ bx = sqrt(hx * hx + hy * hy);
+ bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) +
+ mz * (0.5f - q1q1 - q2q2));
+
+ // Estimated direction of gravity and magnetic field
+ halfvx = q1q3 - q0q2;
+ halfvy = q0q1 + q2q3;
+ halfvz = q0q0 - 0.5f + q3q3;
+ halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+ halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+ halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+
+ // Error is sum of cross product between estimated direction and
+ // measured direction of field vectors
+ halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
+ halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
+ halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
+
+ // Compute and apply integral feedback if enabled
+ if (twoKi > 0.0f) {
+ integralFBx += twoKi * halfex *
+ (1.0f / sampleFreq); // integral error scaled by Ki
+ integralFBy += twoKi * halfey * (1.0f / sampleFreq);
+ integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+ gx += integralFBx; // apply integral feedback
+ gy += integralFBy;
+ gz += integralFBz;
+ } else {
+ integralFBx = 0.0f; // prevent integral windup
+ integralFBy = 0.0f;
+ integralFBz = 0.0f;
+ }
+
+ // Apply proportional feedback
+ gx += twoKp * halfex;
+ gy += twoKp * halfey;
+ gz += twoKp * halfez;
+ }
+
+ // Integrate rate of change of quaternion
+ gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
+ gy *= (0.5f * (1.0f / sampleFreq));
+ gz *= (0.5f * (1.0f / sampleFreq));
+ qa = q0;
+ qb = q1;
+ qc = q2;
+ q0 += (-qb * gx - qc * gy - q3 * gz);
+ q1 += (qa * gx + qc * gz - q3 * gy);
+ q2 += (qa * gy - qb * gz + q3 * gx);
+ q3 += (qa * gz + qb * gy - qc * gx);
+
+ // Normalise quaternion
+ recipNorm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay,
+ float az, float *pitch, float *roll, float *yaw) {
+ float recipNorm;
+ float halfvx, halfvy, halfvz;
+ float halfex, halfey, halfez;
+ float qa, qb, qc;
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in
+ // accelerometer normalisation)
+ if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+ // Normalise accelerometer measurement
+ recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Estimated direction of gravity and vector perpendicular to magnetic
+ // flux
+ halfvx = q1 * q3 - q0 * q2;
+ halfvy = q0 * q1 + q2 * q3;
+ halfvz = q0 * q0 - 0.5f + q3 * q3;
+
+ // Error is sum of cross product between estimated and measured
+ // direction of gravity
+ halfex = (ay * halfvz - az * halfvy);
+ halfey = (az * halfvx - ax * halfvz);
+ halfez = (ax * halfvy - ay * halfvx);
+
+ // Compute and apply integral feedback if enabled
+ if (twoKi > 0.0f) {
+ integralFBx += twoKi * halfex *
+ (1.0f / sampleFreq); // integral error scaled by Ki
+ integralFBy += twoKi * halfey * (1.0f / sampleFreq);
+ integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+ gx += integralFBx; // apply integral feedback
+ gy += integralFBy;
+ gz += integralFBz;
+ } else {
+ integralFBx = 0.0f; // prevent integral windup
+ integralFBy = 0.0f;
+ integralFBz = 0.0f;
+ }
+
+ // Apply proportional feedback
+ gx += twoKp * halfex;
+ gy += twoKp * halfey;
+ gz += twoKp * halfez;
+ }
+
+ // Integrate rate of change of quaternion
+ gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
+ gy *= (0.5f * (1.0f / sampleFreq));
+ gz *= (0.5f * (1.0f / sampleFreq));
+ qa = q0;
+ qb = q1;
+ qc = q2;
+ q0 += (-qb * gx - qc * gy - q3 * gz);
+ q1 += (qa * gx + qc * gz - q3 * gy);
+ q2 += (qa * gy - qb * gz + q3 * gx);
+ q3 += (qa * gz + qb * gy - qc * gx);
+
+ // Normalise quaternion
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+
+ *pitch = asin(-2 * q1 * q3 + 2 * q0 * q2); // pitch
+ *roll = atan2(2 * q2 * q3 + 2 * q0 * q1,
+ -2 * q1 * q1 - 2 * q2 * q2 + 1); // roll
+ *yaw = atan2(2 * (q1 * q2 + q0 * q3),
+ q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); // yaw
+
+ *pitch *= RAD_TO_DEG;
+ *yaw *= RAD_TO_DEG;
+ // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
+ // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19
+ // - http://www.ngdc.noaa.gov/geomag-web/#declination
+ *yaw -= 8.5;
+ *roll *= RAD_TO_DEG;
+
+ /// Serial.printf("%f %f %f \r\n", pitch, roll, yaw);
+}
+
+//---------------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstrict-aliasing"
+float invSqrt(float x) {
+ float halfx = 0.5f * x;
+ float y = x;
+ long i = *(long *)&y;
+ i = 0x5f3759df - (i >> 1);
+ y = *(float *)&i;
+ y = y * (1.5f - (halfx * y * y));
+ return y;
+}
+#pragma GCC diagnostic pop
+//====================================================================================================
+// END OF CODE
+//====================================================================================================
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/MahonyAHRS.h b/lib/M5Core2/src/utility/MahonyAHRS.h
new file mode 100644
index 000000000..cede8d781
--- /dev/null
+++ b/lib/M5Core2/src/utility/MahonyAHRS.h
@@ -0,0 +1,39 @@
+#if defined (CORE2)
+//=====================================================================================================
+// MahonyAHRS.h
+//=====================================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date Author Notes
+// 29/09/2011 SOH Madgwick Initial release
+// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
+//
+//=====================================================================================================
+#ifndef MahonyAHRS_h
+#define MahonyAHRS_h
+
+//----------------------------------------------------------------------------------------------------
+// Variable declaration
+
+extern volatile float twoKp; // 2 * proportional gain (Kp)
+extern volatile float twoKi; // 2 * integral gain (Ki)
+// volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to
+// auxiliary frame
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay,
+ float az, float mx, float my, float mz);
+// void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay,
+// float az);
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay,
+ float az, float *pitch, float *roll, float *yaw);
+float invSqrt(float x);
+#endif
+//=====================================================================================================
+// End of file
+//=====================================================================================================
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/PointAndZone.cpp b/lib/M5Core2/src/utility/PointAndZone.cpp
new file mode 100644
index 000000000..58b80dafa
--- /dev/null
+++ b/lib/M5Core2/src/utility/PointAndZone.cpp
@@ -0,0 +1,201 @@
+#if defined (CORE2)
+#include "PointAndZone.h"
+
+// Point class
+
+Point::Point(int16_t x_ /* = INVALID_VALUE */,
+ int16_t y_ /* = INVALID_VALUE */) {
+ x = x_;
+ y = y_;
+}
+
+bool Point::operator==(const Point& p) {
+ return (Equals(p));
+}
+
+bool Point::operator!=(const Point& p) {
+ return (!Equals(p));
+}
+
+Point::operator char*() {
+ if (valid()) {
+ sprintf(_text, "(%d, %d)", x, y);
+ } else {
+ strncpy(_text, "(invalid)", 12);
+ }
+ return _text;
+}
+
+Point::operator bool() {
+ return !(x == INVALID_VALUE && y == INVALID_VALUE);
+}
+
+void Point::set(int16_t x_ /* = INVALID_VALUE */,
+ int16_t y_ /* = INVALID_VALUE */) {
+ x = x_;
+ y = y_;
+}
+
+bool Point::Equals(const Point& p) {
+ return (x == p.x && y == p.y);
+}
+
+bool Point::in(Zone& z) {
+ return (z.contains(x, y));
+}
+
+bool Point::valid() {
+ return !(x == INVALID_VALUE && y == INVALID_VALUE);
+}
+
+uint16_t Point::distanceTo(const Point& p) {
+ int16_t dx = x - p.x;
+ int16_t dy = y - p.y;
+ return sqrt(dx * dx + dy * dy) + 0.5;
+}
+
+uint16_t Point::directionTo(const Point& p, bool rot1 /* = false */) {
+ // 57.29578 =~ 180/pi, see https://stackoverflow.com/a/62486304
+ uint16_t a = (uint16_t)(450.5 + (atan2(p.y - y, p.x - x) * 57.29578));
+#ifdef TFT
+ if (rot1) {
+ if (TFT->rotation < 4) {
+ a = (((TFT->rotation + 3) % 4) * 90) + a;
+ } else {
+ a = (((TFT->rotation + 1) % 4) * 90) - a;
+ }
+ }
+#endif /* TFT */
+ a = (360 + a) % 360;
+ return a;
+}
+
+bool Point::isDirectionTo(const Point& p, int16_t wanted,
+ uint8_t plusminus /* = PLUSMINUS */,
+ bool rot1 /* = false */) {
+ uint16_t a = directionTo(p, rot1);
+ return (min(abs(wanted - a), 360 - abs(wanted - a)) <= plusminus);
+}
+
+void Point::rotate(uint8_t m) {
+ if (m == 1 || !valid()) return;
+ int16_t normal_x = x;
+ int16_t normal_y = y;
+ int16_t inv_x = HIGHEST_X - x;
+ int16_t inv_y = HIGHEST_Y - y;
+ // inv_y can be negative for area below screen of M5Core2
+ switch (m) {
+ case 0:
+ x = inv_y;
+ y = normal_x;
+ break;
+ case 2:
+ x = normal_y;
+ y = inv_x;
+ break;
+ case 3:
+ x = inv_x;
+ y = inv_y;
+ break;
+ // rotations 4-7 are mirrored
+ case 4:
+ x = inv_y;
+ y = inv_x;
+ break;
+ case 5:
+ x = normal_x;
+ y = inv_y;
+ break;
+ case 6:
+ x = normal_y;
+ y = normal_x;
+ break;
+ case 7:
+ x = inv_x;
+ y = normal_y;
+ break;
+ }
+}
+
+// Zone class
+
+Zone::Zone(int16_t x_ /* = INVALID_VALUE */, int16_t y_ /* = INVALID_VALUE */,
+ int16_t w_ /* = 0 */, int16_t h_ /* = 0 */, bool rot1_ /* = false */
+) {
+ set(x_, y_, w_, h_, rot1_);
+}
+
+Zone::operator bool() {
+ return !(x == INVALID_VALUE && y == INVALID_VALUE);
+}
+
+void Zone::set(int16_t x_ /* = INVALID_VALUE */,
+ int16_t y_ /* = INVALID_VALUE */, int16_t w_ /* = 0 */,
+ int16_t h_ /* = 0 */, bool rot1_ /* = false */) {
+ x = x_;
+ y = y_;
+ w = w_;
+ h = h_;
+ rot1 = rot1_;
+}
+
+bool Zone::valid() {
+ return !(x == INVALID_VALUE && y == INVALID_VALUE);
+}
+
+bool Zone::contains(const Point& p) {
+ return contains(p.x, p.y);
+}
+
+bool Zone::contains(int16_t x_, int16_t y_) {
+#ifdef TFT
+ if (rot1 && TFT->rotation != 1) {
+ Zone t = *this;
+ t.rotate(TFT->rotation);
+ return (y_ >= t.y && y_ <= t.y + t.h && x_ >= t.x && x_ <= t.x + t.w);
+ }
+#endif /* TFT */
+
+ return (y_ >= y && y_ <= y + h && x_ >= x && x_ <= x + w);
+}
+
+void Zone::rotate(uint8_t m) {
+ if (m == 1) return;
+ int16_t normal_x = x;
+ int16_t normal_y = y;
+ int16_t inv_x = TFT_WIDTH - 1 - x - w;
+ int16_t inv_y = TFT_HEIGHT - 1 - y - h; // negative for area below screen
+ switch (m) {
+ case 0:
+ x = inv_y;
+ y = normal_x;
+ break;
+ case 2:
+ x = normal_y;
+ y = inv_x;
+ break;
+ case 3:
+ x = inv_x;
+ y = inv_y;
+ break;
+ // rotations 4-7 are mirrored
+ case 4:
+ x = inv_y;
+ y = inv_x;
+ break;
+ case 5:
+ x = normal_x;
+ y = inv_y;
+ break;
+ case 6:
+ x = normal_y;
+ y = normal_x;
+ break;
+ case 7:
+ x = inv_x;
+ y = normal_y;
+ break;
+ }
+ if (!(m % 2)) std::swap(w, h);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/PointAndZone.h b/lib/M5Core2/src/utility/PointAndZone.h
new file mode 100644
index 000000000..e2ef55015
--- /dev/null
+++ b/lib/M5Core2/src/utility/PointAndZone.h
@@ -0,0 +1,192 @@
+#if defined (CORE2)
+/*
+
+== The PointAndZone Library ==
+
+ This library was written to supply Point and Zone, common primitives for
+ M5Display and M5Button, libraries originally written for the M5Stack series
+ of devices. They could be useful for many other applications, especially
+ anything based on a TFT_eSPI display driver.
+
+
+== Point and Zone: Describing Points and Areas on the Screen ==
+
+ The Point and Zone classes allow you to create variables that hold a point
+ or an area on the screen.
+
+ Point(x, y)
+
+ Holds a point on the screen. Has members x and y that hold the coordinates
+ of a touch. Values INVALID_VALUE for x and y indicate an invalid value,
+ and that's what a point starts out with if you declare it without
+ parameters. The 'valid()' method tests if a point is valid. If you
+ explicitly evaluate a Point as a boolean ("if (p) ..."), you also get
+ whether the point is valid, so this is equivalent to writing "if
+ (p.valid()) ...".
+
+ Zone(x, y, w, h)
+
+ Holds a rectangular area on the screen. Members x, y, w and h are for the
+ x and y coordinate of the top-left corner and the width and height of the
+ rectangle.
+
+ The 'set' method allows you to change the properties of an existing Point
+ or Zone. Using the 'in' or 'contains' method you can test if a point lies
+ in a zone.
+
+ The PointAndZone library also provides the low-level support for direction
+ from one point to another and for screen rotation translations.
+
+
+== Looking for Directions? ==
+
+ This library allows you to find the distance in pixels between two Point
+ objects with "A.distanceTo(B)". Using "A.directionTo(B)" will output a
+ compass course in degrees from A to B. That is, if A lies directly above A
+ on the screen the output will be 0, if B lies to the left of A, the output
+ will be 270. You can also test whether a direction matches some other
+ direction by using "A.isDirectionTo(B, 0, 30)". What this does is take the
+ direction from A to B and output 'true' if it is 0, plus or minus 30
+ degrees. (So either between 330 and 359 or between 0 and 30).
+
+ Do note that unlike in math, on a display the Y-axis points downwards. So
+ pixel coordinates (10, 10) are at direction 135 deg from (0, 0).
+
+ As a last argument to the direction functions, you can supply 'rot1' again
+ (default is 'false'). Just like in zones and buttons, what that means is
+ that the direction is output as if the rotation 1 coordinate system was
+ used.
+
+ Distance and direction functionality is used in Gesture recognition in the
+ M5Button highler level library. Its 'Event' objects have methods that look
+ very much like these, except the 'To' in the name is missing because Events
+ have a starting and ending point so you can just print
+ "myEvent.direction()" or state "if (myEvent.isDirection(0,30) ..."
+
+
+== Some Examples ==
+
+ Point a;
+ Point b(50, 120);
+ Serial.println(a.valid()); // 0
+ Serial.println(a); // (invalid)
+ a.set(10, 30);
+ Serial.println(a); // (10,30)
+ Serial.println(a.valid()); // 1
+ Serial.println(b.y); // 120
+ Serial.println(a.distanceTo(b)); // 98
+ Serial.println(a.directionTo(b)); // 156
+ Serial.println(a.isDirectionTo(b, 0, 30)); // 0
+ Zone z(0,0,100, 100);
+ Serial.println(z.w); // 100
+ Serial.println(z.contains(a)); // 1
+ Serial.println(b.in(z)); // 0
+
+
+== Screen Rotation and the 'rot1' Property ==
+
+ TL;DR: just set it to 'false' if you don't need anything special,
+ otherwise read the rest of this section.
+
+ The TFT_eSPI library allows you to rotate the screen. On M5Stack devices,
+ you do this with "M5.Lcd.SetRotation", supplying a number between 0 and 7.
+ Numbers 0-3 are the obvious rotations, with 1 being the default. 4-7 are
+ the other 4 mirrored, so you would not be using these unless you want to
+ look at the display through a mirror for a homebrew heads-up display or
+ mini-teleprompter.
+
+ The M5Touch library for the Core2 device rotates the data it reads from the
+ sensor to match the display using the 'rotate' function on the Point
+ objects it reads. So if the display is upside down (rotation 3), the (0,
+ 0)-point is diagonally accross from where it is in rotation 1. (And the
+ area that was below the screen at y 240-279 now has negative y-values.)
+
+ Zones (and buttons, because they are extensions of zones) have a 'rot1'
+ property. This would normally set to false, meaning the coordinates are
+ treated as specified in the current rotation. If it is set to 'true'
+ however, the library treats the cordinates as if they are specified in
+ rotation 1 and internally rotates them before evaluating whether a Point is
+ in a Zone. The Button object also rotates the coordinates before drawing
+ the button.
+
+ So by setting 'rot1' to true, you can create zones and buttons that stay in
+ the same place even if the screen is rotated. On the Core2, this is used to
+ define the BtnA through BtnC virtual below-screen buttons, which should
+ always be in the area below the screen where the circles are printed,
+ regardless of rotation.
+
+
+== Porting ==
+
+ To use this library on other devices, simply replace these two lines
+
+ #include <M5Display.h> // so that we can get the rotation
+ #include "utility/Config.h" // Defines 'TFT', a pointer to the display
+
+ by whatever you need to do to make the symbol 'TFT' be a pointer to a
+ TFT_eSPI-derived display device that has a 'rotation' variable. If you
+ don't need rotation just delete the lines: the direction functions and the
+ 'contains' function will now simply ignore their 'rot1' parameters.
+
+*/
+
+#ifndef _POINTANDZONE_H_
+#define _POINTANDZONE_H_
+
+#include <Arduino.h>
+#include <M5Display.h> // so that we can get the rotation
+#include "utility/Config.h" // Defines 'TFT', a pointer to the display
+
+#define INVALID_VALUE -32768
+#define PLUSMINUS 45 // default value for isDirectionTo
+
+#define DIR_UP 0
+#define DIR_RIGHT 90
+#define DIR_DOWN 180
+#define DIR_LEFT 270
+#define DIR_ANY INVALID_VALUE
+
+#define HIGHEST_X 319 // Can't trust TFT_WIDTH, driver is portrait
+#define HIGHEST_Y 239
+
+class Zone;
+
+class Point {
+ public:
+ Point(int16_t x_ = INVALID_VALUE, int16_t y_ = INVALID_VALUE);
+ bool operator==(const Point& p);
+ bool operator!=(const Point& p);
+ explicit operator bool();
+ operator char*();
+ void set(int16_t x_ = INVALID_VALUE, int16_t y_ = INVALID_VALUE);
+ bool valid();
+ bool in(Zone& z);
+ bool Equals(const Point& p);
+ uint16_t distanceTo(const Point& p);
+ uint16_t directionTo(const Point& p, bool rot1 = false);
+ bool isDirectionTo(const Point& p, int16_t wanted,
+ uint8_t plusminus = PLUSMINUS, bool rot1 = false);
+ void rotate(uint8_t m);
+ int16_t x, y;
+
+ private:
+ char _text[12];
+};
+
+class Zone {
+ public:
+ Zone(int16_t x_ = INVALID_VALUE, int16_t y_ = INVALID_VALUE, int16_t w_ = 0,
+ int16_t h_ = 0, bool rot1_ = false);
+ explicit operator bool();
+ bool valid();
+ void set(int16_t x_ = INVALID_VALUE, int16_t y_ = INVALID_VALUE,
+ int16_t w_ = 0, int16_t h_ = 0, bool rot1_ = false);
+ bool contains(const Point& p);
+ bool contains(int16_t x, int16_t y);
+ void rotate(uint8_t m);
+ int16_t x, y, w, h;
+ bool rot1;
+};
+
+#endif /* _POINTANDZONE_H_ */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/pngle.c b/lib/M5Core2/src/utility/pngle.c
new file mode 100644
index 000000000..e0796c43c
--- /dev/null
+++ b/lib/M5Core2/src/utility/pngle.c
@@ -0,0 +1,983 @@
+#if defined (CORE2)
+/*-
+ * MIT License
+ *
+ * Copyright (c) 2019 kikuchan
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <rom/miniz.h>
+#include "pngle.h"
+
+#ifndef MIN
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#endif
+
+#ifdef PNGLE_DEBUG
+#define debug_printf(...) fprintf(stderr, __VA_ARGS__)
+#else
+#define debug_printf(...) ((void)0)
+#endif
+
+#define PNGLE_ERROR(s) \
+ (pngle->error = (s), pngle->state = PNGLE_STATE_ERROR, -1)
+#define PNGLE_CALLOC(a, b, name) \
+ (debug_printf("[pngle] Allocating %zu bytes for %s\n", \
+ (size_t)(a) * (size_t)(b), (name)), \
+ calloc((size_t)(a), (size_t)(b)))
+
+#define PNGLE_UNUSED(x) (void)(x)
+
+typedef enum {
+ PNGLE_STATE_ERROR = -2,
+ PNGLE_STATE_EOF = -1,
+ PNGLE_STATE_INITIAL = 0,
+
+ PNGLE_STATE_FIND_CHUNK_HEADER,
+ PNGLE_STATE_HANDLE_CHUNK,
+ PNGLE_STATE_CRC,
+} pngle_state_t;
+
+typedef enum {
+ // Supported chunks
+ // Filter chunk names by following command to (re)generate hex constants;
+ // % perl -ne 'chomp; s/.*\s*\/\/\s*//; print "\tPNGLE_CHUNK_$_ = 0x" .
+ // unpack("H*") . "UL, // $_\n";'
+ PNGLE_CHUNK_IHDR = 0x49484452UL, // IHDR
+ PNGLE_CHUNK_PLTE = 0x504c5445UL, // PLTE
+ PNGLE_CHUNK_IDAT = 0x49444154UL, // IDAT
+ PNGLE_CHUNK_IEND = 0x49454e44UL, // IEND
+ PNGLE_CHUNK_tRNS = 0x74524e53UL, // tRNS
+ PNGLE_CHUNK_gAMA = 0x67414d41UL, // gAMA
+} pngle_chunk_t;
+
+// typedef struct _pngle_t pngle_t; // declared in pngle.h
+struct _pngle_t {
+ pngle_ihdr_t hdr;
+
+ uint_fast8_t channels; // 0 indicates IHDR hasn't been processed yet
+
+ // PLTE chunk
+ size_t n_palettes;
+ uint8_t *palette;
+
+ // tRNS chunk
+ size_t n_trans_palettes;
+ uint8_t *trans_palette;
+
+ // parser state (reset on every chunk header)
+ pngle_state_t state;
+ uint32_t chunk_type;
+ uint32_t chunk_remain;
+ mz_ulong crc32;
+
+ // decompression state (reset on IHDR)
+ tinfl_decompressor inflator; // 11000 bytes
+ uint8_t lz_buf[TINFL_LZ_DICT_SIZE]; // 32768 bytes
+ uint8_t *next_out; // NULL indicates IDAT hasn't been processed yet
+ size_t avail_out;
+
+ // scanline decoder (reset on every set_interlace_pass() call)
+ uint8_t *scanline_ringbuf;
+ size_t scanline_ringbuf_size;
+ size_t scanline_ringbuf_cidx;
+ int_fast8_t scanline_remain_bytes_to_render;
+ int_fast8_t filter_type;
+ uint32_t drawing_x;
+ uint32_t drawing_y;
+
+ // interlace
+ uint_fast8_t interlace_pass;
+
+ const char *error;
+
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ uint8_t *gamma_table;
+ double display_gamma;
+#endif
+
+ pngle_init_callback_t init_callback;
+ pngle_draw_callback_t draw_callback;
+ pngle_done_callback_t done_callback;
+
+ void *user_data;
+};
+
+// magic
+static const uint8_t png_sig[] = {137, 80, 78, 71, 13, 10, 26, 10};
+static uint32_t interlace_off_x[8] = {0, 0, 4, 0, 2, 0, 1, 0};
+static uint32_t interlace_off_y[8] = {0, 0, 0, 4, 0, 2, 0, 1};
+static uint32_t interlace_div_x[8] = {1, 8, 8, 4, 4, 2, 2, 1};
+static uint32_t interlace_div_y[8] = {1, 8, 8, 8, 4, 4, 2, 2};
+
+static inline uint8_t read_uint8(const uint8_t *p) {
+ return *p;
+}
+
+static inline uint32_t read_uint32(const uint8_t *p) {
+ return (p[0] << 24) | (p[1] << 16) | (p[2] << 8) | (p[3] << 0);
+}
+
+static inline uint32_t U32_CLAMP_ADD(uint32_t a, uint32_t b, uint32_t top) {
+ uint32_t v = a + b;
+ if (v < a) return top; // uint32 overflow
+ if (v > top) return top; // clamp
+ return v;
+}
+
+void pngle_reset(pngle_t *pngle) {
+ if (!pngle) return;
+
+ pngle->state = PNGLE_STATE_INITIAL;
+ pngle->error = "No error";
+
+ if (pngle->scanline_ringbuf) free(pngle->scanline_ringbuf);
+ if (pngle->palette) free(pngle->palette);
+ if (pngle->trans_palette) free(pngle->trans_palette);
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ if (pngle->gamma_table) free(pngle->gamma_table);
+#endif
+
+ pngle->scanline_ringbuf = NULL;
+ pngle->palette = NULL;
+ pngle->trans_palette = NULL;
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ pngle->gamma_table = NULL;
+#endif
+
+ pngle->channels = 0; // indicates IHDR hasn't been processed yet
+ pngle->next_out = NULL; // indicates IDAT hasn't been processed yet
+
+ // clear them just in case...
+ memset(&pngle->hdr, 0, sizeof(pngle->hdr));
+ pngle->n_palettes = 0;
+ pngle->n_trans_palettes = 0;
+
+ tinfl_init(&pngle->inflator);
+}
+
+pngle_t *pngle_new() {
+ pngle_t *pngle = (pngle_t *)PNGLE_CALLOC(1, sizeof(pngle_t), "pngle_t");
+ if (!pngle) return NULL;
+
+ pngle_reset(pngle);
+
+ return pngle;
+}
+
+void pngle_destroy(pngle_t *pngle) {
+ if (pngle) {
+ pngle_reset(pngle);
+ free(pngle);
+ }
+}
+
+const char *pngle_error(pngle_t *pngle) {
+ if (!pngle) return "Uninitialized";
+ return pngle->error;
+}
+
+uint32_t pngle_get_width(pngle_t *pngle) {
+ if (!pngle) return 0;
+ return pngle->hdr.width;
+}
+
+uint32_t pngle_get_height(pngle_t *pngle) {
+ if (!pngle) return 0;
+ return pngle->hdr.height;
+}
+
+pngle_ihdr_t *pngle_get_ihdr(pngle_t *pngle) {
+ if (!pngle) return NULL;
+ if (pngle->channels == 0) return NULL;
+ return &pngle->hdr;
+}
+
+static int is_trans_color(pngle_t *pngle, uint16_t *value, size_t n) {
+ if (pngle->n_trans_palettes != 1) return 0; // false (none or indexed)
+
+ for (size_t i = 0; i < n; i++) {
+ if (value[i] != (pngle->trans_palette[i * 2 + 0] * 0x100 +
+ pngle->trans_palette[i * 2 + 1]))
+ return 0; // false
+ }
+ return 1; // true
+}
+
+static inline void scanline_ringbuf_push(pngle_t *pngle, uint8_t value) {
+ pngle->scanline_ringbuf[pngle->scanline_ringbuf_cidx] = value;
+ pngle->scanline_ringbuf_cidx =
+ (pngle->scanline_ringbuf_cidx + 1) % pngle->scanline_ringbuf_size;
+}
+
+static inline uint16_t get_value(pngle_t *pngle, size_t *ridx, int *bitcount,
+ int depth) {
+ uint16_t v;
+
+ switch (depth) {
+ case 1:
+ case 2:
+ case 4:
+ if (*bitcount >= 8) {
+ *bitcount = 0;
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+ }
+ *bitcount += depth;
+ uint8_t mask = ((1UL << depth) - 1);
+ uint8_t shift = (8 - *bitcount);
+ return (pngle->scanline_ringbuf[*ridx] >> shift) & mask;
+
+ case 8:
+ v = pngle->scanline_ringbuf[*ridx];
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+ return v;
+
+ case 16:
+ v = pngle->scanline_ringbuf[*ridx];
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+
+ v = v * 0x100 + pngle->scanline_ringbuf[*ridx];
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+ return v;
+ }
+
+ return 0;
+}
+
+static int pngle_draw_pixels(pngle_t *pngle, size_t scanline_ringbuf_xidx) {
+ uint16_t v[4]; // MAX_CHANNELS
+ int bitcount = 0;
+ uint8_t pixel_depth = (pngle->hdr.color_type & 1) ? 8 : pngle->hdr.depth;
+ uint16_t maxval = (1UL << pixel_depth) - 1;
+
+ int n_pixels = pngle->hdr.depth == 16 ? 1 : (8 / pngle->hdr.depth);
+
+ for (; n_pixels-- > 0 && pngle->drawing_x < pngle->hdr.width;
+ pngle->drawing_x = U32_CLAMP_ADD(
+ pngle->drawing_x, interlace_div_x[pngle->interlace_pass],
+ pngle->hdr.width)) {
+ for (uint_fast8_t c = 0; c < pngle->channels; c++) {
+ v[c] = get_value(pngle, &scanline_ringbuf_xidx, &bitcount,
+ pngle->hdr.depth);
+ }
+
+ // color type: 0000 0111
+ // ^-- indexed color (palette)
+ // ^--- Color
+ // ^---- Alpha channel
+
+ if (pngle->hdr.color_type & 2) {
+ // color
+ if (pngle->hdr.color_type & 1) {
+ // indexed color: type 3
+
+ // lookup palette info
+ uint16_t pidx = v[0];
+ if (pidx >= pngle->n_palettes)
+ return PNGLE_ERROR("Color index is out of range");
+
+ v[0] = pngle->palette[pidx * 3 + 0];
+ v[1] = pngle->palette[pidx * 3 + 1];
+ v[2] = pngle->palette[pidx * 3 + 2];
+
+ // tRNS as an indexed alpha value table (for color type 3)
+ v[3] = pidx < pngle->n_trans_palettes
+ ? pngle->trans_palette[pidx]
+ : maxval;
+ } else {
+ // true color: 2, and 6
+ v[3] = (pngle->hdr.color_type & 4) ? v[3]
+ : is_trans_color(pngle, v, 3) ? 0
+ : maxval;
+ }
+ } else {
+ // alpha, tRNS, or opaque
+ v[3] = (pngle->hdr.color_type & 4) ? v[1]
+ : is_trans_color(pngle, v, 1) ? 0
+ : maxval;
+
+ // monochrome
+ v[1] = v[2] = v[0];
+ }
+
+ if (pngle->draw_callback) {
+ uint8_t rgba[4] = {(v[0] * 255 + maxval / 2) / maxval,
+ (v[1] * 255 + maxval / 2) / maxval,
+ (v[2] * 255 + maxval / 2) / maxval,
+ (v[3] * 255 + maxval / 2) / maxval};
+
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ if (pngle->gamma_table) {
+ for (int i = 0; i < 3; i++) {
+ rgba[i] = pngle->gamma_table[v[i]];
+ }
+ }
+#endif
+
+ pngle->draw_callback(pngle, pngle->drawing_x, pngle->drawing_y,
+ MIN(interlace_div_x[pngle->interlace_pass] -
+ interlace_off_x[pngle->interlace_pass],
+ pngle->hdr.width - pngle->drawing_x),
+ MIN(interlace_div_y[pngle->interlace_pass] -
+ interlace_off_y[pngle->interlace_pass],
+ pngle->hdr.height - pngle->drawing_y),
+ rgba);
+ }
+ }
+
+ return 0;
+}
+
+static inline int paeth(int a, int b, int c) {
+ int p = a + b - c;
+ int pa = abs(p - a);
+ int pb = abs(p - b);
+ int pc = abs(p - c);
+
+ if (pa <= pb && pa <= pc) return a;
+ if (pb <= pc) return b;
+ return c;
+}
+
+static int set_interlace_pass(pngle_t *pngle, uint_fast8_t pass) {
+ pngle->interlace_pass = pass;
+
+ uint_fast8_t bytes_per_pixel =
+ (pngle->channels * pngle->hdr.depth + 7) / 8; // 1 if depth <= 8
+ size_t scanline_pixels =
+ (pngle->hdr.width - interlace_off_x[pngle->interlace_pass] +
+ interlace_div_x[pngle->interlace_pass] - 1) /
+ interlace_div_x[pngle->interlace_pass];
+ size_t scanline_stride =
+ (scanline_pixels * pngle->channels * pngle->hdr.depth + 7) / 8;
+
+ pngle->scanline_ringbuf_size =
+ scanline_stride + bytes_per_pixel * 2; // 2 rooms for c/x and a
+
+ if (pngle->scanline_ringbuf) free(pngle->scanline_ringbuf);
+ if ((pngle->scanline_ringbuf = PNGLE_CALLOC(pngle->scanline_ringbuf_size, 1,
+ "scanline ringbuf")) == NULL)
+ return PNGLE_ERROR("Insufficient memory");
+
+ pngle->drawing_x = interlace_off_x[pngle->interlace_pass];
+ pngle->drawing_y = interlace_off_y[pngle->interlace_pass];
+ pngle->filter_type = -1;
+
+ pngle->scanline_ringbuf_cidx = 0;
+ pngle->scanline_remain_bytes_to_render = -1;
+
+ return 0;
+}
+
+static int setup_gamma_table(pngle_t *pngle, uint32_t png_gamma) {
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ if (pngle->gamma_table) free(pngle->gamma_table);
+
+ if (pngle->display_gamma <= 0) return 0; // disable gamma correction
+ if (png_gamma == 0) return 0;
+
+ uint8_t pixel_depth = (pngle->hdr.color_type & 1) ? 8 : pngle->hdr.depth;
+ uint16_t maxval = (1UL << pixel_depth) - 1;
+
+ pngle->gamma_table = PNGLE_CALLOC(1, maxval + 1, "gamma table");
+ if (!pngle->gamma_table) return PNGLE_ERROR("Insufficient memory");
+
+ for (int i = 0; i < maxval + 1; i++) {
+ pngle->gamma_table[i] =
+ (uint8_t)floor(pow(i / (double)maxval,
+ 100000.0 / png_gamma / pngle->display_gamma) *
+ 255.0 +
+ 0.5);
+ }
+ debug_printf("[pngle] gamma value = %d\n", png_gamma);
+#else
+ PNGLE_UNUSED(pngle);
+ PNGLE_UNUSED(png_gamma);
+#endif
+ return 0;
+}
+
+static int pngle_on_data(pngle_t *pngle, const uint8_t *p, int len) {
+ const uint8_t *ep = p + len;
+
+ uint_fast8_t bytes_per_pixel =
+ (pngle->channels * pngle->hdr.depth + 7) / 8; // 1 if depth <= 8
+
+ while (p < ep) {
+ if (pngle->drawing_x >= pngle->hdr.width) {
+ // New row
+ pngle->drawing_x = interlace_off_x[pngle->interlace_pass];
+ pngle->drawing_y = U32_CLAMP_ADD(
+ pngle->drawing_y, interlace_div_y[pngle->interlace_pass],
+ pngle->hdr.height);
+ pngle->filter_type = -1; // Indicate new line
+ }
+
+ if (pngle->drawing_x >= pngle->hdr.width ||
+ pngle->drawing_y >= pngle->hdr.height) {
+ if (pngle->interlace_pass == 0 || pngle->interlace_pass >= 7)
+ return len; // Do nothing further
+
+ // Interlace: Next pass
+ if (set_interlace_pass(pngle, pngle->interlace_pass + 1) < 0)
+ return -1;
+ debug_printf("[pngle] interlace pass changed to: %d\n",
+ pngle->interlace_pass);
+
+ continue; // This is required because "No filter type bytes are
+ // present in an empty pass".
+ }
+
+ if (pngle->filter_type < 0) {
+ if (*p > 4) {
+ debug_printf("[pngle] Invalid filter type is found; 0x%02x\n",
+ *p);
+ return PNGLE_ERROR("Invalid filter type is found");
+ }
+
+ pngle->filter_type = (int_fast8_t)*p++; // 0 - 4
+
+ // push sentinel bytes for new line
+ for (uint_fast8_t i = 0; i < bytes_per_pixel; i++) {
+ scanline_ringbuf_push(pngle, 0);
+ }
+
+ continue;
+ }
+
+ size_t cidx = pngle->scanline_ringbuf_cidx;
+ size_t bidx = (pngle->scanline_ringbuf_cidx + bytes_per_pixel) %
+ pngle->scanline_ringbuf_size;
+ size_t aidx = (pngle->scanline_ringbuf_cidx +
+ pngle->scanline_ringbuf_size - bytes_per_pixel) %
+ pngle->scanline_ringbuf_size;
+ // debug_printf("[pngle] cidx = %zd, bidx = %zd, aidx = %zd\n", cidx,
+ // bidx, aidx);
+
+ uint8_t c = pngle->scanline_ringbuf[cidx]; // left-up
+ uint8_t b = pngle->scanline_ringbuf[bidx]; // up
+ uint8_t a = pngle->scanline_ringbuf[aidx]; // left
+ uint8_t x = *p++; // target
+ // debug_printf("[pngle] c = 0x%02x, b = 0x%02x, a = 0x%02x, x =
+ // 0x%02x\n", c, b, a, x);
+
+ // Reverse the filter
+ switch (pngle->filter_type) {
+ case 0:
+ break; // None
+ case 1:
+ x += a;
+ break; // Sub
+ case 2:
+ x += b;
+ break; // Up
+ case 3:
+ x += (a + b) / 2;
+ break; // Average
+ case 4:
+ x += paeth(a, b, c);
+ break; // Paeth
+ }
+
+ scanline_ringbuf_push(pngle, x); // updates scanline_ringbuf_cidx
+
+ if (pngle->scanline_remain_bytes_to_render < 0)
+ pngle->scanline_remain_bytes_to_render = bytes_per_pixel;
+ if (--pngle->scanline_remain_bytes_to_render == 0) {
+ size_t xidx = (pngle->scanline_ringbuf_cidx +
+ pngle->scanline_ringbuf_size - bytes_per_pixel) %
+ pngle->scanline_ringbuf_size;
+
+ if (pngle_draw_pixels(pngle, xidx) < 0) return -1;
+
+ pngle->scanline_remain_bytes_to_render = -1; // reset
+ }
+ }
+
+ return len;
+}
+
+static int pngle_handle_chunk(pngle_t *pngle, const uint8_t *buf, size_t len) {
+ size_t consume = 0;
+
+ switch (pngle->chunk_type) {
+ case PNGLE_CHUNK_IHDR:
+ // parse IHDR
+ consume = 13;
+ if (len < consume) return 0;
+
+ debug_printf("[pngle] Parse IHDR\n");
+
+ pngle->hdr.width = read_uint32(buf + 0);
+ pngle->hdr.height = read_uint32(buf + 4);
+ pngle->hdr.depth = read_uint8(buf + 8);
+ pngle->hdr.color_type = read_uint8(buf + 9);
+ pngle->hdr.compression = read_uint8(buf + 10);
+ pngle->hdr.filter = read_uint8(buf + 11);
+ pngle->hdr.interlace = read_uint8(buf + 12);
+
+ debug_printf("[pngle] width : %d\n", pngle->hdr.width);
+ debug_printf("[pngle] height : %d\n", pngle->hdr.height);
+ debug_printf("[pngle] depth : %d\n", pngle->hdr.depth);
+ debug_printf("[pngle] color_type : %d\n",
+ pngle->hdr.color_type);
+ debug_printf("[pngle] compression: %d\n",
+ pngle->hdr.compression);
+ debug_printf("[pngle] filter : %d\n", pngle->hdr.filter);
+ debug_printf("[pngle] interlace : %d\n", pngle->hdr.interlace);
+
+ /*
+ Color Allowed Interpretation channels Type Bit Depths
+
+ 0 1,2,4,8,16 Each pixel is a grayscale sample. 1
+ channels (Brightness)
+
+ 2 8,16 Each pixel is an R,G,B triple. 3 channels
+ (R, G, B)
+
+ 3 1,2,4,8 Each pixel is a palette index; 1 channels
+ (palette info) a PLTE chunk must appear.
+
+ 4 8,16 Each pixel is a grayscale sample, 2
+ channels (Brightness, Alpha) followed by an alpha sample.
+
+ 6 8,16 Each pixel is an R,G,B triple, 4 channels
+ (R, G, B, Alpha) followed by an alpha sample.
+ */
+ // 111
+ // ^-- indexed color (palette)
+ // ^--- Color
+ // ^---- Alpha channel
+
+ switch (pngle->hdr.color_type) {
+ case 0:
+ pngle->channels = 1;
+ if (pngle->hdr.depth != 1 && pngle->hdr.depth != 2 &&
+ pngle->hdr.depth != 4 && pngle->hdr.depth != 8 &&
+ pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // grayscale
+ case 2:
+ pngle->channels = 3;
+ if (pngle->hdr.depth != 8 && pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // truecolor
+ case 3:
+ pngle->channels = 1;
+ if (pngle->hdr.depth != 1 && pngle->hdr.depth != 2 &&
+ pngle->hdr.depth != 4 && pngle->hdr.depth != 8)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // indexed color
+ case 4:
+ pngle->channels = 2;
+ if (pngle->hdr.depth != 8 && pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // grayscale + alpha
+ case 6:
+ pngle->channels = 4;
+ if (pngle->hdr.depth != 8 && pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // truecolor + alpha
+ default:
+ return PNGLE_ERROR("Incorrect IHDR info");
+ }
+
+ if (pngle->hdr.compression != 0)
+ return PNGLE_ERROR("Unsupported compression type in IHDR");
+ if (pngle->hdr.filter != 0)
+ return PNGLE_ERROR("Unsupported filter type in IHDR");
+
+ // interlace
+ if (set_interlace_pass(pngle, pngle->hdr.interlace ? 1 : 0) < 0)
+ return -1;
+
+ // callback
+ if (pngle->init_callback)
+ pngle->init_callback(pngle, pngle->hdr.width,
+ pngle->hdr.height);
+
+ break;
+
+ case PNGLE_CHUNK_IDAT:
+ // parse & decode IDAT chunk
+ if (len < 1) return 0;
+
+ debug_printf("[pngle] Reading IDAT (len %zd / chunk remain %u)\n",
+ len, pngle->chunk_remain);
+
+ size_t in_bytes = len;
+ size_t out_bytes = pngle->avail_out;
+
+ // debug_printf("[pngle] in_bytes %zd, out_bytes %zd, next_out
+ // %p\n", in_bytes, out_bytes, pngle->next_out);
+
+ // XXX: tinfl_decompress always requires (next_out - lz_buf +
+ // avail_out) == TINFL_LZ_DICT_SIZE
+ tinfl_status status = tinfl_decompress(
+ &pngle->inflator, (const mz_uint8 *)buf, &in_bytes,
+ pngle->lz_buf, (mz_uint8 *)pngle->next_out, &out_bytes,
+ TINFL_FLAG_HAS_MORE_INPUT | TINFL_FLAG_PARSE_ZLIB_HEADER);
+
+ // debug_printf("[pngle] tinfl_decompress\n");
+ // debug_printf("[pngle] => in_bytes %zd, out_bytes %zd,
+ // next_out %p, status %d\n", in_bytes, out_bytes, pngle->next_out,
+ // status);
+
+ if (status < TINFL_STATUS_DONE) {
+ // Decompression failed.
+ debug_printf(
+ "[pngle] tinfl_decompress() failed with status %d!\n",
+ status);
+ return PNGLE_ERROR("Failed to decompress the IDAT stream");
+ }
+
+ pngle->next_out += out_bytes;
+ pngle->avail_out -= out_bytes;
+
+ // debug_printf("[pngle] => avail_out %zd, next_out %p\n",
+ // pngle->avail_out, pngle->next_out);
+
+ if (status == TINFL_STATUS_DONE || pngle->avail_out == 0) {
+ // Output buffer is full, or decompression is done, so write
+ // buffer to output file.
+ // XXX: This is the only chance to process the buffer.
+ uint8_t *read_ptr = pngle->lz_buf;
+ size_t n = TINFL_LZ_DICT_SIZE - (size_t)pngle->avail_out;
+
+ // pngle_on_data() usually returns n, otherwise -1 on error
+ if (pngle_on_data(pngle, read_ptr, n) < 0) return -1;
+
+ // XXX: tinfl_decompress always requires (next_out - lz_buf +
+ // avail_out) == TINFL_LZ_DICT_SIZE
+ pngle->next_out = pngle->lz_buf;
+ pngle->avail_out = TINFL_LZ_DICT_SIZE;
+ }
+
+ consume = in_bytes;
+ break;
+
+ case PNGLE_CHUNK_PLTE:
+ consume = 3;
+ if (len < consume) return 0;
+
+ memcpy(pngle->palette + pngle->n_palettes * 3, buf, 3);
+
+ debug_printf("[pngle] PLTE[%zd]: (%d, %d, %d)\n", pngle->n_palettes,
+ pngle->palette[pngle->n_palettes * 3 + 0],
+ pngle->palette[pngle->n_palettes * 3 + 1],
+ pngle->palette[pngle->n_palettes * 3 + 2]);
+
+ pngle->n_palettes++;
+
+ break;
+
+ case PNGLE_CHUNK_IEND:
+ consume = 0;
+ break;
+
+ case PNGLE_CHUNK_tRNS:
+ switch (pngle->hdr.color_type) {
+ case 3:
+ consume = 1;
+ break;
+ case 0:
+ consume = 2 * 1;
+ break;
+ case 2:
+ consume = 2 * 3;
+ break;
+ default:
+ return PNGLE_ERROR(
+ "tRNS chunk is prohibited on the color type");
+ }
+ if (len < consume) return 0;
+
+ memcpy(pngle->trans_palette + pngle->n_trans_palettes, buf,
+ consume);
+
+ pngle->n_trans_palettes++;
+
+ break;
+
+ case PNGLE_CHUNK_gAMA:
+ consume = 4;
+ if (len < consume) return 0;
+
+ if (setup_gamma_table(pngle, read_uint32(buf)) < 0) return -1;
+
+ break;
+
+ default:
+ // unknown chunk
+ consume = len;
+
+ debug_printf("[pngle] Unknown chunk; %zd bytes discarded\n",
+ consume);
+ break;
+ }
+
+ return consume;
+}
+
+static int pngle_feed_internal(pngle_t *pngle, const uint8_t *buf, size_t len) {
+ if (!pngle) return -1;
+
+ switch (pngle->state) {
+ case PNGLE_STATE_ERROR:
+ return -1;
+
+ case PNGLE_STATE_EOF:
+ return len;
+
+ case PNGLE_STATE_INITIAL:
+ // find PNG header
+ if (len < sizeof(png_sig)) return 0;
+
+ if (memcmp(png_sig, buf, sizeof(png_sig)))
+ return PNGLE_ERROR("Incorrect PNG signature");
+
+ debug_printf("[pngle] PNG signature found\n");
+
+ pngle->state = PNGLE_STATE_FIND_CHUNK_HEADER;
+ return sizeof(png_sig);
+
+ case PNGLE_STATE_FIND_CHUNK_HEADER:
+ if (len < 8) return 0;
+
+ pngle->chunk_remain = read_uint32(buf);
+ pngle->chunk_type = read_uint32(buf + 4);
+
+ pngle->crc32 =
+ mz_crc32(MZ_CRC32_INIT, (const mz_uint8 *)(buf + 4), 4);
+
+ debug_printf("[pngle] Chunk '%.4s' len %u\n", buf + 4,
+ pngle->chunk_remain);
+
+ pngle->state = PNGLE_STATE_HANDLE_CHUNK;
+
+ // initialize & sanity check
+ switch (pngle->chunk_type) {
+ case PNGLE_CHUNK_IHDR:
+ if (pngle->chunk_remain != 13)
+ return PNGLE_ERROR("Invalid IHDR chunk size");
+ if (pngle->channels != 0)
+ return PNGLE_ERROR(
+ "Multiple IHDR chunks are not allowed");
+ break;
+
+ case PNGLE_CHUNK_IDAT:
+ if (pngle->chunk_remain <= 0)
+ return PNGLE_ERROR("Invalid IDAT chunk size");
+ if (pngle->channels == 0)
+ return PNGLE_ERROR("No IHDR chunk is found");
+ if (pngle->hdr.color_type == 3 && pngle->palette == NULL)
+ return PNGLE_ERROR("No PLTE chunk is found");
+
+ if (pngle->next_out == NULL) {
+ // Very first IDAT
+ pngle->next_out = pngle->lz_buf;
+ pngle->avail_out = TINFL_LZ_DICT_SIZE;
+ }
+ break;
+
+ case PNGLE_CHUNK_PLTE:
+ if (pngle->chunk_remain <= 0)
+ return PNGLE_ERROR("Invalid PLTE chunk size");
+ if (pngle->channels == 0)
+ return PNGLE_ERROR("No IHDR chunk is found");
+ if (pngle->palette)
+ return PNGLE_ERROR("Too many PLTE chunk");
+
+ switch (pngle->hdr.color_type) {
+ case 3: // indexed color
+ break;
+ case 2: // truecolor
+ case 6: // truecolor + alpha
+ // suggested palettes
+ break;
+ default:
+ return PNGLE_ERROR(
+ "PLTE chunk is prohibited on the color type");
+ }
+
+ if (pngle->chunk_remain % 3)
+ return PNGLE_ERROR("Invalid PLTE chunk size");
+ if (pngle->chunk_remain / 3 >
+ MIN(256, (1UL << pngle->hdr.depth)))
+ return PNGLE_ERROR("Too many palettes in PLTE");
+ if ((pngle->palette = PNGLE_CALLOC(pngle->chunk_remain / 3,
+ 3, "palette")) == NULL)
+ return PNGLE_ERROR("Insufficient memory");
+ pngle->n_palettes = 0;
+ break;
+
+ case PNGLE_CHUNK_IEND:
+ if (pngle->next_out == NULL)
+ return PNGLE_ERROR("No IDAT chunk is found");
+ if (pngle->chunk_remain > 0)
+ return PNGLE_ERROR("Invalid IEND chunk size");
+ break;
+
+ case PNGLE_CHUNK_tRNS:
+ if (pngle->chunk_remain <= 0)
+ return PNGLE_ERROR("Invalid tRNS chunk size");
+ if (pngle->channels == 0)
+ return PNGLE_ERROR("No IHDR chunk is found");
+ if (pngle->trans_palette)
+ return PNGLE_ERROR("Too many tRNS chunk");
+
+ switch (pngle->hdr.color_type) {
+ case 3: // indexed color
+ if (pngle->chunk_remain > (1UL << pngle->hdr.depth))
+ return PNGLE_ERROR("Too many palettes in tRNS");
+ break;
+ case 0: // grayscale
+ if (pngle->chunk_remain != 2)
+ return PNGLE_ERROR("Invalid tRNS chunk size");
+ break;
+ case 2: // truecolor
+ if (pngle->chunk_remain != 6)
+ return PNGLE_ERROR("Invalid tRNS chunk size");
+ break;
+
+ default:
+ return PNGLE_ERROR(
+ "tRNS chunk is prohibited on the color type");
+ }
+ if ((pngle->trans_palette = PNGLE_CALLOC(
+ pngle->chunk_remain, 1, "trans palette")) == NULL)
+ return PNGLE_ERROR("Insufficient memory");
+ pngle->n_trans_palettes = 0;
+ break;
+
+ default:
+ break;
+ }
+
+ return 8;
+
+ case PNGLE_STATE_HANDLE_CHUNK:
+ len = MIN(len, pngle->chunk_remain);
+
+ int consumed = pngle_handle_chunk(pngle, buf, len);
+
+ if (consumed > 0) {
+ if (pngle->chunk_remain < (uint32_t)consumed)
+ return PNGLE_ERROR("Chunk data has been consumed too much");
+
+ pngle->chunk_remain -= consumed;
+ pngle->crc32 =
+ mz_crc32(pngle->crc32, (const mz_uint8 *)buf, consumed);
+ }
+ if (pngle->chunk_remain <= 0) pngle->state = PNGLE_STATE_CRC;
+
+ return consumed;
+
+ case PNGLE_STATE_CRC:
+ if (len < 4) return 0;
+
+ uint32_t crc32 = read_uint32(buf);
+
+ if (crc32 != pngle->crc32) {
+ debug_printf("[pngle] CRC: %08x vs %08x => NG\n", crc32,
+ (uint32_t)pngle->crc32);
+ return PNGLE_ERROR("CRC mismatch");
+ }
+
+ debug_printf("[pngle] CRC: %08x vs %08x => OK\n", crc32,
+ (uint32_t)pngle->crc32);
+ pngle->state = PNGLE_STATE_FIND_CHUNK_HEADER;
+
+ // XXX:
+ if (pngle->chunk_type == PNGLE_CHUNK_IEND) {
+ pngle->state = PNGLE_STATE_EOF;
+ if (pngle->done_callback) pngle->done_callback(pngle);
+ debug_printf("[pngle] DONE\n");
+ }
+
+ return 4;
+
+ default:
+ break;
+ }
+
+ return PNGLE_ERROR("Invalid state");
+}
+
+int pngle_feed(pngle_t *pngle, const void *buf, size_t len) {
+ size_t pos = 0;
+ pngle_state_t last_state = pngle->state;
+
+ while (pos < len) {
+ int r =
+ pngle_feed_internal(pngle, (const uint8_t *)buf + pos, len - pos);
+ if (r < 0) return r; // error
+
+ if (r == 0 && last_state == pngle->state) break;
+ last_state = pngle->state;
+
+ pos += r;
+ }
+
+ return pos;
+}
+
+void pngle_set_display_gamma(pngle_t *pngle, double display_gamma) {
+ if (!pngle) return;
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ pngle->display_gamma = display_gamma;
+#else
+ PNGLE_UNUSED(display_gamma);
+#endif
+}
+
+void pngle_set_init_callback(pngle_t *pngle, pngle_init_callback_t callback) {
+ if (!pngle) return;
+ pngle->init_callback = callback;
+}
+
+void pngle_set_draw_callback(pngle_t *pngle, pngle_draw_callback_t callback) {
+ if (!pngle) return;
+ pngle->draw_callback = callback;
+}
+
+void pngle_set_done_callback(pngle_t *pngle, pngle_done_callback_t callback) {
+ if (!pngle) return;
+ pngle->done_callback = callback;
+}
+
+void pngle_set_user_data(pngle_t *pngle, void *user_data) {
+ if (!pngle) return;
+ pngle->user_data = user_data;
+}
+
+void *pngle_get_user_data(pngle_t *pngle) {
+ if (!pngle) return NULL;
+ return pngle->user_data;
+}
+
+/* vim: set ts=4 sw=4 noexpandtab: */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/pngle.h b/lib/M5Core2/src/utility/pngle.h
new file mode 100644
index 000000000..b0fe47518
--- /dev/null
+++ b/lib/M5Core2/src/utility/pngle.h
@@ -0,0 +1,94 @@
+#if defined (CORE2)
+/*-
+ * MIT License
+ *
+ * Copyright (c) 2019 kikuchan
+ *
+ * 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.
+ */
+
+#ifndef __PNGLE_H__
+#define __PNGLE_H__
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Main Pngle object
+typedef struct _pngle_t pngle_t;
+
+// Callback signatures
+typedef void (*pngle_init_callback_t)(pngle_t *pngle, uint32_t w, uint32_t h);
+typedef void (*pngle_draw_callback_t)(pngle_t *pngle, uint32_t x, uint32_t y,
+ uint32_t w, uint32_t h, uint8_t rgba[4]);
+typedef void (*pngle_done_callback_t)(pngle_t *pngle);
+
+// ----------------
+// Basic interfaces
+// ----------------
+pngle_t *pngle_new();
+void pngle_destroy(pngle_t *pngle);
+void pngle_reset(pngle_t *pngle); // clear its internal state (not applied to
+ // pngle_set_* functions)
+const char *pngle_error(pngle_t *pngle);
+int pngle_feed(
+ pngle_t *pngle, const void *buf,
+ size_t len); // returns -1: On error, 0: Need more data, n: n bytes eaten
+
+uint32_t pngle_get_width(pngle_t *pngle);
+uint32_t pngle_get_height(pngle_t *pngle);
+
+void pngle_set_init_callback(pngle_t *png, pngle_init_callback_t callback);
+void pngle_set_draw_callback(pngle_t *png, pngle_draw_callback_t callback);
+void pngle_set_done_callback(pngle_t *png, pngle_done_callback_t callback);
+
+void pngle_set_display_gamma(
+ pngle_t *pngle,
+ double display_gamma); // enables gamma correction by specifying display
+ // gamma, typically 2.2. No effect when gAMA chunk
+ // is missing
+
+void pngle_set_user_data(pngle_t *pngle, void *user_data);
+void *pngle_get_user_data(pngle_t *pngle);
+
+// ----------------
+// Debug interfaces
+// ----------------
+
+typedef struct _pngle_ihdr_t {
+ uint32_t width;
+ uint32_t height;
+ uint8_t depth;
+ uint8_t color_type;
+ uint8_t compression;
+ uint8_t filter;
+ uint8_t interlace;
+} pngle_ihdr_t;
+
+// Get IHDR information
+pngle_ihdr_t *pngle_get_ihdr(pngle_t *pngle);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __PNGLE_H__ */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/qrcode.c b/lib/M5Core2/src/utility/qrcode.c
new file mode 100644
index 000000000..8070282fe
--- /dev/null
+++ b/lib/M5Core2/src/utility/qrcode.c
@@ -0,0 +1,970 @@
+#if defined (CORE2)
+/**
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2017 Richard Moore
+ *
+ * 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.
+ */
+
+/**
+ * Special thanks to Nayuki (https://www.nayuki.io/) from which this library
+ * was heavily inspired and compared against.
+ *
+ * See: https://github.com/nayuki/QR-Code-generator/tree/master/cpp
+ */
+
+#include "qrcode.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+// #pragma mark - Error Correction Lookup tables
+
+#if LOCK_VERSION == 0
+
+static const uint16_t NUM_ERROR_CORRECTION_CODEWORDS[4][40] = {
+ // 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+ // 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30,
+ // 31, 32, 33, 34, 35, 36, 37, 38, 39, 40 Error
+ // correction level
+ {
+ 10, 16, 26, 36, 48, 64, 72, 88, 110, 130,
+ 150, 176, 198, 216, 240, 280, 308, 338, 364, 416,
+ 442, 476, 504, 560, 588, 644, 700, 728, 784, 812,
+ 868, 924, 980, 1036, 1064, 1120, 1204, 1260, 1316, 1372}, // Medium
+ {7, 10, 15, 20, 26, 36, 40, 48, 60, 72, 80, 96, 104, 120,
+ 132, 144, 168, 180, 196, 224, 224, 252, 270, 300, 312, 336, 360, 390,
+ 420, 450, 480, 510, 540, 570, 570, 600, 630, 660, 720, 750}, // Low
+ {17, 28, 44, 64, 88, 112, 130, 156, 192, 224,
+ 264, 308, 352, 384, 432, 480, 532, 588, 650, 700,
+ 750, 816, 900, 960, 1050, 1110, 1200, 1260, 1350, 1440,
+ 1530, 1620, 1710, 1800, 1890, 1980, 2100, 2220, 2310, 2430}, // High
+ {13, 22, 36, 52, 72, 96, 108, 132, 160, 192,
+ 224, 260, 288, 320, 360, 408, 448, 504, 546, 600,
+ 644, 690, 750, 810, 870, 952, 1020, 1050, 1140, 1200,
+ 1290, 1350, 1440, 1530, 1590, 1680, 1770, 1860, 1950, 2040}, // Quartile
+};
+
+static const uint8_t NUM_ERROR_CORRECTION_BLOCKS[4][40] = {
+ // Version: (note that index 0 is for padding, and is set to an illegal
+ // value)
+ // 1, 2, 3, 4, 5, 6, 7, 8, 9,10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21,
+ // 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39,
+ // 40 Error correction level
+ {1, 1, 1, 2, 2, 4, 4, 4, 5, 5, 5, 8, 9, 9,
+ 10, 10, 11, 13, 14, 16, 17, 17, 18, 20, 21, 23, 25, 26,
+ 28, 29, 31, 33, 35, 37, 38, 40, 43, 45, 47, 49}, // Medium
+ {1, 1, 1, 1, 1, 2, 2, 2, 2, 4, 4, 4, 4, 4,
+ 6, 6, 6, 6, 7, 8, 8, 9, 9, 10, 12, 12, 12, 13,
+ 14, 15, 16, 17, 18, 19, 19, 20, 21, 22, 24, 25}, // Low
+ {1, 1, 2, 4, 4, 4, 5, 6, 8, 8, 11, 11, 16, 16,
+ 18, 16, 19, 21, 25, 25, 25, 34, 30, 32, 35, 37, 40, 42,
+ 45, 48, 51, 54, 57, 60, 63, 66, 70, 74, 77, 81}, // High
+ {1, 1, 2, 2, 4, 4, 6, 6, 8, 8, 8, 10, 12, 16,
+ 12, 17, 16, 18, 21, 20, 23, 23, 25, 27, 29, 34, 34, 35,
+ 38, 40, 43, 45, 48, 51, 53, 56, 59, 62, 65, 68}, // Quartile
+};
+
+static const uint16_t NUM_RAW_DATA_MODULES[40] = {
+ // 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13,
+ // 14, 15, 16, 17,
+ 208, 359, 567, 807, 1079, 1383, 1568, 1936, 2336, 2768, 3232, 3728, 4256,
+ 4651, 5243, 5867, 6523,
+ // 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28,
+ // 29, 30, 31,
+ 7211, 7931, 8683, 9252, 10068, 10916, 11796, 12708, 13652, 14628, 15371,
+ 16411, 17483, 18587,
+ // 32, 33, 34, 35, 36, 37, 38, 39, 40
+ 19723, 20891, 22091, 23008, 24272, 25568, 26896, 28256, 29648};
+
+// @TODO: Put other LOCK_VERSIONS here
+#elif LOCK_VERSION == 3
+
+static const int16_t NUM_ERROR_CORRECTION_CODEWORDS[4] = {26, 15, 44, 36};
+
+static const int8_t NUM_ERROR_CORRECTION_BLOCKS[4] = {1, 1, 2, 2};
+
+static const uint16_t NUM_RAW_DATA_MODULES = 567;
+#else
+#error Unsupported LOCK_VERSION (add it...)
+#endif
+
+static int max(int a, int b) {
+ if (a > b) {
+ return a;
+ }
+ return b;
+}
+
+/*
+static int abs(int value) {
+ if (value < 0) { return -value; }
+ return value;
+}
+*/
+
+// #pragma mark - Mode testing and conversion
+
+static int8_t getAlphanumeric(char c) {
+ if (c >= '0' && c <= '9') {
+ return (c - '0');
+ }
+ if (c >= 'A' && c <= 'Z') {
+ return (c - 'A' + 10);
+ }
+
+ switch (c) {
+ case ' ':
+ return 36;
+ case '$':
+ return 37;
+ case '%':
+ return 38;
+ case '*':
+ return 39;
+ case '+':
+ return 40;
+ case '-':
+ return 41;
+ case '.':
+ return 42;
+ case '/':
+ return 43;
+ case ':':
+ return 44;
+ }
+ return -1;
+}
+
+static bool isAlphanumeric(const char *text, uint16_t length) {
+ while (length != 0) {
+ if (getAlphanumeric(text[--length]) == -1) {
+ return false;
+ }
+ }
+ return true;
+}
+
+static bool isNumeric(const char *text, uint16_t length) {
+ while (length != 0) {
+ char c = text[--length];
+ if (c < '0' || c > '9') {
+ return false;
+ }
+ }
+ return true;
+}
+
+// #pragma mark - Counting
+
+// We store the following tightly packed (less 8) in modeInfo
+// <=9 <=26 <= 40
+// NUMERIC ( 10, 12, 14);
+// ALPHANUMERIC ( 9, 11, 13);
+// BYTE ( 8, 16, 16);
+static char getModeBits(uint8_t version, uint8_t mode) {
+ // Note: We use 15 instead of 16; since 15 doesn't exist and we cannot store
+ // 16 (8 + 8) in 3 bits hex(int("".join(reversed([('00' + bin(x -
+ // 8)[2:])[-3:] for x in [10, 9, 8, 12, 11, 15, 14, 13, 15]])), 2))
+ unsigned int modeInfo = 0x7bbb80a;
+
+#if LOCK_VERSION == 0 || LOCK_VERSION > 9
+ if (version > 9) {
+ modeInfo >>= 9;
+ }
+#endif
+
+#if LOCK_VERSION == 0 || LOCK_VERSION > 26
+ if (version > 26) {
+ modeInfo >>= 9;
+ }
+#endif
+
+ char result = 8 + ((modeInfo >> (3 * mode)) & 0x07);
+ if (result == 15) {
+ result = 16;
+ }
+ return result;
+}
+
+// #pragma mark - BitBucket
+typedef struct BitBucket {
+ uint32_t bitOffsetOrWidth;
+ uint16_t capacityBytes;
+ uint8_t *data;
+} BitBucket;
+
+/*
+void bb_dump(BitBucket *bitBuffer) {
+ printf("Buffer: ");
+ for (uint32_t i = 0; i < bitBuffer->capacityBytes; i++) {
+ printf("%02x", bitBuffer->data[i]);
+ if ((i % 4) == 3) { printf(" "); }
+ }
+ printf("\n");
+}
+*/
+
+static uint16_t bb_getGridSizeBytes(uint8_t size) {
+ return (((size * size) + 7) / 8);
+}
+
+static uint16_t bb_getBufferSizeBytes(uint32_t bits) {
+ return ((bits + 7) / 8);
+}
+
+static void bb_initBuffer(BitBucket *bitBuffer, uint8_t *data,
+ int32_t capacityBytes) {
+ bitBuffer->bitOffsetOrWidth = 0;
+ bitBuffer->capacityBytes = capacityBytes;
+ bitBuffer->data = data;
+
+ memset(data, 0, bitBuffer->capacityBytes);
+}
+
+static void bb_initGrid(BitBucket *bitGrid, uint8_t *data, uint8_t size) {
+ bitGrid->bitOffsetOrWidth = size;
+ bitGrid->capacityBytes = bb_getGridSizeBytes(size);
+ bitGrid->data = data;
+ memset(data, 0, bitGrid->capacityBytes);
+}
+
+static void bb_appendBits(BitBucket *bitBuffer, uint32_t val, uint8_t length) {
+ uint32_t offset = bitBuffer->bitOffsetOrWidth;
+ for (int8_t i = length - 1; i >= 0; i--, offset++) {
+ bitBuffer->data[offset >> 3] |= ((val >> i) & 1) << (7 - (offset & 7));
+ }
+ bitBuffer->bitOffsetOrWidth = offset;
+}
+/*
+void bb_setBits(BitBucket *bitBuffer, uint32_t val, int offset, uint8_t length)
+{ for (int8_t i = length - 1; i >= 0; i--, offset++) { bitBuffer->data[offset >>
+3] |= ((val >> i) & 1) << (7 - (offset & 7));
+ }
+}
+*/
+static void bb_setBit(BitBucket *bitGrid, uint8_t x, uint8_t y, bool on) {
+ uint32_t offset = y * bitGrid->bitOffsetOrWidth + x;
+ uint8_t mask = 1 << (7 - (offset & 0x07));
+ if (on) {
+ bitGrid->data[offset >> 3] |= mask;
+ } else {
+ bitGrid->data[offset >> 3] &= ~mask;
+ }
+}
+
+static void bb_invertBit(BitBucket *bitGrid, uint8_t x, uint8_t y,
+ bool invert) {
+ uint32_t offset = y * bitGrid->bitOffsetOrWidth + x;
+ uint8_t mask = 1 << (7 - (offset & 0x07));
+ bool on =
+ ((bitGrid->data[offset >> 3] & (1 << (7 - (offset & 0x07)))) != 0);
+ if (on ^ invert) {
+ bitGrid->data[offset >> 3] |= mask;
+ } else {
+ bitGrid->data[offset >> 3] &= ~mask;
+ }
+}
+
+static bool bb_getBit(BitBucket *bitGrid, uint8_t x, uint8_t y) {
+ uint32_t offset = y * bitGrid->bitOffsetOrWidth + x;
+ return (bitGrid->data[offset >> 3] & (1 << (7 - (offset & 0x07)))) != 0;
+}
+
+// #pragma mark - Drawing Patterns
+
+// XORs the data modules in this QR Code with the given mask pattern. Due to
+// XOR's mathematical properties, calling applyMask(m) twice with the same value
+// is equivalent to no change at all. This means it is possible to apply a mask,
+// undo it, and try another mask. Note that a final well-formed QR Code symbol
+// needs exactly one mask applied (not zero, not two, etc.).
+static void applyMask(BitBucket *modules, BitBucket *isFunction, uint8_t mask) {
+ uint8_t size = modules->bitOffsetOrWidth;
+
+ for (uint8_t y = 0; y < size; y++) {
+ for (uint8_t x = 0; x < size; x++) {
+ if (bb_getBit(isFunction, x, y)) {
+ continue;
+ }
+
+ bool invert = 0;
+ switch (mask) {
+ case 0:
+ invert = (x + y) % 2 == 0;
+ break;
+ case 1:
+ invert = y % 2 == 0;
+ break;
+ case 2:
+ invert = x % 3 == 0;
+ break;
+ case 3:
+ invert = (x + y) % 3 == 0;
+ break;
+ case 4:
+ invert = (x / 3 + y / 2) % 2 == 0;
+ break;
+ case 5:
+ invert = x * y % 2 + x * y % 3 == 0;
+ break;
+ case 6:
+ invert = (x * y % 2 + x * y % 3) % 2 == 0;
+ break;
+ case 7:
+ invert = ((x + y) % 2 + x * y % 3) % 2 == 0;
+ break;
+ }
+ bb_invertBit(modules, x, y, invert);
+ }
+ }
+}
+
+static void setFunctionModule(BitBucket *modules, BitBucket *isFunction,
+ uint8_t x, uint8_t y, bool on) {
+ bb_setBit(modules, x, y, on);
+ bb_setBit(isFunction, x, y, true);
+}
+
+// Draws a 9*9 finder pattern including the border separator, with the center
+// module at (x, y).
+static void drawFinderPattern(BitBucket *modules, BitBucket *isFunction,
+ uint8_t x, uint8_t y) {
+ uint8_t size = modules->bitOffsetOrWidth;
+
+ for (int8_t i = -4; i <= 4; i++) {
+ for (int8_t j = -4; j <= 4; j++) {
+ uint8_t dist = max(abs(i), abs(j)); // Chebyshev/infinity norm
+ int16_t xx = x + j, yy = y + i;
+ if (0 <= xx && xx < size && 0 <= yy && yy < size) {
+ setFunctionModule(modules, isFunction, xx, yy,
+ dist != 2 && dist != 4);
+ }
+ }
+ }
+}
+
+// Draws a 5*5 alignment pattern, with the center module at (x, y).
+static void drawAlignmentPattern(BitBucket *modules, BitBucket *isFunction,
+ uint8_t x, uint8_t y) {
+ for (int8_t i = -2; i <= 2; i++) {
+ for (int8_t j = -2; j <= 2; j++) {
+ setFunctionModule(modules, isFunction, x + j, y + i,
+ max(abs(i), abs(j)) != 1);
+ }
+ }
+}
+
+// Draws two copies of the format bits (with its own error correction code)
+// based on the given mask and this object's error correction level field.
+static void drawFormatBits(BitBucket *modules, BitBucket *isFunction,
+ uint8_t ecc, uint8_t mask) {
+ uint8_t size = modules->bitOffsetOrWidth;
+
+ // Calculate error correction code and pack bits
+ uint32_t data = ecc << 3 | mask; // errCorrLvl is uint2, mask is uint3
+ uint32_t rem = data;
+ for (int i = 0; i < 10; i++) {
+ rem = (rem << 1) ^ ((rem >> 9) * 0x537);
+ }
+
+ data = data << 10 | rem;
+ data ^= 0x5412; // uint15
+
+ // Draw first copy
+ for (uint8_t i = 0; i <= 5; i++) {
+ setFunctionModule(modules, isFunction, 8, i, ((data >> i) & 1) != 0);
+ }
+
+ setFunctionModule(modules, isFunction, 8, 7, ((data >> 6) & 1) != 0);
+ setFunctionModule(modules, isFunction, 8, 8, ((data >> 7) & 1) != 0);
+ setFunctionModule(modules, isFunction, 7, 8, ((data >> 8) & 1) != 0);
+
+ for (int8_t i = 9; i < 15; i++) {
+ setFunctionModule(modules, isFunction, 14 - i, 8,
+ ((data >> i) & 1) != 0);
+ }
+
+ // Draw second copy
+ for (int8_t i = 0; i <= 7; i++) {
+ setFunctionModule(modules, isFunction, size - 1 - i, 8,
+ ((data >> i) & 1) != 0);
+ }
+
+ for (int8_t i = 8; i < 15; i++) {
+ setFunctionModule(modules, isFunction, 8, size - 15 + i,
+ ((data >> i) & 1) != 0);
+ }
+
+ setFunctionModule(modules, isFunction, 8, size - 8, true);
+}
+
+// Draws two copies of the version bits (with its own error correction code),
+// based on this object's version field (which only has an effect for 7 <=
+// version <= 40).
+static void drawVersion(BitBucket *modules, BitBucket *isFunction,
+ uint8_t version) {
+ int8_t size = modules->bitOffsetOrWidth;
+
+#if LOCK_VERSION != 0 && LOCK_VERSION < 7
+ return;
+#else
+ if (version < 7) {
+ return;
+ }
+
+ // Calculate error correction code and pack bits
+ uint32_t rem = version; // version is uint6, in the range [7, 40]
+ for (uint8_t i = 0; i < 12; i++) {
+ rem = (rem << 1) ^ ((rem >> 11) * 0x1F25);
+ }
+
+ uint32_t data = version << 12 | rem; // uint18
+
+ // Draw two copies
+ for (uint8_t i = 0; i < 18; i++) {
+ bool bit = ((data >> i) & 1) != 0;
+ uint8_t a = size - 11 + i % 3, b = i / 3;
+ setFunctionModule(modules, isFunction, a, b, bit);
+ setFunctionModule(modules, isFunction, b, a, bit);
+ }
+#endif
+}
+
+static void drawFunctionPatterns(BitBucket *modules, BitBucket *isFunction,
+ uint8_t version, uint8_t ecc) {
+ uint8_t size = modules->bitOffsetOrWidth;
+
+ // Draw the horizontal and vertical timing patterns
+ for (uint8_t i = 0; i < size; i++) {
+ setFunctionModule(modules, isFunction, 6, i, i % 2 == 0);
+ setFunctionModule(modules, isFunction, i, 6, i % 2 == 0);
+ }
+
+ // Draw 3 finder patterns (all corners except bottom right; overwrites some
+ // timing modules)
+ drawFinderPattern(modules, isFunction, 3, 3);
+ drawFinderPattern(modules, isFunction, size - 4, 3);
+ drawFinderPattern(modules, isFunction, 3, size - 4);
+
+#if LOCK_VERSION == 0 || LOCK_VERSION > 1
+
+ if (version > 1) {
+ // Draw the numerous alignment patterns
+
+ uint8_t alignCount = version / 7 + 2;
+ uint8_t step;
+ if (version != 32) {
+ step = (version * 4 + alignCount * 2 + 1) / (2 * alignCount - 2) *
+ 2; // ceil((size - 13) / (2*numAlign - 2)) * 2
+ } else { // C-C-C-Combo breaker!
+ step = 26;
+ }
+
+ uint8_t alignPositionIndex = alignCount - 1;
+ uint8_t alignPosition[alignCount];
+
+ alignPosition[0] = 6;
+
+ uint8_t size = version * 4 + 17;
+ for (uint8_t i = 0, pos = size - 7; i < alignCount - 1;
+ i++, pos -= step) {
+ alignPosition[alignPositionIndex--] = pos;
+ }
+
+ for (uint8_t i = 0; i < alignCount; i++) {
+ for (uint8_t j = 0; j < alignCount; j++) {
+ if ((i == 0 && j == 0) || (i == 0 && j == alignCount - 1) ||
+ (i == alignCount - 1 && j == 0)) {
+ continue; // Skip the three finder corners
+ } else {
+ drawAlignmentPattern(modules, isFunction, alignPosition[i],
+ alignPosition[j]);
+ }
+ }
+ }
+ }
+#endif
+
+ // Draw configuration data
+ drawFormatBits(
+ modules, isFunction, ecc,
+ 0); // Dummy mask value; overwritten later in the constructor
+ drawVersion(modules, isFunction, version);
+}
+
+// Draws the given sequence of 8-bit codewords (data and error correction) onto
+// the entire data area of this QR Code symbol. Function modules need to be
+// marked off before this is called.
+static void drawCodewords(BitBucket *modules, BitBucket *isFunction,
+ BitBucket *codewords) {
+ uint32_t bitLength = codewords->bitOffsetOrWidth;
+ uint8_t *data = codewords->data;
+
+ uint8_t size = modules->bitOffsetOrWidth;
+
+ // Bit index into the data
+ uint32_t i = 0;
+
+ // Do the funny zigzag scan
+ for (int16_t right = size - 1; right >= 1;
+ right -= 2) { // Index of right column in each column pair
+ if (right == 6) {
+ right = 5;
+ }
+
+ for (uint8_t vert = 0; vert < size; vert++) { // Vertical counter
+ for (int j = 0; j < 2; j++) {
+ uint8_t x = right - j; // Actual x coordinate
+ bool upwards = ((right & 2) == 0) ^ (x < 6);
+ uint8_t y =
+ upwards ? size - 1 - vert : vert; // Actual y coordinate
+ if (!bb_getBit(isFunction, x, y) && i < bitLength) {
+ bb_setBit(modules, x, y,
+ ((data[i >> 3] >> (7 - (i & 7))) & 1) != 0);
+ i++;
+ }
+ // If there are any remainder bits (0 to 7), they are already
+ // set to 0/false/white when the grid of modules was initialized
+ }
+ }
+ }
+}
+
+// #pragma mark - Penalty Calculation
+
+#define PENALTY_N1 3
+#define PENALTY_N2 3
+#define PENALTY_N3 40
+#define PENALTY_N4 10
+
+// Calculates and returns the penalty score based on state of this QR Code's
+// current modules. This is used by the automatic mask choice algorithm to find
+// the mask pattern that yields the lowest score.
+// @TODO: This can be optimized by working with the bytes instead of bits.
+static uint32_t getPenaltyScore(BitBucket *modules) {
+ uint32_t result = 0;
+
+ uint8_t size = modules->bitOffsetOrWidth;
+
+ // Adjacent modules in row having same color
+ for (uint8_t y = 0; y < size; y++) {
+ bool colorX = bb_getBit(modules, 0, y);
+ for (uint8_t x = 1, runX = 1; x < size; x++) {
+ bool cx = bb_getBit(modules, x, y);
+ if (cx != colorX) {
+ colorX = cx;
+ runX = 1;
+ } else {
+ runX++;
+ if (runX == 5) {
+ result += PENALTY_N1;
+ } else if (runX > 5) {
+ result++;
+ }
+ }
+ }
+ }
+
+ // Adjacent modules in column having same color
+ for (uint8_t x = 0; x < size; x++) {
+ bool colorY = bb_getBit(modules, x, 0);
+ for (uint8_t y = 1, runY = 1; y < size; y++) {
+ bool cy = bb_getBit(modules, x, y);
+ if (cy != colorY) {
+ colorY = cy;
+ runY = 1;
+ } else {
+ runY++;
+ if (runY == 5) {
+ result += PENALTY_N1;
+ } else if (runY > 5) {
+ result++;
+ }
+ }
+ }
+ }
+
+ uint16_t black = 0;
+ for (uint8_t y = 0; y < size; y++) {
+ uint16_t bitsRow = 0, bitsCol = 0;
+ for (uint8_t x = 0; x < size; x++) {
+ bool color = bb_getBit(modules, x, y);
+
+ // 2*2 blocks of modules having same color
+ if (x > 0 && y > 0) {
+ bool colorUL = bb_getBit(modules, x - 1, y - 1);
+ bool colorUR = bb_getBit(modules, x, y - 1);
+ bool colorL = bb_getBit(modules, x - 1, y);
+ if (color == colorUL && color == colorUR && color == colorL) {
+ result += PENALTY_N2;
+ }
+ }
+
+ // Finder-like pattern in rows and columns
+ bitsRow = ((bitsRow << 1) & 0x7FF) | color;
+ bitsCol = ((bitsCol << 1) & 0x7FF) | bb_getBit(modules, y, x);
+
+ // Needs 11 bits accumulated
+ if (x >= 10) {
+ if (bitsRow == 0x05D || bitsRow == 0x5D0) {
+ result += PENALTY_N3;
+ }
+ if (bitsCol == 0x05D || bitsCol == 0x5D0) {
+ result += PENALTY_N3;
+ }
+ }
+
+ // Balance of black and white modules
+ if (color) {
+ black++;
+ }
+ }
+ }
+
+ // Find smallest k such that (45-5k)% <= dark/total <= (55+5k)%
+ uint16_t total = size * size;
+ for (uint16_t k = 0;
+ black * 20 < (9 - k) * total || black * 20 > (11 + k) * total; k++) {
+ result += PENALTY_N4;
+ }
+ return result;
+}
+
+// #pragma mark - Reed-Solomon Generator
+
+static uint8_t rs_multiply(uint8_t x, uint8_t y) {
+ // Russian peasant multiplication
+ // See: https://en.wikipedia.org/wiki/Ancient_Egyptian_multiplication
+ uint16_t z = 0;
+ for (int8_t i = 7; i >= 0; i--) {
+ z = (z << 1) ^ ((z >> 7) * 0x11D);
+ z ^= ((y >> i) & 1) * x;
+ }
+ return z;
+}
+
+static void rs_init(uint8_t degree, uint8_t *coeff) {
+ memset(coeff, 0, degree);
+ coeff[degree - 1] = 1;
+
+ // Compute the product polynomial (x - r^0) * (x - r^1) * (x - r^2) * ... *
+ // (x - r^{degree-1}), drop the highest term, and store the rest of the
+ // coefficients in order of descending powers. Note that r = 0x02, which is
+ // a generator element of this field GF(2^8/0x11D).
+ uint16_t root = 1;
+ for (uint8_t i = 0; i < degree; i++) {
+ // Multiply the current product by (x - r^i)
+ for (uint8_t j = 0; j < degree; j++) {
+ coeff[j] = rs_multiply(coeff[j], root);
+ if (j + 1 < degree) {
+ coeff[j] ^= coeff[j + 1];
+ }
+ }
+ root = (root << 1) ^
+ ((root >> 7) * 0x11D); // Multiply by 0x02 mod GF(2^8/0x11D)
+ }
+}
+
+static void rs_getRemainder(uint8_t degree, uint8_t *coeff, uint8_t *data,
+ uint8_t length, uint8_t *result, uint8_t stride) {
+ // Compute the remainder by performing polynomial division
+
+ // for (uint8_t i = 0; i < degree; i++) { result[] = 0; }
+ // memset(result, 0, degree);
+
+ for (uint8_t i = 0; i < length; i++) {
+ uint8_t factor = data[i] ^ result[0];
+ for (uint8_t j = 1; j < degree; j++) {
+ result[(j - 1) * stride] = result[j * stride];
+ }
+ result[(degree - 1) * stride] = 0;
+
+ for (uint8_t j = 0; j < degree; j++) {
+ result[j * stride] ^= rs_multiply(coeff[j], factor);
+ }
+ }
+}
+
+// #pragma mark - QrCode
+
+static int8_t encodeDataCodewords(BitBucket *dataCodewords, const uint8_t *text,
+ uint16_t length, uint8_t version) {
+ int8_t mode = MODE_BYTE;
+
+ if (isNumeric((char *)text, length)) {
+ mode = MODE_NUMERIC;
+ bb_appendBits(dataCodewords, 1 << MODE_NUMERIC, 4);
+ bb_appendBits(dataCodewords, length,
+ getModeBits(version, MODE_NUMERIC));
+
+ uint16_t accumData = 0;
+ uint8_t accumCount = 0;
+ for (uint16_t i = 0; i < length; i++) {
+ accumData = accumData * 10 + ((char)(text[i]) - '0');
+ accumCount++;
+ if (accumCount == 3) {
+ bb_appendBits(dataCodewords, accumData, 10);
+ accumData = 0;
+ accumCount = 0;
+ }
+ }
+
+ // 1 or 2 digits remaining
+ if (accumCount > 0) {
+ bb_appendBits(dataCodewords, accumData, accumCount * 3 + 1);
+ }
+
+ } else if (isAlphanumeric((char *)text, length)) {
+ mode = MODE_ALPHANUMERIC;
+ bb_appendBits(dataCodewords, 1 << MODE_ALPHANUMERIC, 4);
+ bb_appendBits(dataCodewords, length,
+ getModeBits(version, MODE_ALPHANUMERIC));
+
+ uint16_t accumData = 0;
+ uint8_t accumCount = 0;
+ for (uint16_t i = 0; i < length; i++) {
+ accumData = accumData * 45 + getAlphanumeric((char)(text[i]));
+ accumCount++;
+ if (accumCount == 2) {
+ bb_appendBits(dataCodewords, accumData, 11);
+ accumData = 0;
+ accumCount = 0;
+ }
+ }
+
+ // 1 character remaining
+ if (accumCount > 0) {
+ bb_appendBits(dataCodewords, accumData, 6);
+ }
+
+ } else {
+ bb_appendBits(dataCodewords, 1 << MODE_BYTE, 4);
+ bb_appendBits(dataCodewords, length, getModeBits(version, MODE_BYTE));
+ for (uint16_t i = 0; i < length; i++) {
+ bb_appendBits(dataCodewords, (char)(text[i]), 8);
+ }
+ }
+ // bb_setBits(dataCodewords, length, 4, getModeBits(version, mode));
+ return mode;
+}
+
+static void performErrorCorrection(uint8_t version, uint8_t ecc,
+ BitBucket *data) {
+ // See: http://www.thonky.com/qr-code-tutorial/structure-final-message
+
+#if LOCK_VERSION == 0
+ uint8_t numBlocks = NUM_ERROR_CORRECTION_BLOCKS[ecc][version - 1];
+ uint16_t totalEcc = NUM_ERROR_CORRECTION_CODEWORDS[ecc][version - 1];
+ uint16_t moduleCount = NUM_RAW_DATA_MODULES[version - 1];
+#else
+ uint8_t numBlocks = NUM_ERROR_CORRECTION_BLOCKS[ecc];
+ uint16_t totalEcc = NUM_ERROR_CORRECTION_CODEWORDS[ecc];
+ uint16_t moduleCount = NUM_RAW_DATA_MODULES;
+#endif
+
+ uint8_t blockEccLen = totalEcc / numBlocks;
+ uint8_t numShortBlocks = numBlocks - moduleCount / 8 % numBlocks;
+ uint8_t shortBlockLen = moduleCount / 8 / numBlocks;
+
+ uint8_t shortDataBlockLen = shortBlockLen - blockEccLen;
+
+ uint8_t result[data->capacityBytes];
+ memset(result, 0, sizeof(result));
+
+ uint8_t coeff[blockEccLen];
+ rs_init(blockEccLen, coeff);
+
+ uint16_t offset = 0;
+ uint8_t *dataBytes = data->data;
+
+ // Interleave all short blocks
+ for (uint8_t i = 0; i < shortDataBlockLen; i++) {
+ uint16_t index = i;
+ uint8_t stride = shortDataBlockLen;
+ for (uint8_t blockNum = 0; blockNum < numBlocks; blockNum++) {
+ result[offset++] = dataBytes[index];
+
+#if LOCK_VERSION == 0 || LOCK_VERSION >= 5
+ if (blockNum == numShortBlocks) {
+ stride++;
+ }
+#endif
+ index += stride;
+ }
+ }
+// Version less than 5 only have short blocks
+#if LOCK_VERSION == 0 || LOCK_VERSION >= 5
+ {
+ // Interleave long blocks
+ uint16_t index = shortDataBlockLen * (numShortBlocks + 1);
+ uint8_t stride = shortDataBlockLen;
+ for (uint8_t blockNum = 0; blockNum < numBlocks - numShortBlocks;
+ blockNum++) {
+ result[offset++] = dataBytes[index];
+
+ if (blockNum == 0) {
+ stride++;
+ }
+ index += stride;
+ }
+ }
+#endif
+
+ // Add all ecc blocks, interleaved
+ uint8_t blockSize = shortDataBlockLen;
+ for (uint8_t blockNum = 0; blockNum < numBlocks; blockNum++) {
+#if LOCK_VERSION == 0 || LOCK_VERSION >= 5
+ if (blockNum == numShortBlocks) {
+ blockSize++;
+ }
+#endif
+ rs_getRemainder(blockEccLen, coeff, dataBytes, blockSize,
+ &result[offset + blockNum], numBlocks);
+ dataBytes += blockSize;
+ }
+
+ memcpy(data->data, result, data->capacityBytes);
+ data->bitOffsetOrWidth = moduleCount;
+}
+
+// We store the Format bits tightly packed into a single byte (each of the 4
+// modes is 2 bits) The format bits can be determined by ECC_FORMAT_BITS >> (2 *
+// ecc)
+static const uint8_t ECC_FORMAT_BITS =
+ (0x02 << 6) | (0x03 << 4) | (0x00 << 2) | (0x01 << 0);
+
+// #pragma mark - Public QRCode functions
+
+uint16_t qrcode_getBufferSize(uint8_t version) {
+ return bb_getGridSizeBytes(4 * version + 17);
+}
+
+// @TODO: Return error if data is too big.
+int8_t qrcode_initBytes(QRCode *qrcode, uint8_t *modules, uint8_t version,
+ uint8_t ecc, uint8_t *data, uint16_t length) {
+ uint8_t size = version * 4 + 17;
+ qrcode->version = version;
+ qrcode->size = size;
+ qrcode->ecc = ecc;
+ qrcode->modules = modules;
+
+ uint8_t eccFormatBits = (ECC_FORMAT_BITS >> (2 * ecc)) & 0x03;
+
+#if LOCK_VERSION == 0
+ uint16_t moduleCount = NUM_RAW_DATA_MODULES[version - 1];
+ uint16_t dataCapacity =
+ moduleCount / 8 -
+ NUM_ERROR_CORRECTION_CODEWORDS[eccFormatBits][version - 1];
+#else
+ version = LOCK_VERSION;
+ uint16_t moduleCount = NUM_RAW_DATA_MODULES;
+ uint16_t dataCapacity =
+ moduleCount / 8 - NUM_ERROR_CORRECTION_CODEWORDS[eccFormatBits];
+#endif
+
+ struct BitBucket codewords;
+ uint8_t codewordBytes[bb_getBufferSizeBytes(moduleCount)];
+ bb_initBuffer(&codewords, codewordBytes, (int32_t)sizeof(codewordBytes));
+
+ // Place the data code words into the buffer
+ int8_t mode = encodeDataCodewords(&codewords, data, length, version);
+
+ if (mode < 0) {
+ return -1;
+ }
+ qrcode->mode = mode;
+
+ // Add terminator and pad up to a byte if applicable
+ uint32_t padding = (dataCapacity * 8) - codewords.bitOffsetOrWidth;
+ if (padding > 4) {
+ padding = 4;
+ }
+ bb_appendBits(&codewords, 0, padding);
+ bb_appendBits(&codewords, 0, (8 - codewords.bitOffsetOrWidth % 8) % 8);
+
+ // Pad with alternate bytes until data capacity is reached
+ for (uint8_t padByte = 0xEC;
+ codewords.bitOffsetOrWidth < (dataCapacity * 8);
+ padByte ^= 0xEC ^ 0x11) {
+ bb_appendBits(&codewords, padByte, 8);
+ }
+
+ BitBucket modulesGrid;
+ bb_initGrid(&modulesGrid, modules, size);
+
+ BitBucket isFunctionGrid;
+ uint8_t isFunctionGridBytes[bb_getGridSizeBytes(size)];
+ bb_initGrid(&isFunctionGrid, isFunctionGridBytes, size);
+
+ // Draw function patterns, draw all codewords, do masking
+ drawFunctionPatterns(&modulesGrid, &isFunctionGrid, version, eccFormatBits);
+ performErrorCorrection(version, eccFormatBits, &codewords);
+ drawCodewords(&modulesGrid, &isFunctionGrid, &codewords);
+
+ // Find the best (lowest penalty) mask
+ uint8_t mask = 0;
+ int32_t minPenalty = INT32_MAX;
+ for (uint8_t i = 0; i < 8; i++) {
+ drawFormatBits(&modulesGrid, &isFunctionGrid, eccFormatBits, i);
+ applyMask(&modulesGrid, &isFunctionGrid, i);
+ int penalty = getPenaltyScore(&modulesGrid);
+ if (penalty < minPenalty) {
+ mask = i;
+ minPenalty = penalty;
+ }
+ applyMask(&modulesGrid, &isFunctionGrid,
+ i); // Undoes the mask due to XOR
+ }
+
+ qrcode->mask = mask;
+
+ // Overwrite old format bits
+ drawFormatBits(&modulesGrid, &isFunctionGrid, eccFormatBits, mask);
+
+ // Apply the final choice of mask
+ applyMask(&modulesGrid, &isFunctionGrid, mask);
+
+ return 0;
+}
+
+int8_t qrcode_initText(QRCode *qrcode, uint8_t *modules, uint8_t version,
+ uint8_t ecc, const char *data) {
+ return qrcode_initBytes(qrcode, modules, version, ecc, (uint8_t *)data,
+ strlen(data));
+}
+
+bool qrcode_getModule(QRCode *qrcode, uint8_t x, uint8_t y) {
+ if (x >= qrcode->size || y >= qrcode->size) {
+ return false;
+ }
+ uint32_t offset = y * qrcode->size + x;
+ return (qrcode->modules[offset >> 3] & (1 << (7 - (offset & 0x07)))) != 0;
+}
+
+/*
+uint8_t qrcode_getHexLength(QRCode *qrcode) {
+ return ((qrcode->size * qrcode->size) + 7) / 4;
+}
+
+void qrcode_getHex(QRCode *qrcode, char *result) {
+
+}
+*/
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/qrcode.h b/lib/M5Core2/src/utility/qrcode.h
new file mode 100644
index 000000000..6201d86cc
--- /dev/null
+++ b/lib/M5Core2/src/utility/qrcode.h
@@ -0,0 +1,88 @@
+#if defined (CORE2)
+/**
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2017 Richard Moore
+ *
+ * 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.
+ */
+
+/**
+ * Special thanks to Nayuki (https://www.nayuki.io/) from which this library
+ * was heavily inspired and compared against.
+ *
+ * See: https://github.com/nayuki/QR-Code-generator/tree/master/cpp
+ */
+
+#ifndef __QRCODE_H_
+#define __QRCODE_H_
+
+#ifndef __cplusplus
+typedef unsigned char bool;
+static const bool false = 0;
+static const bool true = 1;
+#endif
+
+#include <stdint.h>
+
+// QR Code Format Encoding
+#define MODE_NUMERIC 0
+#define MODE_ALPHANUMERIC 1
+#define MODE_BYTE 2
+
+// Error Correction Code Levels
+#define ECC_LOW 0
+#define ECC_MEDIUM 1
+#define ECC_QUARTILE 2
+#define ECC_HIGH 3
+
+// If set to non-zero, this library can ONLY produce QR codes at that version
+// This saves a lot of dynamic memory, as the codeword tables are skipped
+#ifndef LOCK_VERSION
+#define LOCK_VERSION 0
+#endif
+
+typedef struct QRCode {
+ uint8_t version;
+ uint8_t size;
+ uint8_t ecc;
+ uint8_t mode;
+ uint8_t mask;
+ uint8_t *modules;
+} QRCode;
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+uint16_t qrcode_getBufferSize(uint8_t version);
+
+int8_t qrcode_initText(QRCode *qrcode, uint8_t *modules, uint8_t version,
+ uint8_t ecc, const char *data);
+int8_t qrcode_initBytes(QRCode *qrcode, uint8_t *modules, uint8_t version,
+ uint8_t ecc, uint8_t *data, uint16_t length);
+
+bool qrcode_getModule(QRCode *qrcode, uint8_t x, uint8_t y);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* __QRCODE_H_ */
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/quaternionFilters.cpp b/lib/M5Core2/src/utility/quaternionFilters.cpp
new file mode 100644
index 000000000..127691103
--- /dev/null
+++ b/lib/M5Core2/src/utility/quaternionFilters.cpp
@@ -0,0 +1,275 @@
+#if defined (CORE2)
+// Implementation of Sebastian Madgwick's "...efficient orientation filter
+// for... inertial/magnetic sensor arrays"
+// (see http://www.x-io.co.uk/category/open-source/ for examples & more details)
+// which fuses acceleration, rotation rate, and magnetic moments to produce a
+// quaternion-based estimate of absolute device orientation -- which can be
+// converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
+// The performance of the orientation filter is at least as good as conventional
+// Kalman-based filtering algorithms but is much less computationally
+// intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
+
+#include "quaternionFilters.h"
+
+// These are the free parameters in the Mahony filter and fusion scheme, Kp
+// for proportional feedback, Ki for integral
+#define Kp 2.0f * 5.0f
+#define Ki 0.0f
+
+static float GyroMeasError = PI * (40.0f / 180.0f);
+// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+// static float GyroMeasDrift = PI * (0.0f / 180.0f);
+// There is a tradeoff in the beta parameter between accuracy and response
+// speed. In the original Madgwick study, beta of 0.041 (corresponding to
+// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
+// However, with this value, the LSM9SD0 response time is about 10 seconds
+// to a stable initial quaternion. Subsequent changes also require a
+// longish lag time to a stable output, not fast enough for a quadcopter or
+// robot car! By increasing beta (GyroMeasError) by about a factor of
+// fifteen, the response time constant is reduced to ~2 sec. I haven't
+// noticed any reduction in solution accuracy. This is essentially the I
+// coefficient in a PID control sense; the bigger the feedback coefficient,
+// the faster the solution converges, usually at the expense of accuracy.
+// In any case, this is the free parameter in the Madgwick filtering and
+// fusion scheme.
+static float beta_ = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta
+// Compute zeta, the other free parameter in the Madgwick scheme usually
+// set to a small or zero value
+// static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
+
+// Vector to hold integral error for Mahony method
+static float eInt[3] = {0.0f, 0.0f, 0.0f};
+// Vector to hold quaternion
+static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
+
+void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat) {
+ // short name local variable for readability
+ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
+ float norm;
+ float hx, hy, _2bx, _2bz;
+ float s1, s2, s3, s4;
+ float qDot1, qDot2, qDot3, qDot4;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float _2q1mx;
+ float _2q1my;
+ float _2q1mz;
+ float _2q2mx;
+ float _4bx;
+ float _4bz;
+ float _2q1 = 2.0f * q1;
+ float _2q2 = 2.0f * q2;
+ float _2q3 = 2.0f * q3;
+ float _2q4 = 2.0f * q4;
+ float _2q1q3 = 2.0f * q1 * q3;
+ float _2q3q4 = 2.0f * q3 * q4;
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q1q4 = q1 * q4;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q2q4 = q2 * q4;
+ float q3q3 = q3 * q3;
+ float q3q4 = q3 * q4;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0.0f) {
+ return; // handle NaN
+ }
+ norm = 1.0f / norm;
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Normalise magnetometer measurement
+ norm = sqrt(mx * mx + my * my + mz * mz);
+ if (norm == 0.0f) {
+ return; // handle NaN
+ }
+ norm = 1.0f / norm;
+ mx *= norm;
+ my *= norm;
+ mz *= norm;
+
+ // Reference direction of Earth's magnetic field
+ _2q1mx = 2.0f * q1 * mx;
+ _2q1my = 2.0f * q1 * my;
+ _2q1mz = 2.0f * q1 * mz;
+ _2q2mx = 2.0f * q2 * mx;
+ hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 +
+ _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+ hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 +
+ my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+ _2bx = sqrt(hx * hx + hy * hy);
+ _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 +
+ _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+ _4bx = 2.0f * _2bx;
+ _4bz = 2.0f * _2bz;
+
+ // Gradient decent algorithm corrective step
+ s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q2 * (2.0f * q1q2 + _2q3q4 - ay) -
+ _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (-_2bx * q4 + _2bz * q2) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q1 * (2.0f * q1q2 + _2q3q4 - ay) -
+ 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) +
+ _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (_2bx * q3 + _2bz * q1) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ (_2bx * q4 - _4bz * q2) *
+ (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q4 * (2.0f * q1q2 + _2q3q4 - ay) -
+ 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) +
+ (-_4bx * q3 - _2bz * q1) *
+ (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (_2bx * q2 + _2bz * q4) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ (_2bx * q1 - _4bz * q3) *
+ (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q3 * (2.0f * q1q2 + _2q3q4 - ay) +
+ (-_4bx * q4 + _2bz * q2) *
+ (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (-_2bx * q1 + _2bz * q3) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 +
+ s4 * s4); // normalise step magnitude
+ norm = 1.0f / norm;
+ s1 *= norm;
+ s2 *= norm;
+ s3 *= norm;
+ s4 *= norm;
+
+ // Compute rate of change of quaternion
+ qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta_ * s1;
+ qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta_ * s2;
+ qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta_ * s3;
+ qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta_ * s4;
+
+ // Integrate to yield quaternion
+ q1 += qDot1 * deltat;
+ q2 += qDot2 * deltat;
+ q3 += qDot3 * deltat;
+ q4 += qDot4 * deltat;
+ norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
+ norm = 1.0f / norm;
+ q[0] = q1 * norm;
+ q[1] = q2 * norm;
+ q[2] = q3 * norm;
+ q[3] = q4 * norm;
+}
+
+// Similar to Madgwick scheme but uses proportional and integral filtering on
+// the error between estimated reference vectors and measured ones.
+void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat) {
+ // short name local variable for readability
+ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
+ float norm;
+ float hx, hy, bx, bz;
+ float vx, vy, vz, wx, wy, wz;
+ float ex, ey, ez;
+ float pa, pb, pc;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q1q4 = q1 * q4;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q2q4 = q2 * q4;
+ float q3q3 = q3 * q3;
+ float q3q4 = q3 * q4;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0.0f) {
+ return; // Handle NaN
+ }
+ norm = 1.0f / norm; // Use reciprocal for division
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Normalise magnetometer measurement
+ norm = sqrt(mx * mx + my * my + mz * mz);
+ if (norm == 0.0f) {
+ return; // Handle NaN
+ }
+ norm = 1.0f / norm; // Use reciprocal for division
+ mx *= norm;
+ my *= norm;
+ mz *= norm;
+
+ // Reference direction of Earth's magnetic field
+ hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) +
+ 2.0f * mz * (q2q4 + q1q3);
+ hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) +
+ 2.0f * mz * (q3q4 - q1q2);
+ bx = sqrt((hx * hx) + (hy * hy));
+ bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) +
+ 2.0f * mz * (0.5f - q2q2 - q3q3);
+
+ // Estimated direction of gravity and magnetic field
+ vx = 2.0f * (q2q4 - q1q3);
+ vy = 2.0f * (q1q2 + q3q4);
+ vz = q1q1 - q2q2 - q3q3 + q4q4;
+ wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
+ wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
+ wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
+
+ // Error is cross product between estimated direction and measured direction
+ // of gravity
+ ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+ ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+ ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+ if (Ki > 0.0f) {
+ eInt[0] += ex; // accumulate integral error
+ eInt[1] += ey;
+ eInt[2] += ez;
+ } else {
+ eInt[0] = 0.0f; // prevent integral wind up
+ eInt[1] = 0.0f;
+ eInt[2] = 0.0f;
+ }
+
+ // Apply feedback terms
+ gx = gx + Kp * ex + Ki * eInt[0];
+ gy = gy + Kp * ey + Ki * eInt[1];
+ gz = gz + Kp * ez + Ki * eInt[2];
+
+ // Integrate rate of change of quaternion
+ pa = q2;
+ pb = q3;
+ pc = q4;
+ q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
+ q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
+ q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
+ q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
+
+ // Normalise quaternion
+ norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
+ norm = 1.0f / norm;
+ q[0] = q1 * norm;
+ q[1] = q2 * norm;
+ q[2] = q3 * norm;
+ q[3] = q4 * norm;
+}
+
+const float* getQ() {
+ return q;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Core2/src/utility/quaternionFilters.h b/lib/M5Core2/src/utility/quaternionFilters.h
new file mode 100644
index 000000000..f1d624768
--- /dev/null
+++ b/lib/M5Core2/src/utility/quaternionFilters.h
@@ -0,0 +1,16 @@
+#if defined (CORE2)
+#ifndef _QUATERNIONFILTERS_H_
+#define _QUATERNIONFILTERS_H_
+
+#include <Arduino.h>
+
+void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat);
+void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat);
+const float* getQ();
+
+#endif // _QUATERNIONFILTERS_H_
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/readme.md b/lib/M5Stack/readme.md
new file mode 100644
index 000000000..27b172825
--- /dev/null
+++ b/lib/M5Stack/readme.md
@@ -0,0 +1,8 @@
+Library from https://github.com/m5stack/M5Stack
+
+Changed to use TFT_eSPI from this project
+
+Delete all other files to save space and avoid Class redeclarations.
+
+
+
diff --git a/lib/M5Stack/src/Free_Fonts.h b/lib/M5Stack/src/Free_Fonts.h
new file mode 100644
index 000000000..38e88d372
--- /dev/null
+++ b/lib/M5Stack/src/Free_Fonts.h
@@ -0,0 +1,381 @@
+// Attach this header file to your sketch to use the GFX Free Fonts. You can
+// write sketches without it, but it makes referencing them easier.
+
+// This calls up ALL the fonts but they only get loaded if you actually
+// use them in your sketch.
+//
+// No changes are needed to this header file unless new fonts are added to the
+// library "Fonts/GFXFF" folder.
+//
+// To save a lot of typing long names, each font can easily be referenced in the
+// sketch in three ways, either with:
+//
+// 1. Font file name with the & in front such as &FreeSansBoldOblique24pt7b
+// an example being:
+//
+// tft.setFreeFont(&FreeSansBoldOblique24pt7b);
+//
+// 2. FF# where # is a number determined by looking at the list below
+// an example being:
+//
+// tft.setFreeFont(FF32);
+//
+// 3. An abbreviation of the file name. Look at the list below to see
+// the abbreviations used, for example:
+//
+// tft.setFreeFont(FSSBO24)
+//
+// Where the letters mean:
+// F = Free font
+// M = Mono
+// SS = Sans Serif (double S to distinguish is form serif fonts)
+// S = Serif
+// B = Bold
+// O = Oblique (letter O not zero)
+// I = Italic
+// # = point size, either 9, 12, 18 or 24
+//
+// Setting the font to NULL will select the GLCD font:
+//
+// tft.setFreeFont(NULL); // Set font to GLCD
+
+#define LOAD_GFXFF
+
+#ifdef LOAD_GFXFF // Only include the fonts if LOAD_GFXFF is defined in
+ // User_Setup.h
+
+// Use these when printing or drawing text in GLCD and high rendering speed
+// fonts
+#define GFXFF 1
+#define GLCD 0
+#define FONT2 2
+#define FONT4 4
+#define FONT6 6
+#define FONT7 7
+#define FONT8 8
+
+// Use the following when calling setFont()
+//
+// Reserved for GLCD font // FF0
+//
+
+#define TT1 &TomThumb
+
+#define FM9 &FreeMono9pt7b
+#define FM12 &FreeMono12pt7b
+#define FM18 &FreeMono18pt7b
+#define FM24 &FreeMono24pt7b
+
+#define FMB9 &FreeMonoBold9pt7b
+#define FMB12 &FreeMonoBold12pt7b
+#define FMB18 &FreeMonoBold18pt7b
+#define FMB24 &FreeMonoBold24pt7b
+
+#define FMO9 &FreeMonoOblique9pt7b
+#define FMO12 &FreeMonoOblique12pt7b
+#define FMO18 &FreeMonoOblique18pt7b
+#define FMO24 &FreeMonoOblique24pt7b
+
+#define FMBO9 &FreeMonoBoldOblique9pt7b
+#define FMBO12 &FreeMonoBoldOblique12pt7b
+#define FMBO18 &FreeMonoBoldOblique18pt7b
+#define FMBO24 &FreeMonoBoldOblique24pt7b
+
+#define FSS9 &FreeSans9pt7b
+#define FSS12 &FreeSans12pt7b
+#define FSS18 &FreeSans18pt7b
+#define FSS24 &FreeSans24pt7b
+
+#define FSSB9 &FreeSansBold9pt7b
+#define FSSB12 &FreeSansBold12pt7b
+#define FSSB18 &FreeSansBold18pt7b
+#define FSSB24 &FreeSansBold24pt7b
+
+#define FSSO9 &FreeSansOblique9pt7b
+#define FSSO12 &FreeSansOblique12pt7b
+#define FSSO18 &FreeSansOblique18pt7b
+#define FSSO24 &FreeSansOblique24pt7b
+
+#define FSSBO9 &FreeSansBoldOblique9pt7b
+#define FSSBO12 &FreeSansBoldOblique12pt7b
+#define FSSBO18 &FreeSansBoldOblique18pt7b
+#define FSSBO24 &FreeSansBoldOblique24pt7b
+
+#define FS9 &FreeSerif9pt7b
+#define FS12 &FreeSerif12pt7b
+#define FS18 &FreeSerif18pt7b
+#define FS24 &FreeSerif24pt7b
+
+#define FSI9 &FreeSerifItalic9pt7b
+#define FSI12 &FreeSerifItalic12pt7b
+#define FSI19 &FreeSerifItalic18pt7b
+#define FSI24 &FreeSerifItalic24pt7b
+
+#define FSB9 &FreeSerifBold9pt7b
+#define FSB12 &FreeSerifBold12pt7b
+#define FSB18 &FreeSerifBold18pt7b
+#define FSB24 &FreeSerifBold24pt7b
+
+#define FSBI9 &FreeSerifBoldItalic9pt7b
+#define FSBI12 &FreeSerifBoldItalic12pt7b
+#define FSBI18 &FreeSerifBoldItalic18pt7b
+#define FSBI24 &FreeSerifBoldItalic24pt7b
+
+#define FF0 NULL // ff0 reserved for GLCD
+#define FF1 &FreeMono9pt7b
+#define FF2 &FreeMono12pt7b
+#define FF3 &FreeMono18pt7b
+#define FF4 &FreeMono24pt7b
+
+#define FF5 &FreeMonoBold9pt7b
+#define FF6 &FreeMonoBold12pt7b
+#define FF7 &FreeMonoBold18pt7b
+#define FF8 &FreeMonoBold24pt7b
+
+#define FF9 &FreeMonoOblique9pt7b
+#define FF10 &FreeMonoOblique12pt7b
+#define FF11 &FreeMonoOblique18pt7b
+#define FF12 &FreeMonoOblique24pt7b
+
+#define FF13 &FreeMonoBoldOblique9pt7b
+#define FF14 &FreeMonoBoldOblique12pt7b
+#define FF15 &FreeMonoBoldOblique18pt7b
+#define FF16 &FreeMonoBoldOblique24pt7b
+
+#define FF17 &FreeSans9pt7b
+#define FF18 &FreeSans12pt7b
+#define FF19 &FreeSans18pt7b
+#define FF20 &FreeSans24pt7b
+
+#define FF21 &FreeSansBold9pt7b
+#define FF22 &FreeSansBold12pt7b
+#define FF23 &FreeSansBold18pt7b
+#define FF24 &FreeSansBold24pt7b
+
+#define FF25 &FreeSansOblique9pt7b
+#define FF26 &FreeSansOblique12pt7b
+#define FF27 &FreeSansOblique18pt7b
+#define FF28 &FreeSansOblique24pt7b
+
+#define FF29 &FreeSansBoldOblique9pt7b
+#define FF30 &FreeSansBoldOblique12pt7b
+#define FF31 &FreeSansBoldOblique18pt7b
+#define FF32 &FreeSansBoldOblique24pt7b
+
+#define FF33 &FreeSerif9pt7b
+#define FF34 &FreeSerif12pt7b
+#define FF35 &FreeSerif18pt7b
+#define FF36 &FreeSerif24pt7b
+
+#define FF37 &FreeSerifItalic9pt7b
+#define FF38 &FreeSerifItalic12pt7b
+#define FF39 &FreeSerifItalic18pt7b
+#define FF40 &FreeSerifItalic24pt7b
+
+#define FF41 &FreeSerifBold9pt7b
+#define FF42 &FreeSerifBold12pt7b
+#define FF43 &FreeSerifBold18pt7b
+#define FF44 &FreeSerifBold24pt7b
+
+#define FF45 &FreeSerifBoldItalic9pt7b
+#define FF46 &FreeSerifBoldItalic12pt7b
+#define FF47 &FreeSerifBoldItalic18pt7b
+#define FF48 &FreeSerifBoldItalic24pt7b
+
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+// Now we define "s"tring versions for easy printing of the font name so:
+// tft.println(sFF5);
+// will print
+// Mono bold 9
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+
+#define sFF0 "GLCD"
+#define sTT1 "Tom Thumb"
+#define sFF1 "Mono 9"
+#define sFF2 "Mono 12"
+#define sFF3 "Mono 18"
+#define sFF4 "Mono 24"
+
+#define sFF5 "Mono bold 9"
+#define sFF6 "Mono bold 12"
+#define sFF7 "Mono bold 18"
+#define sFF8 "Mono bold 24"
+
+#define sFF9 "Mono oblique 9"
+#define sFF10 "Mono oblique 12"
+#define sFF11 "Mono oblique 18"
+#define sFF12 "Mono oblique 24"
+
+#define sFF13 "Mono bold oblique 9"
+#define sFF14 "Mono bold oblique 12"
+#define sFF15 "Mono bold oblique 18"
+#define sFF16 \
+ "Mono bold oblique 24" // Full text line is too big for 480 pixel wide
+ // screen
+
+#define sFF17 "Sans 9"
+#define sFF18 "Sans 12"
+#define sFF19 "Sans 18"
+#define sFF20 "Sans 24"
+
+#define sFF21 "Sans bold 9"
+#define sFF22 "Sans bold 12"
+#define sFF23 "Sans bold 18"
+#define sFF24 "Sans bold 24"
+
+#define sFF25 "Sans oblique 9"
+#define sFF26 "Sans oblique 12"
+#define sFF27 "Sans oblique 18"
+#define sFF28 "Sans oblique 24"
+
+#define sFF29 "Sans bold oblique 9"
+#define sFF30 "Sans bold oblique 12"
+#define sFF31 "Sans bold oblique 18"
+#define sFF32 "Sans bold oblique 24"
+
+#define sFF33 "Serif 9"
+#define sFF34 "Serif 12"
+#define sFF35 "Serif 18"
+#define sFF36 "Serif 24"
+
+#define sFF37 "Serif italic 9"
+#define sFF38 "Serif italic 12"
+#define sFF39 "Serif italic 18"
+#define sFF40 "Serif italic 24"
+
+#define sFF41 "Serif bold 9"
+#define sFF42 "Serif bold 12"
+#define sFF43 "Serif bold 18"
+#define sFF44 "Serif bold 24"
+
+#define sFF45 "Serif bold italic 9"
+#define sFF46 "Serif bold italic 12"
+#define sFF47 "Serif bold italic 18"
+#define sFF48 "Serif bold italic 24"
+
+#else // LOAD_GFXFF not defined so setup defaults to prevent error messages
+
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+// Free fonts are not loaded in User_Setup.h so we must define all as font 1
+// to prevent compile error messages
+// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
+
+#define GFXFF 1
+#define GLCD 1
+#define FONT2 2
+#define FONT4 4
+#define FONT6 6
+#define FONT7 7
+#define FONT8 8
+
+#define FF0 1
+#define FF1 1
+#define FF2 1
+#define FF3 1
+#define FF4 1
+#define FF5 1
+#define FF6 1
+#define FF7 1
+#define FF8 1
+#define FF9 1
+#define FF10 1
+#define FF11 1
+#define FF12 1
+#define FF13 1
+#define FF14 1
+#define FF15 1
+#define FF16 1
+#define FF17 1
+#define FF18 1
+#define FF19 1
+#define FF20 1
+#define FF21 1
+#define FF22 1
+#define FF23 1
+#define FF24 1
+#define FF25 1
+#define FF26 1
+#define FF27 1
+#define FF28 1
+#define FF29 1
+#define FF30 1
+#define FF31 1
+#define FF32 1
+#define FF33 1
+#define FF34 1
+#define FF35 1
+#define FF36 1
+#define FF37 1
+#define FF38 1
+#define FF39 1
+#define FF40 1
+#define FF41 1
+#define FF42 1
+#define FF43 1
+#define FF44 1
+#define FF45 1
+#define FF46 1
+#define FF47 1
+#define FF48 1
+
+#define FM9 1
+#define FM12 1
+#define FM18 1
+#define FM24 1
+
+#define FMB9 1
+#define FMB12 1
+#define FMB18 1
+#define FMB24 1
+
+#define FMO9 1
+#define FMO12 1
+#define FMO18 1
+#define FMO24 1
+
+#define FMBO9 1
+#define FMBO12 1
+#define FMBO18 1
+#define FMBO24 1
+
+#define FSS9 1
+#define FSS12 1
+#define FSS18 1
+#define FSS24 1
+
+#define FSSB9 1
+#define FSSB12 1
+#define FSSB18 1
+#define FSSB24 1
+
+#define FSSO9 1
+#define FSSO12 1
+#define FSSO18 1
+#define FSSO24 1
+
+#define FSSBO9 1
+#define FSSBO12 1
+#define FSSBO18 1
+#define FSSBO24 1
+
+#define FS9 1
+#define FS12 1
+#define FS18 1
+#define FS24 1
+
+#define FSI9 1
+#define FSI12 1
+#define FSI19 1
+#define FSI24 1
+
+#define FSB9 1
+#define FSB12 1
+#define FSB18 1
+#define FSB24 1
+
+#define FSBI9 1
+#define FSBI12 1
+#define FSBI18 1
+#define FSBI24 1
+
+#endif // LOAD_GFXFF
diff --git a/lib/M5Stack/src/IMU.cpp b/lib/M5Stack/src/IMU.cpp
new file mode 100644
index 000000000..6a96c5e09
--- /dev/null
+++ b/lib/M5Stack/src/IMU.cpp
@@ -0,0 +1,157 @@
+#if defined (CORE)
+#include "IMU.h"
+
+#include <Arduino.h>
+#include <math.h>
+
+#include "M5Stack.h"
+#undef IMU
+
+IMU::IMU() {
+}
+
+int IMU::Init(void) {
+ int imu_flag = M5.Sh200Q.Init();
+ Serial.printf("imu_flag:%d", imu_flag);
+ if (imu_flag != 0) {
+ imu_flag = M5.Mpu6886.Init();
+ if (imu_flag == 0) {
+ imuType = IMU_MPU6886;
+ Serial.printf("IMU_MPU6886");
+ } else {
+ imuType = IMU_UNKNOWN;
+ Serial.printf("IMU_UNKNOWN");
+ return -1;
+ }
+ } else {
+ imuType = IMU_SH200Q;
+ }
+ return 0;
+}
+
+void IMU::getGres() {
+ if (imuType == IMU_SH200Q) {
+ gRes = M5.Sh200Q.gRes;
+ } else if (imuType == IMU_MPU6886) {
+ gRes = M5.Mpu6886.gRes;
+ }
+}
+
+void IMU::getAres() {
+ if (imuType == IMU_SH200Q) {
+ aRes = M5.Sh200Q.aRes;
+ } else if (imuType == IMU_MPU6886) {
+ aRes = M5.Mpu6886.aRes;
+ }
+}
+
+void IMU::getAccelAdc(int16_t *ax, int16_t *ay, int16_t *az) {
+ if (imuType == IMU_SH200Q) {
+ M5.Sh200Q.getAccelAdc(ax, ay, az);
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.getAccelAdc(ax, ay, az);
+ }
+}
+
+void IMU::getAccelData(float *ax, float *ay, float *az) {
+ if (imuType == IMU_SH200Q) {
+ M5.Sh200Q.getAccelData(ax, ay, az);
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.getAccelData(ax, ay, az);
+ }
+}
+
+void IMU::getGyroAdc(int16_t *gx, int16_t *gy, int16_t *gz) {
+ if (imuType == IMU_SH200Q) {
+ M5.Sh200Q.getGyroAdc(gx, gy, gz);
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.getGyroAdc(gx, gy, gz);
+ }
+}
+
+void IMU::getGyroData(float *gx, float *gy, float *gz) {
+ if (imuType == IMU_SH200Q) {
+ M5.Sh200Q.getGyroData(gx, gy, gz);
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.getGyroData(gx, gy, gz);
+ }
+}
+
+void IMU::getTempAdc(int16_t *t) {
+ if (imuType == IMU_SH200Q) {
+ M5.Sh200Q.getTempAdc(t);
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.getTempAdc(t);
+ }
+}
+
+void IMU::getTempData(float *t) {
+ if (imuType == IMU_SH200Q) {
+ M5.Sh200Q.getTempData(t);
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.getTempData(t);
+ }
+}
+
+void IMU::getAhrsData(float *pitch, float *roll, float *yaw) {
+ float accX = 0;
+ float accY = 0;
+ float accZ = 0;
+
+ float gyroX = 0;
+ float gyroY = 0;
+ float gyroZ = 0;
+
+ getGyroData(&gyroX, &gyroY, &gyroZ);
+ getAccelData(&accX, &accY, &accZ);
+
+ MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD,
+ gyroZ * DEG_TO_RAD, accX, accY, accZ, pitch, roll, yaw);
+}
+
+void IMU::setFIFOEnable(bool enable_flag) {
+ if (imuType == IMU_SH200Q) {
+ Serial.println("IMU_SH200Q: setFIFOEnable() not implemented");
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.setFIFOEnable(enable_flag);
+ }
+}
+
+uint8_t IMU::ReadFIFO() {
+ uint8_t read_fifo = 0;
+
+ if (imuType == IMU_SH200Q) {
+ Serial.println("IMU_SH200Q: ReadFIFO() not implemented");
+ } else if (imuType == IMU_MPU6886) {
+ read_fifo = M5.Mpu6886.ReadFIFO();
+ }
+ return read_fifo;
+}
+
+void IMU::ReadFIFOBuff(uint8_t *data_buf, uint16_t length) {
+ if (imuType == IMU_SH200Q) {
+ Serial.println("IMU_SH200Q: ReadFIFOBuff() not implemented");
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.ReadFIFOBuff(data_buf, length);
+ }
+}
+
+uint16_t IMU::ReadFIFOCount() {
+ uint16_t fifo_count = 0;
+
+ if (imuType == IMU_SH200Q) {
+ Serial.println("IMU_SH200Q: ReadFIFOCount() not implemented");
+ } else if (imuType == IMU_MPU6886) {
+ fifo_count = M5.Mpu6886.ReadFIFOCount();
+ }
+ return fifo_count;
+}
+
+void IMU::RestFIFO() {
+ if (imuType == IMU_SH200Q) {
+ Serial.println("IMU_SH200Q: RestFIFO() not implemented");
+ } else if (imuType == IMU_MPU6886) {
+ M5.Mpu6886.RestFIFO();
+ }
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/IMU.h b/lib/M5Stack/src/IMU.h
new file mode 100644
index 000000000..65bd510e9
--- /dev/null
+++ b/lib/M5Stack/src/IMU.h
@@ -0,0 +1,42 @@
+#if defined (CORE)
+#ifndef __IMU_H__
+#define __IMU_H__
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "utility/MahonyAHRS.h"
+
+class IMU {
+ public:
+ enum ImuType { IMU_UNKNOWN = 0, IMU_SH200Q, IMU_MPU6886 };
+
+ IMU();
+
+ int Init(void);
+
+ void getGres();
+ void getAres();
+
+ void getAccelAdc(int16_t *ax, int16_t *ay, int16_t *az);
+ void getGyroAdc(int16_t *gx, int16_t *gy, int16_t *gz);
+ void getTempAdc(int16_t *t);
+
+ void getAccelData(float *ax, float *ay, float *az);
+ void getGyroData(float *gx, float *gy, float *gz);
+ void getTempData(float *t);
+
+ void getAhrsData(float *pitch, float *roll, float *yaw);
+
+ void setFIFOEnable(bool enable_flag);
+ uint8_t ReadFIFO();
+ void ReadFIFOBuff(uint8_t *data_buf, uint16_t length);
+ uint16_t ReadFIFOCount();
+ void RestFIFO();
+
+ ImuType imuType;
+ float aRes, gRes;
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/LoRaWan.cpp b/lib/M5Stack/src/LoRaWan.cpp
new file mode 100644
index 000000000..1bf94dc9e
--- /dev/null
+++ b/lib/M5Stack/src/LoRaWan.cpp
@@ -0,0 +1,1006 @@
+#if defined (CORE)
+/*
+ LoRaWAN.cpp for M5Stack (fork from
+ https://github.com/toddkrein/OTAA-LoRaWAN-Seeed)
+
+ 2013 Copyright (c) Seeed Technology Inc. All right reserved.
+ 2017 Copyright (c) Todd Krein. All rights reserved.
+
+ Original Author: Wayne Weng
+ Date: 2016-10-17
+
+ Greatly overhauled 2017 by Todd Krein (todd@krein.org)
+
+ The MIT License (MIT)
+
+ 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.1 USA
+*/
+
+#include "LoRaWan.h"
+
+const char *physTypeStr[10] = {"EU434", "EU868", "US915", "US915HYBRID",
+ "AU915", "AS923", "CN470", "KR920",
+ "CN470PREQUEL", "STE920"};
+
+LoRaWanClass::LoRaWanClass(void) {
+ memset(_buffer, 0, 256);
+ debug = false;
+}
+
+void LoRaWanClass::init(void) {
+ Serial2.begin(9600, SERIAL_8N1, 16, 17);
+ // Serial2.begin(9600, SERIAL_8N1, 2, 5);
+ // SerialLoRa.begin(9600);
+}
+
+void LoRaWanClass::getVersion(char *buffer, short length,
+ unsigned char timeout) {
+ if (buffer) {
+ while (SerialLoRa.available()) SerialLoRa.read();
+ sendCommand("AT+VER=?\r\n");
+ readBuffer(buffer, length, timeout);
+ }
+}
+
+void LoRaWanClass::getId(char *buffer, short length, unsigned char timeout) {
+ if (buffer) {
+ while (SerialLoRa.available()) SerialLoRa.read();
+ sendCommand("AT+ID=?\r\n");
+ readBuffer(buffer, length, timeout);
+ }
+}
+
+void LoRaWanClass::setId(char *DevAddr, char *DevEUI, char *AppEUI) {
+ char cmd[64];
+
+ if (DevAddr) {
+ memset(cmd, 0, 64);
+ sprintf(cmd, "AT+ID=DevAddr,\"%s\"\r\n", DevAddr);
+ sendCommand(cmd);
+ delay(DEFAULT_TIMEWAIT);
+ }
+
+ if (DevEUI) {
+ memset(cmd, 0, 64);
+ sprintf(cmd, "AT+ID=DevEui,\"%s\"\r\n", DevEUI);
+ sendCommand(cmd);
+ delay(DEFAULT_TIMEWAIT);
+ }
+
+ if (AppEUI) {
+ memset(cmd, 0, 64);
+ sprintf(cmd, "AT+ID=AppEui,\"%s\"\r\n", AppEUI);
+ sendCommand(cmd);
+ delay(DEFAULT_TIMEWAIT);
+ }
+}
+
+void LoRaWanClass::setKey(char *NwkSKey, char *AppSKey, char *AppKey) {
+ char cmd[64];
+
+ if (NwkSKey) {
+ memset(cmd, 0, 64);
+ sprintf(cmd, "AT+KEY=NWKSKEY,\"%s\"\r\n", NwkSKey);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+ }
+
+ if (AppSKey) {
+ memset(cmd, 0, 64);
+ sprintf(cmd, "AT+KEY=APPSKEY,\"%s\"\r\n", AppSKey);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+ }
+
+ if (AppKey) {
+ memset(cmd, 0, 64);
+ sprintf(cmd, "AT+KEY= APPKEY,\"%s\"\r\n", AppKey);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+ }
+}
+
+bool LoRaWanClass::setDataRate(_data_rate_t dataRate,
+ _physical_type_t physicalType) {
+ char cmd[32];
+ // const char *str;
+
+ if ((physicalType <= UNINIT) && (physicalType >= UNDEF)) {
+ myType = UNINIT;
+ debugPrint("Unknown datarate\n");
+ return false;
+ }
+
+ myType = physicalType;
+ sendCommand(F("AT+DR="));
+ // str = (const char*)(physTypeStr[(int)myType]);
+ sendCommand(physTypeStr[myType]);
+ // sendCommand(str);
+ sendCommand(F("\r\n"));
+// if(physicalType == EU434)sendCommand("AT+DR=EU433\r\n");
+// else if(physicalType == EU868)sendCommand("AT+DR=EU868\r\n");
+// else if(physicalType == US915)sendCommand("AT+DR=US915\r\n");
+// else if(physicalType == AU920)sendCommand("AT+DR=AU920\r\n");
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+DR=%d\r\n", dataRate);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+ return true;
+}
+
+void LoRaWanClass::setPower(short power) {
+ char cmd[32];
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+POWER=%d\r\n", power);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setPort(unsigned char port) {
+ char cmd[32];
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+PORT=%d\r\n", port);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setAdaptiveDataRate(bool command) {
+ if (command)
+ sendCommand("AT+ADR=ON\r\n");
+ else
+ sendCommand("AT+ADR=OFF\r\n");
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::getChannel(void) {
+ sendCommand("AT+CH\r\n");
+
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setChannel(unsigned char channel, float frequency) {
+ char cmd[32];
+
+ // if(channel > 16) channel = 16; // ??? this is wrong for US915
+
+ memset(cmd, 0, 32);
+ if (frequency == 0)
+ sprintf(cmd, "AT+CH=%d,0\r\n", channel);
+ else
+ sprintf(cmd, "AT+CH=%d,%d.%d\r\n", channel, (short)frequency,
+ short(frequency * 10) % 10);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setChannel(unsigned char channel, float frequency,
+ _data_rate_t dataRata) {
+ char cmd[32];
+
+ if (channel > 16) channel = 16;
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+CH=%d,%d.%d,%d\r\n", channel, (short)frequency,
+ short(frequency * 10) % 10, dataRata);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setChannel(unsigned char channel, float frequency,
+ _data_rate_t dataRataMin,
+ _data_rate_t dataRataMax) {
+ char cmd[32];
+
+ if (channel > 16) channel = 16;
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+CH=%d,%d.%d,%d,%d\r\n", channel, (short)frequency,
+ short(frequency * 10) % 10, dataRataMin, dataRataMax);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+bool LoRaWanClass::transferPacket(char *buffer, unsigned char timeout) {
+ unsigned char length = strlen(buffer);
+ int count;
+
+ while (SerialLoRa.available()) SerialLoRa.read();
+
+ sendCommand("AT+MSG=\"");
+ for (int i = 0; i < length; i++) SerialLoRa.write(buffer[i]);
+ sendCommand("\"\r\n");
+
+ while (true) {
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ count = readLine(_buffer, BEFFER_LENGTH_MAX, timeout);
+ if (count == 0) continue;
+ // handle timout!
+#if _DEBUG_SERIAL_
+ SerialUSB.print(_buffer);
+#endif
+ if (strstr(_buffer, "+MSG: Done")) return true;
+ }
+
+ // memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ // readBuffer(_buffer, BEFFER_LENGTH_MAX, timeout);
+ //#if _DEBUG_SERIAL_
+ // SerialUSB.print(_buffer);
+ //#endif
+ // if(strstr(_buffer, "+MSG: Done"))return true;
+ // return false;
+}
+
+bool LoRaWanClass::transferPacket(unsigned char *buffer, unsigned char length,
+ unsigned char timeout) {
+ char temp[3] = {0};
+
+ while (SerialLoRa.available()) SerialLoRa.read();
+
+ sendCommand("AT+MSGHEX=\"");
+ for (int i = 0; i < length; i++) {
+ sprintf(temp, "%02x", buffer[i]);
+ SerialLoRa.write(temp);
+ }
+ sendCommand("\"\r\n");
+
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ readBuffer(_buffer, BEFFER_LENGTH_MAX, timeout);
+#if _DEBUG_SERIAL_
+ SerialUSB.print(_buffer);
+#endif
+ if (strstr(_buffer, "+MSGHEX: Done")) return true;
+ return false;
+}
+
+bool LoRaWanClass::transferPacketWithConfirmed(char *buffer,
+ unsigned char timeout) {
+ unsigned char length = strlen(buffer);
+ int i;
+ bool sentOK;
+
+ sentOK = false;
+
+ while (SerialLoRa.available()) SerialLoRa.read();
+
+ sendCommand("AT+CMSG=\"");
+ for (int i = 0; i < length; i++) SerialLoRa.write(buffer[i]);
+ sendCommand("\"\r\n");
+
+#ifdef deadcode
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ i = readBuffer(_buffer, BEFFER_LENGTH_MAX, timeout);
+ _buffer[i] = 0;
+#if _DEBUG_SERIAL_
+ SerialUSB.print(_buffer);
+#endif
+ if (strstr(_buffer, "+CMSG: ACK Received"))
+ return true;
+ else
+ return false;
+#endif
+
+ while (true) {
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ i = readLine(_buffer, BEFFER_LENGTH_MAX, timeout);
+ if (i == 0) continue;
+ _buffer[i] = 0;
+
+ // !!! handle timeout
+#if _DEBUG_SERIAL_
+ SerialUSB.print(_buffer);
+#endif
+ if (strstr(_buffer, "+CMSG: Start")) continue;
+ if (strstr(_buffer, "+CMSG: Wait ACK")) continue;
+ if (strstr(_buffer, "+CMSG: TX")) continue;
+ if (strstr(_buffer, "+CMSG: RXWIN")) continue;
+ if (strstr(_buffer, "+CMSG: No free channel")) break;
+ if (strstr(_buffer, "+CMSG: Done")) break;
+
+ if (strstr(_buffer, "+CMSG: ACK Received")) {
+ sentOK = true;
+ continue;
+ }
+ SerialUSB.print("Result didn't match anything I expected.\n");
+ }
+
+ return sentOK;
+}
+
+bool LoRaWanClass::transferPacketWithConfirmed(unsigned char *buffer,
+ unsigned char length,
+ unsigned char timeout) {
+ char temp[3] = {0};
+ int i;
+ unsigned char *ptr;
+
+ while (SerialLoRa.available()) SerialLoRa.read();
+
+ sendCommand("AT+CMSGHEX=\"");
+ for (int i = 0; i < length; i++) {
+ sprintf(temp, "%02x", buffer[i]);
+ SerialLoRa.write(temp);
+ }
+ sendCommand("\"\r\n");
+#if _DEBUG_SERIAL_
+ ptr = buffer;
+ for (i = 0; i < length; i++) SerialUSB.print(*(ptr++));
+
+ SerialUSB.println("");
+#endif
+
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ i = readBuffer(_buffer, BEFFER_LENGTH_MAX, timeout);
+ _buffer[i] = 0;
+
+ SerialUSB.print(_buffer);
+
+ if (strstr(_buffer, "+CMSGHEX: ACK Received"))
+ return true;
+ else
+ return false;
+}
+
+short LoRaWanClass::receivePacket(char *buffer, short length, short *rssi) {
+ char *ptr;
+ short number = 0;
+
+ ptr = strstr(_buffer, "RSSI ");
+ if (ptr)
+ *rssi = atoi(ptr + 5);
+ else
+ *rssi = -255;
+
+ ptr = strstr(_buffer, "RX: \"");
+ if (ptr) {
+ ptr += 5;
+ for (short i = 0;; i++) {
+ char temp[3] = {0, 0};
+ unsigned char tmp = '?', result = 0;
+
+ temp[0] = *(ptr + i * 3);
+ temp[1] = *(ptr + i * 3 + 1);
+
+ for (unsigned char j = 0; j < 2; j++) {
+ if ((temp[j] >= '0') && (temp[j] <= '9'))
+ tmp = temp[j] - '0';
+ else if ((temp[j] >= 'A') && (temp[j] <= 'F'))
+ tmp = temp[j] - 'A' + 10;
+ else if ((temp[j] >= 'a') && (temp[j] <= 'f'))
+ tmp = temp[j] - 'a' + 10;
+
+ result = result * 16 + tmp;
+ }
+
+ if (i < length) buffer[i] = result;
+
+ if (*(ptr + i * 3 + 3) == '\"' && *(ptr + i * 3 + 4) == '\r' &&
+ *(ptr + i * 3 + 5) == '\n') {
+ number = i + 1;
+ break;
+ }
+ }
+ }
+
+ ptr = strstr(_buffer, "MACCMD: \"");
+ if (ptr) {
+ buffer[0] = 'M';
+ buffer[1] = 'A';
+ buffer[2] = 'C';
+ buffer[3] = 'C';
+ buffer[4] = 'M';
+ buffer[5] = 'D';
+ buffer[6] = ':';
+
+ ptr += 9;
+ for (short i = 0;; i++) {
+ char temp[3] = {0};
+ unsigned char tmp = '?', result = 0;
+
+ temp[0] = *(ptr + i * 3);
+ temp[1] = *(ptr + i * 3 + 1);
+
+ for (unsigned char j = 0; j < 2; j++) {
+ if ((temp[j] >= '0') && (temp[j] <= '9'))
+ tmp = temp[j] - '0';
+ else if ((temp[j] >= 'A') && (temp[j] <= 'F'))
+ tmp = temp[j] - 'A' + 10;
+ else if ((temp[j] >= 'a') && (temp[j] <= 'f'))
+ tmp = temp[j] - 'a' + 10;
+
+ result = result * 16 + tmp;
+ }
+
+ if ((i + 7) < length) buffer[i + 7] = result;
+
+ if (*(ptr + i * 3 + 3) == '\"' && *(ptr + i * 3 + 4) == '\r' &&
+ *(ptr + i * 3 + 5) == '\n') {
+ number = i + 1 + 7;
+ break;
+ }
+ }
+ }
+
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+
+ return number;
+}
+
+bool LoRaWanClass::transferProprietaryPacket(char *buffer,
+ unsigned char timeout) {
+ unsigned char length = strlen(buffer);
+
+ while (SerialLoRa.available()) SerialLoRa.read();
+
+ sendCommand("AT+PMSG=\"");
+ for (int i = 0; i < length; i++) SerialLoRa.write(buffer[i]);
+ sendCommand("\"\r\n");
+
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ readBuffer(_buffer, BEFFER_LENGTH_MAX, timeout);
+#if _DEBUG_SERIAL_
+ SerialUSB.print(_buffer);
+#endif
+ if (strstr(_buffer, "+PMSG: Done")) return true;
+ return false;
+}
+
+bool LoRaWanClass::transferProprietaryPacket(unsigned char *buffer,
+ unsigned char length,
+ unsigned char timeout) {
+ char temp[3] = {0};
+
+ while (SerialLoRa.available()) SerialLoRa.read();
+
+ sendCommand("AT+PMSGHEX=\"");
+ for (int i = 0; i < length; i++) {
+ sprintf(temp, "%02x", buffer[i]);
+ SerialLoRa.write(temp);
+ }
+ sendCommand("\"\r\n");
+
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ readBuffer(_buffer, BEFFER_LENGTH_MAX, timeout);
+#if _DEBUG_SERIAL_
+ SerialUSB.print(_buffer);
+#endif
+ if (strstr(_buffer, "+PMSGHEX: Done")) return true;
+ return false;
+}
+
+void LoRaWanClass::setUnconfirmedMessageRepeatTime(unsigned char time) {
+ char cmd[32];
+
+ if (time > 15)
+ time = 15;
+ else if (time == 0)
+ time = 1;
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+REPT=%d\r\n", time);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setConfirmedMessageRetryTime(unsigned char time) {
+ char cmd[32];
+
+ if (time > 15)
+ time = 15;
+ else if (time == 0)
+ time = 1;
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+RETRY=%d\r\n", time);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::getReceiveWindowFirst(void) {
+ sendCommand("AT+RXWIN1\r\n");
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setReceiveWindowFirst(bool command) {
+ if (command)
+ sendCommand("AT+RXWIN1=ON\r\n");
+ else
+ sendCommand("AT+RXWIN1=OFF\r\n");
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+void LoRaWanClass::setReceiveWindowFirst(unsigned char channel,
+ float frequency) {
+ char cmd[32];
+
+ // if(channel > 16) channel = 16;
+
+ memset(cmd, 0, 32);
+ if (frequency == 0)
+ sprintf(cmd, "AT+RXWIN1=%d,0\r\n", channel);
+ else
+ sprintf(cmd, "AT+RXWIN1=%d,%d.%d\r\n", channel, (short)frequency,
+ short(frequency * 10) % 10);
+ sendCommand(cmd);
+ SerialUSB.print(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME *
+ 4); // this can have a lot of data to dump
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setReceiveWindowSecond(float frequency,
+ _data_rate_t dataRate) {
+ char cmd[32];
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+RXWIN2=%d.%d,%d\r\n", (short)frequency,
+ short(frequency * 10) % 10, dataRate);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setReceiveWindowSecond(float frequency,
+ _spreading_factor_t spreadingFactor,
+ _band_width_t bandwidth) {
+ char cmd[32];
+
+ memset(cmd, 0, 32);
+ sprintf(cmd, "AT+RXWIN2=%d.%d,%d,%d\r\n", (short)frequency,
+ short(frequency * 10) % 10, spreadingFactor, bandwidth);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setReceiveWindowDelay(_window_delay_t command,
+ unsigned short _delay) {
+ char cmd[32];
+
+ memset(cmd, 0, 32);
+ if (command == RECEIVE_DELAY1)
+ sprintf(cmd, "AT+DELAY=RX1,%d\r\n", _delay);
+ else if (command == RECEIVE_DELAY2)
+ sprintf(cmd, "AT+DELAY=RX2,%d\r\n", _delay);
+ else if (command == JOIN_ACCEPT_DELAY1)
+ sprintf(cmd, "AT+DELAY=JRX1,%d\r\n", _delay);
+ else if (command == JOIN_ACCEPT_DELAY2)
+ sprintf(cmd, "AT+DELAY=JRX2,%d\r\n", _delay);
+ sendCommand(cmd);
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::setClassType(_class_type_t type) {
+ if (type == CLASS_A)
+ sendCommand("AT+CLASS=A\r\n");
+ else if (type == CLASS_C)
+ sendCommand("AT+CLASS=C\r\n");
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+//
+// set the JOIN mode to either LWOTAA or LWABP
+// does a half-hearted attempt to check the results
+//
+bool LoRaWanClass::setDeviceMode(_device_mode_t mode) {
+ char buffer[kLOCAL_BUFF_MAX];
+ int timeout = 1;
+
+ if (mode == LWABP)
+ sendCommand("AT+MODE=LWABP\r\n");
+ else if (mode == LWOTAA)
+ sendCommand("AT+MODE=LWOTAA\r\n");
+ else
+ return false;
+
+ memset(buffer, 0, kLOCAL_BUFF_MAX);
+ readBuffer(buffer, kLOCAL_BUFF_MAX - 1, timeout);
+
+#if _DEBUG_SERIAL_
+ SerialUSB.print(buffer);
+// loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+
+ return strstr(
+ buffer, "+MODE:"); // if it works, response is of form "+MODE: LWOTTA"
+}
+
+//
+// JOIN with the application
+//
+// setDeviceMode should have been called before this.
+bool LoRaWanClass::setOTAAJoin(_otaa_join_cmd_t command,
+ unsigned char timeout) {
+ // char *ptr;
+ short count;
+ bool joined = false;
+
+ if (command == JOIN)
+ sendCommand("AT+JOIN\r\n");
+ else if (command == FORCE)
+ sendCommand("AT+JOIN=FORCE\r\n");
+ else {
+ SerialUSB.print("Bad command to setOTAAJoin\n");
+ return false;
+ }
+
+#if _DEBUG_SERIAL_
+// loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ // delay(DEFAULT_TIMEWAIT);
+
+ while (true) {
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ count = readLine(_buffer, BEFFER_LENGTH_MAX, timeout);
+ if (count == 0) continue;
+
+ // !!! handle timeout
+#if _DEBUG_SERIAL_
+ SerialUSB.print(_buffer);
+#endif
+ if (strstr(_buffer, "+JOIN: Join failed")) continue;
+ if (strstr(_buffer, "+JOIN: LoRaWAN modem is busy")) continue;
+ if (strstr(_buffer, "+JOIN: NORMAL")) continue;
+ if (strstr(_buffer, "+JOIN: FORCE")) continue;
+ if (strstr(_buffer, "+JOIN: Start")) continue;
+ if (strstr(_buffer, "+JOIN: Done")) break;
+ if (strstr(_buffer, "+JOIN: No free channel")) break;
+ if (strstr(_buffer, "+JOIN: Network joined")) {
+ joined = true;
+ continue;
+ }
+ if (strstr(_buffer, "+JOIN: NetID")) {
+ joined = true;
+ continue;
+ }
+
+ SerialUSB.print("Result didn't match anything I expected.\n");
+ }
+
+ SerialUSB.print("Done with Join\n");
+ return joined;
+}
+
+void LoRaWanClass::setDeviceLowPower(void) {
+ sendCommand("AT+LOWPOWER\r\n");
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+//
+// Reset the LoRa module. Does not factory reset
+//
+void LoRaWanClass::setDeviceReset(void) {
+ sendCommand("AT+RESET\r\n");
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+//
+// Factory reset the module.
+//
+void LoRaWanClass::setDeviceDefault(void) {
+ sendCommand("AT+FDEFAULT=RISINGHF\r\n");
+#if _DEBUG_SERIAL_
+ loraDebugPrint(DEFAULT_DEBUGTIME);
+#endif
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::initP2PMode(unsigned short frequency,
+ _spreading_factor_t spreadingFactor,
+ _band_width_t bandwidth,
+ unsigned char txPreamble,
+ unsigned char rxPreamble, short power) {
+ char cmd[64] = {
+ 0,
+ };
+ sprintf(cmd, "AT+TEST=RFCFG,%d,%d,%d,%d,%d,%d\r\n", frequency,
+ spreadingFactor, bandwidth, txPreamble, rxPreamble, power);
+
+ sendCommand("AT+MODE=TEST\r\n");
+ delay(DEFAULT_TIMEWAIT);
+ sendCommand(cmd);
+ delay(DEFAULT_TIMEWAIT);
+ sendCommand("AT+TEST=RXLRPKT\r\n");
+ delay(DEFAULT_TIMEWAIT);
+}
+
+void LoRaWanClass::transferPacketP2PMode(char *buffer) {
+ unsigned char length = strlen(buffer);
+
+ sendCommand("AT+TEST=TXLRSTR,\"");
+ for (int i = 0; i < length; i++) SerialLoRa.write(buffer[i]);
+ sendCommand("\"\r\n");
+}
+
+void LoRaWanClass::transferPacketP2PMode(unsigned char *buffer,
+ unsigned char length) {
+ char temp[3] = {0};
+
+ sendCommand("AT+TEST=TXLRPKT,\"");
+ for (int i = 0; i < length; i++) {
+ sprintf(temp, "%02x", buffer[i]);
+ SerialLoRa.write(temp);
+ }
+ sendCommand("\"\r\n");
+}
+
+short LoRaWanClass::receivePacketP2PMode(unsigned char *buffer, short length,
+ short *rssi, unsigned char timeout) {
+ char *ptr;
+ short number;
+
+ while (SerialLoRa.available()) SerialLoRa.read();
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+ readBuffer(_buffer, BEFFER_LENGTH_MAX, timeout);
+
+ ptr = strstr(_buffer, "LEN");
+ if (ptr)
+ number = atoi(ptr + 4);
+ else
+ number = 0;
+
+ if (number <= 0) return 0;
+
+ ptr = strstr(_buffer, "RSSI:");
+ if (ptr)
+ *rssi = atoi(ptr + 5);
+ else
+ *rssi = -255;
+
+ ptr = strstr(_buffer, "RX \"");
+ if (ptr) {
+ ptr += 4;
+ for (short i = 0; i < number; i++) {
+ char temp[3] = {0};
+ unsigned char tmp = '?', result = 0;
+
+ temp[0] = *(ptr + i * 2);
+ temp[1] = *(ptr + i * 2 + 1);
+
+ for (unsigned char j = 0; j < 2; j++) {
+ if ((temp[j] >= '0') && (temp[j] <= '9'))
+ tmp = temp[j] - '0';
+ else if ((temp[j] >= 'A') && (temp[j] <= 'F'))
+ tmp = temp[j] - 'A' + 10;
+ else if ((temp[j] >= 'a') && (temp[j] <= 'f'))
+ tmp = temp[j] - 'a' + 10;
+
+ result = result * 16 + tmp;
+ }
+
+ if (i < length) buffer[i] = result;
+ }
+ }
+
+ memset(_buffer, 0, BEFFER_LENGTH_MAX);
+
+ return number;
+}
+
+// short LoRaWanClass::getBatteryVoltage(void)
+// {
+// int battery;
+
+// pinMode(CHARGE_STATUS_PIN, OUTPUT);
+// digitalWrite(CHARGE_STATUS_PIN, LOW);
+// delay(DEFAULT_TIMEWAIT);
+// battery = (analogRead(BATTERY_POWER_PIN) * 3300 * 11) >> 10;
+// pinMode(CHARGE_STATUS_PIN, INPUT);
+
+// return battery;
+// }
+
+// ??? I think this essentially connects the serial port to the LoRa module.
+// @@@ typing a "~" will exit
+//
+void LoRaWanClass::loraDebug(void) {
+ char c;
+
+ while (true) {
+ if (SerialUSB.available()) {
+ c = SerialUSB.read();
+ if (c == '~') return;
+ SerialLoRa.write(c);
+ }
+ if (SerialLoRa.available()) SerialUSB.write(SerialLoRa.read());
+ }
+}
+
+#if _DEBUG_SERIAL_
+//
+// timeout is the total amount of time allowed for collecting data
+// Would it make more sense if it was the time w/o a character?
+void LoRaWanClass::loraDebugPrint(unsigned int timeout) {
+ unsigned long timerStart, timerEnd;
+ char c;
+
+ timerStart = millis();
+
+ while (1) {
+ while (SerialLoRa.available()) {
+ SerialUSB.write(c = SerialLoRa.read());
+ if (c == '\n')
+ return; // !!! This won't work for commands that return
+ // multiple lines.
+ timerStart = millis();
+ }
+
+ timerEnd = millis();
+ // if(timerEnd - timerStart > 1000 * timeout)break;
+ if (timerEnd - timerStart > timeout) break;
+ }
+}
+#endif
+
+void LoRaWanClass::debugPrint(const char *str) {
+ SerialUSB.print(str);
+}
+
+void LoRaWanClass::sendCommand(const char *command) {
+ SerialLoRa.print(command);
+}
+
+void LoRaWanClass::sendCommand(const __FlashStringHelper *command) {
+ SerialLoRa.print(command);
+}
+
+short LoRaWanClass::readBuffer(char *buffer, short length,
+ unsigned char timeout) {
+ short i = 0;
+ unsigned long timerStart, timerEnd;
+
+ timerStart = millis();
+
+ while (1) {
+ if (i < length) {
+ while (SerialLoRa.available()) {
+ char c = SerialLoRa.read();
+ buffer[i++] = c;
+ }
+ }
+
+ timerEnd = millis();
+ if (timerEnd - timerStart > 1000 * timeout) break;
+ }
+
+ return i;
+}
+
+short LoRaWanClass::readLine(char *buffer, short length,
+ unsigned char timeout) {
+ short i = 0;
+ unsigned long timerStart, timerEnd;
+ char c = '\n';
+
+ timerStart = millis();
+
+ while (1) {
+ if (i < length - 1) {
+ while (SerialLoRa.available()) {
+ c = SerialLoRa.read();
+ buffer[i++] = c;
+ if (c == '\n') break;
+ timerStart = millis();
+ }
+ }
+ if (c == '\n') break; // @@@ barf
+ timerEnd = millis();
+ if (timerEnd - timerStart > 1000 * timeout) break;
+ }
+
+ buffer[i] = 0; // terminate the string
+ return i;
+}
+short LoRaWanClass::waitForResponse(char *response, unsigned char timeout) {
+ short len = strlen(response);
+ short sum = 0;
+ unsigned long timerStart, timerEnd;
+
+ timerStart = millis();
+
+ while (1) {
+ if (SerialLoRa.available()) {
+ char c = SerialLoRa.read();
+
+ sum = (c == response[sum]) ? sum + 1 : 0;
+ if (sum == len) break;
+ }
+
+ timerEnd = millis();
+ if (timerEnd - timerStart > 1000 * timeout) return -1;
+ }
+
+ return 0;
+}
+
+short LoRaWanClass::sendCommandAndWaitForResponse(char *command, char *response,
+ unsigned char timeout) {
+ sendCommand(command);
+
+ return waitForResponse(response, timeout);
+}
+
+LoRaWanClass lora;
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/LoRaWan.h b/lib/M5Stack/src/LoRaWan.h
new file mode 100644
index 000000000..2bce8c7d2
--- /dev/null
+++ b/lib/M5Stack/src/LoRaWan.h
@@ -0,0 +1,562 @@
+#if defined (CORE)
+/*
+ LoRaWAN.cpp for M5Stack (fork from
+ https://github.com/toddkrein/OTAA-LoRaWAN-Seeed)
+
+ 2013 Copyright (c) Seeed Technology Inc. All right reserved.
+ Author: Wayne Weng
+ Date: 2016-10-17
+ add rgb backlight fucnction @ 2013-10-15
+
+ The MIT License (MIT)
+ 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.1 USA
+*/
+
+#ifndef _LORAWAN_H_
+#define _LORAWAN_H_
+
+#include <Arduino.h>
+
+#define SerialUSB Serial
+#define SerialLoRa Serial2
+
+#define _DEBUG_SERIAL_ 1
+//#define DEFAULT_TIMEOUT 5 // second
+#define DEFAULT_TIMEOUT 3 // second
+#define DEFAULT_TIMEWAIT \
+ 200 // milliseconds to wait after issuing command via serial
+#define DEFAULT_DEBUGTIME \
+ 500 // milliseconds to wait for a response after command
+
+// #define BATTERY_POWER_PIN A4
+// #define CHARGE_STATUS_PIN A5
+
+#define BEFFER_LENGTH_MAX 256
+
+#define MAC_COMMAND_FLAG "MACCMD:"
+#define kLOCAL_BUFF_MAX 64
+
+enum _class_type_t { CLASS_A = 0, CLASS_C };
+enum _physical_type_t {
+ UNINIT = -1,
+ EU434,
+ EU868,
+ US915,
+ US915HYBRID,
+ AU915,
+ AS923,
+ CN470,
+ KR920,
+ CN470PREQUEL,
+ STE920,
+ UNDEF
+};
+enum _device_mode_t { LWABP = 0, LWOTAA, TEST };
+enum _otaa_join_cmd_t { JOIN = 0, FORCE };
+enum _window_delay_t {
+ RECEIVE_DELAY1 = 0,
+ RECEIVE_DELAY2,
+ JOIN_ACCEPT_DELAY1,
+ JOIN_ACCEPT_DELAY2
+};
+enum _band_width_t { BW125 = 125, BW250 = 250, BW500 = 500 };
+enum _spreading_factor_t {
+ SF12 = 12,
+ SF11 = 11,
+ SF10 = 10,
+ SF9 = 9,
+ SF8 = 8,
+ SF7 = 7
+};
+enum _data_rate_t {
+ DR0 = 0,
+ DR1,
+ DR2,
+ DR3,
+ DR4,
+ DR5,
+ DR6,
+ DR7,
+ DR8,
+ DR9,
+ DR10,
+ DR11,
+ DR12,
+ DR13,
+ DR14,
+ DR15
+};
+
+/*****************************************************************
+Type DataRate Configuration BitRate| TxPower Configuration
+EU434 0 SF12/125 kHz 250 | 0 10dBm
+ 1 SF11/125 kHz 440 | 1 7 dBm
+ 2 SF10/125 kHz 980 | 2 4 dBm
+ 3 SF9 /125 kHz 1760 | 3 1 dBm
+ 4 SF8 /125 kHz 3125 | 4 -2dBm
+ 5 SF7 /125 kHz 5470 | 5 -5dBm
+ 6 SF7 /250 kHz 11000 | 6:15 RFU
+ 7 FSK:50 kbps 50000 |
+ 8:15 RFU |
+******************************************************************
+Type DataRate Configuration BitRate| TxPower Configuration
+EU868 0 SF12/125 kHz 250 | 0 20dBm
+ 1 SF11/125 kHz 440 | 1 14dBm
+ 2 SF10/125 kHz 980 | 2 11dBm
+ 3 SF9 /125 kHz 1760 | 3 8 dBm
+ 4 SF8 /125 kHz 3125 | 4 5 dBm
+ 5 SF7 /125 kHz 5470 | 5 2 dBm
+ 6 SF7 /250 kHz 11000 | 6:15 RFU
+ 7 FSK:50 kbps 50000 |
+ 8:15 RFU |
+******************************************************************
+Type DataRate Configuration BitRate| TxPower Configuration
+US915 0 SF10/125 kHz 980 | 0 30dBm
+ 1 SF9 /125 kHz 1760 | 1 28dBm
+ 2 SF8 /125 kHz 3125 | 2 26dBm
+ 3 SF7 /125 kHz 5470 | 3 24dBm
+ 4 SF8 /500 kHz 12500 | 4 22dBm
+ 5:7 RFU | 5 20dBm
+ 8 SF12/500 kHz 980 | 6 18dBm
+ 9 SF11/500 kHz 1760 | 7 16dBm
+ 10 SF10/500 kHz 3900 | 8 14dBm
+ 11 SF9 /500 kHz 7000 | 9 12dBm
+ 12 SF8 /500 kHz 12500 | 10 10dBm
+ 13 SF7 /500 kHz 21900 | 11:15 RFU
+ 14:15 RFU |
+*******************************************************************
+Type DataRate Configuration BitRate| TxPower Configuration
+CN780 0 SF12/125 kHz 250 | 0 10dBm
+ 1 SF11/125 kHz 440 | 1 7 dBm
+ 2 SF10/125 kHz 980 | 2 4 dBm
+ 3 SF9 /125 kHz 1760 | 3 1 dBm
+ 4 SF8 /125 kHz 3125 | 4 -2dBm
+ 5 SF7 /125 kHz 5470 | 5 -5dBm
+ 6 SF7 /250 kHz 11000 | 6:15 RFU
+ 7 FSK:50 kbps 50000 |
+ 8:15 RFU |
+******************************************************************/
+
+class LoRaWanClass {
+ public:
+ LoRaWanClass(void);
+
+ /**
+ * \brief Initialize the conmunication interface
+ *
+ * \return Return null
+ */
+ void init(void);
+
+ /**
+ * \brief Read the version from device
+ *
+ * \param [in] *buffer The output data cache
+ * \param [in] length The length of data cache
+ * \param [in] timeout The over time of read
+ *
+ * \return Return null.
+ */
+ void getVersion(char *buffer, short length,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+
+ /**
+ * \brief Read the ID from device
+ *
+ * \param [in] *buffer The output data cache
+ * \param [in] length The length of data cache
+ * \param [in] timeout The over time of read
+ *
+ * \return Return null.
+ */
+ void getId(char *buffer, short length,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+
+ /**
+ * \brief Set the ID
+ *
+ * \param [in] *DevAddr The end-device address
+ * \param [in] *DevEUI The end-device identifier
+ * \param [in] *AppEUI The application identifier
+ *
+ * \return Return null.
+ */
+ void setId(char *DevAddr, char *DevEUI, char *AppEUI);
+
+ /**
+ * \brief Set the key
+ *
+ * \param [in] *NwkSKey The network session key
+ * \param [in] *AppSKey The application session key
+ * \param [in] *AppKey The Application key
+ *
+ * \return Return null.
+ */
+ void setKey(char *NwkSKey, char *AppSKey, char *AppKey);
+
+ /**
+ * \brief Set the data rate
+ *
+ * \param [in] dataRate The date rate of encoding
+ * \param [in] physicalType The type of ISM
+ *
+ * \return Return null.
+ */
+ bool setDataRate(_data_rate_t dataRate = DR0,
+ _physical_type_t physicalType = EU434);
+
+ /**
+ * \brief ON/OFF adaptive data rate mode
+ *
+ * \param [in] command The date rate of encoding
+ *
+ * \return Return null.
+ */
+ void setAdaptiveDataRate(bool command);
+
+ /**
+ * \brief Set the output power
+ *
+ * \param [in] power The output power value
+ *
+ * \return Return null.
+ */
+ void setPower(short power);
+
+ /**
+ * \brief Set the port number
+ *
+ * \param [in] port The port number, range from 1 to 255
+ *
+ * \return Return null.
+ */
+ void setPort(unsigned char port);
+
+ /**
+ * \brief Set the channel parameter
+ *
+ * \param [in] channel The channel number, range from 0 to 15
+ * \param [in] frequency The frequency value
+ *
+ * \return Return null.
+ */
+ void getChannel(void);
+ void setChannel(unsigned char channel, float frequency);
+ /**
+ * \brief Set the channel parameter
+ *
+ * \param [in] channel The channel number, range from 0 to 15
+ * \param [in] frequency The frequency value. Set frequecy zero to disable
+ * one channel \param [in] dataRata The date rate of channel
+ *
+ * \return Return null.
+ */
+ void setChannel(unsigned char channel, float frequency,
+ _data_rate_t dataRata);
+ /**
+ * \brief Set the channel parameter
+ *
+ * \param [in] channel The channel number, range from 0 to 15
+ * \param [in] frequency The frequency value
+ * \param [in] dataRataMin The minimum date rate of channel
+ * \param [in] dataRataMax The maximum date rate of channel
+ *
+ * \return Return null.
+ */
+ void setChannel(unsigned char channel, float frequency,
+ _data_rate_t dataRataMin, _data_rate_t dataRataMax);
+
+ /**
+ * \brief Transfer the data
+ *
+ * \param [in] *buffer The transfer data cache
+ * \param [in] timeout The over time of transfer
+ *
+ * \return Return bool. Ture : transfer done, false : transfer failed
+ */
+ bool transferPacket(char *buffer, unsigned char timeout = DEFAULT_TIMEOUT);
+ /**
+ * \brief Transfer the data
+ *
+ * \param [in] *buffer The transfer data cache
+ * \param [in] length The length of data cache
+ * \param [in] timeout The over time of transfer
+ *
+ * \return Return bool. Ture : transfer done, false : transfer failed
+ */
+ bool transferPacket(unsigned char *buffer, unsigned char length,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+ /**
+ * \brief Transfer the packet data
+ *
+ * \param [in] *buffer The transfer data cache
+ * \param [in] timeout The over time of transfer
+ *
+ * \return Return bool. Ture : Confirmed ACK, false : Confirmed NOT ACK
+ */
+ bool transferPacketWithConfirmed(char *buffer,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+ /**
+ * \brief Transfer the data
+ *
+ * \param [in] *buffer The transfer data cache
+ * \param [in] length The length of data cache
+ * \param [in] timeout The over time of transfer
+ *
+ * \return Return bool. Ture : Confirmed ACK, false : Confirmed NOT ACK
+ */
+ bool transferPacketWithConfirmed(unsigned char *buffer,
+ unsigned char length,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+
+ /**
+ * \brief Receive the data
+ *
+ * \param [in] *buffer The receive data cache
+ * \param [in] length The length of data cache
+ * \param [in] *rssi The RSSI cache
+ *
+ * \return Return Receive data number
+ */
+ short receivePacket(char *buffer, short length, short *rssi);
+
+ /**
+ * \brief Transfer the proprietary data
+ *
+ * \param [in] *buffer The transfer data cache
+ * \param [in] timeout The over time of transfer
+ *
+ * \return Return bool. Ture : transfer done, false : transfer failed
+ */
+ bool transferProprietaryPacket(char *buffer,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+ /**
+ * \brief Transfer the proprietary data
+ *
+ * \param [in] *buffer The transfer data cache
+ * \param [in] length The length of data cache
+ * \param [in] timeout The over time of transfer
+ *
+ * \return Return bool. Ture : transfer done, false : transfer failed
+ */
+ bool transferProprietaryPacket(unsigned char *buffer, unsigned char length,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+
+ /**
+ * \brief Set device mode
+ *
+ * \param [in] mode The mode of device
+ *
+ * \return Return null
+ */
+ bool setDeviceMode(_device_mode_t mode);
+
+ /**
+ * \brief Set device join a network
+ *
+ * \param [in] command The type of join
+ * \param [in] timeout The over time of join
+ *
+ * \return Return bool. True : join OK, false : join NOT OK
+ */
+ bool setOTAAJoin(_otaa_join_cmd_t command,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+
+ /**
+ * \brief Set message unconfirmed repeat time
+ *
+ * \param [in] time The repeat time, range from 1 to 15
+ *
+ * \return Return null
+ */
+ void setUnconfirmedMessageRepeatTime(unsigned char time);
+
+ /**
+ * \brief Set message retry times time
+ *
+ * \param [in] time The retry time, range from 0 to 254
+ *
+ * \return Return null
+ */
+ void setConfirmedMessageRetryTime(unsigned char time);
+
+ /**
+ * \brief ON/OFF receice window 1
+ *
+ * \param [in] command The true : ON, false OFF
+ *
+ * \return Return null
+ */
+ void getReceiveWindowFirst(void);
+ void setReceiveWindowFirst(bool command);
+ /**
+ * \brief Set receice window 1 channel mapping
+ *
+ * \param [in] channel The channel number, range from 0 to 71
+ * \param [in] frequency The frequency value of channel
+ *
+ * \return Return null
+ */
+ void setReceiveWindowFirst(unsigned char channel, float frequency);
+
+ /**
+ * \brief Set receice window 2 channel mapping
+ *
+ * \param [in] frequency The frequency value of channel
+ * \param [in] dataRate The date rate value
+ *
+ * \return Return null
+ */
+ void setReceiveWindowSecond(float frequency, _data_rate_t dataRate);
+ /**
+ * \brief Set receice window 2 channel mapping
+ *
+ * \param [in] frequency The frequency value of channel
+ * \param [in] spreadingFactor The spreading factor value
+ * \param [in] bandwidth The band width value
+ *
+ * \return Return null
+ */
+ void setReceiveWindowSecond(float frequency,
+ _spreading_factor_t spreadingFactor,
+ _band_width_t bandwidth);
+
+ /**
+ * \brief Set receice window delay
+ *
+ * \param [in] command The delay type
+ * \param [in] _delay The delay value(millisecond)
+ *
+ * \return Return null
+ */
+ void setReceiveWindowDelay(_window_delay_t command, unsigned short _delay);
+
+ /**
+ * \brief Set LoRaWAN class type
+ *
+ * \param [in] type The class type
+ *
+ * \return Return null
+ */
+ void setClassType(_class_type_t type);
+
+ /**
+ * \brief Set device into low power mode
+ *
+ * \return Return null
+ */
+ void setDeviceLowPower(void);
+
+ /**
+ * \brief Reset device
+ *
+ * \return Return null
+ */
+ void setDeviceReset(void);
+
+ /**
+ * \brief Setup device default
+ *
+ * \return Return null
+ */
+ void setDeviceDefault(void);
+
+ /**
+ * \brief Initialize device into P2P mode
+ *
+ * \param [in] frequency The ISM frequency value
+ * \param [in] spreadingFactor The spreading factor value
+ * \param [in] bandwidth The band width value
+ * \param [in] txPreamble The transfer packet preamble number
+ * \param [in] rxPreamble The receive packet preamble number
+ * \param [in] power The output power
+ *
+ * \return Return null
+ */
+ void initP2PMode(unsigned short frequency = 433,
+ _spreading_factor_t spreadingFactor = SF12,
+ _band_width_t bandwidth = BW125,
+ unsigned char txPreamble = 8, unsigned char rxPreamble = 8,
+ short power = 20);
+ /**
+ * \brief Transfer the data
+ *
+ * \param [in] *buffer The transfer data cache
+ *
+ * \return Return bool. Ture : transfer done, false : transfer failed
+ */
+ void transferPacketP2PMode(char *buffer);
+ /**
+ * \brief Transfer the data
+ *
+ * \param [in] *buffer The transfer data cache
+ * \param [in] length The length of data cache
+ *
+ * \return Return bool. Ture : transfer done, false : transfer failed
+ */
+ void transferPacketP2PMode(unsigned char *buffer, unsigned char length);
+ /**
+ * \brief Receive the data
+ *
+ * \param [in] *buffer The receive data cache
+ * \param [in] length The length of data cache
+ * \param [in] *rssi The RSSI cache
+ * \param [in] timeout The over time of receive
+ *
+ * \return Return Receive data number
+ */
+ short receivePacketP2PMode(unsigned char *buffer, short length, short *rssi,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+
+ /**
+ * \brief LoRaWAN raw data
+ *
+ * \return Return null
+ */
+ void loraDebug(void);
+#if _DEBUG_SERIAL_
+ void loraDebugPrint(unsigned int timeout);
+#endif
+ void debugPrint(const char *str);
+ /**
+ * \brief Read battery voltage
+ *
+ * \return Return battery voltage
+ */
+ // short getBatteryVoltage(void);
+
+ private:
+ void sendCommand(const char *command);
+ void sendCommand(const __FlashStringHelper *command);
+ short readBuffer(char *buffer, short length,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+ short readLine(char *buffer, short length,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+ short waitForResponse(char *response,
+ unsigned char timeout = DEFAULT_TIMEOUT);
+ short sendCommandAndWaitForResponse(
+ char *command, char *response, unsigned char timeout = DEFAULT_TIMEOUT);
+
+ char _buffer[256];
+ short debug;
+ _physical_type_t myType;
+};
+
+extern LoRaWanClass lora;
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5Display.cpp b/lib/M5Stack/src/M5Display.cpp
new file mode 100644
index 000000000..9294e4d07
--- /dev/null
+++ b/lib/M5Stack/src/M5Display.cpp
@@ -0,0 +1,637 @@
+#if defined (CORE)
+#include "M5Display.h"
+
+#define BLK_PWM_CHANNEL 7 // LEDC_CHANNEL_7
+
+M5Display::M5Display() : TFT_eSPI() {
+}
+
+void M5Display::begin() {
+ TFT_eSPI::begin();
+ setRotation(1);
+ fillScreen(0);
+
+ // Init the back-light LED PWM
+ ledcSetup(BLK_PWM_CHANNEL, 44100, 8);
+ ledcAttachPin(TFT_BL, BLK_PWM_CHANNEL);
+ ledcWrite(BLK_PWM_CHANNEL, 80);
+}
+
+void M5Display::sleep() {
+ startWrite();
+ writecommand(ILI9341_SLPIN); // Software reset
+ endWrite();
+}
+
+void M5Display::wakeup() {
+ startWrite();
+ writecommand(ILI9341_SLPOUT);
+ endWrite();
+}
+
+void M5Display::setBrightness(uint8_t brightness) {
+ ledcWrite(BLK_PWM_CHANNEL, brightness);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h, data);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint16_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h, data);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data, uint16_t transparent) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h, data,
+ transparent);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint8_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h,
+ (const uint16_t *)data);
+ setSwapBytes(swap);
+}
+
+void M5Display::drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint8_t *data) {
+ bool swap = getSwapBytes();
+ setSwapBytes(true);
+ pushImage((int32_t)x0, (int32_t)y0, (uint32_t)w, (uint32_t)h,
+ (uint16_t *)data);
+ setSwapBytes(swap);
+}
+
+void M5Display::progressBar(int x, int y, int w, int h, uint8_t val) {
+ drawRect(x, y, w, h, 0x09F1);
+ fillRect(x + 1, y + 1, w * (((float)val) / 100.0), h - 1, 0x09F1);
+}
+/*
+#include "utility/qrcode.h"
+void M5Display::qrcode(const char *string, uint16_t x, uint16_t y,
+ uint8_t width, uint8_t version) {
+ // Create the QR code
+ QRCode qrcode;
+ uint8_t qrcodeData[qrcode_getBufferSize(version)];
+ qrcode_initText(&qrcode, qrcodeData, version, 0, string);
+
+ // Top quiet zone
+ uint8_t thickness = width / qrcode.size;
+ uint16_t lineLength = qrcode.size * thickness;
+ uint8_t xOffset = x + (width - lineLength) / 2;
+ uint8_t yOffset = y + (width - lineLength) / 2;
+ fillRect(x, y, width, width, TFT_WHITE);
+
+ for (uint8_t y = 0; y < qrcode.size; y++) {
+ for (uint8_t x = 0; x < qrcode.size; x++) {
+ uint8_t q = qrcode_getModule(&qrcode, x, y);
+ if (q)
+ fillRect(x * thickness + xOffset, y * thickness + yOffset,
+ thickness, thickness, TFT_BLACK);
+ }
+ }
+}
+
+void M5Display::qrcode(const String &string, uint16_t x, uint16_t y,
+ uint8_t width, uint8_t version) {
+ int16_t len = string.length() + 2;
+ char buffer[len];
+ string.toCharArray(buffer, len);
+ qrcode(buffer, x, y, width, version);
+}
+*/
+// These read 16- and 32-bit types from the SD card file.
+// BMP data is stored little-endian, Arduino is little-endian too.
+// May need to reverse subscript order if porting elsewhere.
+
+uint16_t read16(fs::File &f) {
+ uint16_t result;
+ ((uint8_t *)&result)[0] = f.read(); // LSB
+ ((uint8_t *)&result)[1] = f.read(); // MSB
+ return result;
+}
+
+uint32_t read32(fs::File &f) {
+ uint32_t result;
+ ((uint8_t *)&result)[0] = f.read(); // LSB
+ ((uint8_t *)&result)[1] = f.read();
+ ((uint8_t *)&result)[2] = f.read();
+ ((uint8_t *)&result)[3] = f.read(); // MSB
+ return result;
+}
+
+// Bodmers BMP image rendering function
+void M5Display::drawBmpFile(fs::FS &fs, const char *path, uint16_t x,
+ uint16_t y) {
+ if ((x >= width()) || (y >= height())) return;
+
+ // Open requested file on SD card
+ File bmpFS = fs.open(path, "r");
+
+ if (!bmpFS) {
+ Serial.print("File not found");
+ return;
+ }
+
+ uint32_t seekOffset;
+ uint16_t w, h, row, col;
+ uint8_t r, g, b;
+
+ uint32_t startTime = millis();
+
+ if (read16(bmpFS) == 0x4D42) {
+ read32(bmpFS);
+ read32(bmpFS);
+ seekOffset = read32(bmpFS);
+ read32(bmpFS);
+ w = read32(bmpFS);
+ h = read32(bmpFS);
+
+ if ((read16(bmpFS) == 1) && (read16(bmpFS) == 24) &&
+ (read32(bmpFS) == 0)) {
+ y += h - 1;
+
+ setSwapBytes(true);
+ bmpFS.seek(seekOffset);
+
+ uint16_t padding = (4 - ((w * 3) & 3)) & 3;
+ uint8_t lineBuffer[w * 3 + padding];
+
+ for (row = 0; row < h; row++) {
+ bmpFS.read(lineBuffer, sizeof(lineBuffer));
+ uint8_t *bptr = lineBuffer;
+ uint16_t *tptr = (uint16_t *)lineBuffer;
+ // Convert 24 to 16 bit colours
+ for (col = 0; col < w; col++) {
+ b = *bptr++;
+ g = *bptr++;
+ r = *bptr++;
+ *tptr++ = ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+ }
+
+ // Push the pixel row to screen, pushImage will crop the line if
+ // needed y is decremented as the BMP image is drawn bottom up
+ pushImage(x, y--, w, 1, (uint16_t *)lineBuffer);
+ }
+ Serial.print("Loaded in ");
+ Serial.print(millis() - startTime);
+ Serial.println(" ms");
+ } else
+ Serial.println("BMP format not recognized.");
+ }
+ bmpFS.close();
+}
+
+// void M5Display::drawBmp(fs::FS &fs, const char *path, uint16_t x, uint16_t y)
+// {
+// drawBmpFile(fs, path, x, y);
+// }
+/***************************************************
+ This library is written to be compatible with Adafruit's ILI9341
+ library and automatically detects the display type on ESP_WROVER_KITs
+ Earlier WROVERs had ILI9341, while newer releases have ST7789V
+
+ MIT license, all text above must be included in any redistribution
+ ****************************************************/
+
+/*
+ * JPEG
+ */
+
+#include "rom/tjpgd.h"
+
+#define jpgColor(c) \
+ (((uint16_t)(((uint8_t *)(c))[0] & 0xF8) << 8) | \
+ ((uint16_t)(((uint8_t *)(c))[1] & 0xFC) << 3) | \
+ ((((uint8_t *)(c))[2] & 0xF8) >> 3))
+
+#if ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_ERROR
+const char *jd_errors[] = {"Succeeded",
+ "Interrupted by output function",
+ "Device error or wrong termination of input stream",
+ "Insufficient memory pool for the image",
+ "Insufficient stream input buffer",
+ "Parameter error",
+ "Data format error",
+ "Right format but not supported",
+ "Not supported JPEG standard"};
+#endif
+
+typedef struct {
+ uint16_t x;
+ uint16_t y;
+ uint16_t maxWidth;
+ uint16_t maxHeight;
+ uint16_t offX;
+ uint16_t offY;
+ jpeg_div_t scale;
+ const void *src;
+ size_t len;
+ size_t index;
+ M5Display *tft;
+ uint16_t outWidth;
+ uint16_t outHeight;
+} jpg_file_decoder_t;
+
+static uint32_t jpgReadFile(JDEC *decoder, uint8_t *buf, uint32_t len) {
+ jpg_file_decoder_t *jpeg = (jpg_file_decoder_t *)decoder->device;
+ File *file = (File *)jpeg->src;
+ if (buf) {
+ return file->read(buf, len);
+ } else {
+ file->seek(len, SeekCur);
+ }
+ return len;
+}
+
+static uint32_t jpgRead(JDEC *decoder, uint8_t *buf, uint32_t len) {
+ jpg_file_decoder_t *jpeg = (jpg_file_decoder_t *)decoder->device;
+ if (buf) {
+ memcpy(buf, (const uint8_t *)jpeg->src + jpeg->index, len);
+ }
+ jpeg->index += len;
+ return len;
+}
+
+static uint32_t jpgWrite(JDEC *decoder, void *bitmap, JRECT *rect) {
+ jpg_file_decoder_t *jpeg = (jpg_file_decoder_t *)decoder->device;
+ uint16_t x = rect->left;
+ uint16_t y = rect->top;
+ uint16_t w = rect->right + 1 - x;
+ uint16_t h = rect->bottom + 1 - y;
+ uint16_t oL = 0, oR = 0;
+ uint8_t *data = (uint8_t *)bitmap;
+
+ if (rect->right < jpeg->offX) {
+ return 1;
+ }
+ if (rect->left >= (jpeg->offX + jpeg->outWidth)) {
+ return 1;
+ }
+ if (rect->bottom < jpeg->offY) {
+ return 1;
+ }
+ if (rect->top >= (jpeg->offY + jpeg->outHeight)) {
+ return 1;
+ }
+ if (rect->top < jpeg->offY) {
+ uint16_t linesToSkip = jpeg->offY - rect->top;
+ data += linesToSkip * w * 3;
+ h -= linesToSkip;
+ y += linesToSkip;
+ }
+ if (rect->bottom >= (jpeg->offY + jpeg->outHeight)) {
+ uint16_t linesToSkip =
+ (rect->bottom + 1) - (jpeg->offY + jpeg->outHeight);
+ h -= linesToSkip;
+ }
+ if (rect->left < jpeg->offX) {
+ oL = jpeg->offX - rect->left;
+ }
+ if (rect->right >= (jpeg->offX + jpeg->outWidth)) {
+ oR = (rect->right + 1) - (jpeg->offX + jpeg->outWidth);
+ }
+
+ uint16_t pixBuf[32];
+ uint8_t pixIndex = 0;
+ uint16_t line;
+
+ jpeg->tft->startWrite();
+ // jpeg->tft->setAddrWindow(x - jpeg->offX + jpeg->x + oL, y - jpeg->offY +
+ // jpeg->y, w - (oL + oR), h);
+ jpeg->tft->setWindow(x - jpeg->offX + jpeg->x + oL,
+ y - jpeg->offY + jpeg->y,
+ x - jpeg->offX + jpeg->x + oL + w - (oL + oR) - 1,
+ y - jpeg->offY + jpeg->y + h - 1);
+
+ while (h--) {
+ data += 3 * oL;
+ line = w - (oL + oR);
+ while (line--) {
+ pixBuf[pixIndex++] = jpgColor(data);
+ data += 3;
+ if (pixIndex == 32) {
+ jpeg->tft->writePixels(pixBuf, 32);
+ // SPI.writePixels((uint8_t *)pixBuf, 64);
+ pixIndex = 0;
+ }
+ }
+ data += 3 * oR;
+ }
+ if (pixIndex) {
+ jpeg->tft->writePixels(pixBuf, pixIndex);
+ // SPI.writePixels((uint8_t *)pixBuf, pixIndex * 2);
+ }
+ jpeg->tft->endWrite();
+ return 1;
+}
+
+static bool jpgDecode(jpg_file_decoder_t *jpeg,
+ uint32_t (*reader)(JDEC *, uint8_t *, uint32_t)) {
+ static uint8_t work[3100];
+ JDEC decoder;
+
+ JRESULT jres = jd_prepare(&decoder, reader, work, 3100, jpeg);
+ if (jres != JDR_OK) {
+ log_e("jd_prepare failed! %s", jd_errors[jres]);
+ return false;
+ }
+
+ uint16_t jpgWidth = decoder.width / (1 << (uint8_t)(jpeg->scale));
+ uint16_t jpgHeight = decoder.height / (1 << (uint8_t)(jpeg->scale));
+
+ if (jpeg->offX >= jpgWidth || jpeg->offY >= jpgHeight) {
+ log_e("Offset Outside of JPEG size");
+ return false;
+ }
+
+ size_t jpgMaxWidth = jpgWidth - jpeg->offX;
+ size_t jpgMaxHeight = jpgHeight - jpeg->offY;
+
+ jpeg->outWidth =
+ (jpgMaxWidth > jpeg->maxWidth) ? jpeg->maxWidth : jpgMaxWidth;
+ jpeg->outHeight =
+ (jpgMaxHeight > jpeg->maxHeight) ? jpeg->maxHeight : jpgMaxHeight;
+
+ jres = jd_decomp(&decoder, jpgWrite, (uint8_t)jpeg->scale);
+ if (jres != JDR_OK) {
+ log_e("jd_decomp failed! %s", jd_errors[jres]);
+ return false;
+ }
+
+ return true;
+}
+
+void M5Display::drawJpg(const uint8_t *jpg_data, size_t jpg_len, uint16_t x,
+ uint16_t y, uint16_t maxWidth, uint16_t maxHeight,
+ uint16_t offX, uint16_t offY, jpeg_div_t scale) {
+ if ((x + maxWidth) > width() || (y + maxHeight) > height()) {
+ log_e("Bad dimensions given");
+ return;
+ }
+
+ jpg_file_decoder_t jpeg;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ jpeg.src = jpg_data;
+ jpeg.len = jpg_len;
+ jpeg.index = 0;
+ jpeg.x = x;
+ jpeg.y = y;
+ jpeg.maxWidth = maxWidth;
+ jpeg.maxHeight = maxHeight;
+ jpeg.offX = offX;
+ jpeg.offY = offY;
+ jpeg.scale = scale;
+ jpeg.tft = this;
+
+ jpgDecode(&jpeg, jpgRead);
+}
+
+void M5Display::drawJpgFile(fs::FS &fs, const char *path, uint16_t x,
+ uint16_t y, uint16_t maxWidth, uint16_t maxHeight,
+ uint16_t offX, uint16_t offY, jpeg_div_t scale) {
+ if ((x + maxWidth) > width() || (y + maxHeight) > height()) {
+ log_e("Bad dimensions given");
+ return;
+ }
+
+ File file = fs.open(path);
+ if (!file) {
+ log_e("Failed to open file for reading");
+ return;
+ }
+
+ jpg_file_decoder_t jpeg;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ jpeg.src = &file;
+ jpeg.len = file.size();
+ jpeg.index = 0;
+ jpeg.x = x;
+ jpeg.y = y;
+ jpeg.maxWidth = maxWidth;
+ jpeg.maxHeight = maxHeight;
+ jpeg.offX = offX;
+ jpeg.offY = offY;
+ jpeg.scale = scale;
+ jpeg.tft = this;
+
+ jpgDecode(&jpeg, jpgReadFile);
+
+ file.close();
+}
+
+/*
+ * PNG
+ */
+
+#include <HTTPClient.h>
+
+#include "utility/pngle.h"
+
+typedef struct _png_draw_params {
+ uint16_t x;
+ uint16_t y;
+ uint16_t maxWidth;
+ uint16_t maxHeight;
+ uint16_t offX;
+ uint16_t offY;
+ double scale;
+ uint8_t alphaThreshold;
+
+ M5Display *tft;
+} png_file_decoder_t;
+
+static void pngle_draw_callback(pngle_t *pngle, uint32_t x, uint32_t y,
+ uint32_t w, uint32_t h, uint8_t rgba[4]) {
+ png_file_decoder_t *p = (png_file_decoder_t *)pngle_get_user_data(pngle);
+ uint16_t color = jpgColor(rgba); // XXX: It's PNG ;)
+
+ if (x < p->offX || y < p->offY) return;
+ x -= p->offX;
+ y -= p->offY;
+
+ // An interlaced file with alpha channel causes disaster, so use 1 here for
+ // simplicity
+ w = 1;
+ h = 1;
+
+ if (p->scale != 1.0) {
+ x = (uint32_t)ceil(x * p->scale);
+ y = (uint32_t)ceil(y * p->scale);
+ w = (uint32_t)ceil(w * p->scale);
+ h = (uint32_t)ceil(h * p->scale);
+ }
+
+ if (x >= p->maxWidth || y >= p->maxHeight) return;
+ if (x + w >= p->maxWidth) w = p->maxWidth - x;
+ if (y + h >= p->maxHeight) h = p->maxHeight - y;
+
+ x += p->x;
+ y += p->y;
+
+ if (rgba[3] >= p->alphaThreshold) {
+ p->tft->fillRect(x, y, w, h, color);
+ }
+}
+
+void M5Display::drawPngFile(fs::FS &fs, const char *path, uint16_t x,
+ uint16_t y, uint16_t maxWidth, uint16_t maxHeight,
+ uint16_t offX, uint16_t offY, double scale,
+ uint8_t alphaThreshold) {
+ File file = fs.open(path);
+ if (!file) {
+ log_e("Failed to open file for reading");
+ return;
+ }
+
+ pngle_t *pngle = pngle_new();
+
+ png_file_decoder_t png;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ png.x = x;
+ png.y = y;
+ png.maxWidth = maxWidth;
+ png.maxHeight = maxHeight;
+ png.offX = offX;
+ png.offY = offY;
+ png.scale = scale;
+ png.alphaThreshold = alphaThreshold;
+ png.tft = this;
+
+ pngle_set_user_data(pngle, &png);
+ pngle_set_draw_callback(pngle, pngle_draw_callback);
+
+ // Feed data to pngle
+ uint8_t buf[1024];
+ int remain = 0;
+ int len;
+ while ((len = file.read(buf + remain, sizeof(buf) - remain)) > 0) {
+ int fed = pngle_feed(pngle, buf, remain + len);
+ if (fed < 0) {
+ log_e("[pngle error] %s", pngle_error(pngle));
+ break;
+ }
+
+ remain = remain + len - fed;
+ if (remain > 0) memmove(buf, buf + fed, remain);
+ }
+
+ pngle_destroy(pngle);
+ file.close();
+}
+
+void M5Display::drawPngUrl(const char *url, uint16_t x, uint16_t y,
+ uint16_t maxWidth, uint16_t maxHeight, uint16_t offX,
+ uint16_t offY, double scale,
+ uint8_t alphaThreshold) {
+ HTTPClient http;
+
+ if (WiFi.status() != WL_CONNECTED) {
+ log_e("Not connected");
+ return;
+ }
+
+ http.begin(url);
+
+ int httpCode = http.GET();
+ if (httpCode != HTTP_CODE_OK) {
+ log_e("HTTP ERROR: %d\n", httpCode);
+ http.end();
+ return;
+ }
+ // get length of document (is -1 when Server sends no Content-Length header)
+ int len = http.getSize();
+
+ WiFiClient *stream = http.getStreamPtr();
+
+ pngle_t *pngle = pngle_new();
+
+ png_file_decoder_t png;
+
+ if (!maxWidth) {
+ maxWidth = width() - x;
+ }
+ if (!maxHeight) {
+ maxHeight = height() - y;
+ }
+
+ png.x = x;
+ png.y = y;
+ png.maxWidth = maxWidth;
+ png.maxHeight = maxHeight;
+ png.offX = offX;
+ png.offY = offY;
+ png.scale = scale;
+ png.alphaThreshold = alphaThreshold;
+ png.tft = this;
+
+ pngle_set_user_data(pngle, &png);
+ pngle_set_draw_callback(pngle, pngle_draw_callback);
+
+ // Feed data to pngle
+ uint8_t buf[1024];
+ int remain = 0;
+ int c;
+ while (http.connected() && (len > 0 || len == -1)) {
+ size_t size = stream->available();
+ if (!size) {
+ delay(1);
+ continue;
+ }
+
+ if (size > sizeof(buf) - remain) size = sizeof(buf) - remain;
+ if ((c = stream->readBytes(buf + remain, size)) > 0) {
+ int fed = pngle_feed(pngle, buf, remain + c);
+ if (fed < 0) {
+ log_e("[pngle error] %s", pngle_error(pngle));
+ break;
+ }
+
+ remain = remain + c - fed;
+ if (remain > 0) memmove(buf, buf + fed, remain);
+
+ if (len > 0) {
+ len -= c;
+ }
+ }
+ }
+
+ pngle_destroy(pngle);
+ http.end();
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5Display.h b/lib/M5Stack/src/M5Display.h
new file mode 100644
index 000000000..4ce289b6a
--- /dev/null
+++ b/lib/M5Stack/src/M5Display.h
@@ -0,0 +1,118 @@
+#if defined (CORE)
+#ifndef _M5DISPLAY_H_
+#define _M5DISPLAY_H_
+
+#include <Arduino.h>
+#include <FS.h>
+#include <SPI.h>
+
+#include <TFT_eSPI.h>
+//#include "utility/In_eSPI.h"
+//#include "utility/Sprite.h"
+
+typedef enum {
+ JPEG_DIV_NONE,
+ JPEG_DIV_2,
+ JPEG_DIV_4,
+ JPEG_DIV_8,
+ JPEG_DIV_MAX
+} jpeg_div_t;
+
+class M5Display : public TFT_eSPI {
+ public:
+ M5Display();
+ void begin();
+ void sleep();
+ void wakeup();
+ void setBrightness(uint8_t brightness);
+ void clearDisplay(uint32_t color = ILI9341_BLACK) {
+ fillScreen(color);
+ }
+ void clear(uint32_t color = ILI9341_BLACK) {
+ fillScreen(color);
+ }
+ void display() {
+ }
+
+ inline void startWrite(void) {
+#if defined(SPI_HAS_TRANSACTION) && defined(SUPPORT_TRANSACTIONS) && \
+ !defined(ESP32_PARALLEL)
+ if (locked) {
+ locked = false;
+ SPI.beginTransaction(
+ SPISettings(SPI_FREQUENCY, MSBFIRST, SPI_MODE0));
+ }
+#endif
+ CS_L;
+ }
+ inline void endWrite(void) {
+#if defined(SPI_HAS_TRANSACTION) && defined(SUPPORT_TRANSACTIONS) && \
+ !defined(ESP32_PARALLEL)
+ if (!inTransaction) {
+ if (!locked) {
+ locked = true;
+ SPI.endTransaction();
+ }
+ }
+#endif
+ CS_H;
+ }
+ inline void writePixel(uint16_t color) {
+ SPI.write16(color);
+ }
+ inline void writePixels(uint16_t *colors, uint32_t len) {
+ SPI.writePixels((uint8_t *)colors, len * 2);
+ }
+ void progressBar(int x, int y, int w, int h, uint8_t val);
+
+#define setFont setFreeFont
+
+ void qrcode(const char *string, uint16_t x = 50, uint16_t y = 10,
+ uint8_t width = 220, uint8_t version = 6);
+ void qrcode(const String &string, uint16_t x = 50, uint16_t y = 10,
+ uint8_t width = 220, uint8_t version = 6);
+
+ void drawBmp(fs::FS &fs, const char *path, uint16_t x, uint16_t y);
+ void drawBmpFile(fs::FS &fs, const char *path, uint16_t x, uint16_t y);
+
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint8_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint16_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ uint8_t *data);
+ void drawBitmap(int16_t x0, int16_t y0, int16_t w, int16_t h,
+ const uint16_t *data, uint16_t transparent);
+
+ void drawJpg(const uint8_t *jpg_data, size_t jpg_len, uint16_t x = 0,
+ uint16_t y = 0, uint16_t maxWidth = 0, uint16_t maxHeight = 0,
+ uint16_t offX = 0, uint16_t offY = 0,
+ jpeg_div_t scale = JPEG_DIV_NONE);
+
+ void drawJpg(fs::FS &fs, const char *path, uint16_t x = 0, uint16_t y = 0,
+ uint16_t maxWidth = 0, uint16_t maxHeight = 0,
+ uint16_t offX = 0, uint16_t offY = 0,
+ jpeg_div_t scale = JPEG_DIV_NONE);
+
+ void drawJpgFile(fs::FS &fs, const char *path, uint16_t x = 0,
+ uint16_t y = 0, uint16_t maxWidth = 0,
+ uint16_t maxHeight = 0, uint16_t offX = 0,
+ uint16_t offY = 0, jpeg_div_t scale = JPEG_DIV_NONE);
+
+ void drawPngFile(fs::FS &fs, const char *path, uint16_t x = 0,
+ uint16_t y = 0, uint16_t maxWidth = 0,
+ uint16_t maxHeight = 0, uint16_t offX = 0,
+ uint16_t offY = 0, double scale = 1.0,
+ uint8_t alphaThreshold = 127);
+
+ void drawPngUrl(const char *url, uint16_t x = 0, uint16_t y = 0,
+ uint16_t maxWidth = 0, uint16_t maxHeight = 0,
+ uint16_t offX = 0, uint16_t offY = 0, double scale = 1.0,
+ uint8_t alphaThreshold = 127);
+
+ private:
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5Faces.cpp b/lib/M5Stack/src/M5Faces.cpp
new file mode 100644
index 000000000..aadffd9d8
--- /dev/null
+++ b/lib/M5Stack/src/M5Faces.cpp
@@ -0,0 +1,42 @@
+#if defined (CORE)
+// Copyright (c) M5Stack. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full
+// license information.
+
+#include "M5Faces.h"
+
+// ================ Faces Keyboard ===================
+#define KEYBOARD_I2C_ADDR 0x08
+#define KEYBOARD_INT 5
+#define READI2CSUBADDR 0
+
+/*----------------------------------------------------------------------*
+ * Extentions for Facces *
+ *----------------------------------------------------------------------*/
+bool M5Faces::canControlFaces() {
+ return M5.I2C.writeCommand(KEYBOARD_I2C_ADDR, READI2CSUBADDR);
+}
+
+M5Faces::M5Faces() {
+ pinMode(KEYBOARD_INT, INPUT_PULLUP);
+}
+
+uint8_t M5Faces::getch(void) {
+ uint8_t data = 0x00;
+
+ if (kbhit()) {
+ while (data == 0x00) {
+ if (!M5.I2C.readByte(KEYBOARD_I2C_ADDR, &data)) {
+ return 0x00;
+ }
+ delay(1);
+ }
+ return data;
+ }
+ return 0x00;
+}
+
+bool M5Faces::kbhit(void) {
+ return (digitalRead(KEYBOARD_INT) == LOW);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5Faces.h b/lib/M5Stack/src/M5Faces.h
new file mode 100644
index 000000000..2626565bb
--- /dev/null
+++ b/lib/M5Stack/src/M5Faces.h
@@ -0,0 +1,21 @@
+#if defined (CORE)
+// Copyright (c) M5Stack. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full
+// license information.
+#ifndef M5Faces_h
+#define M5Faces_h
+
+#include "M5Stack.h"
+
+class M5Faces {
+ public:
+ M5Faces();
+ bool canControlFaces();
+ uint8_t getch(void);
+ bool kbhit(void);
+
+ private:
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5LoRa.cpp b/lib/M5Stack/src/M5LoRa.cpp
new file mode 100644
index 000000000..9b74e2c01
--- /dev/null
+++ b/lib/M5Stack/src/M5LoRa.cpp
@@ -0,0 +1,508 @@
+#if defined (CORE)
+// Copyright (c) Sandeep Mistry. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full
+// license information.
+
+#include "M5LoRa.h"
+
+// registers
+#define REG_FIFO 0x00
+#define REG_OP_MODE 0x01
+#define REG_FRF_MSB 0x06
+#define REG_FRF_MID 0x07
+#define REG_FRF_LSB 0x08
+#define REG_PA_CONFIG 0x09
+#define REG_LNA 0x0c
+#define REG_FIFO_ADDR_PTR 0x0d
+#define REG_FIFO_TX_BASE_ADDR 0x0e
+#define REG_FIFO_RX_BASE_ADDR 0x0f
+#define REG_FIFO_RX_CURRENT_ADDR 0x10
+#define REG_IRQ_FLAGS 0x12
+#define REG_RX_NB_BYTES 0x13
+#define REG_PKT_RSSI_VALUE 0x1a
+#define REG_PKT_SNR_VALUE 0x1b
+#define REG_MODEM_CONFIG_1 0x1d
+#define REG_MODEM_CONFIG_2 0x1e
+#define REG_PREAMBLE_MSB 0x20
+#define REG_PREAMBLE_LSB 0x21
+#define REG_PAYLOAD_LENGTH 0x22
+#define REG_MODEM_CONFIG_3 0x26
+#define REG_RSSI_WIDEBAND 0x2c
+#define REG_DETECTION_OPTIMIZE 0x31
+#define REG_DETECTION_THRESHOLD 0x37
+#define REG_SYNC_WORD 0x39
+#define REG_DIO_MAPPING_1 0x40
+#define REG_VERSION 0x42
+
+// modes
+#define MODE_LONG_RANGE_MODE 0x80
+#define MODE_SLEEP 0x00
+#define MODE_STDBY 0x01
+#define MODE_TX 0x03
+#define MODE_RX_CONTINUOUS 0x05
+#define MODE_RX_SINGLE 0x06
+
+// PA config
+#define PA_BOOST 0x80
+
+// IRQ masks
+#define IRQ_TX_DONE_MASK 0x08
+#define IRQ_PAYLOAD_CRC_ERROR_MASK 0x20
+#define IRQ_RX_DONE_MASK 0x40
+
+#define MAX_PKT_LENGTH 255
+
+LoRaClass::LoRaClass()
+ : _spiSettings(8E6, MSBFIRST, SPI_MODE0),
+ _ss(LORA_DEFAULT_SS_PIN),
+ _reset(LORA_DEFAULT_RESET_PIN),
+ _dio0(LORA_DEFAULT_DIO0_PIN),
+ _frequency(0),
+ _packetIndex(0),
+ _implicitHeaderMode(0),
+ _onReceive(NULL) {
+ // overide Stream timeout value
+ setTimeout(0);
+}
+
+int LoRaClass::begin(long frequency) {
+ // setup pins
+ pinMode(_ss, OUTPUT);
+ // set SS high
+ digitalWrite(_ss, HIGH);
+
+ if (_reset != -1) {
+ pinMode(_reset, OUTPUT);
+
+ // perform reset
+ digitalWrite(_reset, LOW);
+ delay(10);
+ digitalWrite(_reset, HIGH);
+ delay(10);
+ }
+
+ // start SPI
+ SPI.begin();
+
+ // check version
+ uint8_t version = readRegister(REG_VERSION);
+ if (version != 0x12) {
+ return 0;
+ }
+
+ // put in sleep mode
+ sleep();
+
+ // set frequency
+ setFrequency(frequency);
+
+ // set base addresses
+ writeRegister(REG_FIFO_TX_BASE_ADDR, 0);
+ writeRegister(REG_FIFO_RX_BASE_ADDR, 0);
+
+ // set LNA boost
+ writeRegister(REG_LNA, readRegister(REG_LNA) | 0x03);
+
+ // set auto AGC
+ writeRegister(REG_MODEM_CONFIG_3, 0x04);
+
+ // set output power to 17 dBm
+ setTxPower(17);
+
+ // put in standby mode
+ idle();
+
+ return 1;
+}
+
+void LoRaClass::end() {
+ // put in sleep mode
+ sleep();
+
+ // stop SPI
+ SPI.end();
+}
+
+int LoRaClass::beginPacket(int implicitHeader) {
+ // put in standby mode
+ idle();
+
+ if (implicitHeader) {
+ implicitHeaderMode();
+ } else {
+ explicitHeaderMode();
+ }
+
+ // reset FIFO address and paload length
+ writeRegister(REG_FIFO_ADDR_PTR, 0);
+ writeRegister(REG_PAYLOAD_LENGTH, 0);
+
+ return 1;
+}
+
+int LoRaClass::endPacket() {
+ // put in TX mode
+ writeRegister(REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_TX);
+
+ // wait for TX done
+ while ((readRegister(REG_IRQ_FLAGS) & IRQ_TX_DONE_MASK) == 0) {
+ yield();
+ }
+
+ // clear IRQ's
+ writeRegister(REG_IRQ_FLAGS, IRQ_TX_DONE_MASK);
+
+ return 1;
+}
+
+int LoRaClass::parsePacket(int size) {
+ int packetLength = 0;
+ int irqFlags = readRegister(REG_IRQ_FLAGS);
+
+ if (size > 0) {
+ implicitHeaderMode();
+
+ writeRegister(REG_PAYLOAD_LENGTH, size & 0xff);
+ } else {
+ explicitHeaderMode();
+ }
+
+ // clear IRQ's
+ writeRegister(REG_IRQ_FLAGS, irqFlags);
+
+ if ((irqFlags & IRQ_RX_DONE_MASK) &&
+ (irqFlags & IRQ_PAYLOAD_CRC_ERROR_MASK) == 0) {
+ // received a packet
+ _packetIndex = 0;
+
+ // read packet length
+ if (_implicitHeaderMode) {
+ packetLength = readRegister(REG_PAYLOAD_LENGTH);
+ } else {
+ packetLength = readRegister(REG_RX_NB_BYTES);
+ }
+
+ // set FIFO address to current RX address
+ writeRegister(REG_FIFO_ADDR_PTR,
+ readRegister(REG_FIFO_RX_CURRENT_ADDR));
+
+ // put in standby mode
+ idle();
+ } else if (readRegister(REG_OP_MODE) !=
+ (MODE_LONG_RANGE_MODE | MODE_RX_SINGLE)) {
+ // not currently in RX mode
+
+ // reset FIFO address
+ writeRegister(REG_FIFO_ADDR_PTR, 0);
+
+ // put in single RX mode
+ writeRegister(REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_RX_SINGLE);
+ }
+
+ return packetLength;
+}
+
+int LoRaClass::packetRssi() {
+ return (readRegister(REG_PKT_RSSI_VALUE) -
+ (_frequency < 868E6 ? 164 : 157));
+}
+
+float LoRaClass::packetSnr() {
+ return ((int8_t)readRegister(REG_PKT_SNR_VALUE)) * 0.25;
+}
+
+size_t LoRaClass::write(uint8_t byte) {
+ return write(&byte, sizeof(byte));
+}
+
+size_t LoRaClass::write(const uint8_t* buffer, size_t size) {
+ int currentLength = readRegister(REG_PAYLOAD_LENGTH);
+
+ // check size
+ if ((currentLength + size) > MAX_PKT_LENGTH) {
+ size = MAX_PKT_LENGTH - currentLength;
+ }
+
+ // write data
+ for (size_t i = 0; i < size; i++) {
+ writeRegister(REG_FIFO, buffer[i]);
+ }
+
+ // update length
+ writeRegister(REG_PAYLOAD_LENGTH, currentLength + size);
+
+ return size;
+}
+
+int LoRaClass::available() {
+ return (readRegister(REG_RX_NB_BYTES) - _packetIndex);
+}
+
+int LoRaClass::read() {
+ if (!available()) {
+ return -1;
+ }
+
+ _packetIndex++;
+
+ return readRegister(REG_FIFO);
+}
+
+int LoRaClass::peek() {
+ if (!available()) {
+ return -1;
+ }
+
+ // store current FIFO address
+ int currentAddress = readRegister(REG_FIFO_ADDR_PTR);
+
+ // read
+ uint8_t b = readRegister(REG_FIFO);
+
+ // restore FIFO address
+ writeRegister(REG_FIFO_ADDR_PTR, currentAddress);
+
+ return b;
+}
+
+void LoRaClass::flush() {
+}
+
+void LoRaClass::onReceive(void (*callback)(int)) {
+ _onReceive = callback;
+
+ if (callback) {
+ pinMode(_dio0, INPUT);
+
+ writeRegister(REG_DIO_MAPPING_1, 0x00);
+
+ attachInterrupt(digitalPinToInterrupt(_dio0), LoRaClass::onDio0Rise,
+ RISING);
+ } else {
+ detachInterrupt(digitalPinToInterrupt(_dio0));
+ }
+}
+
+void LoRaClass::receive(int size) {
+ if (size > 0) {
+ implicitHeaderMode();
+
+ writeRegister(REG_PAYLOAD_LENGTH, size & 0xff);
+ } else {
+ explicitHeaderMode();
+ }
+
+ writeRegister(REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_RX_CONTINUOUS);
+}
+
+void LoRaClass::idle() {
+ writeRegister(REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_STDBY);
+}
+
+void LoRaClass::sleep() {
+ writeRegister(REG_OP_MODE, MODE_LONG_RANGE_MODE | MODE_SLEEP);
+}
+
+void LoRaClass::setTxPower(int level, int outputPin) {
+ if (PA_OUTPUT_RFO_PIN == outputPin) {
+ // RFO
+ if (level < 0) {
+ level = 0;
+ } else if (level > 14) {
+ level = 14;
+ }
+
+ writeRegister(REG_PA_CONFIG, 0x70 | level);
+ } else {
+ // PA BOOST
+ if (level < 2) {
+ level = 2;
+ } else if (level > 17) {
+ level = 17;
+ }
+
+ writeRegister(REG_PA_CONFIG, PA_BOOST | (level - 2));
+ }
+}
+
+void LoRaClass::setFrequency(long frequency) {
+ _frequency = frequency;
+
+ uint64_t frf = ((uint64_t)frequency << 19) / 32000000;
+
+ writeRegister(REG_FRF_MSB, (uint8_t)(frf >> 16));
+ writeRegister(REG_FRF_MID, (uint8_t)(frf >> 8));
+ writeRegister(REG_FRF_LSB, (uint8_t)(frf >> 0));
+}
+
+void LoRaClass::setSpreadingFactor(int sf) {
+ if (sf < 6) {
+ sf = 6;
+ } else if (sf > 12) {
+ sf = 12;
+ }
+
+ if (sf == 6) {
+ writeRegister(REG_DETECTION_OPTIMIZE, 0xc5);
+ writeRegister(REG_DETECTION_THRESHOLD, 0x0c);
+ } else {
+ writeRegister(REG_DETECTION_OPTIMIZE, 0xc3);
+ writeRegister(REG_DETECTION_THRESHOLD, 0x0a);
+ }
+
+ writeRegister(
+ REG_MODEM_CONFIG_2,
+ (readRegister(REG_MODEM_CONFIG_2) & 0x0f) | ((sf << 4) & 0xf0));
+}
+
+void LoRaClass::setSignalBandwidth(long sbw) {
+ int bw;
+
+ if (sbw <= 7.8E3) {
+ bw = 0;
+ } else if (sbw <= 10.4E3) {
+ bw = 1;
+ } else if (sbw <= 15.6E3) {
+ bw = 2;
+ } else if (sbw <= 20.8E3) {
+ bw = 3;
+ } else if (sbw <= 31.25E3) {
+ bw = 4;
+ } else if (sbw <= 41.7E3) {
+ bw = 5;
+ } else if (sbw <= 62.5E3) {
+ bw = 6;
+ } else if (sbw <= 125E3) {
+ bw = 7;
+ } else if (sbw <= 250E3) {
+ bw = 8;
+ } else /*if (sbw <= 250E3)*/ {
+ bw = 9;
+ }
+
+ writeRegister(REG_MODEM_CONFIG_1,
+ (readRegister(REG_MODEM_CONFIG_1) & 0x0f) | (bw << 4));
+}
+
+void LoRaClass::setCodingRate4(int denominator) {
+ if (denominator < 5) {
+ denominator = 5;
+ } else if (denominator > 8) {
+ denominator = 8;
+ }
+
+ int cr = denominator - 4;
+
+ writeRegister(REG_MODEM_CONFIG_1,
+ (readRegister(REG_MODEM_CONFIG_1) & 0xf1) | (cr << 1));
+}
+
+void LoRaClass::setPreambleLength(long length) {
+ writeRegister(REG_PREAMBLE_MSB, (uint8_t)(length >> 8));
+ writeRegister(REG_PREAMBLE_LSB, (uint8_t)(length >> 0));
+}
+
+void LoRaClass::setSyncWord(int sw) {
+ writeRegister(REG_SYNC_WORD, sw);
+}
+
+void LoRaClass::enableCrc() {
+ writeRegister(REG_MODEM_CONFIG_2, readRegister(REG_MODEM_CONFIG_2) | 0x04);
+}
+
+void LoRaClass::disableCrc() {
+ writeRegister(REG_MODEM_CONFIG_2, readRegister(REG_MODEM_CONFIG_2) & 0xfb);
+}
+
+byte LoRaClass::random() {
+ return readRegister(REG_RSSI_WIDEBAND);
+}
+
+void LoRaClass::setPins(int ss, int reset, int dio0) {
+ _ss = ss;
+ _reset = reset;
+ _dio0 = dio0;
+}
+
+void LoRaClass::setSPIFrequency(uint32_t frequency) {
+ _spiSettings = SPISettings(frequency, MSBFIRST, SPI_MODE0);
+}
+
+void LoRaClass::dumpRegisters(Stream& out) {
+ for (int i = 0; i < 128; i++) {
+ out.print("0x");
+ out.print(i, HEX);
+ out.print(": 0x");
+ out.println(readRegister(i), HEX);
+ }
+}
+
+void LoRaClass::explicitHeaderMode() {
+ _implicitHeaderMode = 0;
+
+ writeRegister(REG_MODEM_CONFIG_1, readRegister(REG_MODEM_CONFIG_1) & 0xfe);
+}
+
+void LoRaClass::implicitHeaderMode() {
+ _implicitHeaderMode = 1;
+
+ writeRegister(REG_MODEM_CONFIG_1, readRegister(REG_MODEM_CONFIG_1) | 0x01);
+}
+
+void LoRaClass::handleDio0Rise() {
+ int irqFlags = readRegister(REG_IRQ_FLAGS);
+
+ // clear IRQ's
+ writeRegister(REG_IRQ_FLAGS, irqFlags);
+
+ if ((irqFlags & IRQ_PAYLOAD_CRC_ERROR_MASK) == 0) {
+ // received a packet
+ _packetIndex = 0;
+
+ // read packet length
+ int packetLength = _implicitHeaderMode
+ ? readRegister(REG_PAYLOAD_LENGTH)
+ : readRegister(REG_RX_NB_BYTES);
+
+ // set FIFO address to current RX address
+ writeRegister(REG_FIFO_ADDR_PTR,
+ readRegister(REG_FIFO_RX_CURRENT_ADDR));
+
+ if (_onReceive) {
+ _onReceive(packetLength);
+ }
+
+ // reset FIFO address
+ writeRegister(REG_FIFO_ADDR_PTR, 0);
+ }
+}
+
+uint8_t LoRaClass::readRegister(uint8_t address) {
+ return singleTransfer(address & 0x7f, 0x00);
+}
+
+void LoRaClass::writeRegister(uint8_t address, uint8_t value) {
+ singleTransfer(address | 0x80, value);
+}
+
+uint8_t LoRaClass::singleTransfer(uint8_t address, uint8_t value) {
+ uint8_t response;
+
+ digitalWrite(_ss, LOW);
+
+ SPI.beginTransaction(_spiSettings);
+ SPI.transfer(address);
+ response = SPI.transfer(value);
+ SPI.endTransaction();
+
+ digitalWrite(_ss, HIGH);
+
+ return response;
+}
+
+void LoRaClass::onDio0Rise() {
+ LoRa.handleDio0Rise();
+}
+
+LoRaClass LoRa;
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5LoRa.h b/lib/M5Stack/src/M5LoRa.h
new file mode 100644
index 000000000..4a1d2d9fc
--- /dev/null
+++ b/lib/M5Stack/src/M5LoRa.h
@@ -0,0 +1,108 @@
+#if defined (CORE)
+// Copyright (c) Sandeep Mistry. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full
+// license information.
+
+// 23FEB18 gojimmypi reversed LORA_DEFAULT_RESET_PIN and LORA_DEFAULT_DIO0_PIN
+// pin numbers reset on pin 36; irq on pin 26
+
+// 12NOV2020 Zontex reversed LORA_DEFAULT_RESET_PIN and LORA_DEFAULT_DIO0_PIN
+// pin numbers reset on pin 26; irq on pin 36
+
+#ifndef M5LORA_H
+#define M5LORA_H
+
+#include <Arduino.h>
+#include <SPI.h>
+
+#define LORA_DEFAULT_SS_PIN 5
+#define LORA_DEFAULT_RESET_PIN 26
+#define LORA_DEFAULT_DIO0_PIN 36
+
+#define PA_OUTPUT_RFO_PIN 0
+#define PA_OUTPUT_PA_BOOST_PIN 1
+
+class LoRaClass : public Stream {
+ public:
+ LoRaClass();
+
+ int begin(long frequency);
+ void end();
+
+ int beginPacket(int implicitHeader = false);
+ int endPacket();
+
+ int parsePacket(int size = 0);
+ int packetRssi();
+ float packetSnr();
+
+ // from Print
+ virtual size_t write(uint8_t byte);
+ virtual size_t write(const uint8_t* buffer, size_t size);
+
+ // from Stream
+ virtual int available();
+ virtual int read();
+ virtual int peek();
+ virtual void flush();
+
+ void onReceive(void (*callback)(int));
+
+ void receive(int size = 0);
+ void idle();
+ void sleep();
+
+ void setTxPower(int level, int outputPin = PA_OUTPUT_PA_BOOST_PIN);
+ void setFrequency(long frequency);
+ void setSpreadingFactor(int sf);
+ void setSignalBandwidth(long sbw);
+ void setCodingRate4(int denominator);
+ void setPreambleLength(long length);
+ void setSyncWord(int sw);
+ void enableCrc();
+ void disableCrc();
+
+ // deprecated
+ void crc() {
+ enableCrc();
+ }
+ void noCrc() {
+ disableCrc();
+ }
+
+ byte random();
+
+ void setPins(int ss = LORA_DEFAULT_SS_PIN,
+ int reset = LORA_DEFAULT_RESET_PIN,
+ int dio0 = LORA_DEFAULT_DIO0_PIN);
+ void setSPIFrequency(uint32_t frequency);
+
+ void dumpRegisters(Stream& out);
+
+ private:
+ void explicitHeaderMode();
+ void implicitHeaderMode();
+
+ void handleDio0Rise();
+
+ uint8_t readRegister(uint8_t address);
+ void writeRegister(uint8_t address, uint8_t value);
+ uint8_t singleTransfer(uint8_t address, uint8_t value);
+
+ static void onDio0Rise();
+
+ private:
+ SPISettings _spiSettings;
+ int _ss;
+ int _reset;
+ int _dio0;
+ int _frequency;
+ int _packetIndex;
+ int _implicitHeaderMode;
+ void (*_onReceive)(int);
+};
+
+extern LoRaClass LoRa;
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5Stack.cpp b/lib/M5Stack/src/M5Stack.cpp
new file mode 100644
index 000000000..ee77574ff
--- /dev/null
+++ b/lib/M5Stack/src/M5Stack.cpp
@@ -0,0 +1,96 @@
+#if defined (CORE)
+// Copyright (c) M5Stack. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full
+// license information.
+
+#include "M5Stack.h"
+
+M5Stack::M5Stack() : isInited(0) {
+}
+
+void M5Stack::begin(bool LCDEnable, bool SDEnable, bool SerialEnable, bool I2CEnable) {
+ // Correct init once
+ if (isInited == true) {
+ return;
+ } else {
+ isInited = true;
+ }
+
+ for (auto gpio : (const uint8_t[]){18, 19, 23}) {
+ *(volatile uint32_t*)(GPIO_PIN_MUX_REG[gpio]) |= FUN_DRV_M;
+ gpio_pulldown_dis((gpio_num_t)gpio);
+ gpio_pullup_en((gpio_num_t)gpio);
+ }
+
+ // UART
+ if (SerialEnable == true) {
+ Serial.begin(115200);
+ Serial.flush();
+ delay(50);
+ Serial.println("M5Stack initializing...");
+ }
+
+ // TF Card
+ if (SDEnable == true) {
+ SD.begin(TFCARD_CS_PIN, SPI, 40000000);
+ }
+
+ // LCD INIT
+ if (LCDEnable == true) {
+ Lcd.begin();
+ }
+
+ // TONE
+ // Speaker.begin();
+
+ // Set wakeup button
+ Power.setWakeupButton(BUTTON_A_PIN);
+
+ // I2C init
+ if (I2CEnable == true) {
+ Wire.begin(21, 22);
+ }
+
+ if (SerialEnable == true) {
+ Serial.println("OK");
+ }
+
+ // if use M5GO button, need set gpio15 OD or PP mode to avoid affecting the
+ // wifi signal
+ pinMode(15, OUTPUT_OPEN_DRAIN);
+}
+
+void M5Stack::update() {
+ // Button update
+ BtnA.read();
+ BtnB.read();
+ BtnC.read();
+
+ // Speaker update
+ Speaker.update();
+}
+
+/**
+ * Function has been move to Power class.(for compatibility)
+ * This name will be removed in a future release.
+ */
+void M5Stack::setPowerBoostKeepOn(bool en) {
+ M5.Power.setPowerBoostKeepOn(en);
+}
+/**
+ * Function has been move to Power class.(for compatibility)
+ * This name will be removed in a future release.
+ */
+void M5Stack::setWakeupButton(uint8_t button) {
+ M5.Power.setWakeupButton(button);
+}
+/**
+ * Function has been move to Power class.(for compatibility)
+ * This name will be removed in a future release.
+ */
+void M5Stack::powerOFF() {
+ M5.Power.deepSleep();
+}
+
+M5Stack M5;
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/M5Stack.h b/lib/M5Stack/src/M5Stack.h
new file mode 100644
index 000000000..7f413ec2c
--- /dev/null
+++ b/lib/M5Stack/src/M5Stack.h
@@ -0,0 +1,186 @@
+#if defined (CORE)
+// Copyright (c) M5Stack. All rights reserved.
+
+// Licensed under the MIT license. See LICENSE file in the project root for full
+// license information.
+/**
+ * \par Copyright (C), 2016-2017, M5Stack
+ * \class M5Stack
+ * \brief M5Stack library.
+ * @file M5Stack.h
+ * @author M5Stack
+ * @version V0.2.4
+ * @date 2018/10/29
+ * @brief Header for M5Stack.cpp module
+ *
+ * \par Description
+ * This file is a driver for M5Stack Core/Basic/Gray.
+ *
+ * \par Method List:
+ *
+ * System:
+ M5.begin();
+ M5.update();
+
+ Power:
+ M5.Power.setPowerBoostKeepOn()
+ M5.Power.setCharge(uint8_t mode);
+ M5.Power.setPowerBoostKeepOn(bool en);
+ M5.Power.isChargeFull();
+ M5.Power.setWakeupButton(uint8_t button);
+ M5.Power.powerOFF();
+
+ bool setPowerBoostOnOff(bool en);
+ bool setPowerBoostSet(bool en);
+ bool setPowerVin(bool en);
+ bool setPowerWLEDSet(bool en);
+
+ LCD:
+ M5.lcd.setBrightness(uint8_t brightness);
+ M5.Lcd.drawPixel(int16_t x, int16_t y, uint16_t color);
+ M5.Lcd.drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint16_t color);
+ M5.Lcd.fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color);
+ M5.Lcd.fillScreen(uint16_t color); M5.Lcd.drawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color);
+ M5.Lcd.drawCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername,uint16_t color);
+ M5.Lcd.fillCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color);
+ M5.Lcd.fillCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername,int16_t delta, uint16_t color);
+ M5.Lcd.drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color);
+ M5.Lcd.fillTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color);
+ M5.Lcd.drawRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h, int16_t radius, uint16_t color);
+ M5.Lcd.fillRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h, int16_t radius, uint16_t color);
+ M5.Lcd.drawBitmap(int16_t x, int16_t y, const uint8_t bitmap[], int16_t w, int16_t h, uint16_t color);
+ M5.Lcd.drawRGBBitmap(int16_t x, int16_t y, const uint16_t bitmap[], int16_t w, int16_t h),
+ M5.Lcd.drawChar(uint16_t x, uint16_t y, char c, uint16_t color, uint16_t bg, uint8_t size);
+ M5.Lcd.setCursor(uint16_t x0, uint16_t y0);
+ M5.Lcd.setTextColor(uint16_t color);
+ M5.Lcd.setTextColor(uint16_t color, uint16_t backgroundcolor);
+ M5.Lcd.setTextSize(uint8_t size);
+ M5.Lcd.setTextWrap(boolean w);
+ M5.Lcd.printf();
+ M5.Lcd.print();
+ M5.Lcd.println();
+ M5.Lcd.drawCentreString(const char *string, int dX, int poY, int font);
+ M5.Lcd.drawRightString(const char *string, int dX, int poY, int font);
+ M5.Lcd.drawJpg(const uint8_t *jpg_data, size_t jpg_len, uint16_t x, uint16_t y);
+ M5.Lcd.drawJpgFile(fs::FS &fs, const char *path, uint16_t x, uint16_t y);
+ M5.Lcd.drawBmpFile(fs::FS &fs, const char *path, uint16_t x, uint16_t y);
+
+ Button:
+ M5.BtnA/B/C.read(); // Must be called in loop(), or via update()
+ M5.BtnA/B/C.isPressed();
+ M5.BtnA/B/C.isReleased();
+ M5.BtnA/B/C.wasPressed();
+ M5.BtnA/B/C.wasReleased();
+ M5.BtnA/B/C.wasreleasedFor()
+ M5.BtnA/B/C.pressedFor(uint32_t ms);
+ M5.BtnA/B/C.releasedFor(uint32_t ms);
+ M5.BtnA/B/C.lastChange();
+
+ Speaker:
+ M5.Speaker.tone(uint32_t freq);
+ M5.Speaker.tone(freq, time);
+ M5.Speaker.beep();
+ M5.Speaker.setBeep(uint16_t frequency, uint16_t duration);
+ M5.Speaker.mute();
+
+ *
+ * \par History:
+ * <pre>
+ * `<Author>` `<Time>` `<Version>` `<Descr>`
+ * Zibin Zheng 2017/07/14 0.0.1 Rebuild the new.
+ * Bin 2018/10/29 0.2.4 Add Button API
+ * </pre>
+ *
+ */
+// #define ESP32
+
+#ifndef _M5STACK_H_
+#define _M5STACK_H_
+
+#if defined(ESP32)
+
+#include <Arduino.h>
+#include <SPI.h>
+#include <Wire.h>
+
+#include "FS.h"
+#include "M5Display.h"
+#include "SD.h"
+#include "gitTagVersion.h"
+#include "IMU.h"
+#include "utility/Button.h"
+#include "utility/CommUtil.h"
+#include "utility/Config.h"
+#include "utility/Power.h"
+#include "utility/Speaker.h"
+#include "utility/MPU6886.h"
+#include "utility/SH200Q.h"
+
+class M5Stack {
+ public:
+ M5Stack();
+ void begin(bool LCDEnable = true, bool SDEnable = true,
+ bool SerialEnable = true, bool I2CEnable = false);
+
+ // Updates the status of hardware buttons, and ends any completed tone on the speaker.
+ // Recommended to be placed in loop()
+ void update();
+
+ // LCD display, derived from TFT_eSPI. See examples
+ M5Display Lcd = M5Display();
+
+ // Power and battery charge control. Call Power.begin() in setup().
+ POWER Power;
+
+// Button API
+#define DEBOUNCE_MS 10
+
+ // Hardware button A. Call read() before checking if isPressed()
+ Button BtnA = Button(BUTTON_A_PIN, true, DEBOUNCE_MS);
+
+ // Hardware button B. Call read() before checking if isPressed()
+ Button BtnB = Button(BUTTON_B_PIN, true, DEBOUNCE_MS);
+
+ // Hardware button C. Call read() before checking if isPressed()
+ Button BtnC = Button(BUTTON_C_PIN, true, DEBOUNCE_MS);
+
+ // SPEAKER on DAC pin 25
+ SPEAKER Speaker;
+
+ // UART
+ // HardwareSerial Serial0 = HardwareSerial(0);
+ // HardwareSerial Serial2 = HardwareSerial(2);
+
+ // I2C
+ IMU Imu;
+ CommUtil I2C = CommUtil();
+
+ MPU6886 Mpu6886;
+ SH200Q Sh200Q;
+
+ /**
+ * Function has been moved to Power class (for compatibility)
+ * This name will be removed in a future release.
+ */
+ void setPowerBoostKeepOn(bool en) __attribute__((deprecated));
+ void setWakeupButton(uint8_t button) __attribute__((deprecated));
+ void powerOFF() __attribute__((deprecated));
+
+ private:
+ bool isInited;
+};
+
+extern M5Stack M5;
+#define m5 M5
+#define lcd Lcd
+#define imu Imu
+#define IMU Imu
+#define MPU6886 Mpu6886
+#define mpu6886 Mpu6886
+#define SH200Q Sh200Q
+#define sh200q Sh200Q
+#else
+#error "This library only supports boards with ESP32 processor."
+#endif
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/gitTagVersion.h b/lib/M5Stack/src/gitTagVersion.h
new file mode 100644
index 000000000..05d00fff2
--- /dev/null
+++ b/lib/M5Stack/src/gitTagVersion.h
@@ -0,0 +1 @@
+#define M5_LIB_VERSION F("0.2.3-dirty")
diff --git a/lib/M5Stack/src/utility/Button.cpp b/lib/M5Stack/src/utility/Button.cpp
new file mode 100644
index 000000000..fd281164d
--- /dev/null
+++ b/lib/M5Stack/src/utility/Button.cpp
@@ -0,0 +1,143 @@
+#if defined (CORE)
+/*----------------------------------------------------------------------*
+ * Arduino Button Library v1.0 *
+ * Jack Christensen May 2011, published Mar 2012 *
+ * *
+ * Library for reading momentary contact switches like tactile button *
+ * switches. Intended for use in state machine constructs. *
+ * Use the read() function to read all buttons in the main loop, *
+ * which should execute as fast as possible. *
+ * *
+ * This work is licensed under the Creative Commons Attribution- *
+ * ShareAlike 3.0 Unported License. To view a copy of this license, *
+ * visit http://creativecommons.org/licenses/by-sa/3.0/ or send a *
+ * letter to Creative Commons, 171 Second Street, Suite 300, *
+ * San Francisco, California, 94105, USA. *
+ *----------------------------------------------------------------------*/
+
+#include "Button.h"
+
+/*----------------------------------------------------------------------*
+ * Button(pin, puEnable, invert, dbTime) instantiates a button object. *
+ * pin Is the Arduino pin the button is connected to. *
+ * puEnable Enables the AVR internal pullup resistor if != 0 (can also *
+ * use true or false). *
+ * invert If invert == 0, interprets a high state as pressed, low as *
+ * released. If invert != 0, interprets a high state as *
+ * released, low as pressed (can also use true or false). *
+ * dbTime Is the debounce time in milliseconds. *
+ * *
+ * (Note that invert cannot be implied from puEnable since an external *
+ * pullup could be used.) *
+ *----------------------------------------------------------------------*/
+Button::Button(uint8_t pin, uint8_t invert, uint32_t dbTime) {
+ _pin = pin;
+ _invert = invert;
+ _dbTime = dbTime;
+ pinMode(_pin, INPUT_PULLUP);
+ _state = digitalRead(_pin);
+ if (_invert != 0) _state = !_state;
+ _time = millis();
+ _lastState = _state;
+ _changed = 0;
+ _hold_time = -1;
+ _lastTime = _time;
+ _lastChange = _time;
+ _pressTime = _time;
+}
+
+/*----------------------------------------------------------------------*
+ * read() returns the state of the button, 1==pressed, 0==released, *
+ * does debouncing, captures and maintains times, previous states, etc. *
+ *----------------------------------------------------------------------*/
+uint8_t Button::read(void) {
+ static uint32_t ms;
+ static uint8_t pinVal;
+
+ ms = millis();
+ pinVal = digitalRead(_pin);
+ if (_invert != 0) pinVal = !pinVal;
+ if (ms - _lastChange < _dbTime) {
+ _lastTime = _time;
+ _time = ms;
+ _changed = 0;
+ return _state;
+ } else {
+ _lastTime = _time;
+ _time = ms;
+ _lastState = _state;
+ _state = pinVal;
+ if (_state != _lastState) {
+ _lastChange = ms;
+ _changed = 1;
+ if (_state) {
+ _pressTime = _time;
+ }
+ } else {
+ _changed = 0;
+ }
+ return _state;
+ }
+}
+
+/*----------------------------------------------------------------------*
+ * isPressed() and isReleased() check the button state when it was last *
+ * read, and return false (0) or true (!=0) accordingly. *
+ * These functions do not cause the button to be read. *
+ *----------------------------------------------------------------------*/
+uint8_t Button::isPressed(void) {
+ return _state == 0 ? 0 : 1;
+}
+
+uint8_t Button::isReleased(void) {
+ return _state == 0 ? 1 : 0;
+}
+
+/*----------------------------------------------------------------------*
+ * wasPressed() and wasReleased() check the button state to see if it *
+ * changed between the last two reads and return false (0) or *
+ * true (!=0) accordingly. *
+ * These functions do not cause the button to be read. *
+ *----------------------------------------------------------------------*/
+uint8_t Button::wasPressed(void) {
+ return _state && _changed;
+}
+
+uint8_t Button::wasReleased(void) {
+ return !_state && _changed && millis() - _pressTime < _hold_time;
+}
+
+uint8_t Button::wasReleasefor(uint32_t ms) {
+ _hold_time = ms;
+ return !_state && _changed && millis() - _pressTime >= ms;
+}
+/*----------------------------------------------------------------------*
+ * pressedFor(ms) and releasedFor(ms) check to see if the button is *
+ * pressed (or released), and has been in that state for the specified *
+ * time in milliseconds. Returns false (0) or true (1) accordingly. *
+ * These functions do not cause the button to be read. *
+ *----------------------------------------------------------------------*/
+uint8_t Button::pressedFor(uint32_t ms) {
+ return (_state == 1 && _time - _lastChange >= ms) ? 1 : 0;
+}
+
+uint8_t Button::pressedFor(uint32_t ms, uint32_t continuous_time) {
+ if (_state == 1 && _time - _lastChange >= ms &&
+ _time - _lastLongPress >= continuous_time) {
+ _lastLongPress = _time;
+ return 1;
+ }
+ return 0;
+}
+
+uint8_t Button::releasedFor(uint32_t ms) {
+ return (_state == 0 && _time - _lastChange >= ms) ? 1 : 0;
+}
+/*----------------------------------------------------------------------*
+ * lastChange() returns the time the button last changed state, *
+ * in milliseconds. *
+ *----------------------------------------------------------------------*/
+uint32_t Button::lastChange(void) {
+ return _lastChange;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/Button.h b/lib/M5Stack/src/utility/Button.h
new file mode 100644
index 000000000..a8e82d23e
--- /dev/null
+++ b/lib/M5Stack/src/utility/Button.h
@@ -0,0 +1,50 @@
+
+#if defined (CORE)/*----------------------------------------------------------------------*
+ * Arduino Button Library v1.0 *
+ * Jack Christensen Mar 2012 *
+ * *
+ * This work is licensed under the Creative Commons Attribution- *
+ * ShareAlike 3.0 Unported License. To view a copy of this license, *
+ * visit http://creativecommons.org/licenses/by-sa/3.0/ or send a *
+ * letter to Creative Commons, 171 Second Street, Suite 300, *
+ * San Francisco, California, 94105, USA. *
+ *----------------------------------------------------------------------*/
+#ifndef Button_h
+#define Button_h
+// #if ARDUINO >= 100
+#include <Arduino.h>
+// #else
+// #include <WProgram.h>
+// #endif
+class Button {
+ public:
+ Button(uint8_t pin, uint8_t invert, uint32_t dbTime);
+ uint8_t read();
+ uint8_t isPressed();
+ uint8_t isReleased();
+ uint8_t wasPressed();
+ uint8_t wasReleased();
+ uint8_t pressedFor(uint32_t ms);
+ uint8_t pressedFor(uint32_t ms, uint32_t continuous_time);
+ uint8_t releasedFor(uint32_t ms);
+ uint8_t wasReleasefor(uint32_t ms);
+ uint32_t lastChange();
+
+ private:
+ uint8_t _pin; // arduino pin number
+ uint8_t _puEnable; // internal pullup resistor enabled
+ uint8_t _invert; // if 0, interpret high state as pressed, else interpret
+ // low state as pressed
+ uint8_t _state; // current button state
+ uint8_t _lastState; // previous button state
+ uint8_t _changed; // state changed since last read
+ uint32_t _time; // time of current state (all times are in ms)
+ uint32_t _lastTime; // time of previous state
+ uint32_t _lastChange; // time of last state change
+ uint32_t _lastLongPress; // time of last state change
+ uint32_t _dbTime; // debounce time
+ uint32_t _pressTime; // press time
+ uint32_t _hold_time; // hold time call wasreleasefor
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/CommUtil.cpp b/lib/M5Stack/src/utility/CommUtil.cpp
new file mode 100644
index 000000000..52a6c1ecd
--- /dev/null
+++ b/lib/M5Stack/src/utility/CommUtil.cpp
@@ -0,0 +1,165 @@
+#if defined (CORE)
+/*----------------------------------------------------------------------*
+ * M5Stack I2C Common Library v1.0 *
+ * *
+ * This work is licensed under the GNU Lesser General Public *
+ * License v2.1 *
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.en.html *
+ *----------------------------------------------------------------------*/
+#include "CommUtil.h"
+
+#include "../M5Stack.h"
+
+extern M5Stack M5;
+
+// debug for message of I2C ( bypass message to serial)
+//#define I2C_DEBUG_TO_SERIAL
+
+CommUtil::CommUtil() {
+}
+
+// Wire.h read and write protocols
+bool CommUtil::writeCommand(uint8_t address, uint8_t subAddress) {
+ bool function_result = false;
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ function_result = (Wire.endTransmission() == 0); // Send the Tx buffer
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("writeCommand:send to 0x%02x [0x%02x] result:%s\n", address,
+ subAddress, function_result ? "OK" : "NG");
+#endif
+
+ return (function_result);
+}
+
+// Wire.h read and write protocols
+bool CommUtil::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
+ bool function_result = false;
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ Wire.write(data); // Put data in Tx buffer
+ function_result = (Wire.endTransmission() == 0); // Send the Tx buffer
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("writeByte:send to 0x%02x [0x%2x] data=0x%02x result:%s\n",
+ address, subAddress, data, function_result ? "OK" : "NG");
+#endif
+
+ return (function_result);
+}
+
+// Wire.h read and write protocols
+bool CommUtil::writeBytes(uint8_t address, uint8_t subAddress, uint8_t *data,
+ uint8_t length) {
+ bool function_result = false;
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("writeBytes:send to 0x%02x [0x%02x] data=", address,
+ subAddress);
+#endif
+
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ for (int i = 0; i < length; i++) {
+ Wire.write(*(data + i)); // Put data in Tx buffer
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x ", *(data + i));
+#endif
+ }
+ function_result = (Wire.endTransmission() == 0); // Send the Tx buffer
+
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("result:%s\n", function_result ? "OK" : "NG");
+#endif
+
+ return function_result; // Send the Tx buffer
+}
+
+bool CommUtil::readByte(uint8_t address, uint8_t *result) {
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("readByte :read from 0x%02x requestByte=1 receive=", address);
+#endif
+
+ if (Wire.requestFrom(address, (uint8_t)1)) {
+ *result = Wire.read(); // Fill Rx buffer with result
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x\n", result);
+#endif
+ return true;
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("none\n");
+#endif
+ return false;
+}
+
+bool CommUtil::readByte(uint8_t address, uint8_t subAddress, uint8_t *result) {
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("readByte :read from 0x%02x [0x%02x] requestByte=1 receive=",
+ address, subAddress);
+#endif
+
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ if (Wire.endTransmission(false) == 0 &&
+ Wire.requestFrom(address, (uint8_t)1)) {
+ *result = Wire.read(); // Fill Rx buffer with result
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x\n", *result);
+#endif
+ return true;
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("none\n");
+#endif
+ return false;
+}
+
+bool CommUtil::readBytes(uint8_t address, uint8_t subAddress, uint8_t count,
+ uint8_t *dest) {
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("readBytes:read from 0x%02x [0x%02x] requestByte=%d receive=",
+ address, subAddress, count);
+#endif
+
+ Wire.beginTransmission(address); // Initialize the Tx buffer
+ Wire.write(subAddress); // Put slave register address in Tx buffer
+ uint8_t i = 0;
+ if (Wire.endTransmission(false) == 0 &&
+ Wire.requestFrom(address, (uint8_t)count)) {
+ while (Wire.available()) {
+ dest[i++] = Wire.read(); // Put read results in the Rx buffer
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("%02x ", dest[i - 1]);
+#endif
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf(" (len:%d)\n", i);
+#endif
+ return true;
+ }
+#ifdef I2C_DEBUG_TO_SERIAL
+ Serial.printf("none\n");
+#endif
+ return false;
+}
+
+bool CommUtil::readBytes(uint8_t address, uint8_t count, uint8_t *dest) {
+ uint8_t i = 0;
+ if (Wire.requestFrom(address, (uint8_t)count)) {
+ while (Wire.available()) {
+ // Put read results in the Rx buffer
+ dest[i++] = Wire.read();
+ }
+ return true;
+ }
+ return false;
+}
+
+void CommUtil::scanID(bool *result) {
+ for (int i = 0x00; i <= 0x7f; i++) {
+ *(result + i) = writeCommand(i, 0x00);
+ }
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/CommUtil.h b/lib/M5Stack/src/utility/CommUtil.h
new file mode 100644
index 000000000..b01cc4ffe
--- /dev/null
+++ b/lib/M5Stack/src/utility/CommUtil.h
@@ -0,0 +1,32 @@
+#if defined (CORE)
+/*----------------------------------------------------------------------*
+ * M5Stack I2C Common Library v1.0 *
+ * *
+ * This work is licensed under the GNU Lesser General Public *
+ * License v2.1 *
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.en.html *
+ *----------------------------------------------------------------------*/
+#ifndef CommUtil_h
+#define CommUtil_h
+
+#include <Arduino.h>
+#include <Wire.h>
+
+class CommUtil {
+ public:
+ CommUtil();
+ bool writeCommand(uint8_t address, uint8_t subAddress);
+ bool writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+ bool writeBytes(uint8_t address, uint8_t subAddress, uint8_t *data,
+ uint8_t length);
+ bool readByte(uint8_t address, uint8_t *result);
+ bool readByte(uint8_t address, uint8_t subAddress, uint8_t *result);
+ bool readBytes(uint8_t address, uint8_t count, uint8_t *dest);
+ bool readBytes(uint8_t address, uint8_t subAddress, uint8_t count,
+ uint8_t *dest);
+ void scanID(bool *result);
+
+ private:
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/Config.h b/lib/M5Stack/src/utility/Config.h
new file mode 100644
index 000000000..f434044e3
--- /dev/null
+++ b/lib/M5Stack/src/utility/Config.h
@@ -0,0 +1,41 @@
+#if defined (CORE)
+#ifndef _CONFIG_H_
+#define _CONFIG_H_
+
+// Screen
+#define TFT_LED_PIN 32
+#define TFT_DC_PIN 27
+#define TFT_CS_PIN 14
+#define TFT_MOSI_PIN 23
+#define TFT_CLK_PIN 18
+#define TFT_RST_PIN 33
+#define TFT_MISO_PIN 19
+
+// SD card
+#define TFCARD_CS_PIN 4
+
+// Buttons
+#define BTN_A 0
+#define BTN_B 1
+#define BTN_C 2
+#define BUTTON_A 0
+#define BUTTON_B 1
+#define BUTTON_C 2
+#define BUTTON_A_PIN 39
+#define BUTTON_B_PIN 38
+#define BUTTON_C_PIN 37
+
+// BEEP PIN
+#define SPEAKER_PIN 25
+#define TONE_PIN_CHANNEL 0
+
+// LORA
+#define LORA_CS_PIN 5
+#define LORA_RST_PIN 26
+#define LORA_IRQ_PIN 36
+
+// UART
+#define USE_SERIAL Serial
+
+#endif /* SETTINGS_C */
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/M5Timer.cpp b/lib/M5Stack/src/utility/M5Timer.cpp
new file mode 100644
index 000000000..eff9d7c73
--- /dev/null
+++ b/lib/M5Stack/src/utility/M5Timer.cpp
@@ -0,0 +1,224 @@
+#if defined (CORE)
+/*
+ * M5Timer.cpp
+ *
+ * M5Timer - A timer library for Arduino.
+ * Author: mromani@ottotecnica.com
+ * Copyright (c) 2010 OTTOTECNICA Italy
+ *
+ * This library is free software; you can redistribute it
+ * and/or modify it under the terms of the GNU Lesser
+ * General Public License as published by the Free Software
+ * Foundation; either version 2.1 of the License, or (at
+ * your option) any later version.
+ *
+ * This library is distributed in the hope that it will
+ * be useful, but WITHOUT ANY WARRANTY; without even the
+ * implied warranty of MERCHANTABILITY or FITNESS FOR A
+ * PARTICULAR PURPOSE. See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser
+ * General Public License along with this library; if not,
+ * write to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include "M5Timer.h"
+
+// Select time function:
+// static inline unsigned long elapsed() { return micros(); }
+static inline unsigned long elapsed() {
+ return millis();
+}
+
+M5Timer::M5Timer() {
+ unsigned long current_millis = elapsed();
+
+ for (int i = 0; i < MAX_TIMERS; i++) {
+ enabled[i] = false;
+ callbacks[i] = 0; // if the callback pointer is zero, the slot is free,
+ // i.e. doesn't "contain" any timer
+ prev_millis[i] = current_millis;
+ numRuns[i] = 0;
+ }
+ numTimers = 0;
+}
+
+void M5Timer::run() {
+ int i;
+ unsigned long current_millis;
+
+ // get current time
+ current_millis = elapsed();
+
+ for (i = 0; i < MAX_TIMERS; i++) {
+ toBeCalled[i] = DEFCALL_DONTRUN;
+
+ // no callback == no timer, i.e. jump over empty slots
+ if (callbacks[i] != 0) {
+ // is it time to process this timer ?
+ // see
+ // http://arduino.cc/forum/index.php/topic,124048.msg932592.html#msg932592
+
+ if (current_millis - prev_millis[i] >= delays[i]) {
+ // update time
+ // prev_millis[i] = current_millis;
+ prev_millis[i] += delays[i];
+
+ // check if the timer callback has to be executed
+ if (enabled[i] == true) {
+ // "run forever" timers must always be executed
+ if (maxNumRuns[i] == RUN_FOREVER) {
+ toBeCalled[i] = DEFCALL_RUNONLY;
+ } else if (numRuns[i] < maxNumRuns[i]) {
+ // other timers get executed the specified number of
+ // times
+ toBeCalled[i] = DEFCALL_RUNONLY;
+ numRuns[i]++;
+ // after the last run, delete the timer
+ if (numRuns[i] >= maxNumRuns[i]) {
+ toBeCalled[i] = DEFCALL_RUNANDDEL;
+ }
+ }
+ }
+ }
+ }
+ }
+ for (i = 0; i < MAX_TIMERS; i++) {
+ switch (toBeCalled[i]) {
+ case DEFCALL_DONTRUN:
+ break;
+
+ case DEFCALL_RUNONLY:
+ callbacks[i]();
+ break;
+
+ case DEFCALL_RUNANDDEL:
+ callbacks[i]();
+ deleteTimer(i);
+ break;
+ }
+ }
+}
+
+// find the first available slot
+// return -1 if none found
+int M5Timer::findFirstFreeSlot() {
+ int i;
+
+ // all slots are used
+ if (numTimers >= MAX_TIMERS) {
+ return -1;
+ }
+
+ // return the first slot with no callback (i.e. free)
+ for (i = 0; i < MAX_TIMERS; i++) {
+ if (callbacks[i] == 0) {
+ return i;
+ }
+ }
+ // no free slots found
+ return -1;
+}
+
+int M5Timer::setTimer(long d, timer_callback f, int n) {
+ int freeTimer;
+
+ freeTimer = findFirstFreeSlot();
+ if (freeTimer < 0) {
+ return -1;
+ }
+
+ if (f == NULL) {
+ return -1;
+ }
+
+ delays[freeTimer] = d;
+ callbacks[freeTimer] = f;
+ maxNumRuns[freeTimer] = n;
+ enabled[freeTimer] = true;
+ prev_millis[freeTimer] = elapsed();
+
+ numTimers++;
+
+ return freeTimer;
+}
+
+int M5Timer::setInterval(long d, timer_callback f) {
+ return setTimer(d, f, RUN_FOREVER);
+}
+
+int M5Timer::setTimeout(long d, timer_callback f) {
+ return setTimer(d, f, RUN_ONCE);
+}
+
+void M5Timer::deleteTimer(int timerId) {
+ if (timerId >= MAX_TIMERS) {
+ return;
+ }
+
+ // nothing to delete if no timers are in use
+ if (numTimers == 0) {
+ return;
+ }
+
+ // don't decrease the number of timers if the
+ // specified slot is already empty
+ if (callbacks[timerId] != NULL) {
+ callbacks[timerId] = 0;
+ enabled[timerId] = false;
+ toBeCalled[timerId] = DEFCALL_DONTRUN;
+ delays[timerId] = 0;
+ numRuns[timerId] = 0;
+
+ // update number of timers
+ numTimers--;
+ }
+}
+
+// function contributed by code@rowansimms.com
+void M5Timer::restartTimer(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ prev_millis[numTimer] = elapsed();
+}
+
+boolean M5Timer::isEnabled(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return false;
+ }
+
+ return enabled[numTimer];
+}
+
+void M5Timer::enable(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ enabled[numTimer] = true;
+}
+
+void M5Timer::disable(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ enabled[numTimer] = false;
+}
+
+void M5Timer::toggle(int numTimer) {
+ if (numTimer >= MAX_TIMERS) {
+ return;
+ }
+
+ enabled[numTimer] = !enabled[numTimer];
+}
+
+int M5Timer::getNumTimers() {
+ return numTimers;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/M5Timer.h b/lib/M5Stack/src/utility/M5Timer.h
new file mode 100644
index 000000000..c7e3edd97
--- /dev/null
+++ b/lib/M5Stack/src/utility/M5Timer.h
@@ -0,0 +1,126 @@
+#if defined (CORE)
+/*
+ * M5Timer.h
+ *
+ * M5Timer - A timer library for Arduino.
+ * Author: mromani@ottotecnica.com
+ * Copyright (c) 2010 OTTOTECNICA Italy
+ *
+ * This library is free software; you can redistribute it
+ * and/or modify it under the terms of the GNU Lesser
+ * General Public License as published by the Free Software
+ * Foundation; either version 2.1 of the License, or (at
+ * your option) any later version.
+ *
+ * This library is distributed in the hope that it will
+ * be useful, but WITHOUT ANY WARRANTY; without even the
+ * implied warranty of MERCHANTABILITY or FITNESS FOR A
+ * PARTICULAR PURPOSE. See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser
+ * General Public License along with this library; if not,
+ * write to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifndef M5Timer_H
+#define M5Timer_H
+
+#include <Arduino.h>
+
+#include <functional>
+
+typedef std::function<void(void)> timer_callback;
+
+class M5Timer {
+ public:
+ // maximum number of timers
+ const static int MAX_TIMERS = 10;
+
+ // setTimer() constants
+ const static int RUN_FOREVER = 0;
+ const static int RUN_ONCE = 1;
+
+ // constructor
+ M5Timer();
+
+ // this function must be called inside loop()
+ void run();
+
+ // call function f every d milliseconds
+ int setInterval(long d, timer_callback f);
+
+ // call function f once after d milliseconds
+ int setTimeout(long d, timer_callback f);
+
+ // call function f every d milliseconds for n times
+ int setTimer(long d, timer_callback f, int n);
+
+ // destroy the specified timer
+ void deleteTimer(int numTimer);
+
+ // restart the specified timer
+ void restartTimer(int numTimer);
+
+ // returns true if the specified timer is enabled
+ boolean isEnabled(int numTimer);
+
+ // enables the specified timer
+ void enable(int numTimer);
+
+ // disables the specified timer
+ void disable(int numTimer);
+
+ // enables the specified timer if it's currently disabled,
+ // and vice-versa
+ void toggle(int numTimer);
+
+ // returns the number of used timers
+ int getNumTimers();
+
+ // returns the number of available timers
+ int getNumAvailableTimers() {
+ return MAX_TIMERS - numTimers;
+ };
+
+ private:
+ // deferred call constants
+ const static int DEFCALL_DONTRUN = 0; // don't call the callback function
+ const static int DEFCALL_RUNONLY =
+ 1; // call the callback function but don't delete the timer
+ const static int DEFCALL_RUNANDDEL =
+ 2; // call the callback function and delete the timer
+
+ // find the first available slot
+ int findFirstFreeSlot();
+
+ // value returned by the millis() function
+ // in the previous run() call
+ unsigned long prev_millis[MAX_TIMERS];
+
+ // pointers to the callback functions
+ timer_callback callbacks[MAX_TIMERS];
+
+ // delay values
+ long delays[MAX_TIMERS];
+
+ // number of runs to be executed for each timer
+ int maxNumRuns[MAX_TIMERS];
+
+ // number of executed runs for each timer
+ int numRuns[MAX_TIMERS];
+
+ // which timers are enabled
+ boolean enabled[MAX_TIMERS];
+
+ // deferred function call (sort of) - N.B.: this array is only used in run()
+ int toBeCalled[MAX_TIMERS];
+
+ // actual number of timers in use
+ int numTimers;
+};
+
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/MPU6886.cpp b/lib/M5Stack/src/utility/MPU6886.cpp
new file mode 100644
index 000000000..59d3da6c7
--- /dev/null
+++ b/lib/M5Stack/src/utility/MPU6886.cpp
@@ -0,0 +1,284 @@
+#if defined (CORE)
+#include "MPU6886.h"
+
+#include <Arduino.h>
+#include <math.h>
+
+MPU6886::MPU6886() {
+}
+
+void MPU6886::I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* read_Buffer) {
+ Wire.beginTransmission(driver_Addr);
+ Wire.write(start_Addr);
+ Wire.endTransmission(false);
+ uint8_t i = 0;
+ Wire.requestFrom(driver_Addr, number_Bytes);
+
+ //! Put read results in the Rx buffer
+ while (Wire.available()) {
+ read_Buffer[i++] = Wire.read();
+ }
+}
+
+void MPU6886::I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* write_Buffer) {
+ Wire.beginTransmission(driver_Addr);
+ Wire.write(start_Addr);
+ Wire.write(*write_Buffer);
+ Wire.endTransmission();
+}
+
+int MPU6886::Init(void) {
+ unsigned char tempdata[1];
+ unsigned char regdata;
+
+ Gyscale = GFS_2000DPS;
+ Acscale = AFS_8G;
+
+ Wire.begin(21, 22);
+
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_WHOAMI, 1, tempdata);
+ imuId = tempdata[0];
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ delay(10);
+
+ regdata = (0x01 << 7);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ delay(10);
+
+ regdata = (0x01 << 0);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, ®data);
+ delay(10);
+
+ // +- 8g
+ regdata = 0x10;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, ®data);
+ delay(1);
+
+ // +- 2000 dps
+ regdata = 0x18;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data);
+ delay(1);
+
+ // 1khz output
+ regdata = 0x01;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, ®data);
+ delay(1);
+
+ // 2 div, FIFO 500hz out
+ regdata = 0x01;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_SMPLRT_DIV, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, ®data);
+ delay(1);
+
+ regdata = 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_EN, 1, ®data);
+ delay(1);
+
+ regdata = 0x22;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_PIN_CFG, 1, ®data);
+ delay(1);
+
+ regdata = 0x01;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, ®data);
+
+ delay(10);
+
+ setGyroFsr(Gyscale);
+ setAccelFsr(Acscale);
+ return 0;
+}
+
+void MPU6886::getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_XOUT_H, 6, buf);
+
+ *ax = ((int16_t)buf[0] << 8) | buf[1];
+ *ay = ((int16_t)buf[2] << 8) | buf[3];
+ *az = ((int16_t)buf[4] << 8) | buf[5];
+}
+
+void MPU6886::getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_XOUT_H, 6, buf);
+
+ *gx = ((uint16_t)buf[0] << 8) | buf[1];
+ *gy = ((uint16_t)buf[2] << 8) | buf[3];
+ *gz = ((uint16_t)buf[4] << 8) | buf[5];
+}
+
+void MPU6886::getTempAdc(int16_t* t) {
+ uint8_t buf[14];
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_TEMP_OUT_H, 14, buf);
+
+ *t = ((uint16_t)buf[6] << 8) | buf[7];
+}
+
+//!俯仰，航向，横滚: pitch，yaw，roll，指三维空间中飞行器的旋转状态。
+void MPU6886::getAhrsData(float* pitch, float* roll, float* yaw) {
+ float accX = 0;
+ float accY = 0;
+ float accZ = 0;
+
+ float gyroX = 0;
+ float gyroY = 0;
+ float gyroZ = 0;
+
+ getGyroData(&gyroX, &gyroY, &gyroZ);
+ getAccelData(&accX, &accY, &accZ);
+ MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD,
+ gyroZ * DEG_TO_RAD, accX, accY, accZ, pitch, roll, yaw);
+}
+
+// Possible gyro scales (and their register bit settings)
+void MPU6886::updateGres() {
+ switch (Gyscale) {
+ case GFS_250DPS:
+ gRes = 250.0 / 32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0 / 32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0 / 32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0 / 32768.0;
+ break;
+ }
+}
+
+// Possible accelerometer scales (and their register bit settings) are:
+// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
+// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit
+// value:
+void MPU6886::updateAres() {
+ switch (Acscale) {
+ case AFS_2G:
+ aRes = 2.0 / 32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0 / 32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0 / 32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0 / 32768.0;
+ break;
+ }
+}
+
+void MPU6886::setGyroFsr(Gscale scale) {
+ unsigned char regdata;
+ regdata = (scale << 3);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, ®data);
+ delay(10);
+ Gyscale = scale;
+ updateGres();
+}
+
+void MPU6886::setAccelFsr(Ascale scale) {
+ unsigned char regdata;
+ regdata = (scale << 3);
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, ®data);
+ delay(10);
+ Acscale = scale;
+ updateAres();
+}
+
+void MPU6886::getAccelData(float* ax, float* ay, float* az) {
+ int16_t accX = 0;
+ int16_t accY = 0;
+ int16_t accZ = 0;
+ getAccelAdc(&accX, &accY, &accZ);
+
+ *ax = (float)accX * aRes;
+ *ay = (float)accY * aRes;
+ *az = (float)accZ * aRes;
+}
+
+void MPU6886::getGyroData(float* gx, float* gy, float* gz) {
+ int16_t gyroX = 0;
+ int16_t gyroY = 0;
+ int16_t gyroZ = 0;
+ getGyroAdc(&gyroX, &gyroY, &gyroZ);
+
+ *gx = (float)gyroX * gRes;
+ *gy = (float)gyroY * gRes;
+ *gz = (float)gyroZ * gRes;
+}
+
+void MPU6886::getTempData(float* t) {
+ int16_t temp = 0;
+ getTempAdc(&temp);
+
+ *t = (float)temp / 326.8 + 25.0;
+}
+
+void MPU6886::setGyroOffset(uint16_t x, uint16_t y, uint16_t z) {
+ uint8_t buf_out[6];
+ buf_out[0] = x >> 8;
+ buf_out[1] = x & 0xff;
+ buf_out[2] = y >> 8;
+ buf_out[3] = y & 0xff;
+ buf_out[4] = z >> 8;
+ buf_out[5] = z & 0xff;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_OFFSET, 6, buf_out);
+}
+
+void MPU6886::setFIFOEnable(bool enableflag) {
+ uint8_t regdata = 0;
+ regdata = enableflag ? 0x18 : 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_ENABLE, 1, ®data);
+ regdata = enableflag ? 0x40 : 0x00;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, ®data);
+}
+
+uint8_t MPU6886::ReadFIFO() {
+ uint8_t ReData = 0;
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_R_W, 1, &ReData);
+ return ReData;
+}
+
+void MPU6886::ReadFIFOBuff(uint8_t* DataBuff, uint16_t Length) {
+ uint8_t number = Length / 210;
+ for (uint8_t i = 0; i < number; i++) {
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_R_W, 210,
+ &DataBuff[i * 210]);
+ }
+
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_R_W, Length % 210,
+ &DataBuff[number * 210]);
+}
+
+void MPU6886::RestFIFO() {
+ uint8_t buf_out;
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, &buf_out);
+ buf_out |= 0x04;
+ I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, &buf_out);
+}
+
+uint16_t MPU6886::ReadFIFOCount() {
+ uint8_t Buff[2];
+ uint16_t ReData = 0;
+ I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_COUNT, 2, Buff);
+ ReData = (Buff[0] << 8) | Buff[1];
+ return ReData;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/MPU6886.h b/lib/M5Stack/src/utility/MPU6886.h
new file mode 100644
index 000000000..5ad75fb5b
--- /dev/null
+++ b/lib/M5Stack/src/utility/MPU6886.h
@@ -0,0 +1,101 @@
+#if defined (CORE)
+/*
+ Note: The MPU6886 is an I2C sensor and uses the Arduino Wire library.
+ Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or
+ a 3.3 V Teensy 3.1. We have disabled the internal pull-ups used by the Wire
+ library in the Wire.h/twi.c utility file. We are also using the 400 kHz fast
+ I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
+ */
+#ifndef _MPU6886_H_
+#define _MPU6886_H_
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "MahonyAHRS.h"
+
+#define MPU6886_ADDRESS 0x68
+#define MPU6886_WHOAMI 0x75
+#define MPU6886_ACCEL_INTEL_CTRL 0x69
+#define MPU6886_SMPLRT_DIV 0x19
+#define MPU6886_INT_PIN_CFG 0x37
+#define MPU6886_INT_ENABLE 0x38
+#define MPU6886_ACCEL_XOUT_H 0x3B
+#define MPU6886_ACCEL_XOUT_L 0x3C
+#define MPU6886_ACCEL_YOUT_H 0x3D
+#define MPU6886_ACCEL_YOUT_L 0x3E
+#define MPU6886_ACCEL_ZOUT_H 0x3F
+#define MPU6886_ACCEL_ZOUT_L 0x40
+
+#define MPU6886_TEMP_OUT_H 0x41
+#define MPU6886_TEMP_OUT_L 0x42
+
+#define MPU6886_GYRO_XOUT_H 0x43
+#define MPU6886_GYRO_XOUT_L 0x44
+#define MPU6886_GYRO_YOUT_H 0x45
+#define MPU6886_GYRO_YOUT_L 0x46
+#define MPU6886_GYRO_ZOUT_H 0x47
+#define MPU6886_GYRO_ZOUT_L 0x48
+
+#define MPU6886_USER_CTRL 0x6A
+#define MPU6886_PWR_MGMT_1 0x6B
+#define MPU6886_PWR_MGMT_2 0x6C
+#define MPU6886_CONFIG 0x1A
+#define MPU6886_GYRO_CONFIG 0x1B
+#define MPU6886_ACCEL_CONFIG 0x1C
+#define MPU6886_ACCEL_CONFIG2 0x1D
+#define MPU6886_FIFO_EN 0x23
+
+#define MPU6886_FIFO_ENABLE 0x23
+#define MPU6886_FIFO_COUNT 0x72
+#define MPU6886_FIFO_R_W 0x74
+#define MPU6886_GYRO_OFFSET 0x13
+//#define G (9.8)
+#define RtA 57.324841
+#define AtR 0.0174533
+#define Gyro_Gr 0.0010653
+
+class MPU6886 {
+ public:
+ enum Ascale { AFS_2G = 0, AFS_4G, AFS_8G, AFS_16G };
+
+ enum Gscale { GFS_250DPS = 0, GFS_500DPS, GFS_1000DPS, GFS_2000DPS };
+
+ public:
+ MPU6886();
+ int Init(void);
+ void getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az);
+ void getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz);
+ void getTempAdc(int16_t* t);
+
+ void getAccelData(float* ax, float* ay, float* az);
+ void getGyroData(float* gx, float* gy, float* gz);
+ void getTempData(float* t);
+
+ void setGyroFsr(Gscale scale);
+ void setAccelFsr(Ascale scale);
+
+ void getAhrsData(float* pitch, float* roll, float* yaw);
+ void setFIFOEnable(bool enableflag);
+ uint8_t ReadFIFO();
+ void ReadFIFOBuff(uint8_t* DataBuff, uint16_t Length);
+ uint16_t ReadFIFOCount();
+ void RestFIFO();
+ void setGyroOffset(uint16_t x, uint16_t y, uint16_t z);
+
+ public:
+ float aRes, gRes, imuId;
+ Gscale Gyscale;
+ Ascale Acscale;
+
+ private:
+ private:
+ void I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* read_Buffer);
+ void I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* write_Buffer);
+ void updateGres();
+ void updateAres();
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/MPU9250.cpp b/lib/M5Stack/src/utility/MPU9250.cpp
new file mode 100644
index 000000000..f04e68981
--- /dev/null
+++ b/lib/M5Stack/src/utility/MPU9250.cpp
@@ -0,0 +1,576 @@
+#if defined (CORE)
+#include "../M5Stack.h"
+#include "MPU9250.h"
+
+//==============================================================================
+//====== Set of useful function to access acceleration. gyroscope, magnetometer,
+//====== and temperature data
+//==============================================================================
+
+void MPU9250::getMres() {
+ switch (Mscale) {
+ // Possible magnetometer scales (and their register bit settings) are:
+ // 14 bit resolution (0) and 16 bit resolution (1)
+ case MFS_14BITS:
+ mRes = 10. * 4912. / 8190.; // Proper scale to return milliGauss
+ break;
+ case MFS_16BITS:
+ mRes = 10. * 4912. / 32760.0; // Proper scale to return milliGauss
+ break;
+ }
+}
+
+void MPU9250::getGres() {
+ switch (Gscale) {
+ // Possible gyro scales (and their register bit settings) are:
+ // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
+ // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
+ // 2-bit value:
+ case GFS_250DPS:
+ gRes = 250.0 / 32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0 / 32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0 / 32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0 / 32768.0;
+ break;
+ }
+}
+
+void MPU9250::getAres() {
+ switch (Ascale) {
+ // Possible accelerometer scales (and their register bit settings)
+ // are: 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). Here's a
+ // bit of an algorith to calculate DPS/(ADC tick) based on that
+ // 2-bit value:
+ case AFS_2G:
+ aRes = 2.0 / 32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0 / 32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0 / 32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0 / 32768.0;
+ break;
+ }
+}
+
+void MPU9250::readAccelData(int16_t* destination) {
+ uint8_t rawData[6]; // x/y/z accel register data stored here
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6,
+ &rawData[0]); // Read the six raw data registers into data array
+ destination[0] =
+ ((int16_t)rawData[0] << 8) |
+ rawData[1]; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = ((int16_t)rawData[2] << 8) | rawData[3];
+ destination[2] = ((int16_t)rawData[4] << 8) | rawData[5];
+}
+
+void MPU9250::readGyroData(int16_t* destination) {
+ uint8_t rawData[6]; // x/y/z gyro register data stored here
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6,
+ &rawData[0]); // Read the six raw data registers sequentially
+ // into data array
+ destination[0] =
+ ((int16_t)rawData[0] << 8) |
+ rawData[1]; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = ((int16_t)rawData[2] << 8) | rawData[3];
+ destination[2] = ((int16_t)rawData[4] << 8) | rawData[5];
+}
+
+void MPU9250::readMagData(int16_t* destination) {
+ // x/y/z gyro register data, ST2 register stored here, must read ST2 at end
+ // of data acquisition
+ uint8_t rawData[7];
+ // Wait for magnetometer data ready bit to be set
+ if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) {
+ // Read the six raw data and ST2 registers sequentially into data array
+ readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);
+ uint8_t c = rawData[6]; // End data read by reading ST2 register
+ // Check if magnetic sensor overflow set, if not then report data
+ if (!(c & 0x08)) {
+ // Turn the MSB and LSB into a signed 16-bit value
+ destination[0] = ((int16_t)rawData[1] << 8) | rawData[0];
+ // Data stored as little Endian
+ destination[1] = ((int16_t)rawData[3] << 8) | rawData[2];
+ destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
+ }
+ }
+}
+
+int16_t MPU9250::readTempData() {
+ uint8_t rawData[2]; // x/y/z gyro register data stored here
+ readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2,
+ &rawData[0]); // Read the two raw data registers sequentially
+ // into data array
+ return ((int16_t)rawData[0] << 8) |
+ rawData[1]; // Turn the MSB and LSB into a 16-bit value
+}
+
+// Calculate the time the last update took for use in the quaternion filters
+void MPU9250::updateTime() {
+ Now = micros();
+
+ // Set integration time by time elapsed since last filter update
+ deltat = ((Now - lastUpdate) / 1000000.0f);
+ lastUpdate = Now;
+
+ sum += deltat; // sum for averaging filter update rate
+ sumCount++;
+}
+
+void MPU9250::initAK8963(float* destination) {
+ // First extract the factory calibration for each magnetometer axis
+ uint8_t rawData[3]; // x/y/z gyro calibration data stored here
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+ delay(10);
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
+ delay(10);
+ readBytes(AK8963_ADDRESS, AK8963_ASAX, 3,
+ &rawData[0]); // Read the x-, y-, and z-axis calibration values
+ destination[0] = (float)(rawData[0] - 128) / 256. +
+ 1.; // Return x-axis sensitivity adjustment values, etc.
+ destination[1] = (float)(rawData[1] - 128) / 256. + 1.;
+ destination[2] = (float)(rawData[2] - 128) / 256. + 1.;
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+ delay(10);
+ // Configure the magnetometer for continuous read and highest resolution
+ // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL
+ // register, and enable continuous mode data acquisition Mmode (bits [3:0]),
+ // 0010 for 8 Hz and 0110 for 100 Hz sample rates
+ writeByte(AK8963_ADDRESS, AK8963_CNTL,
+ Mscale << 4 |
+ Mmode); // Set magnetometer data resolution and sample ODR
+ delay(10);
+}
+
+void MPU9250::initMPU9250() {
+ // wake up device
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1,
+ 0x00); // Clear sleep mode bit (6), enable all sensors
+ delay(100); // Wait for all registers to reset
+
+ // get stable time source
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1,
+ 0x01); // Auto select clock source to be PLL gyroscope reference
+ // if ready else
+ delay(200);
+
+ // Configure Gyro and Thermometer
+ // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz,
+ // respectively; minimum delay time for this setting is 5.9 ms, which means
+ // sensor fusion update rates cannot be higher than 1 / 0.0059 = 170 Hz
+ // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for
+ // both With the MPU9250, it is possible to get gyro sample rates of 32 kHz
+ // (!), 8 kHz, or 1 kHz
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
+
+ // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV,
+ 0x04); // Use a 200 Hz rate; a rate consistent with the filter
+ // update rate
+ // determined inset in CONFIG above
+
+ // Set gyroscope full scale range
+ // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are
+ // left-shifted into positions 4:3
+ uint8_t c =
+ readByte(MPU9250_ADDRESS,
+ GYRO_CONFIG); // get current GYRO_CONFIG register value
+ // c = c & ~0xE0; // Clear self-test bits [7:5]
+ c = c & ~0x02; // Clear Fchoice bits [1:0]
+ c = c & ~0x18; // Clear AFS bits [4:3]
+ c = c | Gscale << 3; // Set full scale range for the gyro
+ // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to
+ // bits 1:0 of GYRO_CONFIG
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG,
+ c); // Write new GYRO_CONFIG value to register
+
+ // Set accelerometer full-scale range configuration
+ c = readByte(MPU9250_ADDRESS,
+ ACCEL_CONFIG); // get current ACCEL_CONFIG register value
+ // c = c & ~0xE0; // Clear self-test bits [7:5]
+ c = c & ~0x18; // Clear AFS bits [4:3]
+ c = c | Ascale << 3; // Set full scale range for the accelerometer
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG,
+ c); // Write new ACCEL_CONFIG register value
+
+ // Set accelerometer sample rate configuration
+ // It is possible to get a 4 kHz sample rate from the accelerometer by
+ // choosing 1 for accel_fchoice_b bit [3]; in this case the bandwidth
+ // is 1.13 kHz
+ c = readByte(MPU9250_ADDRESS,
+ ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
+ c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
+ c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2,
+ c); // Write new ACCEL_CONFIG2 register value
+ // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
+ // but all these rates are further reduced by a factor of 5 to 200 Hz
+ // because of the SMPLRT_DIV setting
+
+ // Configure Interrupts and Bypass Enable
+ // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH
+ // until interrupt cleared, clear on read of INT_STATUS, and enable
+ // I2C_BYPASS_EN so additional chips can join the I2C bus and all can be
+ // controlled by the Arduino as master
+ writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
+ writeByte(MPU9250_ADDRESS, INT_ENABLE,
+ 0x01); // Enable data ready (bit 0) interrupt
+ delay(100);
+}
+
+// Function which accumulates gyro and accelerometer data after device
+// initialization. It calculates the average of the at-rest readings and then
+// loads the resulting offsets into accelerometer and gyro bias registers.
+void MPU9250::calibrateMPU9250(float* gyroBias, float* accelBias) {
+ uint8_t
+ data[12]; // data array to hold accelerometer and gyro x, y, z, data
+ uint16_t ii, packet_count, fifo_count;
+ int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+
+ // reset device
+ // Write a one to bit 7 reset bit; toggle reset device
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
+ delay(100);
+
+ // get stable time source; Auto select clock source to be PLL gyroscope
+ // reference if ready else use the internal oscillator, bits 2:0 = 001
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
+ delay(200);
+
+ // Configure device for bias calculation
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
+ writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1,
+ 0x00); // Turn on internal clock source
+ writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
+ writeByte(MPU9250_ADDRESS, USER_CTRL,
+ 0x00); // Disable FIFO and I2C master modes
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
+ delay(15);
+
+ // Configure MPU6050 gyro and accelerometer for bias calculation
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG,
+ 0x00); // Set gyro full-scale to 250 degrees per second, maximum
+ // sensitivity
+ writeByte(
+ MPU9250_ADDRESS, ACCEL_CONFIG,
+ 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+
+ uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
+ uint16_t accelsensitivity = 16384; // = 16384 LSB/g
+
+ // Configure FIFO to capture accelerometer and gyro data for bias
+ // calculation
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
+ writeByte(MPU9250_ADDRESS, FIFO_EN,
+ 0x78); // Enable gyro and accelerometer sensors for FIFO (max
+ // size 512 bytes in MPU-9150)
+ delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
+
+ // At end of sample accumulation, turn off FIFO sensor read
+ writeByte(MPU9250_ADDRESS, FIFO_EN,
+ 0x00); // Disable gyro and accelerometer sensors for FIFO
+ readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2,
+ &data[0]); // read FIFO sample count
+ fifo_count = ((uint16_t)data[0] << 8) | data[1];
+ packet_count =
+ fifo_count /
+ 12; // How many sets of full gyro and accelerometer data for averaging
+
+ for (ii = 0; ii < packet_count; ii++) {
+ int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+ readBytes(MPU9250_ADDRESS, FIFO_R_W, 12,
+ &data[0]); // read data for averaging
+ accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) |
+ data[1]); // Form signed 16-bit integer for
+ // each sample in FIFO
+ accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]);
+ accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]);
+ gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]);
+ gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]);
+ gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
+
+ accel_bias[0] +=
+ (int32_t)accel_temp[0]; // Sum individual signed 16-bit biases to
+ // get accumulated signed 32-bit biases
+ accel_bias[1] += (int32_t)accel_temp[1];
+ accel_bias[2] += (int32_t)accel_temp[2];
+ gyro_bias[0] += (int32_t)gyro_temp[0];
+ gyro_bias[1] += (int32_t)gyro_temp[1];
+ gyro_bias[2] += (int32_t)gyro_temp[2];
+ }
+ accel_bias[0] /=
+ (int32_t)packet_count; // Normalize sums to get average count biases
+ accel_bias[1] /= (int32_t)packet_count;
+ accel_bias[2] /= (int32_t)packet_count;
+ gyro_bias[0] /= (int32_t)packet_count;
+ gyro_bias[1] /= (int32_t)packet_count;
+ gyro_bias[2] /= (int32_t)packet_count;
+
+ if (accel_bias[2] > 0L) {
+ // Remove gravity from the z-axis accelerometer bias calculation
+ accel_bias[2] -= (int32_t)accelsensitivity;
+ } else {
+ accel_bias[2] += (int32_t)accelsensitivity;
+ }
+
+ // Construct the gyro biases for push to the hardware gyro bias registers,
+ // which are reset to zero upon device startup
+ data[0] = (-gyro_bias[0] / 4 >> 8) &
+ 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to
+ // expected bias input format
+ data[1] =
+ (-gyro_bias[0] / 4) & 0xFF; // Biases are additive, so change sign on
+ // calculated average gyro biases
+ data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
+ data[3] = (-gyro_bias[1] / 4) & 0xFF;
+ data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
+ data[5] = (-gyro_bias[2] / 4) & 0xFF;
+
+ // Push gyro biases to hardware registers
+ writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
+ writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
+ writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
+ writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
+ writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
+ writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
+
+ // Output scaled gyro biases for display in the main program
+ gyroBias[0] = (float)gyro_bias[0] / (float)gyrosensitivity;
+ gyroBias[1] = (float)gyro_bias[1] / (float)gyrosensitivity;
+ gyroBias[2] = (float)gyro_bias[2] / (float)gyrosensitivity;
+
+ // Construct the accelerometer biases for push to the hardware accelerometer
+ // bias registers. These registers contain factory trim values which must be
+ // added to the calculated accelerometer biases; on boot up these registers
+ // will hold non-zero values. In addition, bit 0 of the lower byte must be
+ // preserved since it is used for temperature compensation calculations.
+ // Accelerometer bias registers expect bias input as 2048 LSB per g, so that
+ // the accelerometer biases calculated above must be divided by 8.
+
+ int32_t accel_bias_reg[3] = {
+ 0, 0, 0}; // A place to hold the factory accelerometer trim biases
+ readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2,
+ &data[0]); // Read factory accelerometer trim values
+ accel_bias_reg[0] = (int32_t)(((int16_t)data[0] << 8) | data[1]);
+ readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
+ accel_bias_reg[1] = (int32_t)(((int16_t)data[0] << 8) | data[1]);
+ readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
+ accel_bias_reg[2] = (int32_t)(((int16_t)data[0] << 8) | data[1]);
+
+ uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of
+ // lower byte of accelerometer bias registers
+ uint8_t mask_bit[3] = {
+ 0, 0,
+ 0}; // Define array to hold mask bit for each accelerometer bias axis
+
+ for (ii = 0; ii < 3; ii++) {
+ if ((accel_bias_reg[ii] & mask)) {
+ mask_bit[ii] = 0x01; // If temperature compensation bit is set,
+ // record that fact in mask_bit
+ }
+ }
+
+ // Construct total accelerometer bias, including calculated average
+ // accelerometer bias from above
+ accel_bias_reg[0] -=
+ (accel_bias[0] / 8); // Subtract calculated averaged accelerometer bias
+ // scaled to 2048 LSB/g (16 g full scale)
+ accel_bias_reg[1] -= (accel_bias[1] / 8);
+ accel_bias_reg[2] -= (accel_bias[2] / 8);
+
+ data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+ data[1] = (accel_bias_reg[0]) & 0xFF;
+ data[1] =
+ data[1] | mask_bit[0]; // preserve temperature compensation bit when
+ // writing back to accelerometer bias registers
+ data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+ data[3] = (accel_bias_reg[1]) & 0xFF;
+ data[3] =
+ data[3] | mask_bit[1]; // preserve temperature compensation bit when
+ // writing back to accelerometer bias registers
+ data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+ data[5] = (accel_bias_reg[2]) & 0xFF;
+ data[5] =
+ data[5] | mask_bit[2]; // preserve temperature compensation bit when
+ // writing back to accelerometer bias registers
+
+ // Apparently this is not working for the acceleration biases in the
+ // MPU-9250 Are we handling the temperature correction bit properly? Push
+ // accelerometer biases to hardware registers
+ writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
+ writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
+ writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
+ writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
+ writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
+ writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
+
+ // Output scaled accelerometer biases for display in the main program
+ accelBias[0] = (float)accel_bias[0] / (float)accelsensitivity;
+ accelBias[1] = (float)accel_bias[1] / (float)accelsensitivity;
+ accelBias[2] = (float)accel_bias[2] / (float)accelsensitivity;
+}
+
+// Accelerometer and gyroscope self test; check calibration wrt factory settings
+void MPU9250::MPU9250SelfTest(
+ float* destination) { // Should return percent deviation from factory trim
+ // values, +/- 14 or less deviation is a pass
+ uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
+ uint8_t selfTest[6];
+ int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
+ float factoryTrim[6];
+ uint8_t FS = 0;
+
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV,
+ 0x00); // Set gyro sample rate to 1 kHz
+ writeByte(MPU9250_ADDRESS, CONFIG,
+ 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG,
+ 1 << FS); // Set full scale range for the gyro to 250 dps
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2,
+ 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG,
+ 1 << FS); // Set full scale range for the accelerometer to 2 g
+
+ for (int ii = 0; ii < 200;
+ ii++) { // get average current values of gyro and acclerometer
+
+ readBytes(
+ MPU9250_ADDRESS, ACCEL_XOUT_H, 6,
+ &rawData[0]); // Read the six raw data registers into data array
+ aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) |
+ rawData[1]); // Turn the MSB and LSB into a signed
+ // 16-bit value
+ aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6,
+ &rawData[0]); // Read the six raw data registers sequentially
+ // into data array
+ gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) |
+ rawData[1]); // Turn the MSB and LSB into a signed
+ // 16-bit value
+ gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+ }
+
+ for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as
+ // average current readings
+ aAvg[ii] /= 200;
+ gAvg[ii] /= 200;
+ }
+
+ // Configure the accelerometer for self-test
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG,
+ 0xE0); // Enable self test on all three axes and set
+ // accelerometer range to +/- 2 g
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG,
+ 0xE0); // Enable self test on all three axes and set gyro range
+ // to +/- 250 degrees/s
+ delay(25); // Delay a while to let the device stabilize
+
+ for (int ii = 0; ii < 200;
+ ii++) { // get average self-test values of gyro and acclerometer
+
+ readBytes(
+ MPU9250_ADDRESS, ACCEL_XOUT_H, 6,
+ &rawData[0]); // Read the six raw data registers into data array
+ aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) |
+ rawData[1]); // Turn the MSB and LSB into a
+ // signed 16-bit value
+ aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6,
+ &rawData[0]); // Read the six raw data registers sequentially
+ // into data array
+ gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) |
+ rawData[1]); // Turn the MSB and LSB into a
+ // signed 16-bit value
+ gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+ }
+
+ for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as
+ // average self-test readings
+ aSTAvg[ii] /= 200;
+ gSTAvg[ii] /= 200;
+ }
+
+ // Configure the gyro and accelerometer for normal operation
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
+ delay(25); // Delay a while to let the device stabilize
+
+ // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
+ selfTest[0] = readByte(
+ MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
+ selfTest[1] = readByte(
+ MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
+ selfTest[2] = readByte(
+ MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
+ selfTest[3] = readByte(MPU9250_ADDRESS,
+ SELF_TEST_X_GYRO); // X-axis gyro self-test results
+ selfTest[4] = readByte(MPU9250_ADDRESS,
+ SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
+ selfTest[5] = readByte(MPU9250_ADDRESS,
+ SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
+
+ // Retrieve factory self-test value from self-test code reads
+ factoryTrim[0] = (float)(2620 / 1 << FS) *
+ (pow(1.01, ((float)selfTest[0] -
+ 1.0))); // FT[Xa] factory trim calculation
+ factoryTrim[1] = (float)(2620 / 1 << FS) *
+ (pow(1.01, ((float)selfTest[1] -
+ 1.0))); // FT[Ya] factory trim calculation
+ factoryTrim[2] = (float)(2620 / 1 << FS) *
+ (pow(1.01, ((float)selfTest[2] -
+ 1.0))); // FT[Za] factory trim calculation
+ factoryTrim[3] = (float)(2620 / 1 << FS) *
+ (pow(1.01, ((float)selfTest[3] -
+ 1.0))); // FT[Xg] factory trim calculation
+ factoryTrim[4] = (float)(2620 / 1 << FS) *
+ (pow(1.01, ((float)selfTest[4] -
+ 1.0))); // FT[Yg] factory trim calculation
+ factoryTrim[5] = (float)(2620 / 1 << FS) *
+ (pow(1.01, ((float)selfTest[5] -
+ 1.0))); // FT[Zg] factory trim calculation
+
+ // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim
+ // of the Self-Test Response To get percent, must multiply by 100
+ for (int i = 0; i < 3; i++) {
+ destination[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i])) /
+ factoryTrim[i]; // Report percent differences
+ destination[i + 3] = 100.0 * ((float)(gSTAvg[i] - gAvg[i])) /
+ factoryTrim[i + 3]; // Report percent differences
+ }
+}
+
+// Wire.h read and write protocols
+void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
+ M5.I2C.writeByte(address, subAddress, data);
+}
+
+uint8_t MPU9250::readByte(uint8_t address, uint8_t subAddress) {
+ uint8_t result;
+ M5.I2C.readByte(address, subAddress, &result);
+ return (result);
+}
+
+void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count,
+ uint8_t* dest) {
+ M5.I2C.readBytes(address, subAddress, count, dest);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/MPU9250.h b/lib/M5Stack/src/utility/MPU9250.h
new file mode 100644
index 000000000..0b6dbd128
--- /dev/null
+++ b/lib/M5Stack/src/utility/MPU9250.h
@@ -0,0 +1,251 @@
+#if defined (CORE)
+/*
+ Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
+ Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or
+ a 3.3 V Teensy 3.1. We have disabled the internal pull-ups used by the Wire
+ library in the Wire.h/twi.c utility file. We are also using the 400 kHz fast
+ I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
+ */
+#ifndef _MPU9250_H_
+#define _MPU9250_H_
+
+#include <SPI.h>
+#include <Wire.h>
+
+// See also MPU-9250 Register Map and Descriptions, Revision 4.0,
+// RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in above
+// document; the MPU9250 and MPU9150 are virtually identical but the latter has
+// a different register map
+
+// Magnetometer Registers
+#define AK8963_ADDRESS 0x0C
+#define WHO_AM_I_AK8963 0x00 // should return 0x48
+#define INFO 0x01
+#define AK8963_ST1 0x02 // data ready status bit 0
+#define AK8963_XOUT_L 0x03 // data
+#define AK8963_XOUT_H 0x04
+#define AK8963_YOUT_L 0x05
+#define AK8963_YOUT_H 0x06
+#define AK8963_ZOUT_L 0x07
+#define AK8963_ZOUT_H 0x08
+#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
+#define AK8963_CNTL \
+ 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and
+ // Fuse ROM (1111) modes on bits 3:0
+#define AK8963_ASTC 0x0C // Self test control
+#define AK8963_I2CDIS 0x0F // I2C disable
+#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
+#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
+#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
+
+#define SELF_TEST_X_GYRO 0x00
+#define SELF_TEST_Y_GYRO 0x01
+#define SELF_TEST_Z_GYRO 0x02
+
+/*#define X_FINE_GAIN 0x03 // [7:0] fine gain
+#define Y_FINE_GAIN 0x04
+#define Z_FINE_GAIN 0x05
+#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
+#define XA_OFFSET_L_TC 0x07
+#define YA_OFFSET_H 0x08
+#define YA_OFFSET_L_TC 0x09
+#define ZA_OFFSET_H 0x0A
+#define ZA_OFFSET_L_TC 0x0B */
+
+#define SELF_TEST_X_ACCEL 0x0D
+#define SELF_TEST_Y_ACCEL 0x0E
+#define SELF_TEST_Z_ACCEL 0x0F
+
+#define SELF_TEST_A 0x10
+
+#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
+#define XG_OFFSET_L 0x14
+#define YG_OFFSET_H 0x15
+#define YG_OFFSET_L 0x16
+#define ZG_OFFSET_H 0x17
+#define ZG_OFFSET_L 0x18
+#define SMPLRT_DIV 0x19
+#define CONFIG 0x1A
+#define GYRO_CONFIG 0x1B
+#define ACCEL_CONFIG 0x1C
+#define ACCEL_CONFIG2 0x1D
+#define LP_ACCEL_ODR 0x1E
+#define WOM_THR 0x1F
+
+// Duration counter threshold for motion interrupt generation, 1 kHz rate,
+// LSB = 1 ms
+#define MOT_DUR 0x20
+// Zero-motion detection threshold bits [7:0]
+#define ZMOT_THR 0x21
+// Duration counter threshold for zero motion interrupt generation, 16 Hz rate,
+// LSB = 64 ms
+#define ZRMOT_DUR 0x22
+
+#define FIFO_EN 0x23
+#define I2C_MST_CTRL 0x24
+#define I2C_SLV0_ADDR 0x25
+#define I2C_SLV0_REG 0x26
+#define I2C_SLV0_CTRL 0x27
+#define I2C_SLV1_ADDR 0x28
+#define I2C_SLV1_REG 0x29
+#define I2C_SLV1_CTRL 0x2A
+#define I2C_SLV2_ADDR 0x2B
+#define I2C_SLV2_REG 0x2C
+#define I2C_SLV2_CTRL 0x2D
+#define I2C_SLV3_ADDR 0x2E
+#define I2C_SLV3_REG 0x2F
+#define I2C_SLV3_CTRL 0x30
+#define I2C_SLV4_ADDR 0x31
+#define I2C_SLV4_REG 0x32
+#define I2C_SLV4_DO 0x33
+#define I2C_SLV4_CTRL 0x34
+#define I2C_SLV4_DI 0x35
+#define I2C_MST_STATUS 0x36
+#define INT_PIN_CFG 0x37
+#define INT_ENABLE 0x38
+#define DMP_INT_STATUS 0x39 // Check DMP interrupt
+#define INT_STATUS 0x3A
+#define ACCEL_XOUT_H 0x3B
+#define ACCEL_XOUT_L 0x3C
+#define ACCEL_YOUT_H 0x3D
+#define ACCEL_YOUT_L 0x3E
+#define ACCEL_ZOUT_H 0x3F
+#define ACCEL_ZOUT_L 0x40
+#define TEMP_OUT_H 0x41
+#define TEMP_OUT_L 0x42
+#define GYRO_XOUT_H 0x43
+#define GYRO_XOUT_L 0x44
+#define GYRO_YOUT_H 0x45
+#define GYRO_YOUT_L 0x46
+#define GYRO_ZOUT_H 0x47
+#define GYRO_ZOUT_L 0x48
+#define EXT_SENS_DATA_00 0x49
+#define EXT_SENS_DATA_01 0x4A
+#define EXT_SENS_DATA_02 0x4B
+#define EXT_SENS_DATA_03 0x4C
+#define EXT_SENS_DATA_04 0x4D
+#define EXT_SENS_DATA_05 0x4E
+#define EXT_SENS_DATA_06 0x4F
+#define EXT_SENS_DATA_07 0x50
+#define EXT_SENS_DATA_08 0x51
+#define EXT_SENS_DATA_09 0x52
+#define EXT_SENS_DATA_10 0x53
+#define EXT_SENS_DATA_11 0x54
+#define EXT_SENS_DATA_12 0x55
+#define EXT_SENS_DATA_13 0x56
+#define EXT_SENS_DATA_14 0x57
+#define EXT_SENS_DATA_15 0x58
+#define EXT_SENS_DATA_16 0x59
+#define EXT_SENS_DATA_17 0x5A
+#define EXT_SENS_DATA_18 0x5B
+#define EXT_SENS_DATA_19 0x5C
+#define EXT_SENS_DATA_20 0x5D
+#define EXT_SENS_DATA_21 0x5E
+#define EXT_SENS_DATA_22 0x5F
+#define EXT_SENS_DATA_23 0x60
+#define MOT_DETECT_STATUS 0x61
+#define I2C_SLV0_DO 0x63
+#define I2C_SLV1_DO 0x64
+#define I2C_SLV2_DO 0x65
+#define I2C_SLV3_DO 0x66
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET 0x68
+#define MOT_DETECT_CTRL 0x69
+#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
+#define PWR_MGMT_2 0x6C
+#define DMP_BANK 0x6D // Activates a specific bank in the DMP
+#define DMP_RW_PNT \
+ 0x6E // Set read/write pointer to a specific start address in specified DMP
+ // bank
+#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
+#define DMP_REG_1 0x70
+#define DMP_REG_2 0x71
+#define FIFO_COUNTH 0x72
+#define FIFO_COUNTL 0x73
+#define FIFO_R_W 0x74
+#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
+#define XA_OFFSET_H 0x77
+#define XA_OFFSET_L 0x78
+#define YA_OFFSET_H 0x7A
+#define YA_OFFSET_L 0x7B
+#define ZA_OFFSET_H 0x7D
+#define ZA_OFFSET_L 0x7E
+
+// Using the MPU-9250 breakout board, ADO is set to 0
+// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
+#define ADO 0
+#if ADO
+#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1
+#else
+#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0
+#define AK8963_ADDRESS 0x0C // Address of magnetometer
+#endif // AD0
+
+class MPU9250 {
+ protected:
+ // Set initial input parameters
+ enum Ascale { AFS_2G = 0, AFS_4G, AFS_8G, AFS_16G };
+
+ enum Gscale { GFS_250DPS = 0, GFS_500DPS, GFS_1000DPS, GFS_2000DPS };
+
+ enum Mscale {
+ MFS_14BITS = 0, // 0.6 mG per LSB
+ MFS_16BITS // 0.15 mG per LSB
+ };
+
+ // Specify sensor full scale
+ uint8_t Gscale = GFS_250DPS;
+ uint8_t Ascale = AFS_2G;
+ // Choose either 14-bit or 16-bit magnetometer resolution
+ uint8_t Mscale = MFS_16BITS;
+ // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
+ uint8_t Mmode = 0x02;
+
+ public:
+ float pitch, yaw, roll;
+ float temperature; // Stores the real internal chip temperature in Celsius
+ int16_t tempCount; // Temperature raw count output
+ uint32_t delt_t = 0; // Used to control display output rate
+
+ uint32_t count = 0, sumCount = 0; // used to control display output rate
+ float deltat = 0.0f,
+ sum = 0.0f; // integration interval for both filter schemes
+ uint32_t lastUpdate = 0,
+ firstUpdate = 0; // used to calculate integration interval
+ uint32_t Now = 0; // used to calculate integration interval
+
+ int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
+ int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
+ // Scale resolutions per LSB for the sensors
+ float aRes, gRes, mRes;
+ // Variables to hold latest sensor data values
+ float ax, ay, az, gx, gy, gz, mx, my, mz;
+ // Factory mag calibration and mag bias
+ float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0};
+ // Bias corrections for gyro and accelerometer
+ float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0};
+ float SelfTest[6];
+ // Stores the 16-bit signed accelerometer sensor output
+ int16_t accelCount[3];
+
+ public:
+ void getMres();
+ void getGres();
+ void getAres();
+ void readAccelData(int16_t *);
+ void readGyroData(int16_t *);
+ void readMagData(int16_t *);
+ int16_t readTempData();
+ void updateTime();
+ void initAK8963(float *);
+ void initMPU9250();
+ void calibrateMPU9250(float *gyroBias, float *accelBias);
+ void MPU9250SelfTest(float *destination);
+ void writeByte(uint8_t, uint8_t, uint8_t);
+ uint8_t readByte(uint8_t, uint8_t);
+ void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *);
+}; // class MPU9250
+
+#endif // _MPU9250_H_
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/MahonyAHRS.cpp b/lib/M5Stack/src/utility/MahonyAHRS.cpp
new file mode 100644
index 000000000..f863f991d
--- /dev/null
+++ b/lib/M5Stack/src/utility/MahonyAHRS.cpp
@@ -0,0 +1,269 @@
+#if defined (CORE)
+//=====================================================================================================
+// MahonyAHRS.c
+//=====================================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date Author Notes
+// 29/09/2011 SOH Madgwick Initial release
+// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
+//
+//=====================================================================================================
+
+//---------------------------------------------------------------------------------------------------
+// Header files
+
+#include "MahonyAHRS.h"
+
+#include <math.h>
+
+#include "Arduino.h"
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreq 25.0f // sample frequency in Hz
+#define twoKpDef (2.0f * 1.0f) // 2 * proportional gain
+#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
+
+//#define twoKiDef (0.0f * 0.0f)
+
+//---------------------------------------------------------------------------------------------------
+// Variable definitions
+
+volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp)
+volatile float twoKi = twoKiDef; // 2 * integral gain (Ki)
+volatile float
+ q0 = 1.0,
+ q1 = 0.0, q2 = 0.0,
+ q3 = 0.0; // quaternion of sensor frame relative to auxiliary frame
+volatile float integralFBx = 0.0f, integralFBy = 0.0f,
+ integralFBz = 0.0f; // integral error terms scaled by Ki
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+// float invSqrt(float x);
+
+//====================================================================================================
+// Functions
+
+//---------------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay,
+ float az, float mx, float my, float mz) {
+ float recipNorm;
+ float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+ float hx, hy, bx, bz;
+ float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
+ float halfex, halfey, halfez;
+ float qa, qb, qc;
+
+ // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
+ // magnetometer normalisation)
+ if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+ // MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+ return;
+ }
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in
+ // accelerometer normalisation)
+ if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+ // Normalise accelerometer measurement
+ recipNorm = sqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Normalise magnetometer measurement
+ recipNorm = sqrt(mx * mx + my * my + mz * mz);
+ mx *= recipNorm;
+ my *= recipNorm;
+ mz *= recipNorm;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ q0q0 = q0 * q0;
+ q0q1 = q0 * q1;
+ q0q2 = q0 * q2;
+ q0q3 = q0 * q3;
+ q1q1 = q1 * q1;
+ q1q2 = q1 * q2;
+ q1q3 = q1 * q3;
+ q2q2 = q2 * q2;
+ q2q3 = q2 * q3;
+ q3q3 = q3 * q3;
+
+ // Reference direction of Earth's magnetic field
+ hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) +
+ mz * (q1q3 + q0q2));
+ hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) +
+ mz * (q2q3 - q0q1));
+ bx = sqrt(hx * hx + hy * hy);
+ bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) +
+ mz * (0.5f - q1q1 - q2q2));
+
+ // Estimated direction of gravity and magnetic field
+ halfvx = q1q3 - q0q2;
+ halfvy = q0q1 + q2q3;
+ halfvz = q0q0 - 0.5f + q3q3;
+ halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+ halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+ halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+
+ // Error is sum of cross product between estimated direction and
+ // measured direction of field vectors
+ halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
+ halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
+ halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
+
+ // Compute and apply integral feedback if enabled
+ if (twoKi > 0.0f) {
+ integralFBx += twoKi * halfex *
+ (1.0f / sampleFreq); // integral error scaled by Ki
+ integralFBy += twoKi * halfey * (1.0f / sampleFreq);
+ integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+ gx += integralFBx; // apply integral feedback
+ gy += integralFBy;
+ gz += integralFBz;
+ } else {
+ integralFBx = 0.0f; // prevent integral windup
+ integralFBy = 0.0f;
+ integralFBz = 0.0f;
+ }
+
+ // Apply proportional feedback
+ gx += twoKp * halfex;
+ gy += twoKp * halfey;
+ gz += twoKp * halfez;
+ }
+
+ // Integrate rate of change of quaternion
+ gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
+ gy *= (0.5f * (1.0f / sampleFreq));
+ gz *= (0.5f * (1.0f / sampleFreq));
+ qa = q0;
+ qb = q1;
+ qc = q2;
+ q0 += (-qb * gx - qc * gy - q3 * gz);
+ q1 += (qa * gx + qc * gz - q3 * gy);
+ q2 += (qa * gy - qb * gz + q3 * gx);
+ q3 += (qa * gz + qb * gy - qc * gx);
+
+ // Normalise quaternion
+ recipNorm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay,
+ float az, float *pitch, float *roll, float *yaw) {
+ float recipNorm;
+ float halfvx, halfvy, halfvz;
+ float halfex, halfey, halfez;
+ float qa, qb, qc;
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in
+ // accelerometer normalisation)
+ if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+ // Normalise accelerometer measurement
+ recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Estimated direction of gravity and vector perpendicular to magnetic
+ // flux
+ halfvx = q1 * q3 - q0 * q2;
+ halfvy = q0 * q1 + q2 * q3;
+ halfvz = q0 * q0 - 0.5f + q3 * q3;
+
+ // Error is sum of cross product between estimated and measured
+ // direction of gravity
+ halfex = (ay * halfvz - az * halfvy);
+ halfey = (az * halfvx - ax * halfvz);
+ halfez = (ax * halfvy - ay * halfvx);
+
+ // Compute and apply integral feedback if enabled
+ if (twoKi > 0.0f) {
+ integralFBx += twoKi * halfex *
+ (1.0f / sampleFreq); // integral error scaled by Ki
+ integralFBy += twoKi * halfey * (1.0f / sampleFreq);
+ integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+ gx += integralFBx; // apply integral feedback
+ gy += integralFBy;
+ gz += integralFBz;
+ } else {
+ integralFBx = 0.0f; // prevent integral windup
+ integralFBy = 0.0f;
+ integralFBz = 0.0f;
+ }
+
+ // Apply proportional feedback
+ gx += twoKp * halfex;
+ gy += twoKp * halfey;
+ gz += twoKp * halfez;
+ }
+
+ // Integrate rate of change of quaternion
+ gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
+ gy *= (0.5f * (1.0f / sampleFreq));
+ gz *= (0.5f * (1.0f / sampleFreq));
+ qa = q0;
+ qb = q1;
+ qc = q2;
+ q0 += (-qb * gx - qc * gy - q3 * gz);
+ q1 += (qa * gx + qc * gz - q3 * gy);
+ q2 += (qa * gy - qb * gz + q3 * gx);
+ q3 += (qa * gz + qb * gy - qc * gx);
+
+ // Normalise quaternion
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+
+ *pitch = asin(-2 * q1 * q3 + 2 * q0 * q2); // pitch
+ *roll = atan2(2 * q2 * q3 + 2 * q0 * q1,
+ -2 * q1 * q1 - 2 * q2 * q2 + 1); // roll
+ *yaw = atan2(2 * (q1 * q2 + q0 * q3),
+ q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); // yaw
+
+ *pitch *= RAD_TO_DEG;
+ *yaw *= RAD_TO_DEG;
+ // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
+ // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19
+ // - http://www.ngdc.noaa.gov/geomag-web/#declination
+ *yaw -= 8.5;
+ *roll *= RAD_TO_DEG;
+
+ /// Serial.printf("%f %f %f \r\n", pitch, roll, yaw);
+}
+
+//---------------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstrict-aliasing"
+float invSqrt(float x) {
+ float halfx = 0.5f * x;
+ float y = x;
+ long i = *(long *)&y;
+ i = 0x5f3759df - (i >> 1);
+ y = *(float *)&i;
+ y = y * (1.5f - (halfx * y * y));
+ return y;
+}
+#pragma GCC diagnostic pop
+//====================================================================================================
+// END OF CODE
+//====================================================================================================
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/MahonyAHRS.h b/lib/M5Stack/src/utility/MahonyAHRS.h
new file mode 100644
index 000000000..4983ffad3
--- /dev/null
+++ b/lib/M5Stack/src/utility/MahonyAHRS.h
@@ -0,0 +1,39 @@
+#if defined (CORE)
+//=====================================================================================================
+// MahonyAHRS.h
+//=====================================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date Author Notes
+// 29/09/2011 SOH Madgwick Initial release
+// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
+//
+//=====================================================================================================
+#ifndef MahonyAHRS_h
+#define MahonyAHRS_h
+
+//----------------------------------------------------------------------------------------------------
+// Variable declaration
+
+extern volatile float twoKp; // 2 * proportional gain (Kp)
+extern volatile float twoKi; // 2 * integral gain (Ki)
+// volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to
+// auxiliary frame
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay,
+ float az, float mx, float my, float mz);
+// void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay,
+// float az);
+void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay,
+ float az, float *pitch, float *roll, float *yaw);
+float invSqrt(float x);
+#endif
+//=====================================================================================================
+// End of file
+//=====================================================================================================
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/Power.cpp b/lib/M5Stack/src/utility/Power.cpp
new file mode 100644
index 000000000..add16b4dc
--- /dev/null
+++ b/lib/M5Stack/src/utility/Power.cpp
@@ -0,0 +1,452 @@
+#if defined (CORE)
+/*----------------------------------------------------------------------*
+ * M5Stack Bettery/Power Control Library v1.0 *
+ * *
+ * This work is licensed under the GNU Lesser General Public *
+ * License v2.1 *
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.en.html *
+ *----------------------------------------------------------------------*/
+#include "Power.h"
+
+#include <esp_bt_main.h>
+#include <esp_sleep.h>
+#include <esp_wifi.h>
+
+#ifndef ESP_ARDUINO_VERSION_VAL
+#define ESP_ARDUINO_VERSION_VAL(major, minor, patch) \
+ ((major << 16) | (minor << 8) | (patch))
+#endif
+
+#if ESP_ARDUINO_VERSION >= ESP_ARDUINO_VERSION_VAL(2, 0, 0)
+#include "esp32/rom/rtc.h"
+#else
+#include <rom/rtc.h>
+#endif // ESP_ARDUINO_VERSION
+
+#include "../M5Stack.h"
+
+// ================ Power IC IP5306 ===================
+#define IP5306_ADDR (117) // 0x75
+#define IP5306_REG_SYS_CTL0 (0x00)
+#define IP5306_REG_SYS_CTL1 (0x01)
+#define IP5306_REG_SYS_CTL2 (0x02)
+#define IP5306_REG_READ0 (0x70)
+#define IP5306_REG_READ1 (0x71)
+#define IP5306_REG_READ3 (0x78)
+#define IP5306_REG_CHG_CTL0 (0x20)
+#define IP5306_REG_CHG_CTL1 (0x21)
+#define IP5306_REG_CHG_DIG (0x24)
+
+//- REG_CTL0
+#define BOOST_ENABLE_BIT (0x20)
+#define CHARGE_OUT_BIT (0x10)
+#define BOOT_ON_LOAD_BIT (0x04)
+#define BOOST_OUT_BIT (0x02)
+#define BOOST_BUTTON_EN_BIT (0x01)
+
+//- REG_CTL1
+#define BOOST_SET_BIT (0x80)
+#define WLED_SET_BIT (0x40)
+#define SHORT_BOOST_BIT (0x20)
+#define VIN_ENABLE_BIT (0x04)
+
+//- REG_CTL2
+#define SHUTDOWNTIME_MASK (0x0c)
+#define SHUTDOWNTIME_64S (0x0c)
+#define SHUTDOWNTIME_32S (0x04)
+#define SHUTDOWNTIME_16S (0x08)
+#define SHUTDOWNTIME_8S (0x00)
+
+//- REG_READ0
+#define CHARGE_ENABLE_BIT (0x08)
+
+//- REG_READ1
+#define CHARGE_FULL_BIT (0x08)
+
+//- REG_READ2
+#define LIGHT_LOAD_BIT (0x20)
+#define LOWPOWER_SHUTDOWN_BIT (0x01)
+
+//- CHG
+#define CURRENT_100MA (0x01 << 0)
+#define CURRENT_200MA (0x01 << 1)
+#define CURRENT_400MA (0x01 << 2)
+#define CURRENT_800MA (0x01 << 3)
+#define CURRENT_1600MA (0x01 << 4)
+
+#define BAT_4_2V (0x00)
+#define BAT_4_3V (0x01)
+#define BAT_4_3_5V (0x02)
+#define BAT_4_4V (0x03)
+
+#define CHG_CC_BIT (0x20)
+
+extern M5Stack M5;
+
+POWER::POWER() {
+}
+
+void POWER::begin() {
+ uint8_t data;
+
+ // Initial I2C
+ Wire.begin(21, 22);
+
+ // 450ma
+ setVinMaxCurrent(CURRENT_400MA);
+
+ setChargeVolt(BAT_4_2V);
+
+ // End charge current 200ma
+ if (M5.I2C.readByte(IP5306_ADDR, 0x21, &data) == true) {
+ M5.I2C.writeByte(IP5306_ADDR, 0x21, (data & 0x3f) | 0x00);
+ }
+
+ // Add volt 28mv
+ if (M5.I2C.readByte(IP5306_ADDR, 0x22, &data) == true) {
+ M5.I2C.writeByte(IP5306_ADDR, 0x22, (data & 0xfc) | 0x02);
+ }
+
+ // Vin charge CC
+ if (M5.I2C.readByte(IP5306_ADDR, 0x23, &data) == true) {
+ M5.I2C.writeByte(IP5306_ADDR, 0x23, (data & 0xdf) | 0x20);
+ }
+}
+
+bool POWER::setPowerBoostOnOff(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL1, &data) == true) {
+ return M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL1,
+ en ? (data | BOOST_SET_BIT) : (data & (~BOOST_SET_BIT)));
+ }
+ return false;
+}
+
+bool POWER::setPowerBoostSet(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL1, &data) == true) {
+ return M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL1,
+ en ? (data | SHORT_BOOST_BIT) : (data & (~SHORT_BOOST_BIT)));
+ }
+ return false;
+}
+
+bool POWER::setPowerVin(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL1, &data) == true) {
+ return M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL1,
+ en ? (data | VIN_ENABLE_BIT) : (data & (~VIN_ENABLE_BIT)));
+ }
+ return false;
+}
+
+bool POWER::setPowerWLEDSet(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL1, &data) == true) {
+ return M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL1,
+ en ? (data | WLED_SET_BIT) : (data & (~WLED_SET_BIT)));
+ }
+ return false;
+}
+
+bool POWER::setPowerBtnEn(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data) == true) {
+ return M5.I2C.writeByte(IP5306_ADDR, IP5306_REG_SYS_CTL0,
+ en ? (data | BOOST_BUTTON_EN_BIT)
+ : (data & (~BOOST_BUTTON_EN_BIT)));
+ }
+ return false;
+}
+
+bool POWER::setLowPowerShutdownTime(ShutdownTime time) {
+ uint8_t data;
+ bool ret;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL2, &data) == true) {
+ switch (time) {
+ case ShutdownTime::SHUTDOWN_8S:
+ ret = M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL2,
+ ((data & (~SHUTDOWNTIME_MASK)) | SHUTDOWNTIME_8S));
+ break;
+ case ShutdownTime::SHUTDOWN_16S:
+ ret = M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL2,
+ ((data & (~SHUTDOWNTIME_MASK)) | SHUTDOWNTIME_16S));
+ break;
+ case ShutdownTime::SHUTDOWN_32S:
+ ret = M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL2,
+ ((data & (~SHUTDOWNTIME_MASK)) | SHUTDOWNTIME_32S));
+ break;
+ case ShutdownTime::SHUTDOWN_64S:
+ ret = M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL2,
+ ((data & (~SHUTDOWNTIME_MASK)) | SHUTDOWNTIME_64S));
+ break;
+ default:
+ ret = false;
+ break;
+ }
+ return ret;
+ }
+ return false;
+}
+
+/*
+ default: false
+ false: when the current is too small, ip5306 will automatically shut down
+ note: it seem not work and has problems
+ Function has disabled.(Stab for compatibility)
+ This function will be removed in a future release.
+*/
+bool POWER::setKeepLightLoad(bool en) {
+ // uint8_t data;
+ // if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data) == true) {
+ // return M5.I2C.writeByte(IP5306_ADDR, IP5306_REG_SYS_CTL0, !en ? (data
+ // | LIGHT_LOAD_BIT) : (data & (~LIGHT_LOAD_BIT)));
+ // }
+ return false;
+}
+
+// true: Output normally open
+bool POWER::setPowerBoostKeepOn(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data) == true) {
+ return M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL0,
+ en ? (data | BOOST_OUT_BIT) : (data & (~BOOST_OUT_BIT)));
+ }
+ return false;
+}
+
+/**
+ * Function has disabled.(Stab for compatibility)
+ * This function will be removed in a future release.
+ */
+bool POWER::setLowPowerShutdown(bool en) {
+ // uint8_t data;
+ // if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL1, &data) == true) {
+ // return M5.I2C.writeByte(IP5306_ADDR, IP5306_REG_SYS_CTL1, en ? (data |
+ // LOWPOWER_SHUTDOWN_BIT) : (data & (~LOWPOWER_SHUTDOWN_BIT)));
+ //}
+ return setPowerBoostKeepOn(!en);
+}
+/*
+ default: true
+ true: If enough load is connected, the power will turn on.
+*/
+bool POWER::setAutoBootOnLoad(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data) == true) {
+ return M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL0,
+ en ? (data | BOOT_ON_LOAD_BIT) : (data & (~BOOT_ON_LOAD_BIT)));
+ }
+ return false;
+}
+
+bool POWER::setVinMaxCurrent(uint8_t cur) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_CHG_DIG, &data) == true) {
+ return M5.I2C.writeByte(IP5306_ADDR, IP5306_REG_CHG_DIG,
+ (data & 0xe0) | cur);
+ }
+ return false;
+}
+
+bool POWER::setChargeVolt(uint8_t volt) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_CHG_CTL0, &data) == true) {
+ return M5.I2C.writeByte(IP5306_ADDR, IP5306_REG_CHG_CTL0,
+ (data & 0xfc) | volt);
+ }
+ return false;
+}
+
+// if charge full,try set charge enable->disable->enable,can be recharged
+bool POWER::setCharge(bool en) {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL0, &data) == true) {
+ return M5.I2C.writeByte(
+ IP5306_ADDR, IP5306_REG_SYS_CTL0,
+ en ? (data | CHARGE_OUT_BIT) : (data & (~CHARGE_OUT_BIT)));
+ }
+ return false;
+}
+
+// full return true, else return false
+bool POWER::isChargeFull() {
+ uint8_t data;
+ return (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_READ1, &data))
+ ? (data & CHARGE_FULL_BIT)
+ : false;
+}
+
+// test if ip5306 is an i2c version
+bool POWER::canControl() {
+ return M5.I2C.writeCommand(IP5306_ADDR, IP5306_REG_READ0);
+}
+
+// true:charge, false:discharge
+bool POWER::isCharging() {
+ uint8_t data;
+ return (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_READ0, &data))
+ ? (data & CHARGE_ENABLE_BIT)
+ : false;
+}
+
+// Return percentage * 100
+int8_t POWER::getBatteryLevel() {
+ uint8_t data;
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_READ3, &data) == true) {
+ switch (data & 0xF0) {
+ case 0x00:
+ return 100;
+ case 0x80:
+ return 75;
+ case 0xC0:
+ return 50;
+ case 0xE0:
+ return 25;
+ default:
+ return 0;
+ }
+ }
+ return -1;
+}
+
+void POWER::setWakeupButton(uint8_t button) {
+ _wakeupPin = button;
+}
+
+void POWER::reset() {
+ esp_restart();
+}
+
+bool POWER::isResetbySoftware() {
+ RESET_REASON reset_reason = rtc_get_reset_reason(0);
+ return (reset_reason == SW_RESET || reset_reason == SW_CPU_RESET);
+}
+
+bool POWER::isResetbyWatchdog() {
+ RESET_REASON reset_reason = rtc_get_reset_reason(0);
+ return (
+ reset_reason == TG0WDT_SYS_RESET || reset_reason == TG1WDT_SYS_RESET ||
+ reset_reason == OWDT_RESET || reset_reason == RTCWDT_SYS_RESET ||
+ reset_reason == RTCWDT_CPU_RESET || reset_reason == RTCWDT_RTC_RESET ||
+ reset_reason == TGWDT_CPU_RESET);
+}
+
+bool POWER::isResetbyDeepsleep() {
+ RESET_REASON reset_reason = rtc_get_reset_reason(0);
+ return (reset_reason == DEEPSLEEP_RESET);
+}
+
+bool POWER::isResetbyPowerSW() {
+ RESET_REASON reset_reason = rtc_get_reset_reason(0);
+ return (reset_reason == POWERON_RESET);
+}
+
+// note:
+// If the IP5306 I2C communication is not available,
+// such as the old model, there is a limit to the maximum time for sleep return.
+// When using this function, pay attention to the constraints.
+void POWER::deepSleep(uint64_t time_in_us) {
+ // Keep power keep boost on
+ setPowerBoostKeepOn(true);
+
+ // power off the Lcd
+ M5.Lcd.setBrightness(0);
+ M5.Lcd.sleep();
+
+ // ESP32 into deep sleep
+ esp_sleep_enable_ext0_wakeup((gpio_num_t)_wakeupPin, LOW);
+
+ if (time_in_us > 0) {
+ esp_sleep_enable_timer_wakeup(time_in_us);
+ } else {
+ esp_sleep_disable_wakeup_source(ESP_SLEEP_WAKEUP_TIMER);
+ }
+
+ while (digitalRead(_wakeupPin) == LOW) {
+ delay(10);
+ }
+
+ (time_in_us == 0) ? esp_deep_sleep_start() : esp_deep_sleep(time_in_us);
+}
+
+// note:
+// If the IP5306 I2C communication is not available,
+// such as the old model, there is a limit to the maximum time for sleep return.
+// When using this function, pay attention to the constraints.
+void POWER::lightSleep(uint64_t time_in_us) {
+ // Keep power keep boost on
+ setPowerBoostKeepOn(true);
+
+ // power off the Lcd
+ M5.Lcd.setBrightness(0);
+ M5.Lcd.sleep();
+
+ // ESP32 into deep sleep
+ esp_sleep_enable_ext0_wakeup((gpio_num_t)_wakeupPin, LOW);
+ esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_AUTO);
+
+ while (digitalRead(_wakeupPin) == LOW) {
+ delay(10);
+ }
+ if (time_in_us > 0) {
+ esp_sleep_enable_timer_wakeup(time_in_us);
+ } else {
+ esp_sleep_disable_wakeup_source(ESP_SLEEP_WAKEUP_TIMER);
+ }
+ esp_light_sleep_start();
+
+ // power on the Lcd
+ M5.Lcd.wakeup();
+ M5.Lcd.setBrightness(200);
+}
+
+// note:
+// To ensure that the power is turned off,
+// reduce the power consumption according to the specifications of the power
+// supply IC. Otherwise, the power supply IC will continue to supply power.
+void POWER::powerOFF() {
+ uint8_t data;
+ // power off the Lcd
+ M5.Lcd.setBrightness(0);
+ M5.Lcd.sleep();
+
+ // Power off request
+ if (M5.I2C.readByte(IP5306_ADDR, IP5306_REG_SYS_CTL1, &data) == true) {
+ M5.I2C.writeByte(IP5306_ADDR, IP5306_REG_SYS_CTL1,
+ (data & (~BOOST_ENABLE_BIT)));
+ }
+
+ // if wifi was initialized, stop it
+ wifi_mode_t mode;
+ if (esp_wifi_get_mode(&mode) == ESP_OK) {
+ esp_wifi_disconnect();
+ esp_wifi_stop();
+ }
+
+ // stop bt
+ esp_bluedroid_disable();
+
+ // disable interrupt/peripheral
+ esp_sleep_disable_wakeup_source(ESP_SLEEP_WAKEUP_ALL);
+ gpio_deep_sleep_hold_dis();
+
+ // Shutdown setting
+ setPowerBoostKeepOn(false);
+ setLowPowerShutdownTime(ShutdownTime::SHUTDOWN_8S);
+ setPowerBtnEn(true);
+
+ // wait shutdown from IP5306 (low-current shutdown)
+ esp_deep_sleep_start();
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/Power.h b/lib/M5Stack/src/utility/Power.h
new file mode 100644
index 000000000..dc4756b7b
--- /dev/null
+++ b/lib/M5Stack/src/utility/Power.h
@@ -0,0 +1,78 @@
+#if defined (CORE)
+/*----------------------------------------------------------------------*
+ * M5Stack Bettery/Power Control Library v1.0 *
+ * *
+ * This work is licensed under the GNU Lesser General Public *
+ * License v2.1 *
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.en.html *
+ *----------------------------------------------------------------------*/
+#ifndef Power_h
+#define Power_h
+#include <Arduino.h>
+#include <Wire.h>
+
+#define SLEEP_MSEC(us) (((uint64_t)us) * 1000L)
+#define SLEEP_SEC(us) (((uint64_t)us) * 1000000L)
+#define SLEEP_MIN(us) (((uint64_t)us) * 60L * 1000000L)
+#define SLEEP_HR(us) (((uint64_t)us) * 60L * 60L * 1000000L)
+
+class POWER {
+ public:
+ POWER();
+ bool canControl();
+ void begin();
+
+ // -- ShutdownTimeParam
+ enum ShutdownTime {
+ SHUTDOWN_8S = 0,
+ SHUTDOWN_16S,
+ SHUTDOWN_32S,
+ SHUTDOWN_64S
+ };
+
+ // -- control for power
+ bool setKeepLightLoad(bool en) __attribute__((deprecated));
+ bool setPowerBoostKeepOn(bool en);
+ bool setAutoBootOnLoad(bool en);
+ bool setLowPowerShutdown(bool en) __attribute__((deprecated));
+ bool setLowPowerShutdownTime(ShutdownTime time);
+ bool setPowerBoostOnOff(bool en);
+ bool setPowerBoostSet(bool en);
+ bool setPowerVin(bool en);
+ bool setPowerWLEDSet(bool en);
+ bool setPowerBtnEn(bool en);
+
+ // -- control for battery
+ bool setVinMaxCurrent(uint8_t cur);
+ bool setChargeVolt(uint8_t volt);
+
+ bool setCharge(bool en);
+ bool isChargeFull();
+ bool isCharging();
+ int8_t getBatteryLevel();
+ bool batteryMode(bool en);
+
+ // -- configuration for wakeup
+ void setWakeupButton(uint8_t button);
+
+ // -- get resson for startup
+ bool isResetbyWatchdog();
+ bool isResetbyDeepsleep();
+ bool isResetbySoftware();
+ bool isResetbyPowerSW();
+
+ // -- sleep
+ void deepSleep(uint64_t time_in_us = 0);
+ void lightSleep(uint64_t time_in_us = 0);
+
+ // -- power off
+ void powerOFF();
+
+ // -- software reset
+ void reset();
+
+ private:
+ uint8_t _wakeupPin;
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/SH200Q.cpp b/lib/M5Stack/src/utility/SH200Q.cpp
new file mode 100644
index 000000000..b74be348b
--- /dev/null
+++ b/lib/M5Stack/src/utility/SH200Q.cpp
@@ -0,0 +1,268 @@
+#if defined (CORE)
+#include "SH200Q.h"
+
+#include <Arduino.h>
+#include <math.h>
+
+SH200Q::SH200Q() {
+}
+
+void SH200Q::I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* read_Buffer) {
+ Wire.beginTransmission(driver_Addr);
+ Wire.write(start_Addr);
+ Wire.endTransmission();
+ uint8_t i = 0;
+ Wire.requestFrom(driver_Addr, number_Bytes);
+ // byte buf = Wire1.read();
+ //*read_Buffer = buf;
+ //! Put read results in the Rx buffer
+ while (Wire.available()) {
+ read_Buffer[i++] = Wire1.read();
+ }
+}
+
+void SH200Q::I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* write_Buffer) {
+ Wire.beginTransmission(driver_Addr);
+ Wire.write(start_Addr);
+ Wire.write(*write_Buffer);
+ Wire.endTransmission();
+ // Serial.printf("I2C Write OP, ADDR: 0x%02x, ADS: 0x%02x, NumBytes: %u,
+ // Data: 0x%02x\n\r", driver_Addr, start_Addr, number_Bytes, *write_Buffer);
+}
+
+void SH200Q::sh200i_ADCReset(void) {
+ unsigned char tempdata[1];
+ // set 0xC2 bit2 1-->0
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_ADC_RESET, 1, tempdata);
+
+ tempdata[0] = tempdata[0] | 0x04;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_ADC_RESET, 1, tempdata);
+ delay(1);
+
+ tempdata[0] = tempdata[0] & 0xFB;
+ // tempdata[0] = 0x0A; //C8
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_ADC_RESET, 1, tempdata);
+}
+
+void SH200Q::sh200i_Reset(void) {
+ unsigned char tempdata[1];
+
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_RESET, 1, tempdata);
+
+ // SH200I_RESET
+ tempdata[0] = tempdata[0] | 0x80;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_RESET, 1, tempdata);
+
+ delay(1);
+
+ tempdata[0] = tempdata[0] & 0x7F;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_RESET, 1, tempdata);
+}
+
+//初始化
+int SH200Q::Init(void) {
+ unsigned char tempdata[1];
+ Gyscale = GFS_2000DPS;
+ Acscale = AFS_8G;
+
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_WHOAMI, 1, tempdata);
+ if (tempdata[0] != 0x18) {
+ return -1;
+ }
+ imuId = tempdata[0];
+
+ sh200i_ADCReset();
+
+ I2C_Read_NBytes(SH200I_ADDRESS, 0xD8, 1, tempdata);
+
+ tempdata[0] = tempdata[0] | 0x80;
+ I2C_Write_NBytes(SH200I_ADDRESS, 0xD8, 1, tempdata);
+
+ delay(1);
+
+ tempdata[0] = tempdata[0] & 0x7F;
+ I2C_Write_NBytes(SH200I_ADDRESS, 0xD8, 1, tempdata);
+
+ tempdata[0] = 0x61;
+ I2C_Write_NBytes(SH200I_ADDRESS, 0x78, 1, tempdata);
+
+ delay(1);
+
+ tempdata[0] = 0x00;
+ I2C_Write_NBytes(SH200I_ADDRESS, 0x78, 1, tempdata);
+
+ // set acc odr 256hz
+ tempdata[0] = 0x91; // 0x81 1024hz //0x89 512hz //0x91 256hz
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_ACC_CONFIG, 1, tempdata);
+
+ // set gyro odr 500hz
+ tempdata[0] = 0x13; // 0x11 1000hz //0x13 500hz //0x15 256hz
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_GYRO_CONFIG, 1, tempdata);
+
+ // set gyro dlpf 50hz
+ tempdata[0] =
+ 0x03; // 0x00 250hz //0x01 200hz 0x02 100hz 0x03 50hz 0x04 25hz
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_GYRO_DLPF, 1, tempdata);
+
+ // set no buffer mode
+ tempdata[0] = 0x00;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_FIFO_CONFIG, 1, tempdata);
+
+ // set acc range +-8G
+ tempdata[0] = 0x01;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_ACC_RANGE, 1, tempdata);
+
+ // set gyro range +-2000¶È/s
+ tempdata[0] = 0x00;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_GYRO_RANGE, 1, tempdata);
+
+ tempdata[0] = 0xC0;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_REG_SET1, 1, tempdata);
+
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_REG_SET2, 1, tempdata);
+
+ // ADC Reset
+ tempdata[0] = tempdata[0] | 0x10;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_REG_SET2, 1, tempdata);
+
+ delay(1);
+
+ tempdata[0] = tempdata[0] & 0xEF;
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_REG_SET2, 1, tempdata);
+
+ delay(10);
+
+ setGyroFsr(Gyscale);
+ setAccelFsr(Acscale);
+ return 0;
+}
+
+// Possible gyro scales (and their register bit settings) are:
+// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
+// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit
+// value:
+void SH200Q::updateGres() {
+ switch (Gyscale) {
+ case GFS_125DPS:
+ gRes = 125.0 / 32768.0;
+ break;
+ case GFS_250DPS:
+ gRes = 250.0 / 32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0 / 32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0 / 32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0 / 32768.0;
+ break;
+ }
+}
+
+// Possible accelerometer scales (and their register bit settings) are:
+// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
+// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit
+// value:
+void SH200Q::updateAres() {
+ switch (Acscale) {
+ case AFS_4G:
+ aRes = 4.0 / 32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0 / 32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0 / 32768.0;
+ break;
+ }
+}
+
+void SH200Q::setGyroFsr(Gscale scale) {
+ uint8_t regData;
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_GYRO_RANGE, 1, ®Data);
+ regData = (regData & 0xf8) | (scale & 0x07);
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_GYRO_RANGE, 1, ®Data);
+ delay(10);
+ Gyscale = scale;
+ updateGres();
+}
+
+void SH200Q::setAccelFsr(Ascale scale) {
+ uint8_t regData;
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_ACC_RANGE, 1, ®Data);
+ regData = (regData & 0xf8) | (scale & 0x07);
+ I2C_Write_NBytes(SH200I_ADDRESS, SH200I_ACC_RANGE, 1, ®Data);
+ delay(10);
+ Acscale = scale;
+ updateAres();
+}
+
+void SH200Q::getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_OUTPUT_ACC, 6, buf);
+
+ *ax = (int16_t)((buf[1] << 8) | buf[0]);
+ *ay = (int16_t)((buf[3] << 8) | buf[2]);
+ *az = (int16_t)((buf[5] << 8) | buf[4]);
+}
+
+void SH200Q::getAccelData(float* ax, float* ay, float* az) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_OUTPUT_ACC, 6, buf);
+
+ *ax = (int16_t)((buf[1] << 8) | buf[0]) * aRes;
+ *ay = (int16_t)((buf[3] << 8) | buf[2]) * aRes;
+ *az = (int16_t)((buf[5] << 8) | buf[4]) * aRes;
+}
+
+void SH200Q::getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_OUTPUT_GYRO, 6, buf);
+
+ *gx = (int16_t)((buf[1] << 8) | buf[0]);
+ *gy = (int16_t)((buf[3] << 8) | buf[2]);
+ *gz = (int16_t)((buf[5] << 8) | buf[4]);
+}
+
+void SH200Q::getGyroData(float* gx, float* gy, float* gz) {
+ uint8_t buf[6];
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_OUTPUT_GYRO, 6, buf);
+
+ *gx = (int16_t)((buf[1] << 8) | buf[0]) * gRes;
+ *gy = (int16_t)((buf[3] << 8) | buf[2]) * gRes;
+ *gz = (int16_t)((buf[5] << 8) | buf[4]) * gRes;
+}
+
+void SH200Q::getTempAdc(int16_t* t) {
+ uint8_t buf[2];
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_OUTPUT_TEMP, 2, buf);
+ *t = (int16_t)((buf[1] << 8) | buf[0]);
+}
+
+void SH200Q::getTempData(float* t) {
+ uint8_t buf[2];
+ I2C_Read_NBytes(SH200I_ADDRESS, SH200I_OUTPUT_TEMP, 2, buf);
+ *t = (int16_t)((buf[1] << 8) | buf[0]) / 333.87 + 21.0;
+}
+
+//!俯仰，航向，横滚: pitch，yaw，roll，指三维空间中飞行器的旋转状态。
+void SH200Q::getAhrsData(float* pitch, float* roll, float* yaw) {
+ float accX = 0;
+ float accY = 0;
+ float accZ = 0;
+
+ float gyroX = 0;
+ float gyroY = 0;
+ float gyroZ = 0;
+
+ getGyroData(&gyroX, &gyroY, &gyroZ);
+ getAccelData(&accX, &accY, &accZ);
+
+ MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD,
+ gyroZ * DEG_TO_RAD, accX, accY, accZ, pitch, roll, yaw);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/SH200Q.h b/lib/M5Stack/src/utility/SH200Q.h
new file mode 100644
index 000000000..838d2a142
--- /dev/null
+++ b/lib/M5Stack/src/utility/SH200Q.h
@@ -0,0 +1,78 @@
+#if defined (CORE)
+#ifndef _SH200Q_H_
+#define _SH200Q_H_
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#include "MahonyAHRS.h"
+
+#define SH200I_ADDRESS 0x6C // 7bit i2c address
+#define SH200I_WHOAMI 0x30
+#define SH200I_ACC_CONFIG 0x0E
+#define SH200I_GYRO_CONFIG 0x0F
+#define SH200I_GYRO_DLPF 0x11
+#define SH200I_FIFO_CONFIG 0x12
+#define SH200I_ACC_RANGE 0x16
+#define SH200I_GYRO_RANGE 0x2B
+#define SH200I_OUTPUT_ACC 0x00
+#define SH200I_OUTPUT_GYRO 0x06
+#define SH200I_OUTPUT_TEMP 0x0C
+#define SH200I_REG_SET1 0xBA
+#define SH200I_REG_SET2 0xCA // ADC reset
+#define SH200I_ADC_RESET 0xC2 // drive reset
+#define SH200I_SOFT_RESET 0x7F
+#define SH200I_RESET 0x75
+
+//#define G (9.8)
+#define RtA 57.324841
+#define AtR 0.0174533
+#define Gyro_Gr 0.0010653
+
+class SH200Q {
+ public:
+ enum Ascale { AFS_4G = 0, AFS_8G, AFS_16G };
+
+ enum Gscale {
+ GFS_2000DPS = 0,
+ GFS_1000DPS,
+ GFS_500DPS,
+ GFS_250DPS,
+ GFS_125DPS,
+ };
+
+ public:
+ SH200Q();
+
+ int Init(void);
+
+ void getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az);
+ void getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz);
+ void getTempAdc(int16_t* t);
+
+ void getAccelData(float* ax, float* ay, float* az);
+ void getGyroData(float* gx, float* gy, float* gz);
+ void getTempData(float* t);
+ void getAhrsData(float* pitch, float* roll, float* yaw);
+
+ void setGyroFsr(Gscale scale);
+ void setAccelFsr(Ascale scale);
+
+ public:
+ float aRes, gRes;
+ uint8_t imuId;
+ Gscale Gyscale;
+ Ascale Acscale;
+
+ protected:
+ void I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* read_Buffer);
+ void I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr,
+ uint8_t number_Bytes, uint8_t* write_Buffer);
+ void sh200i_ADCReset(void);
+ void sh200i_Reset(void);
+ void updateGres();
+ void updateAres();
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/Speaker.cpp b/lib/M5Stack/src/utility/Speaker.cpp
new file mode 100644
index 000000000..99647f896
--- /dev/null
+++ b/lib/M5Stack/src/utility/Speaker.cpp
@@ -0,0 +1,83 @@
+#if defined (CORE)
+#include "Speaker.h"
+
+SPEAKER::SPEAKER(void) {
+ _volume = 8;
+ _begun = false;
+}
+
+void SPEAKER::begin() {
+ _begun = true;
+ ledcSetup(TONE_PIN_CHANNEL, 0, 13);
+ ledcAttachPin(SPEAKER_PIN, TONE_PIN_CHANNEL);
+ setBeep(1000, 100);
+}
+
+void SPEAKER::end() {
+ mute();
+ ledcDetachPin(SPEAKER_PIN);
+ _begun = false;
+}
+
+void SPEAKER::tone(uint16_t frequency) {
+ if (!_begun) begin();
+ ledcWriteTone(TONE_PIN_CHANNEL, frequency);
+ ledcWrite(TONE_PIN_CHANNEL, 0x400 >> _volume);
+}
+
+void SPEAKER::tone(uint16_t frequency, uint32_t duration) {
+ tone(frequency);
+ _count = millis() + duration;
+ speaker_on = 1;
+}
+
+void SPEAKER::beep() {
+ if (!_begun) begin();
+ tone(_beep_freq, _beep_duration);
+}
+
+void SPEAKER::setBeep(uint16_t frequency, uint16_t duration) {
+ _beep_freq = frequency;
+ _beep_duration = duration;
+}
+
+void SPEAKER::setVolume(uint8_t volume) {
+ _volume = 11 - volume;
+}
+
+void SPEAKER::mute() {
+ ledcWriteTone(TONE_PIN_CHANNEL, 0);
+ digitalWrite(SPEAKER_PIN, 0);
+}
+
+void SPEAKER::update() {
+ if (speaker_on) {
+ if (millis() > _count) {
+ speaker_on = 0;
+ mute();
+ }
+ }
+}
+
+void SPEAKER::write(uint8_t value) {
+ dacWrite(SPEAKER_PIN, value);
+}
+
+void SPEAKER::playMusic(const uint8_t* music_data, uint16_t sample_rate) {
+ uint32_t length = strlen((char*)music_data);
+ uint16_t delay_interval = ((uint32_t)1000000 / sample_rate);
+ if (_volume != 11) {
+ for (int i = 0; i < length; i++) {
+ dacWrite(SPEAKER_PIN, music_data[i] / _volume);
+ delayMicroseconds(delay_interval);
+ }
+
+ for (int t = music_data[length - 1] / _volume; t >= 0; t--) {
+ dacWrite(SPEAKER_PIN, t);
+ delay(2);
+ }
+ }
+ // ledcSetup(TONE_PIN_CHANNEL, 0, 13);
+ ledcAttachPin(SPEAKER_PIN, TONE_PIN_CHANNEL);
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/Speaker.h b/lib/M5Stack/src/utility/Speaker.h
new file mode 100644
index 000000000..285c44191
--- /dev/null
+++ b/lib/M5Stack/src/utility/Speaker.h
@@ -0,0 +1,42 @@
+#if defined (CORE)
+#ifndef _SPEAKER_H_
+#define _SPEAKER_H_
+
+#include "Arduino.h"
+#include "Config.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+#include "esp32-hal-dac.h"
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+class SPEAKER {
+ public:
+ SPEAKER(void);
+
+ void begin();
+ void end();
+ void mute();
+ void tone(uint16_t frequency);
+ void tone(uint16_t frequency, uint32_t duration);
+ void beep();
+ void setBeep(uint16_t frequency, uint16_t duration);
+ void update();
+
+ void write(uint8_t value);
+ void setVolume(uint8_t volume);
+ void playMusic(const uint8_t *music_data, uint16_t sample_rate);
+
+ private:
+ uint32_t _count;
+ uint8_t _volume;
+ uint16_t _beep_duration;
+ uint16_t _beep_freq;
+ bool _begun;
+ bool speaker_on;
+};
+#endif
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/pngle.c b/lib/M5Stack/src/utility/pngle.c
new file mode 100644
index 000000000..4ac9709c8
--- /dev/null
+++ b/lib/M5Stack/src/utility/pngle.c
@@ -0,0 +1,983 @@
+#if defined (CORE)
+/*-
+ * MIT License
+ *
+ * Copyright (c) 2019 kikuchan
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <rom/miniz.h>
+#include "pngle.h"
+
+#ifndef MIN
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#endif
+
+#ifdef PNGLE_DEBUG
+#define debug_printf(...) fprintf(stderr, __VA_ARGS__)
+#else
+#define debug_printf(...) ((void)0)
+#endif
+
+#define PNGLE_ERROR(s) \
+ (pngle->error = (s), pngle->state = PNGLE_STATE_ERROR, -1)
+#define PNGLE_CALLOC(a, b, name) \
+ (debug_printf("[pngle] Allocating %zu bytes for %s\n", \
+ (size_t)(a) * (size_t)(b), (name)), \
+ calloc((size_t)(a), (size_t)(b)))
+
+#define PNGLE_UNUSED(x) (void)(x)
+
+typedef enum {
+ PNGLE_STATE_ERROR = -2,
+ PNGLE_STATE_EOF = -1,
+ PNGLE_STATE_INITIAL = 0,
+
+ PNGLE_STATE_FIND_CHUNK_HEADER,
+ PNGLE_STATE_HANDLE_CHUNK,
+ PNGLE_STATE_CRC,
+} pngle_state_t;
+
+typedef enum {
+ // Supported chunks
+ // Filter chunk names by following command to (re)generate hex constants;
+ // % perl -ne 'chomp; s/.*\s*\/\/\s*//; print "\tPNGLE_CHUNK_$_ = 0x" .
+ // unpack("H*") . "UL, // $_\n";'
+ PNGLE_CHUNK_IHDR = 0x49484452UL, // IHDR
+ PNGLE_CHUNK_PLTE = 0x504c5445UL, // PLTE
+ PNGLE_CHUNK_IDAT = 0x49444154UL, // IDAT
+ PNGLE_CHUNK_IEND = 0x49454e44UL, // IEND
+ PNGLE_CHUNK_tRNS = 0x74524e53UL, // tRNS
+ PNGLE_CHUNK_gAMA = 0x67414d41UL, // gAMA
+} pngle_chunk_t;
+
+// typedef struct _pngle_t pngle_t; // declared in pngle.h
+struct _pngle_t {
+ pngle_ihdr_t hdr;
+
+ uint_fast8_t channels; // 0 indicates IHDR hasn't been processed yet
+
+ // PLTE chunk
+ size_t n_palettes;
+ uint8_t *palette;
+
+ // tRNS chunk
+ size_t n_trans_palettes;
+ uint8_t *trans_palette;
+
+ // parser state (reset on every chunk header)
+ pngle_state_t state;
+ uint32_t chunk_type;
+ uint32_t chunk_remain;
+ mz_ulong crc32;
+
+ // decompression state (reset on IHDR)
+ tinfl_decompressor inflator; // 11000 bytes
+ uint8_t lz_buf[TINFL_LZ_DICT_SIZE]; // 32768 bytes
+ uint8_t *next_out; // NULL indicates IDAT hasn't been processed yet
+ size_t avail_out;
+
+ // scanline decoder (reset on every set_interlace_pass() call)
+ uint8_t *scanline_ringbuf;
+ size_t scanline_ringbuf_size;
+ size_t scanline_ringbuf_cidx;
+ int_fast8_t scanline_remain_bytes_to_render;
+ int_fast8_t filter_type;
+ uint32_t drawing_x;
+ uint32_t drawing_y;
+
+ // interlace
+ uint_fast8_t interlace_pass;
+
+ const char *error;
+
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ uint8_t *gamma_table;
+ double display_gamma;
+#endif
+
+ pngle_init_callback_t init_callback;
+ pngle_draw_callback_t draw_callback;
+ pngle_done_callback_t done_callback;
+
+ void *user_data;
+};
+
+// magic
+static const uint8_t png_sig[] = {137, 80, 78, 71, 13, 10, 26, 10};
+static uint32_t interlace_off_x[8] = {0, 0, 4, 0, 2, 0, 1, 0};
+static uint32_t interlace_off_y[8] = {0, 0, 0, 4, 0, 2, 0, 1};
+static uint32_t interlace_div_x[8] = {1, 8, 8, 4, 4, 2, 2, 1};
+static uint32_t interlace_div_y[8] = {1, 8, 8, 8, 4, 4, 2, 2};
+
+static inline uint8_t read_uint8(const uint8_t *p) {
+ return *p;
+}
+
+static inline uint32_t read_uint32(const uint8_t *p) {
+ return (p[0] << 24) | (p[1] << 16) | (p[2] << 8) | (p[3] << 0);
+}
+
+static inline uint32_t U32_CLAMP_ADD(uint32_t a, uint32_t b, uint32_t top) {
+ uint32_t v = a + b;
+ if (v < a) return top; // uint32 overflow
+ if (v > top) return top; // clamp
+ return v;
+}
+
+void pngle_reset(pngle_t *pngle) {
+ if (!pngle) return;
+
+ pngle->state = PNGLE_STATE_INITIAL;
+ pngle->error = "No error";
+
+ if (pngle->scanline_ringbuf) free(pngle->scanline_ringbuf);
+ if (pngle->palette) free(pngle->palette);
+ if (pngle->trans_palette) free(pngle->trans_palette);
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ if (pngle->gamma_table) free(pngle->gamma_table);
+#endif
+
+ pngle->scanline_ringbuf = NULL;
+ pngle->palette = NULL;
+ pngle->trans_palette = NULL;
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ pngle->gamma_table = NULL;
+#endif
+
+ pngle->channels = 0; // indicates IHDR hasn't been processed yet
+ pngle->next_out = NULL; // indicates IDAT hasn't been processed yet
+
+ // clear them just in case...
+ memset(&pngle->hdr, 0, sizeof(pngle->hdr));
+ pngle->n_palettes = 0;
+ pngle->n_trans_palettes = 0;
+
+ tinfl_init(&pngle->inflator);
+}
+
+pngle_t *pngle_new() {
+ pngle_t *pngle = (pngle_t *)PNGLE_CALLOC(1, sizeof(pngle_t), "pngle_t");
+ if (!pngle) return NULL;
+
+ pngle_reset(pngle);
+
+ return pngle;
+}
+
+void pngle_destroy(pngle_t *pngle) {
+ if (pngle) {
+ pngle_reset(pngle);
+ free(pngle);
+ }
+}
+
+const char *pngle_error(pngle_t *pngle) {
+ if (!pngle) return "Uninitialized";
+ return pngle->error;
+}
+
+uint32_t pngle_get_width(pngle_t *pngle) {
+ if (!pngle) return 0;
+ return pngle->hdr.width;
+}
+
+uint32_t pngle_get_height(pngle_t *pngle) {
+ if (!pngle) return 0;
+ return pngle->hdr.height;
+}
+
+pngle_ihdr_t *pngle_get_ihdr(pngle_t *pngle) {
+ if (!pngle) return NULL;
+ if (pngle->channels == 0) return NULL;
+ return &pngle->hdr;
+}
+
+static int is_trans_color(pngle_t *pngle, uint16_t *value, size_t n) {
+ if (pngle->n_trans_palettes != 1) return 0; // false (none or indexed)
+
+ for (size_t i = 0; i < n; i++) {
+ if (value[i] != (pngle->trans_palette[i * 2 + 0] * 0x100 +
+ pngle->trans_palette[i * 2 + 1]))
+ return 0; // false
+ }
+ return 1; // true
+}
+
+static inline void scanline_ringbuf_push(pngle_t *pngle, uint8_t value) {
+ pngle->scanline_ringbuf[pngle->scanline_ringbuf_cidx] = value;
+ pngle->scanline_ringbuf_cidx =
+ (pngle->scanline_ringbuf_cidx + 1) % pngle->scanline_ringbuf_size;
+}
+
+static inline uint16_t get_value(pngle_t *pngle, size_t *ridx, int *bitcount,
+ int depth) {
+ uint16_t v;
+
+ switch (depth) {
+ case 1:
+ case 2:
+ case 4:
+ if (*bitcount >= 8) {
+ *bitcount = 0;
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+ }
+ *bitcount += depth;
+ uint8_t mask = ((1UL << depth) - 1);
+ uint8_t shift = (8 - *bitcount);
+ return (pngle->scanline_ringbuf[*ridx] >> shift) & mask;
+
+ case 8:
+ v = pngle->scanline_ringbuf[*ridx];
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+ return v;
+
+ case 16:
+ v = pngle->scanline_ringbuf[*ridx];
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+
+ v = v * 0x100 + pngle->scanline_ringbuf[*ridx];
+ *ridx = (*ridx + 1) % pngle->scanline_ringbuf_size;
+ return v;
+ }
+
+ return 0;
+}
+
+static int pngle_draw_pixels(pngle_t *pngle, size_t scanline_ringbuf_xidx) {
+ uint16_t v[4]; // MAX_CHANNELS
+ int bitcount = 0;
+ uint8_t pixel_depth = (pngle->hdr.color_type & 1) ? 8 : pngle->hdr.depth;
+ uint16_t maxval = (1UL << pixel_depth) - 1;
+
+ int n_pixels = pngle->hdr.depth == 16 ? 1 : (8 / pngle->hdr.depth);
+
+ for (; n_pixels-- > 0 && pngle->drawing_x < pngle->hdr.width;
+ pngle->drawing_x = U32_CLAMP_ADD(
+ pngle->drawing_x, interlace_div_x[pngle->interlace_pass],
+ pngle->hdr.width)) {
+ for (uint_fast8_t c = 0; c < pngle->channels; c++) {
+ v[c] = get_value(pngle, &scanline_ringbuf_xidx, &bitcount,
+ pngle->hdr.depth);
+ }
+
+ // color type: 0000 0111
+ // ^-- indexed color (palette)
+ // ^--- Color
+ // ^---- Alpha channel
+
+ if (pngle->hdr.color_type & 2) {
+ // color
+ if (pngle->hdr.color_type & 1) {
+ // indexed color: type 3
+
+ // lookup palette info
+ uint16_t pidx = v[0];
+ if (pidx >= pngle->n_palettes)
+ return PNGLE_ERROR("Color index is out of range");
+
+ v[0] = pngle->palette[pidx * 3 + 0];
+ v[1] = pngle->palette[pidx * 3 + 1];
+ v[2] = pngle->palette[pidx * 3 + 2];
+
+ // tRNS as an indexed alpha value table (for color type 3)
+ v[3] = pidx < pngle->n_trans_palettes
+ ? pngle->trans_palette[pidx]
+ : maxval;
+ } else {
+ // true color: 2, and 6
+ v[3] = (pngle->hdr.color_type & 4) ? v[3]
+ : is_trans_color(pngle, v, 3) ? 0
+ : maxval;
+ }
+ } else {
+ // alpha, tRNS, or opaque
+ v[3] = (pngle->hdr.color_type & 4) ? v[1]
+ : is_trans_color(pngle, v, 1) ? 0
+ : maxval;
+
+ // monochrome
+ v[1] = v[2] = v[0];
+ }
+
+ if (pngle->draw_callback) {
+ uint8_t rgba[4] = {(v[0] * 255 + maxval / 2) / maxval,
+ (v[1] * 255 + maxval / 2) / maxval,
+ (v[2] * 255 + maxval / 2) / maxval,
+ (v[3] * 255 + maxval / 2) / maxval};
+
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ if (pngle->gamma_table) {
+ for (int i = 0; i < 3; i++) {
+ rgba[i] = pngle->gamma_table[v[i]];
+ }
+ }
+#endif
+
+ pngle->draw_callback(pngle, pngle->drawing_x, pngle->drawing_y,
+ MIN(interlace_div_x[pngle->interlace_pass] -
+ interlace_off_x[pngle->interlace_pass],
+ pngle->hdr.width - pngle->drawing_x),
+ MIN(interlace_div_y[pngle->interlace_pass] -
+ interlace_off_y[pngle->interlace_pass],
+ pngle->hdr.height - pngle->drawing_y),
+ rgba);
+ }
+ }
+
+ return 0;
+}
+
+static inline int paeth(int a, int b, int c) {
+ int p = a + b - c;
+ int pa = abs(p - a);
+ int pb = abs(p - b);
+ int pc = abs(p - c);
+
+ if (pa <= pb && pa <= pc) return a;
+ if (pb <= pc) return b;
+ return c;
+}
+
+static int set_interlace_pass(pngle_t *pngle, uint_fast8_t pass) {
+ pngle->interlace_pass = pass;
+
+ uint_fast8_t bytes_per_pixel =
+ (pngle->channels * pngle->hdr.depth + 7) / 8; // 1 if depth <= 8
+ size_t scanline_pixels =
+ (pngle->hdr.width - interlace_off_x[pngle->interlace_pass] +
+ interlace_div_x[pngle->interlace_pass] - 1) /
+ interlace_div_x[pngle->interlace_pass];
+ size_t scanline_stride =
+ (scanline_pixels * pngle->channels * pngle->hdr.depth + 7) / 8;
+
+ pngle->scanline_ringbuf_size =
+ scanline_stride + bytes_per_pixel * 2; // 2 rooms for c/x and a
+
+ if (pngle->scanline_ringbuf) free(pngle->scanline_ringbuf);
+ if ((pngle->scanline_ringbuf = PNGLE_CALLOC(pngle->scanline_ringbuf_size, 1,
+ "scanline ringbuf")) == NULL)
+ return PNGLE_ERROR("Insufficient memory");
+
+ pngle->drawing_x = interlace_off_x[pngle->interlace_pass];
+ pngle->drawing_y = interlace_off_y[pngle->interlace_pass];
+ pngle->filter_type = -1;
+
+ pngle->scanline_ringbuf_cidx = 0;
+ pngle->scanline_remain_bytes_to_render = -1;
+
+ return 0;
+}
+
+static int setup_gamma_table(pngle_t *pngle, uint32_t png_gamma) {
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ if (pngle->gamma_table) free(pngle->gamma_table);
+
+ if (pngle->display_gamma <= 0) return 0; // disable gamma correction
+ if (png_gamma == 0) return 0;
+
+ uint8_t pixel_depth = (pngle->hdr.color_type & 1) ? 8 : pngle->hdr.depth;
+ uint16_t maxval = (1UL << pixel_depth) - 1;
+
+ pngle->gamma_table = PNGLE_CALLOC(1, maxval + 1, "gamma table");
+ if (!pngle->gamma_table) return PNGLE_ERROR("Insufficient memory");
+
+ for (int i = 0; i < maxval + 1; i++) {
+ pngle->gamma_table[i] =
+ (uint8_t)floor(pow(i / (double)maxval,
+ 100000.0 / png_gamma / pngle->display_gamma) *
+ 255.0 +
+ 0.5);
+ }
+ debug_printf("[pngle] gamma value = %d\n", png_gamma);
+#else
+ PNGLE_UNUSED(pngle);
+ PNGLE_UNUSED(png_gamma);
+#endif
+ return 0;
+}
+
+static int pngle_on_data(pngle_t *pngle, const uint8_t *p, int len) {
+ const uint8_t *ep = p + len;
+
+ uint_fast8_t bytes_per_pixel =
+ (pngle->channels * pngle->hdr.depth + 7) / 8; // 1 if depth <= 8
+
+ while (p < ep) {
+ if (pngle->drawing_x >= pngle->hdr.width) {
+ // New row
+ pngle->drawing_x = interlace_off_x[pngle->interlace_pass];
+ pngle->drawing_y = U32_CLAMP_ADD(
+ pngle->drawing_y, interlace_div_y[pngle->interlace_pass],
+ pngle->hdr.height);
+ pngle->filter_type = -1; // Indicate new line
+ }
+
+ if (pngle->drawing_x >= pngle->hdr.width ||
+ pngle->drawing_y >= pngle->hdr.height) {
+ if (pngle->interlace_pass == 0 || pngle->interlace_pass >= 7)
+ return len; // Do nothing further
+
+ // Interlace: Next pass
+ if (set_interlace_pass(pngle, pngle->interlace_pass + 1) < 0)
+ return -1;
+ debug_printf("[pngle] interlace pass changed to: %d\n",
+ pngle->interlace_pass);
+
+ continue; // This is required because "No filter type bytes are
+ // present in an empty pass".
+ }
+
+ if (pngle->filter_type < 0) {
+ if (*p > 4) {
+ debug_printf("[pngle] Invalid filter type is found; 0x%02x\n",
+ *p);
+ return PNGLE_ERROR("Invalid filter type is found");
+ }
+
+ pngle->filter_type = (int_fast8_t)*p++; // 0 - 4
+
+ // push sentinel bytes for new line
+ for (uint_fast8_t i = 0; i < bytes_per_pixel; i++) {
+ scanline_ringbuf_push(pngle, 0);
+ }
+
+ continue;
+ }
+
+ size_t cidx = pngle->scanline_ringbuf_cidx;
+ size_t bidx = (pngle->scanline_ringbuf_cidx + bytes_per_pixel) %
+ pngle->scanline_ringbuf_size;
+ size_t aidx = (pngle->scanline_ringbuf_cidx +
+ pngle->scanline_ringbuf_size - bytes_per_pixel) %
+ pngle->scanline_ringbuf_size;
+ // debug_printf("[pngle] cidx = %zd, bidx = %zd, aidx = %zd\n", cidx,
+ // bidx, aidx);
+
+ uint8_t c = pngle->scanline_ringbuf[cidx]; // left-up
+ uint8_t b = pngle->scanline_ringbuf[bidx]; // up
+ uint8_t a = pngle->scanline_ringbuf[aidx]; // left
+ uint8_t x = *p++; // target
+ // debug_printf("[pngle] c = 0x%02x, b = 0x%02x, a = 0x%02x, x =
+ // 0x%02x\n", c, b, a, x);
+
+ // Reverse the filter
+ switch (pngle->filter_type) {
+ case 0:
+ break; // None
+ case 1:
+ x += a;
+ break; // Sub
+ case 2:
+ x += b;
+ break; // Up
+ case 3:
+ x += (a + b) / 2;
+ break; // Average
+ case 4:
+ x += paeth(a, b, c);
+ break; // Paeth
+ }
+
+ scanline_ringbuf_push(pngle, x); // updates scanline_ringbuf_cidx
+
+ if (pngle->scanline_remain_bytes_to_render < 0)
+ pngle->scanline_remain_bytes_to_render = bytes_per_pixel;
+ if (--pngle->scanline_remain_bytes_to_render == 0) {
+ size_t xidx = (pngle->scanline_ringbuf_cidx +
+ pngle->scanline_ringbuf_size - bytes_per_pixel) %
+ pngle->scanline_ringbuf_size;
+
+ if (pngle_draw_pixels(pngle, xidx) < 0) return -1;
+
+ pngle->scanline_remain_bytes_to_render = -1; // reset
+ }
+ }
+
+ return len;
+}
+
+static int pngle_handle_chunk(pngle_t *pngle, const uint8_t *buf, size_t len) {
+ size_t consume = 0;
+
+ switch (pngle->chunk_type) {
+ case PNGLE_CHUNK_IHDR:
+ // parse IHDR
+ consume = 13;
+ if (len < consume) return 0;
+
+ debug_printf("[pngle] Parse IHDR\n");
+
+ pngle->hdr.width = read_uint32(buf + 0);
+ pngle->hdr.height = read_uint32(buf + 4);
+ pngle->hdr.depth = read_uint8(buf + 8);
+ pngle->hdr.color_type = read_uint8(buf + 9);
+ pngle->hdr.compression = read_uint8(buf + 10);
+ pngle->hdr.filter = read_uint8(buf + 11);
+ pngle->hdr.interlace = read_uint8(buf + 12);
+
+ debug_printf("[pngle] width : %d\n", pngle->hdr.width);
+ debug_printf("[pngle] height : %d\n", pngle->hdr.height);
+ debug_printf("[pngle] depth : %d\n", pngle->hdr.depth);
+ debug_printf("[pngle] color_type : %d\n",
+ pngle->hdr.color_type);
+ debug_printf("[pngle] compression: %d\n",
+ pngle->hdr.compression);
+ debug_printf("[pngle] filter : %d\n", pngle->hdr.filter);
+ debug_printf("[pngle] interlace : %d\n", pngle->hdr.interlace);
+
+ /*
+ Color Allowed Interpretation channels Type Bit Depths
+
+ 0 1,2,4,8,16 Each pixel is a grayscale sample. 1
+ channels (Brightness)
+
+ 2 8,16 Each pixel is an R,G,B triple. 3
+ channels (R, G, B)
+
+ 3 1,2,4,8 Each pixel is a palette index; 1
+ channels (palette info) a PLTE chunk must appear.
+
+ 4 8,16 Each pixel is a grayscale sample, 2
+ channels (Brightness, Alpha) followed by an alpha sample.
+
+ 6 8,16 Each pixel is an R,G,B triple, 4
+ channels (R, G, B, Alpha) followed by an alpha sample.
+ */
+ // 111
+ // ^-- indexed color (palette)
+ // ^--- Color
+ // ^---- Alpha channel
+
+ switch (pngle->hdr.color_type) {
+ case 0:
+ pngle->channels = 1;
+ if (pngle->hdr.depth != 1 && pngle->hdr.depth != 2 &&
+ pngle->hdr.depth != 4 && pngle->hdr.depth != 8 &&
+ pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // grayscale
+ case 2:
+ pngle->channels = 3;
+ if (pngle->hdr.depth != 8 && pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // truecolor
+ case 3:
+ pngle->channels = 1;
+ if (pngle->hdr.depth != 1 && pngle->hdr.depth != 2 &&
+ pngle->hdr.depth != 4 && pngle->hdr.depth != 8)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // indexed color
+ case 4:
+ pngle->channels = 2;
+ if (pngle->hdr.depth != 8 && pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // grayscale + alpha
+ case 6:
+ pngle->channels = 4;
+ if (pngle->hdr.depth != 8 && pngle->hdr.depth != 16)
+ return PNGLE_ERROR("Invalid bit depth");
+ break; // truecolor + alpha
+ default:
+ return PNGLE_ERROR("Incorrect IHDR info");
+ }
+
+ if (pngle->hdr.compression != 0)
+ return PNGLE_ERROR("Unsupported compression type in IHDR");
+ if (pngle->hdr.filter != 0)
+ return PNGLE_ERROR("Unsupported filter type in IHDR");
+
+ // interlace
+ if (set_interlace_pass(pngle, pngle->hdr.interlace ? 1 : 0) < 0)
+ return -1;
+
+ // callback
+ if (pngle->init_callback)
+ pngle->init_callback(pngle, pngle->hdr.width,
+ pngle->hdr.height);
+
+ break;
+
+ case PNGLE_CHUNK_IDAT:
+ // parse & decode IDAT chunk
+ if (len < 1) return 0;
+
+ debug_printf("[pngle] Reading IDAT (len %zd / chunk remain %u)\n",
+ len, pngle->chunk_remain);
+
+ size_t in_bytes = len;
+ size_t out_bytes = pngle->avail_out;
+
+ // debug_printf("[pngle] in_bytes %zd, out_bytes %zd, next_out
+ // %p\n", in_bytes, out_bytes, pngle->next_out);
+
+ // XXX: tinfl_decompress always requires (next_out - lz_buf +
+ // avail_out) == TINFL_LZ_DICT_SIZE
+ tinfl_status status = tinfl_decompress(
+ &pngle->inflator, (const mz_uint8 *)buf, &in_bytes,
+ pngle->lz_buf, (mz_uint8 *)pngle->next_out, &out_bytes,
+ TINFL_FLAG_HAS_MORE_INPUT | TINFL_FLAG_PARSE_ZLIB_HEADER);
+
+ // debug_printf("[pngle] tinfl_decompress\n");
+ // debug_printf("[pngle] => in_bytes %zd, out_bytes %zd,
+ // next_out %p, status %d\n", in_bytes, out_bytes, pngle->next_out,
+ // status);
+
+ if (status < TINFL_STATUS_DONE) {
+ // Decompression failed.
+ debug_printf(
+ "[pngle] tinfl_decompress() failed with status %d!\n",
+ status);
+ return PNGLE_ERROR("Failed to decompress the IDAT stream");
+ }
+
+ pngle->next_out += out_bytes;
+ pngle->avail_out -= out_bytes;
+
+ // debug_printf("[pngle] => avail_out %zd, next_out %p\n",
+ // pngle->avail_out, pngle->next_out);
+
+ if (status == TINFL_STATUS_DONE || pngle->avail_out == 0) {
+ // Output buffer is full, or decompression is done, so write
+ // buffer to output file.
+ // XXX: This is the only chance to process the buffer.
+ uint8_t *read_ptr = pngle->lz_buf;
+ size_t n = TINFL_LZ_DICT_SIZE - (size_t)pngle->avail_out;
+
+ // pngle_on_data() usually returns n, otherwise -1 on error
+ if (pngle_on_data(pngle, read_ptr, n) < 0) return -1;
+
+ // XXX: tinfl_decompress always requires (next_out - lz_buf +
+ // avail_out) == TINFL_LZ_DICT_SIZE
+ pngle->next_out = pngle->lz_buf;
+ pngle->avail_out = TINFL_LZ_DICT_SIZE;
+ }
+
+ consume = in_bytes;
+ break;
+
+ case PNGLE_CHUNK_PLTE:
+ consume = 3;
+ if (len < consume) return 0;
+
+ memcpy(pngle->palette + pngle->n_palettes * 3, buf, 3);
+
+ debug_printf("[pngle] PLTE[%zd]: (%d, %d, %d)\n", pngle->n_palettes,
+ pngle->palette[pngle->n_palettes * 3 + 0],
+ pngle->palette[pngle->n_palettes * 3 + 1],
+ pngle->palette[pngle->n_palettes * 3 + 2]);
+
+ pngle->n_palettes++;
+
+ break;
+
+ case PNGLE_CHUNK_IEND:
+ consume = 0;
+ break;
+
+ case PNGLE_CHUNK_tRNS:
+ switch (pngle->hdr.color_type) {
+ case 3:
+ consume = 1;
+ break;
+ case 0:
+ consume = 2 * 1;
+ break;
+ case 2:
+ consume = 2 * 3;
+ break;
+ default:
+ return PNGLE_ERROR(
+ "tRNS chunk is prohibited on the color type");
+ }
+ if (len < consume) return 0;
+
+ memcpy(pngle->trans_palette + pngle->n_trans_palettes, buf,
+ consume);
+
+ pngle->n_trans_palettes++;
+
+ break;
+
+ case PNGLE_CHUNK_gAMA:
+ consume = 4;
+ if (len < consume) return 0;
+
+ if (setup_gamma_table(pngle, read_uint32(buf)) < 0) return -1;
+
+ break;
+
+ default:
+ // unknown chunk
+ consume = len;
+
+ debug_printf("[pngle] Unknown chunk; %zd bytes discarded\n",
+ consume);
+ break;
+ }
+
+ return consume;
+}
+
+static int pngle_feed_internal(pngle_t *pngle, const uint8_t *buf, size_t len) {
+ if (!pngle) return -1;
+
+ switch (pngle->state) {
+ case PNGLE_STATE_ERROR:
+ return -1;
+
+ case PNGLE_STATE_EOF:
+ return len;
+
+ case PNGLE_STATE_INITIAL:
+ // find PNG header
+ if (len < sizeof(png_sig)) return 0;
+
+ if (memcmp(png_sig, buf, sizeof(png_sig)))
+ return PNGLE_ERROR("Incorrect PNG signature");
+
+ debug_printf("[pngle] PNG signature found\n");
+
+ pngle->state = PNGLE_STATE_FIND_CHUNK_HEADER;
+ return sizeof(png_sig);
+
+ case PNGLE_STATE_FIND_CHUNK_HEADER:
+ if (len < 8) return 0;
+
+ pngle->chunk_remain = read_uint32(buf);
+ pngle->chunk_type = read_uint32(buf + 4);
+
+ pngle->crc32 =
+ mz_crc32(MZ_CRC32_INIT, (const mz_uint8 *)(buf + 4), 4);
+
+ debug_printf("[pngle] Chunk '%.4s' len %u\n", buf + 4,
+ pngle->chunk_remain);
+
+ pngle->state = PNGLE_STATE_HANDLE_CHUNK;
+
+ // initialize & sanity check
+ switch (pngle->chunk_type) {
+ case PNGLE_CHUNK_IHDR:
+ if (pngle->chunk_remain != 13)
+ return PNGLE_ERROR("Invalid IHDR chunk size");
+ if (pngle->channels != 0)
+ return PNGLE_ERROR(
+ "Multiple IHDR chunks are not allowed");
+ break;
+
+ case PNGLE_CHUNK_IDAT:
+ if (pngle->chunk_remain <= 0)
+ return PNGLE_ERROR("Invalid IDAT chunk size");
+ if (pngle->channels == 0)
+ return PNGLE_ERROR("No IHDR chunk is found");
+ if (pngle->hdr.color_type == 3 && pngle->palette == NULL)
+ return PNGLE_ERROR("No PLTE chunk is found");
+
+ if (pngle->next_out == NULL) {
+ // Very first IDAT
+ pngle->next_out = pngle->lz_buf;
+ pngle->avail_out = TINFL_LZ_DICT_SIZE;
+ }
+ break;
+
+ case PNGLE_CHUNK_PLTE:
+ if (pngle->chunk_remain <= 0)
+ return PNGLE_ERROR("Invalid PLTE chunk size");
+ if (pngle->channels == 0)
+ return PNGLE_ERROR("No IHDR chunk is found");
+ if (pngle->palette)
+ return PNGLE_ERROR("Too many PLTE chunk");
+
+ switch (pngle->hdr.color_type) {
+ case 3: // indexed color
+ break;
+ case 2: // truecolor
+ case 6: // truecolor + alpha
+ // suggested palettes
+ break;
+ default:
+ return PNGLE_ERROR(
+ "PLTE chunk is prohibited on the color type");
+ }
+
+ if (pngle->chunk_remain % 3)
+ return PNGLE_ERROR("Invalid PLTE chunk size");
+ if (pngle->chunk_remain / 3 >
+ MIN(256, (1UL << pngle->hdr.depth)))
+ return PNGLE_ERROR("Too many palettes in PLTE");
+ if ((pngle->palette = PNGLE_CALLOC(pngle->chunk_remain / 3,
+ 3, "palette")) == NULL)
+ return PNGLE_ERROR("Insufficient memory");
+ pngle->n_palettes = 0;
+ break;
+
+ case PNGLE_CHUNK_IEND:
+ if (pngle->next_out == NULL)
+ return PNGLE_ERROR("No IDAT chunk is found");
+ if (pngle->chunk_remain > 0)
+ return PNGLE_ERROR("Invalid IEND chunk size");
+ break;
+
+ case PNGLE_CHUNK_tRNS:
+ if (pngle->chunk_remain <= 0)
+ return PNGLE_ERROR("Invalid tRNS chunk size");
+ if (pngle->channels == 0)
+ return PNGLE_ERROR("No IHDR chunk is found");
+ if (pngle->trans_palette)
+ return PNGLE_ERROR("Too many tRNS chunk");
+
+ switch (pngle->hdr.color_type) {
+ case 3: // indexed color
+ if (pngle->chunk_remain > (1UL << pngle->hdr.depth))
+ return PNGLE_ERROR("Too many palettes in tRNS");
+ break;
+ case 0: // grayscale
+ if (pngle->chunk_remain != 2)
+ return PNGLE_ERROR("Invalid tRNS chunk size");
+ break;
+ case 2: // truecolor
+ if (pngle->chunk_remain != 6)
+ return PNGLE_ERROR("Invalid tRNS chunk size");
+ break;
+
+ default:
+ return PNGLE_ERROR(
+ "tRNS chunk is prohibited on the color type");
+ }
+ if ((pngle->trans_palette = PNGLE_CALLOC(
+ pngle->chunk_remain, 1, "trans palette")) == NULL)
+ return PNGLE_ERROR("Insufficient memory");
+ pngle->n_trans_palettes = 0;
+ break;
+
+ default:
+ break;
+ }
+
+ return 8;
+
+ case PNGLE_STATE_HANDLE_CHUNK:
+ len = MIN(len, pngle->chunk_remain);
+
+ int consumed = pngle_handle_chunk(pngle, buf, len);
+
+ if (consumed > 0) {
+ if (pngle->chunk_remain < (uint32_t)consumed)
+ return PNGLE_ERROR("Chunk data has been consumed too much");
+
+ pngle->chunk_remain -= consumed;
+ pngle->crc32 =
+ mz_crc32(pngle->crc32, (const mz_uint8 *)buf, consumed);
+ }
+ if (pngle->chunk_remain <= 0) pngle->state = PNGLE_STATE_CRC;
+
+ return consumed;
+
+ case PNGLE_STATE_CRC:
+ if (len < 4) return 0;
+
+ uint32_t crc32 = read_uint32(buf);
+
+ if (crc32 != pngle->crc32) {
+ debug_printf("[pngle] CRC: %08x vs %08x => NG\n", crc32,
+ (uint32_t)pngle->crc32);
+ return PNGLE_ERROR("CRC mismatch");
+ }
+
+ debug_printf("[pngle] CRC: %08x vs %08x => OK\n", crc32,
+ (uint32_t)pngle->crc32);
+ pngle->state = PNGLE_STATE_FIND_CHUNK_HEADER;
+
+ // XXX:
+ if (pngle->chunk_type == PNGLE_CHUNK_IEND) {
+ pngle->state = PNGLE_STATE_EOF;
+ if (pngle->done_callback) pngle->done_callback(pngle);
+ debug_printf("[pngle] DONE\n");
+ }
+
+ return 4;
+
+ default:
+ break;
+ }
+
+ return PNGLE_ERROR("Invalid state");
+}
+
+int pngle_feed(pngle_t *pngle, const void *buf, size_t len) {
+ size_t pos = 0;
+ pngle_state_t last_state = pngle->state;
+
+ while (pos < len) {
+ int r =
+ pngle_feed_internal(pngle, (const uint8_t *)buf + pos, len - pos);
+ if (r < 0) return r; // error
+
+ if (r == 0 && last_state == pngle->state) break;
+ last_state = pngle->state;
+
+ pos += r;
+ }
+
+ return pos;
+}
+
+void pngle_set_display_gamma(pngle_t *pngle, double display_gamma) {
+ if (!pngle) return;
+#ifndef PNGLE_NO_GAMMA_CORRECTION
+ pngle->display_gamma = display_gamma;
+#else
+ PNGLE_UNUSED(display_gamma);
+#endif
+}
+
+void pngle_set_init_callback(pngle_t *pngle, pngle_init_callback_t callback) {
+ if (!pngle) return;
+ pngle->init_callback = callback;
+}
+
+void pngle_set_draw_callback(pngle_t *pngle, pngle_draw_callback_t callback) {
+ if (!pngle) return;
+ pngle->draw_callback = callback;
+}
+
+void pngle_set_done_callback(pngle_t *pngle, pngle_done_callback_t callback) {
+ if (!pngle) return;
+ pngle->done_callback = callback;
+}
+
+void pngle_set_user_data(pngle_t *pngle, void *user_data) {
+ if (!pngle) return;
+ pngle->user_data = user_data;
+}
+
+void *pngle_get_user_data(pngle_t *pngle) {
+ if (!pngle) return NULL;
+ return pngle->user_data;
+}
+
+/* vim: set ts=4 sw=4 noexpandtab: */
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/pngle.h b/lib/M5Stack/src/utility/pngle.h
new file mode 100644
index 000000000..3c54b8b21
--- /dev/null
+++ b/lib/M5Stack/src/utility/pngle.h
@@ -0,0 +1,94 @@
+#if defined (CORE)
+/*-
+ * MIT License
+ *
+ * Copyright (c) 2019 kikuchan
+ *
+ * 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.
+ */
+
+#ifndef __PNGLE_H__
+#define __PNGLE_H__
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Main Pngle object
+typedef struct _pngle_t pngle_t;
+
+// Callback signatures
+typedef void (*pngle_init_callback_t)(pngle_t *pngle, uint32_t w, uint32_t h);
+typedef void (*pngle_draw_callback_t)(pngle_t *pngle, uint32_t x, uint32_t y,
+ uint32_t w, uint32_t h, uint8_t rgba[4]);
+typedef void (*pngle_done_callback_t)(pngle_t *pngle);
+
+// ----------------
+// Basic interfaces
+// ----------------
+pngle_t *pngle_new();
+void pngle_destroy(pngle_t *pngle);
+void pngle_reset(pngle_t *pngle); // clear its internal state (not applied to
+ // pngle_set_* functions)
+const char *pngle_error(pngle_t *pngle);
+int pngle_feed(
+ pngle_t *pngle, const void *buf,
+ size_t len); // returns -1: On error, 0: Need more data, n: n bytes eaten
+
+uint32_t pngle_get_width(pngle_t *pngle);
+uint32_t pngle_get_height(pngle_t *pngle);
+
+void pngle_set_init_callback(pngle_t *png, pngle_init_callback_t callback);
+void pngle_set_draw_callback(pngle_t *png, pngle_draw_callback_t callback);
+void pngle_set_done_callback(pngle_t *png, pngle_done_callback_t callback);
+
+void pngle_set_display_gamma(
+ pngle_t *pngle,
+ double display_gamma); // enables gamma correction by specifying display
+ // gamma, typically 2.2. No effect when gAMA chunk
+ // is missing
+
+void pngle_set_user_data(pngle_t *pngle, void *user_data);
+void *pngle_get_user_data(pngle_t *pngle);
+
+// ----------------
+// Debug interfaces
+// ----------------
+
+typedef struct _pngle_ihdr_t {
+ uint32_t width;
+ uint32_t height;
+ uint8_t depth;
+ uint8_t color_type;
+ uint8_t compression;
+ uint8_t filter;
+ uint8_t interlace;
+} pngle_ihdr_t;
+
+// Get IHDR information
+pngle_ihdr_t *pngle_get_ihdr(pngle_t *pngle);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __PNGLE_H__ */
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/quaternionFilters.cpp b/lib/M5Stack/src/utility/quaternionFilters.cpp
new file mode 100644
index 000000000..12ffa0e44
--- /dev/null
+++ b/lib/M5Stack/src/utility/quaternionFilters.cpp
@@ -0,0 +1,275 @@
+#if defined (CORE)
+// Implementation of Sebastian Madgwick's "...efficient orientation filter
+// for... inertial/magnetic sensor arrays"
+// (see http://www.x-io.co.uk/category/open-source/ for examples & more details)
+// which fuses acceleration, rotation rate, and magnetic moments to produce a
+// quaternion-based estimate of absolute device orientation -- which can be
+// converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
+// The performance of the orientation filter is at least as good as conventional
+// Kalman-based filtering algorithms but is much less computationally
+// intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
+
+#include "quaternionFilters.h"
+
+// These are the free parameters in the Mahony filter and fusion scheme, Kp
+// for proportional feedback, Ki for integral
+#define Kp 2.0f * 5.0f
+#define Ki 0.0f
+
+static float GyroMeasError = PI * (40.0f / 180.0f);
+// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+// static float GyroMeasDrift = PI * (0.0f / 180.0f);
+// There is a tradeoff in the beta parameter between accuracy and response
+// speed. In the original Madgwick study, beta of 0.041 (corresponding to
+// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
+// However, with this value, the LSM9SD0 response time is about 10 seconds
+// to a stable initial quaternion. Subsequent changes also require a
+// longish lag time to a stable output, not fast enough for a quadcopter or
+// robot car! By increasing beta (GyroMeasError) by about a factor of
+// fifteen, the response time constant is reduced to ~2 sec. I haven't
+// noticed any reduction in solution accuracy. This is essentially the I
+// coefficient in a PID control sense; the bigger the feedback coefficient,
+// the faster the solution converges, usually at the expense of accuracy.
+// In any case, this is the free parameter in the Madgwick filtering and
+// fusion scheme.
+static float gyro_beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta
+// Compute zeta, the other free parameter in the Madgwick scheme usually
+// set to a small or zero value
+// static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
+
+// Vector to hold integral error for Mahony method
+static float eInt[3] = {0.0f, 0.0f, 0.0f};
+// Vector to hold quaternion
+static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
+
+void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat) {
+ // short name local variable for readability
+ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
+ float norm;
+ float hx, hy, _2bx, _2bz;
+ float s1, s2, s3, s4;
+ float qDot1, qDot2, qDot3, qDot4;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float _2q1mx;
+ float _2q1my;
+ float _2q1mz;
+ float _2q2mx;
+ float _4bx;
+ float _4bz;
+ float _2q1 = 2.0f * q1;
+ float _2q2 = 2.0f * q2;
+ float _2q3 = 2.0f * q3;
+ float _2q4 = 2.0f * q4;
+ float _2q1q3 = 2.0f * q1 * q3;
+ float _2q3q4 = 2.0f * q3 * q4;
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q1q4 = q1 * q4;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q2q4 = q2 * q4;
+ float q3q3 = q3 * q3;
+ float q3q4 = q3 * q4;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0.0f) {
+ return; // handle NaN
+ }
+ norm = 1.0f / norm;
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Normalise magnetometer measurement
+ norm = sqrt(mx * mx + my * my + mz * mz);
+ if (norm == 0.0f) {
+ return; // handle NaN
+ }
+ norm = 1.0f / norm;
+ mx *= norm;
+ my *= norm;
+ mz *= norm;
+
+ // Reference direction of Earth's magnetic field
+ _2q1mx = 2.0f * q1 * mx;
+ _2q1my = 2.0f * q1 * my;
+ _2q1mz = 2.0f * q1 * mz;
+ _2q2mx = 2.0f * q2 * mx;
+ hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 +
+ _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+ hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 +
+ my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+ _2bx = sqrt(hx * hx + hy * hy);
+ _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 +
+ _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+ _4bx = 2.0f * _2bx;
+ _4bz = 2.0f * _2bz;
+
+ // Gradient decent algorithm corrective step
+ s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q2 * (2.0f * q1q2 + _2q3q4 - ay) -
+ _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (-_2bx * q4 + _2bz * q2) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q1 * (2.0f * q1q2 + _2q3q4 - ay) -
+ 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) +
+ _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (_2bx * q3 + _2bz * q1) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ (_2bx * q4 - _4bz * q2) *
+ (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q4 * (2.0f * q1q2 + _2q3q4 - ay) -
+ 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) +
+ (-_4bx * q3 - _2bz * q1) *
+ (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (_2bx * q2 + _2bz * q4) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ (_2bx * q1 - _4bz * q3) *
+ (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) +
+ _2q3 * (2.0f * q1q2 + _2q3q4 - ay) +
+ (-_4bx * q4 + _2bz * q2) *
+ (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) +
+ (-_2bx * q1 + _2bz * q3) *
+ (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) +
+ _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 +
+ s4 * s4); // normalise step magnitude
+ norm = 1.0f / norm;
+ s1 *= norm;
+ s2 *= norm;
+ s3 *= norm;
+ s4 *= norm;
+
+ // Compute rate of change of quaternion
+ qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - gyro_beta * s1;
+ qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - gyro_beta * s2;
+ qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - gyro_beta * s3;
+ qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - gyro_beta * s4;
+
+ // Integrate to yield quaternion
+ q1 += qDot1 * deltat;
+ q2 += qDot2 * deltat;
+ q3 += qDot3 * deltat;
+ q4 += qDot4 * deltat;
+ norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
+ norm = 1.0f / norm;
+ q[0] = q1 * norm;
+ q[1] = q2 * norm;
+ q[2] = q3 * norm;
+ q[3] = q4 * norm;
+}
+
+// Similar to Madgwick scheme but uses proportional and integral filtering on
+// the error between estimated reference vectors and measured ones.
+void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat) {
+ // short name local variable for readability
+ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
+ float norm;
+ float hx, hy, bx, bz;
+ float vx, vy, vz, wx, wy, wz;
+ float ex, ey, ez;
+ float pa, pb, pc;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q1q4 = q1 * q4;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q2q4 = q2 * q4;
+ float q3q3 = q3 * q3;
+ float q3q4 = q3 * q4;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0.0f) {
+ return; // Handle NaN
+ }
+ norm = 1.0f / norm; // Use reciprocal for division
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Normalise magnetometer measurement
+ norm = sqrt(mx * mx + my * my + mz * mz);
+ if (norm == 0.0f) {
+ return; // Handle NaN
+ }
+ norm = 1.0f / norm; // Use reciprocal for division
+ mx *= norm;
+ my *= norm;
+ mz *= norm;
+
+ // Reference direction of Earth's magnetic field
+ hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) +
+ 2.0f * mz * (q2q4 + q1q3);
+ hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) +
+ 2.0f * mz * (q3q4 - q1q2);
+ bx = sqrt((hx * hx) + (hy * hy));
+ bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) +
+ 2.0f * mz * (0.5f - q2q2 - q3q3);
+
+ // Estimated direction of gravity and magnetic field
+ vx = 2.0f * (q2q4 - q1q3);
+ vy = 2.0f * (q1q2 + q3q4);
+ vz = q1q1 - q2q2 - q3q3 + q4q4;
+ wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
+ wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
+ wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
+
+ // Error is cross product between estimated direction and measured direction
+ // of gravity
+ ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+ ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+ ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+ if (Ki > 0.0f) {
+ eInt[0] += ex; // accumulate integral error
+ eInt[1] += ey;
+ eInt[2] += ez;
+ } else {
+ eInt[0] = 0.0f; // prevent integral wind up
+ eInt[1] = 0.0f;
+ eInt[2] = 0.0f;
+ }
+
+ // Apply feedback terms
+ gx = gx + Kp * ex + Ki * eInt[0];
+ gy = gy + Kp * ey + Ki * eInt[1];
+ gz = gz + Kp * ez + Ki * eInt[2];
+
+ // Integrate rate of change of quaternion
+ pa = q2;
+ pb = q3;
+ pc = q4;
+ q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
+ q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
+ q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
+ q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
+
+ // Normalise quaternion
+ norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
+ norm = 1.0f / norm;
+ q[0] = q1 * norm;
+ q[1] = q2 * norm;
+ q[2] = q3 * norm;
+ q[3] = q4 * norm;
+}
+
+const float* getQ() {
+ return q;
+}
+#endif
\ No newline at end of file
diff --git a/lib/M5Stack/src/utility/quaternionFilters.h b/lib/M5Stack/src/utility/quaternionFilters.h
new file mode 100644
index 000000000..90caee158
--- /dev/null
+++ b/lib/M5Stack/src/utility/quaternionFilters.h
@@ -0,0 +1,16 @@
+#if defined (CORE)
+#ifndef _QUATERNIONFILTERS_H_
+#define _QUATERNIONFILTERS_H_
+
+#include <Arduino.h>
+
+void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat);
+void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
+ float gz, float mx, float my, float mz,
+ float deltat);
+const float* getQ();
+
+#endif // _QUATERNIONFILTERS_H_
+#endif
\ No newline at end of file
diff --git a/lib/TFT_eSPI/TFT_Drivers/ILI9341_Init.h b/lib/TFT_eSPI/TFT_Drivers/ILI9341_Init.h
index 05a5dda3e..5bb75ec82 100644
--- a/lib/TFT_eSPI/TFT_Drivers/ILI9341_Init.h
+++ b/lib/TFT_eSPI/TFT_Drivers/ILI9341_Init.h
@@ -4,8 +4,87 @@
// This setup information uses simple 8-bit SPI writecommand() and writedata() functions
//
// See ST7735_Setup.h file for an alternative format
+#if defined(CORE2)
+{
+ writecommand(0xC8);
+ writedata(0xFF);
+ writedata(0x93);
+ writedata(0x42);
+
+ writecommand(ILI9341_PWCTR1);
+ writedata(0x12);
+ writedata(0x12);
+
+ writecommand(ILI9341_PWCTR2);
+ writedata(0x03);
+
+ writecommand(0xB0);
+ writedata(0xE0);
+
+ writecommand(0xF6);
+ writedata(0x00);
+ writedata(0x01);
+ writedata(0x01);
-#if defined (ILI9341_DRIVER) || defined (ILI9342_DRIVER)
+ writecommand(ILI9341_MADCTL); // Memory Access Control
+#ifdef M5STACK
+ writedata(TFT_MAD_MY | TFT_MAD_MV |
+ TFT_MAD_COLOR_ORDER); // Rotation 0 (portrait mode)
+#else
+ writedata(TFT_MAD_MX | TFT_MAD_COLOR_ORDER); // Rotation 0 (portrait mode)
+#endif
+
+ writecommand(ILI9341_PIXFMT);
+ writedata(0x55);
+
+ writecommand(ILI9341_DFUNCTR); // Display Function Control
+ writedata(0x08);
+ writedata(0x82);
+ writedata(0x27);
+
+ writecommand(ILI9341_GMCTRP1); // Set Gamma
+ writedata(0x00);
+ writedata(0x0C);
+ writedata(0x11);
+ writedata(0x04);
+ writedata(0x11);
+ writedata(0x08);
+ writedata(0x37);
+ writedata(0x89);
+ writedata(0x4C);
+ writedata(0x06);
+ writedata(0x0C);
+ writedata(0x0A);
+ writedata(0x2E);
+ writedata(0x34);
+ writedata(0x0F);
+
+ writecommand(ILI9341_GMCTRN1); // Set Gamma
+ writedata(0x00);
+ writedata(0x0B);
+ writedata(0x11);
+ writedata(0x05);
+ writedata(0x13);
+ writedata(0x09);
+ writedata(0x33);
+ writedata(0x67);
+ writedata(0x48);
+ writedata(0x07);
+ writedata(0x0E);
+ writedata(0x0B);
+ writedata(0x2E);
+ writedata(0x33);
+ writedata(0x0F);
+
+ writecommand(ILI9341_SLPOUT); // Exit Sleep
+
+ spi_end();
+ delay(120);
+ spi_begin();
+
+ writecommand(ILI9341_DISPON); // Display on
+}
+#elif defined (ILI9341_DRIVER) || defined (ILI9342_DRIVER)
{
writecommand(0xEF);
writedata(0x03);
diff --git a/lib/TFT_eSPI/TFT_eSPI.h b/lib/TFT_eSPI/TFT_eSPI.h
index 51d7e7bd9..1e3a5cb76 100644
--- a/lib/TFT_eSPI/TFT_eSPI.h
+++ b/lib/TFT_eSPI/TFT_eSPI.h
@@ -836,7 +836,10 @@ class TFT_eSPI : public Print { friend class TFT_eSprite; // Sprite class has ac
uint8_t decoderState = 0; // UTF8 decoder state - not for user access
uint16_t decoderBuffer; // Unicode code-point buffer - not for user access
-
+
+ //Moved here to Core2 capture it
+ bool locked, inTransaction, lockTransaction; // SPI transaction and mutex lock flags
+ int32_t cursor_x, cursor_y, padX; // Text cursor x,y and padding setting
//--------------------------------------- private ------------------------------------//
private:
// Legacy begin and end prototypes - deprecated TODO: delete
@@ -902,7 +905,6 @@ class TFT_eSPI : public Print { friend class TFT_eSprite; // Sprite class has ac
getColorCallback getColor = nullptr; // Smooth font callback function pointer
- bool locked, inTransaction, lockTransaction; // SPI transaction and mutex lock flags
//-------------------------------------- protected ----------------------------------//
protected:
@@ -925,7 +927,6 @@ class TFT_eSPI : public Print { friend class TFT_eSprite; // Sprite class has ac
bool _vpDatum;
bool _vpOoB;
- int32_t cursor_x, cursor_y, padX; // Text cursor x,y and padding setting
int32_t bg_cursor_x; // Background fill cursor
int32_t last_cursor_x; // Previous text cursor position when fill used
diff --git a/platformio.ini b/platformio.ini
index 3f249b62b..8b2274fb8 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -401,7 +401,7 @@ build_flags =
-DCORE_DEBUG_LEVEL=5
-DM5STACK=1 ;key for new device,
- -DCORE=1 ;mykeyboard.cpp: need map buttons an/or touchscreen,
+ -DCORE2=1 ;mykeyboard.cpp: need map buttons an/or touchscreen,
;display.cpp: need map battery status value,
;settings.cpp: need map brighness control
;main.cpp: need set startup
@@ -412,7 +412,7 @@ build_flags =
;FM Radio
;-DFM_SI4713=1 ;Uncomment to activate FM Radio using Adafruit Si4713
;Microphone
- ;-DMIC_SPM1423=1 ;uncomment to enable Applicable for SPM1423 device
+ -DMIC_SPM1423=1 ;uncomment to enable Applicable for SPM1423 device
-DPIN_CLK=-1
-DI2S_SCLK_PIN=-1
-DI2S_DATA_PIN=-1
@@ -480,6 +480,9 @@ build_flags =
-DILI9341_DRIVER=1
-DTFT_INVERSION_ON=1
-DM5STACK=1
+ -DTFT_WIDTH=240
+ -DTFT_HEIGHT=320
+ -DTFT_MISO=38
-DTFT_MOSI=23
-DTFT_SCLK=18
-DTFT_CS=5
@@ -488,9 +491,8 @@ build_flags =
-DTFT_BL=-1
-DTOUCH_CS=-1
-DSMOOTH_FONT=1
- -DSPI_FREQUENCY=20000000
- -DSPI_READ_FREQUENCY=20000000
- -DSPI_TOUCH_FREQUENCY=2500000
+ -DSPI_FREQUENCY=40000000
+ -DSPI_READ_FREQUENCY=16000000
;SD Card Setup pins
-DSDCARD_CS=4
@@ -565,7 +567,7 @@ build_flags =
-DBTN_ACT=LOw
;Touchscreen Config
- -DHAS_TOUCH=1
+ ;-DHAS_TOUCH=1
;-DALLOW_ALL_GPIO_FOR_IR_RF=1 ; Set this option to make use of all GPIOs, from 1 to 44 to be chosen, except TFT and SD pins
@@ -599,6 +601,8 @@ build_flags =
-DTFT_INVERSION_ON=1
-DTFT_BACKLIGHT_ON=1
-DM5STACK=1
+ -DTFT_WIDTH=240
+ -DTFT_HEIGHT=320
-DTFT_MOSI=23
-DTFT_SCLK=18
-DTFT_CS=14
@@ -681,7 +685,7 @@ build_flags =
-DBTN_ACT=LOw
;Touchscreen Config
- -DHAS_TOUCH=1
+ ;-DHAS_TOUCH=1
;-DALLOW_ALL_GPIO_FOR_IR_RF=1 ; Set this option to make use of all GPIOs, from 1 to 44 to be chosen, except TFT and SD pins
@@ -715,6 +719,8 @@ build_flags =
-DTFT_INVERSION_ON=1
-DTFT_BACKLIGHT_ON=1
-DM5STACK=1
+ -DTFT_WIDTH=240
+ -DTFT_HEIGHT=320
-DTFT_MOSI=23
-DTFT_SCLK=18
-DTFT_CS=14
@@ -955,6 +961,7 @@ board_upload.maximum_size = 16777216
build_flags =
${common.build_flags}
-DCORE_DEBUG_LEVEL=5
+ -DARDUINO_USB_CDC_ON_BOOT=1
-DM5STACK=1 ;key for new device,
;mykeyboard.cpp: need map buttons an/or touchscreen,
@@ -989,7 +996,7 @@ build_flags =
-DDOUT=-1
;Can run USB as HID
- ;-DUSB_as_HID=1 ;uncomment to enable
+ -DUSB_as_HID=1 ;uncomment to enable
;Battery ADC read pin
;-DBAT_PIN=10
diff --git a/src/core/display.cpp b/src/core/display.cpp
index b130979f1..445102c01 100644
--- a/src/core/display.cpp
+++ b/src/core/display.cpp
@@ -444,6 +444,8 @@ int getBattery() {
percent = (mv - 3300) * 100 / (float)(4150 - 3350);
//#elif defined(NEW_DEVICE)
+ #elif defined(CORE2)
+ percent = M5.Axp.GetBatteryLevel();
#elif defined(M5STACK)
percent = M5.Power.getBatteryLevel();
#else
@@ -534,7 +536,7 @@ void listFiles(int index, String fileList[][3]) {
void drawWifiSmall(int x, int y) {
tft.fillRect(x,y,17,17,BGCOLOR);
- tft.fillCircle(9,14,2,FGCOLOR);
+ tft.fillCircle(9+x,14+y,2,FGCOLOR);
tft.drawArc(9+x,14+y,5,7,130,230,FGCOLOR, BGCOLOR);
tft.drawArc(9+x,14+y,11,13,130,230,FGCOLOR, BGCOLOR);
}
@@ -665,7 +667,7 @@ void drawFM(int x, int y) {
tft.fillRect(x,y,80,80,BGCOLOR);
// Case
- tft.drawRoundRect(-12+x,16+y,110,55,8,FGCOLOR);
+ tft.drawRoundRect(-12+x,16+y,110,55,8,FGCOLOR);
tft.drawRoundRect(-12+x-1,16-1+y,112,57,8,FGCOLOR);
tft.drawRoundRect(-12+x-2,16-2+y,114,59,8,FGCOLOR);
@@ -676,9 +678,9 @@ void drawFM(int x, int y) {
tft.drawRect(7+x,27+y,40,20,FGCOLOR);
// Antenna
- tft.drawLine(tft.width()/4+30,tft.height()/4+7,tft.width()/4+58,tft.height()/5+6,FGCOLOR);
- tft.drawLine(tft.width()/4+31,tft.height()/4+7,tft.width()/4+59,tft.height()/5+6,FGCOLOR);
- tft.fillCircle(tft.width()/4+58,tft.height()/5+6,2,FGCOLOR);
+ tft.drawLine(x ,16+y,x+28,y+3,FGCOLOR);
+ tft.drawLine(x+1,16+y,x+29,y+3,FGCOLOR);
+ tft.fillCircle(x+28,y+3,2,FGCOLOR);
// Buttons
tft.fillCircle(12+x,58+y,5,FGCOLOR);
diff --git a/src/core/globals.h b/src/core/globals.h
index d7c2a8a9e..f15424bd1 100644
--- a/src/core/globals.h
+++ b/src/core/globals.h
@@ -31,12 +31,18 @@ extern char16_t FGCOLOR;
extern Keyboard_Class Keyboard;
#endif
-#if defined(M5STACK)
+#if defined(CORE2)
+ #include <M5Core2.h>
+#elif defined(CORE)
+ #include <M5Stack.h>
+#elif defined(M5STACK)
#include <M5Unified.h>
+
#endif
+
// Declaração dos objetos TFT
#if defined(HAS_SCREEN)
- #if defined(M5STACK)
+ #if defined(M5STACK) && !defined(CORE2) && !defined(CORE)
#define tft M5.Lcd
extern M5Canvas sprite;
extern M5Canvas draw;
diff --git a/src/core/main_menu.cpp b/src/core/main_menu.cpp
index f570d5566..3a0ae7791 100644
--- a/src/core/main_menu.cpp
+++ b/src/core/main_menu.cpp
@@ -76,7 +76,6 @@ void wifiOptions() {
**********************************************************************/
void bleOptions() {
options = {
-#if !defined(CORE)
#if !defined(LITE_VERSION)
{"BLE Beacon", [=]() { ble_test(); }},
{"BLE Scan", [=]() { ble_scan(); }},
@@ -87,9 +86,6 @@ void bleOptions() {
{"SourApple", [=]() { aj_adv(3); }},
{"Android Spam", [=]() { aj_adv(4); }},
{"BT Maelstrom", [=]() { aj_adv(5); }},
-#else
- {"In Development", [=]() { backToMenu(); }},
-#endif
{"Main Menu", [=]() { backToMenu(); }},
};
delay(200);
@@ -240,8 +236,10 @@ void otherOptions(){
#endif
#ifdef USB_as_HID
{"BadUSB", [=]() { usb_setup(); }},
+ #if defined(CARDPUTER)
{"USB Keyboard", [=]() { usb_keyboard(); }},
#endif
+ #endif
#ifdef HAS_RGB_LED
{"LED Control", [=]() { ledrgb_setup(); }}, //IncursioHack
{"LED FLash", [=]() { ledrgb_flash(); }}, // IncursioHack
diff --git a/src/core/mykeyboard.cpp b/src/core/mykeyboard.cpp
index 295af8b66..2ef574012 100644
--- a/src/core/mykeyboard.cpp
+++ b/src/core/mykeyboard.cpp
@@ -45,6 +45,32 @@ struct box_t
static constexpr std::size_t box_count = 52;
static box_t box_list[box_count];
+#define PREV 0
+#define SEL 1
+#define NEXT 2
+#define ALL 3
+
+#if defined(M5STACK) && !defined(CORE2)
+bool menuPress(int bot) {
+ //0 - prev
+ //1 - Sel
+ //2 - next
+ //3 - any
+ int terco=WIDTH/3;
+ M5.update();
+ auto t = M5.Touch.getDetail();
+ if (t.isPressed() || t.isHolding()) {
+ //if(rotation==3) t.x = WIDTH-t.x;
+ //else if (rotation==1) t.y = (HEIGHT+20)-t.y;
+ if(t.y>(HEIGHT) && (t.x>terco*bot && t.x<terco*(1+bot) || bot==ALL)) {
+ t.x=WIDTH+1;
+ t.y=HEIGHT+11;
+ return true;
+ } else return false;
+ } else return false;
+}
+
+#endif
#endif
/* Verifies Upper Btn to go to previous item */
@@ -53,9 +79,12 @@ bool checkNextPress(){
#if defined (CARDPUTER)
Keyboard.update();
if(Keyboard.isKeyPressed('/') || Keyboard.isKeyPressed('.'))
+ #elif defined(CORE2) || defined(CORE)
+ M5.update();
+ if(M5.BtnC.isPressed())
#elif defined(M5STACK)
M5.update();
- if(M5.BtnC.isHolding() || M5.BtnC.isPressed())
+ if(menuPress(NEXT))
#elif ! defined(HAS_SCREEN)
// always return false
if(false)
@@ -80,9 +109,12 @@ bool checkPrevPress() {
#elif defined(CARDPUTER)
Keyboard.update();
if(Keyboard.isKeyPressed(',') || Keyboard.isKeyPressed(';'))
+ #elif defined(CORE2) || defined(CORE)
+ M5.update();
+ if(M5.BtnA.isPressed())
#elif defined(M5STACK)
M5.update();
- if(M5.BtnA.isHolding() || M5.BtnA.isPressed())
+ if(menuPress(PREV))
#elif ! defined(HAS_SCREEN)
// always return false
if(false)
@@ -109,9 +141,12 @@ bool checkSelPress(){
#elif ! defined(HAS_SCREEN)
// always return false
if(false)
+ #elif defined(CORE2) || defined(CORE)
+ M5.update();
+ if(M5.BtnB.isPressed())
#elif defined(M5STACK)
M5.update();
- if(M5.BtnB.isHolding() || M5.BtnB.isPressed())
+ if(menuPress(SEL))
#else
if(digitalRead(SEL_BTN)==LOW)
#endif
@@ -137,9 +172,12 @@ bool checkEscPress(){
#elif ! defined(HAS_SCREEN)
// always return false
if(false)
+ #elif defined(CORE2) || defined(CORE)
+ M5.update();
+ if(M5.BtnA.isPressed())
#elif defined(M5STACK)
M5.update();
- if(M5.BtnA.isHolding() || M5.BtnA.isPressed())
+ if(menuPress(PREV))
#else
if(digitalRead(UP_BTN)==LOW)
#endif
@@ -158,9 +196,12 @@ bool checkAnyKeyPress() {
#if defined (CARDPUTER) // If any key is pressed, it'll jump the boot screen
Keyboard.update();
if(Keyboard.isPressed())
+ #elif defined(CORE2) || defined(CORE)
+ M5.update();
+ if(M5.BtnA.isPressed() || M5.BtnB.isPressed() || M5.BtnC.isPressed())
#elif defined(M5STACK)
M5.update();
- if(M5.BtnA.isHolding() || M5.BtnA.isPressed() || M5.BtnB.isHolding() || M5.BtnB.isPressed() || M5.BtnC.isHolding() || M5.BtnC.isPressed())
+ if(menuPress(ALL))
#elif ! defined(HAS_SCREEN)
// always return false
if(false)
@@ -495,7 +536,10 @@ String keyboard(String mytext, int maxSize, String msg) {
int z=0;
#if defined(HAS_TOUCH)
- #if defined(M5STACK)
+ #if defined(CORE2)
+ M5.update();
+ auto t = M5.Touch.getPressPoint();
+ #elif defined(M5STACK)
M5.update();
auto t = M5.Touch.getDetail();
if (t.isPressed() || t.isHolding())
diff --git a/src/core/sd_functions.cpp b/src/core/sd_functions.cpp
index 260527182..8ff7c91c8 100644
--- a/src/core/sd_functions.cpp
+++ b/src/core/sd_functions.cpp
@@ -63,7 +63,9 @@ bool setupSdCard() {
***************************************************************************************/
void closeSdCard() {
SD.end();
+ #if defined(STICK_C_PLUS) || defined(STICK_C_PLUS2)
sdcardSPI.end(); // Closes SPI connections and release pins.
+ #endif
//Serial.println("SD Card Unmounted...");
sdcardMounted = false;
}
diff --git a/src/core/settings.cpp b/src/core/settings.cpp
index d934e1e58..aaf6b824d 100644
--- a/src/core/settings.cpp
+++ b/src/core/settings.cpp
@@ -56,6 +56,11 @@ void setBrightness(int brightval, bool save) {
}
#elif defined(STICK_C_PLUS)
axp192.ScreenBreath(brightval);
+ #elif defined(CORE2)
+ M5.Axp.ScreenBreath(brightval);
+ #elif defined(CORE)
+ uint8_t _tmp = (255*brightval)/100;
+ M5.Lcd.setBrightness(_tmp);
#elif defined(M5STACK)
M5.Display.setBrightness(brightval);
#endif
@@ -84,6 +89,11 @@ void getBrightness() {
analogWrite(BACKLIGHT, bl);
#elif defined(STICK_C_PLUS)
axp192.ScreenBreath(bright);
+ #elif defined(CORE2)
+ M5.Axp.ScreenBreath(bright);
+ #elif defined(CORE)
+ uint8_t _tmp = (255*bright)/100;
+ M5.Lcd.setBrightness(_tmp);
#elif defined(M5STACK)
M5.Display.setBrightness(bright);
#endif
@@ -95,6 +105,11 @@ void getBrightness() {
analogWrite(BACKLIGHT, bl);
#elif defined(STICK_C_PLUS)
axp192.ScreenBreath(bright);
+ #elif defined(CORE2)
+ M5.Axp.ScreenBreath(bright);
+ #elif defined(CORE)
+ uint8_t _tmp = (255*bright)/100;
+ M5.Lcd.setBrightness(_tmp);
#elif defined(M5STACK)
M5.Display.setBrightness(bright);
#endif
diff --git a/src/main.cpp b/src/main.cpp
index f3630d482..46aa09610 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -54,7 +54,7 @@ const int bufSize = 4096;
uint8_t buff[4096] = {0};
// Protected global variables
#if defined(HAS_SCREEN)
- #if defined(M5STACK)
+ #if defined(M5STACK) && !defined(CORE2) && !defined(CORE)
#define tft M5.Lcd
M5Canvas sprite(&M5.Lcd);
M5Canvas draw(&M5.Lcd);
@@ -108,7 +108,7 @@ void setup_gpio() {
#elif ! defined(HAS_SCREEN)
// do nothing
#elif defined(M5STACK) // init must be done after tft, to make SDCard work
- M5.begin();
+ //M5.begin();
#else
pinMode(UP_BTN, INPUT); // Sets the power btn as an INPUT
pinMode(SEL_BTN, INPUT);
@@ -129,9 +129,12 @@ void setup_gpio() {
void begin_tft(){
#if defined(HAS_SCREEN) && !defined(M5STACK)
tft.init();
-#elif !defined(M5STACK)
- tft.begin(); //115200, 240,320);
- tft.clear();
+#elif defined(CORE2) || defined(CORE)
+ M5.begin();
+ tft.init();
+#elif defined(M5STACK)
+ M5.begin();
+
#endif
rotation = gsetRotation();
tft.setRotation(rotation);
diff --git a/src/modules/others/qrcode_menu.cpp b/src/modules/others/qrcode_menu.cpp
index ddfad6962..55074fdbb 100644
--- a/src/modules/others/qrcode_menu.cpp
+++ b/src/modules/others/qrcode_menu.cpp
@@ -1,4 +1,4 @@
-#if !defined(M5STACK)
+#if !defined(M5STACK) || defined(CORE) || defined(CORE2)
#include "../lib/TFT_eSPI_QRcode/src/qrcode.h"
#endif
#include "core/display.h"
@@ -35,7 +35,7 @@ String calculate_crc(String input) {
void qrcode_display(String qrcodeUrl) {
#ifdef HAS_SCREEN
- #if defined(M5STACK)
+ #if defined(M5STACK) && !defined(CORE2) && !defined(CORE)
tft.qrcode(qrcodeUrl,5,5,HEIGHT);
#else
QRcode qrcode(&tft);
From df647f5d2a7aa3d7575964f695053ab4daa02ea5 Mon Sep 17 00:00:00 2001
From: Pirata <bmorcelli@gmail.com>
Date: Sat, 24 Aug 2024 00:50:31 -0300
Subject: [PATCH 2/2] compatibilization
---
src/main.cpp | 412 +--------------------------------------------------
1 file changed, 2 insertions(+), 410 deletions(-)
diff --git a/src/main.cpp b/src/main.cpp
index b5fc5133d..968c59b42 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -181,416 +181,8 @@ void boot_screen() {
}
}
-#if !defined(LITE_VERSION)
- #if defined(BUZZ_PIN)
- // Bip M5 just because it can. Does not bip if splashscreen is bypassed
- _tone(5000, 50);
- delay(200);
- _tone(5000, 50);
- /* 2fix: menu infinite loop */
- #elif defined(HAS_NS4168_SPKR)
- // play a boot sound
- if(SD.exists("/boot.wav")) playAudioFile(&SD, "/boot.wav");
- else if(LittleFS.exists("/boot.wav")) playAudioFile(&LittleFS, "/boot.wav");
- setup_gpio(); // temp fix for menu inf. loop
- #endif
-#endif
-}
-
-
-/*********************************************************************
-** Function: load_eeprom
-** Load EEPROM data
-*********************************************************************/
-void load_eeprom() {
- EEPROM.begin(EEPROMSIZE); // open eeprom
-
- rotation = EEPROM.read(0);
- dimmerSet = EEPROM.read(1);
- bright = EEPROM.read(2);
- IrTx = EEPROM.read(6);
- IrRx = EEPROM.read(7);
- RfTx = EEPROM.read(8);
- RfRx = EEPROM.read(9);
- tmz = EEPROM.read(10);
- FGCOLOR = EEPROM.read(11) << 8 | EEPROM.read(12);
- RfModule = EEPROM.read(13);
- RfidModule = EEPROM.read(14);
-
- log_i("\
- \n*-*EEPROM Settings*-* \
- \n- rotation =%03d, \
- \n- dimmerSet =%03d, \
- \n- Brightness=%03d, \
- \n- IR Tx Pin =%03d, \
- \n- IR Rx Pin =%03d, \
- \n- RF Tx Pin =%03d, \
- \n- RF Rx Pin =%03d, \
- \n- Time Zone =%03d, \
- \n- FGColor =0x%04X \
- \n- RfModule =%03d, \
- \n- RfidModule=%03d, \
- \n*-*-*-*-*-*-*-*-*-*-*", rotation, dimmerSet, bright,IrTx, IrRx, RfTx, RfRx, tmz, FGCOLOR, RfModule, RfidModule);
- if (rotation>3 || dimmerSet>60 || bright>100 || IrTx>100 || IrRx>100 || RfRx>100 || RfTx>100 || tmz>24) {
- rotation = ROTATION;
- dimmerSet=10;
- bright=100;
- IrTx=LED;
- IrRx=GROVE_SCL;
- RfTx=GROVE_SDA;
- RfRx=GROVE_SCL;
- FGCOLOR=0xA80F;
- tmz=0;
- RfModule=0;
- RfidModule=M5_RFID2_MODULE;
-
- EEPROM.write(0, rotation);
- EEPROM.write(1, dimmerSet);
- EEPROM.write(2, bright);
- EEPROM.write(6, IrTx);
- EEPROM.write(7, IrRx);
- EEPROM.write(8, RfTx);
- EEPROM.write(9, RfRx);
- EEPROM.write(10, tmz);
- EEPROM.write(11, int((FGCOLOR >> 8) & 0x00FF));
- EEPROM.write(12, int(FGCOLOR & 0x00FF));
- EEPROM.write(13, RfModule);
- EEPROM.write(14, RfidModule);
- EEPROM.writeString(20,"");
-
- EEPROM.commit(); // Store data to EEPROM
- EEPROM.end();
- log_w("One of the eeprom values is invalid");
- }
- setBrightness(bright,false);
- EEPROM.end();
-}
-
-/*********************************************************************
-** Function: init_clock
-** Clock initialisation for propper display in menu
-*********************************************************************/
-void init_clock() {
- #if defined(HAS_RTC)
- RTC_TimeTypeDef _time;
- cplus_RTC _rtc;
- _rtc.begin();
- _rtc.GetBm8563Time();
- _rtc.GetTime(&_time);
- #endif
-}
-
-/*********************************************************************
-** Function: setup
-** Where the devices are started and variables set
-*********************************************************************/
-void setup() {
- Serial.begin(115200);
-
- log_d("Total heap: %d", ESP.getHeapSize());
- log_d("Free heap: %d", ESP.getFreeHeap());
- if(psramInit()) log_d("PSRAM Started");
- if(psramFound()) log_d("PSRAM Found");
- else log_d("PSRAM Not Found");
- log_d("Total PSRAM: %d", ESP.getPsramSize());
- log_d("Free PSRAM: %d", ESP.getFreePsram());
-
- // declare variables
- prog_handler=0;
- sdcardMounted=false;
- wifiConnected=false;
- BLEConnected=false;
-
- setup_gpio();
- begin_tft();
- load_eeprom();
- init_clock();
-
- if(!LittleFS.begin(true)) { LittleFS.format(), LittleFS.begin();}
-
- boot_screen();
-
- #if ! defined(HAS_SCREEN)
- // start a task to handle serial commands while the webui is running
- startSerialCommandsHandlerTask();
- #endif
-
- delay(200);
- previousMillis = millis();
-}
-
-/**********************************************************************
-** Function: loop
-** Main loop
-**********************************************************************/
-#if defined(HAS_SCREEN)
-void loop() {
- #if defined(HAS_RTC)
- RTC_TimeTypeDef _time;
- #endif
- bool redraw = true;
- int index = 0;
- int opt = 9;
-
- tft.fillRect(0,0,WIDTH,HEIGHT,BGCOLOR);
- setupSdCard();
- getConfigs();
-
-
- while(1){
- if (returnToMenu) {
- returnToMenu = false;
- tft.fillScreen(BGCOLOR); //fix any problem with the mainMenu screen when coming back from submenus or functions
- redraw=true;
- }
-
- if (redraw) {
- drawMainMenu(index);
- redraw = false;
- delay(200);
- }
-
- handleSerialCommands();
-#ifdef CARDPUTER
- checkShortcutPress(); // shortctus to quickly start apps without navigating the menus
-#endif
-
- if (checkPrevPress()) {
- checkReboot();
- if(index==0) index = opt - 1;
- else if(index>0) index--;
- redraw = true;
- }
- /* DW Btn to next item */
- if (checkNextPress()) {
- index++;
- if((index+1)>opt) index = 0;
- redraw = true;
- }
-
- /* Select and run function */
- if (checkSelPress()) {
- getMainMenuOptions(index);
- drawMainBorder(true);
- redraw=true;
- }
-
- if (clock_set) {
- #if defined(HAS_RTC)
- _rtc.GetTime(&_time);
- setTftDisplay(12, 12, FGCOLOR, 1, BGCOLOR);
- snprintf(timeStr, sizeof(timeStr), "%02d:%02d", _time.Hours, _time.Minutes);
- tft.print(timeStr);
- #else
- updateTimeStr(rtc.getTimeStruct());
- setTftDisplay(12, 12, FGCOLOR, 1, BGCOLOR);
- tft.print(timeStr);
- #endif
- }
- else {
- setTftDisplay(12, 12, FGCOLOR, 1, BGCOLOR);
- tft.print("BRUCE " + String(BRUCE_VERSION));
- }
- }
-}
-#else
-
-// alternative loop function for headless boards
-#include "core/wifi_common.h"
-#include "modules/others/webInterface.h"
-
-void loop() {
- setupSdCard();
- getConfigs();
-
- if(!wifiConnected) {
- Serial.println("wifiConnect");
- wifiConnect("",0,true); // TODO: read mode from settings file
- }
- Serial.println("startWebUi");
- startWebUi(true); // MEMO: will quit when checkEscPress
-}
-#endif
-=======
-#include "core/globals.h"
-
-#include <EEPROM.h>
-#include <iostream>
-#include <functional>
-#include <vector>
-#include <string>
-#include "esp32-hal-psram.h"
-
-
-SPIClass sdcardSPI;
-// Public Globals Variables
-unsigned long previousMillis = millis();
-int prog_handler; // 0 - Flash, 1 - LittleFS, 3 - Download
-int rotation;
-int IrTx;
-int IrRx;
-int RfTx;
-int RfRx;
-int RfModule=0; // 0 - single-pinned, 1 - CC1101+SPI
-float RfFreq=433.92;
-int RfidModule=M5_RFID2_MODULE;
-String cachedPassword="";
-int dimmerSet;
-int bright=100;
-int tmz=3;
-int devMode=0;
-bool sdcardMounted = false;
-bool gpsConnected = false;
-bool wifiConnected = false;
-bool BLEConnected = false;
-bool returnToMenu;
-bool isSleeping = false;
-bool isScreenOff = false;
-bool dimmer = false;
-char timeStr[10];
-time_t localTime;
-struct tm* timeInfo;
-#if defined(HAS_RTC)
- cplus_RTC _rtc;
- bool clock_set = true;
-#else
- ESP32Time rtc;
- bool clock_set = false;
-#endif
-JsonDocument settings;
-
-String wui_usr="admin";
-String wui_pwd="bruce";
-String ssid;
-String pwd;
-std::vector<Option> options;
-const int bufSize = 4096;
-uint8_t buff[4096] = {0};
-// Protected global variables
-#if defined(HAS_SCREEN)
- #if defined(M5STACK)
- #define tft M5.Lcd
- M5Canvas sprite(&M5.Lcd);
- M5Canvas draw(&M5.Lcd);
- #else
- TFT_eSPI tft = TFT_eSPI(); // Invoke custom library
- TFT_eSprite sprite = TFT_eSprite(&tft);
- TFT_eSprite draw = TFT_eSprite(&tft);
- #endif
-#else
- SerialDisplayClass tft;
- SerialDisplayClass& sprite = tft;
- SerialDisplayClass& draw = tft;
-#endif
-
-#if defined(CARDPUTER)
- Keyboard_Class Keyboard = Keyboard_Class();
-#elif defined (STICK_C_PLUS)
- AXP192 axp192;
-#endif
-
-#include "Wire.h"
-#include "core/display.h"
-#include "core/mykeyboard.h"
-#include "core/sd_functions.h"
-#include "core/settings.h"
-#include "core/main_menu.h"
-#include "core/serialcmds.h"
-#include "modules/others/audio.h" // for playAudioFile
-#include "modules/rf/rf.h" // for initCC1101once
-
-
-/*********************************************************************
-** Function: setup_gpio
-** Setup GPIO pins
-*********************************************************************/
-void setup_gpio() {
- #if defined(STICK_C_PLUS2)
- pinMode(UP_BTN, INPUT); // Sets the power btn as an INPUT
- pinMode(SEL_BTN, INPUT);
- pinMode(DW_BTN, INPUT);
- pinMode(4, OUTPUT); // Keeps the Stick alive after take off the USB cable
- digitalWrite(4,HIGH); // Keeps the Stick alive after take off the USB cable
- #elif defined(STICK_C_PLUS)
- pinMode(SEL_BTN, INPUT);
- pinMode(DW_BTN, INPUT);
- axp192.begin(); // Start the energy management of AXP192
- #elif defined(CARDPUTER)
- Keyboard.begin();
- pinMode(0, INPUT);
- pinMode(10, INPUT); // Pin that reads the
- #elif ! defined(HAS_SCREEN)
- // do nothing
- #elif defined(M5STACK) // init must be done after tft, to make SDCard work
- M5.begin();
- #else
- pinMode(UP_BTN, INPUT); // Sets the power btn as an INPUT
- pinMode(SEL_BTN, INPUT);
- pinMode(DW_BTN, INPUT);
- #endif
-
- #if defined(BACKLIGHT)
- pinMode(BACKLIGHT, OUTPUT);
- #endif
- initCC1101once(&sdcardSPI); // Sets GPIO in the CC1101 lib
-}
-
-
-/*********************************************************************
-** Function: begin_tft
-** Config tft
-*********************************************************************/
-void begin_tft(){
-#if defined(HAS_SCREEN) && !defined(M5STACK)
- tft.init();
-#elif !defined(M5STACK)
- tft.begin(); //115200, 240,320);
- tft.clear();
-#endif
- rotation = gsetRotation();
- tft.setRotation(rotation);
- resetTftDisplay();
-}
-
-
-/*********************************************************************
-** Function: boot_screen
-** Draw boot screen
-*********************************************************************/
-void boot_screen() {
- tft.setTextColor(FGCOLOR, TFT_BLACK);
- tft.setTextSize(FM);
- tft.drawCentreString("Bruce", WIDTH / 2, 10, SMOOTH_FONT);
- tft.setTextSize(FP);
- tft.drawCentreString(BRUCE_VERSION, WIDTH / 2, 25, SMOOTH_FONT);
- tft.setTextSize(FM);
-
- tft.drawCentreString("PREDATORY FIRMWARE", WIDTH / 2, HEIGHT+2, SMOOTH_FONT); // will draw outside the screen on non touch devices
-
- int i = millis();
- char16_t bgcolor = BGCOLOR;
- while(millis()<i+7000) { // boot image lasts for 5 secs
- #if !defined(LITE_VERSION)
- if((millis()-i>2000) && (millis()-i)<2200) tft.fillRect(0,45,WIDTH,HEIGHT-45,BGCOLOR);
- if((millis()-i>2200) && (millis()-i)<2700) tft.drawRect(2*WIDTH/3,HEIGHT/2,2,2,FGCOLOR);
- if((millis()-i>2700) && (millis()-i)<2900) tft.fillRect(0,45,WIDTH,HEIGHT-45,BGCOLOR);
- #if defined(M5STACK)
- if((millis()-i>2900) && (millis()-i)<3400) tft.drawXBitmap(2*WIDTH/3 - 30 ,5+HEIGHT/2,bruce_small_bits, bruce_small_width, bruce_small_height,bgcolor,FGCOLOR);
- if((millis()-i>3400) && (millis()-i)<3600) tft.fillRect(0,0,WIDTH,HEIGHT,BGCOLOR);
- if((millis()-i>3600)) tft.drawXBitmap((WIDTH-238)/2,(HEIGHT-133)/2,bits, bits_width, bits_height,bgcolor,FGCOLOR);
- #else
- if((millis()-i>2900) && (millis()-i)<3400) tft.drawXBitmap(2*WIDTH/3 - 30 ,5+HEIGHT/2,bruce_small_bits, bruce_small_width, bruce_small_height,TFT_BLACK,FGCOLOR);
- if((millis()-i>3400) && (millis()-i)<3600) tft.fillRect(0,0,WIDTH,HEIGHT,BGCOLOR);
- if((millis()-i>3600)) tft.drawXBitmap((WIDTH-238)/2,(HEIGHT-133)/2,bits, bits_width, bits_height,TFT_BLACK,FGCOLOR);
- #endif
- #endif
- if(checkAnyKeyPress()) // If any key or M5 key is pressed, it'll jump the boot screen
- {
- tft.fillScreen(TFT_BLACK);
- delay(10);
- return;
- }
- }
+ // Clear splashscreen
+ tft.fillScreen(TFT_BLACK);
// Clear splashscreen
tft.fillScreen(TFT_BLACK);