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 +#include + +#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 +#include + +#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 +#include + +#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 +#include +#include +#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 +#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 + +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 +#include +#include +#include + +#include "utility/Config.h" // This is where Core2 defines would be +//#include "utility/In_eSPI.h" +//#include "utility/Sprite.h" +#include +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 _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 + +// 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 + + 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 + + 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 + +#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 + +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 +#include +#include +#include +#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 +#include + +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::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::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 + + 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 + + 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 + + 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 + + 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 + + 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 + + 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 + + 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 +#include +#include +#include +#include +#include + +#include "PointAndZone.h" +#include "utility/Config.h" + +//#ifdef M5Stack_M5Core2 +#include +//#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 EventHandlerCallback; + +class Button : public Zone { + public: + static std::vector 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 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 _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 +#include + +typedef std::function 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 +#include + +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 +#include +#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 +//--------------------------------------------------------------------------------------------------- +// 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 // 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 +#include // 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 +#include +#include +#include +#include + +#include +#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 + +#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 +#include + +// #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 + +// 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 + +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 +#include + +#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 +#include + +#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 + +#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 + +#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 +#include +#include + +#include +//#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 +#include + +#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: + * 
+ * ``         `
+ * + */ +// #define ESP32 + +#ifndef _M5STACK_H_ +#define _M5STACK_H_ + +#if defined(ESP32) + +#include +#include +#include + +#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 +// #else +// #include +// #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 +#include + +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 + +#include + +typedef std::function 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 +#include + +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 +#include + +#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 +#include + +// 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 + +#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 +#include +#include + +#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 +#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 +#include + +#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 +#include + +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 +#include + +#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 +#include +#include +#include +#include + +#include +#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 + +#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 + +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 da3177425..044f120d5 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 +#elif defined(CORE) + #include +#elif defined(M5STACK) #include + #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 1afcb13eb..25f5fe315 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 -#include -#include -#include -#include -#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