LILYGO TTGOのT-Wristbandを方位磁針にする

TTGOのT-Wristbandには、MPU9250あるいはLSM9DS1という9軸センサがついており、地磁気センサがついているので、T-Wristbandの液晶に方位磁針の絵をリアルタイム表示してみたいと思います。

以下を参考にさせていただきました。

　https://lang-ship.com/blog/work/lilygo-ttgo-t-watch/
　http://kako.com/blog/?p=47302

※この製品は技適をとっていないので、BLEやWiFiは使わないようにしてください！

T-Wristbandって何？

以下にあるように、かのMi Bandに激似の、液晶付きの小型リストバンドです。

　https://ja.aliexpress.com/i/4000527495064.html

0.96インチ液晶ディスプレイや、バッテリ、9軸センサが内蔵されています。メインCPUはなじみのあるESP32-PICOで開発ボードもあるので、ArduinoやPlatform IOを使っていろんなことができそうです。
※リストバンドでよくある脈拍センサやバイブ機能はありません。

公式サンプルソースは以下にあります。回路図も置いてあります。
　https://github.com/Xinyuan-LilyGO/LilyGO-T-Wristband

私が購入したときには、MPU9250でしたが、LSM9DS1でも動くと思います。

ライブラリのインストール

以下の2つのライブラリが必要です。

・TFT-eSPI
・PCF8563_Library

最新のバージョンでない場合は、更新しておきます。

LSM9DS1の場合は、以下も必要かもしれません。
・SparkFun LSM9DS1 IMU

MPU9250では以下のファイルも使いますので公式サンプルから持ってきます。
・MPU9250.h、MPU9250.cpp
・quaternionFilters.h、quaternionFilters.cpp

共通で以下のファイルも使います。
・sensor.h
・ttgo.h
・charge.h

WiFiManagerも公式サンプルでは使っていますが、うまくコンパイルできなかったので、使っていません。WiFiが必要な場合は、手動でアクセスポイントを設定します。とはいっても、技適をとっていないので、WiFiは使えません。

次に、TFT_eSPIをヘッダファイルを変更します。

\Documents\Arduino\libraries\TFT_eSPI
にあるUser_Setup_Select.hをエディタで開き、以下の部分をコメントアウトします。

//#include <User_Setup.h>           // Default setup is root library folder

また、以下の部分がコメントアウトされているのを有効にします。

#include <User_Setups/Setup26_TTGO_T_Wristband.h>  // Setup file for ESP32 and TTGO T-Wristband ST7735 SPI bus TFT

実装の補足

ソースコードをまるまる載せる前に、重要箇所だけ補足します。
方位磁針を液晶に表示する部分です。

void COMPASS_Show(){
    readMPU9250();

    tft.fillScreen(TFT_BLACK);

    float center = 66 / 2;
    tft.drawCircle(center, center, center, TFT_WHITE);

    float scaler = sqrt(IMU.mx * IMU.mx + IMU.my * IMU.my);
    float x = IMU.mx / scaler * 0.9;
    float y = IMU.my / scaler * 0.9;
    tft.fillTriangle( center + x * center, center - y * center,
                      center - y * center / 10, center - x * center / 10,
                      center + y * center / 10, center + x * center / 10, TFT_RED  );

    x = -x;
    y = -y;
    tft.fillTriangle( center + x * center, center - y * center,
                      center - y * center / 10, center - x * center / 10,
                      center + y * center / 10, center + x * center / 10, TFT_BLUE  );

    delay(200);
}

readMPU9250()を呼び出して、最新のセンサ情報を取得します。取得した値は内部に保持してあります。後述の通り、IMU.axやらIMU.gxやらIMU.mxといった形で取得できるようになります。

fillScreen
　再描画のため、いったん画面をクリアします。

drawCircle
　丸い方位磁針のように、白い丸を描きます。

fillTriangle
　2回呼び出しています。1回目は赤色で北を示す針を、2回目は南を示す青色の針を描画しています。針っぽくするため、三角形で描いています。
　IMU.mx、IMU.my、IMU.mzが地磁気センサから取得した情報です。Z軸方向はあまり意味がないので今回は使っていません。
　まず、長さを1に正規化しています。あとは、北を示す針の三角形の各頂点を指定して描画します。長さを1ではなくちょっと短くしています。南を示す針は、北の針の真逆の方向ですね。

delay
　必須ではありませんが、描画タイミングを間隔的にするため、ウェイトを入れています。

ソースコード

2種類あります。

1つは基本となる時刻表示、方位磁針表示、9軸センサ値表示のソースです。もう1つは、さらにBLEペリフェラルとしても稼働し、接続してきたBLEセントラルに9軸センサ値をNotificationで通知する機能を追加したものです。
後者は、技適をとっていないので、動かせません。T-Wristbandでなくとも、MPU9520が接続されたESP32デバイスであれば動くので参考まで。

Arduino IDEでコンパイル時に選択するボードは、「ESP32 Pico Kit」にします。シリアルポートも忘れずに選択します。

操作方法ですが、表示部の下のタッチパッド部をタッチすると、時刻表示、方位磁針表示、センサ値表示が切り替わります。
10秒間何もしないか、3秒間タッチパッド部をタッチすると、スタンバイ状態になります。

（基本版）

#include <pcf8563.h>
#include <TFT_eSPI.h> // Graphics and font library for ST7735 driver chip
#include <SPI.h>
#include <Wire.h>
#include "sensor.h"
#include "esp_adc_cal.h"
#include "charge.h"

#define TP_PIN_PIN          33
#define I2C_SDA_PIN         21
#define I2C_SCL_PIN         22
#define IMU_INT_PIN         38
#define RTC_INT_PIN         34
#define BATT_ADC_PIN        35
#define VBUS_PIN            36
#define TP_PWR_PIN          25
#define LED_PIN             4
#define CHARGE_PIN          32

extern MPU9250 IMU;

TFT_eSPI tft = TFT_eSPI();  // Invoke library, pins defined in User_Setup.h
PCF8563_Class rtc;

char buff[256];
bool rtcIrq = false;
bool initial = true;

uint8_t func_select = 0;
uint8_t omm = 99;
uint8_t xcolon = 0;
uint32_t targetTime = 0;       // for next 1 second timeout
uint32_t colour = 0;
int vref = 1100;

bool pressed = false;
uint32_t pressedTime = 0;
uint32_t releasedTime = 0;
bool charge_indication = false;

uint8_t hh, mm, ss;

#define PRESSED_HOLD_TIME   3000
#define RELEASED_HOLD_TIME  10000

void scanI2Cdevice(void)
{
    uint8_t err, addr;
    int nDevices = 0;
    for (addr = 1; addr < 127; addr++) {
        Wire.beginTransmission(addr);
        err = Wire.endTransmission();
        if (err == 0) {
            Serial.print("I2C device found at address 0x");
            if (addr < 16)
                Serial.print("0");
            Serial.print(addr, HEX);
            Serial.println(" !");
            nDevices++;
        } else if (err == 4) {
            Serial.print("Unknow error at address 0x");
            if (addr < 16)
                Serial.print("0");
            Serial.println(addr, HEX);
        }
    }
    if (nDevices == 0)
        Serial.println("No I2C devices found\n");
    else
        Serial.println("Done\n");
}

void drawProgressBar(uint16_t x0, uint16_t y0, uint16_t w, uint16_t h, uint8_t percentage, uint16_t frameColor, uint16_t barColor)
{
    if (percentage == 0) {
        tft.fillRoundRect(x0, y0, w, h, 3, TFT_BLACK);
    }
    uint8_t margin = 2;
    uint16_t barHeight = h - 2 * margin;
    uint16_t barWidth = w - 2 * margin;
    tft.drawRoundRect(x0, y0, w, h, 3, frameColor);
    tft.fillRect(x0 + margin, y0 + margin, barWidth * percentage / 100.0, barHeight, barColor);
}

void setupADC()
{
    esp_adc_cal_characteristics_t adc_chars;
    esp_adc_cal_value_t val_type = esp_adc_cal_characterize((adc_unit_t)ADC_UNIT_1, (adc_atten_t)ADC1_CHANNEL_6, (adc_bits_width_t)ADC_WIDTH_BIT_12, 1100, &adc_chars);
    //Check type of calibration value used to characterize ADC
    if (val_type == ESP_ADC_CAL_VAL_EFUSE_VREF) {
        Serial.printf("eFuse Vref:%u mV", adc_chars.vref);
        vref = adc_chars.vref;
    } else if (val_type == ESP_ADC_CAL_VAL_EFUSE_TP) {
        Serial.printf("Two Point --> coeff_a:%umV coeff_b:%umV\n", adc_chars.coeff_a, adc_chars.coeff_b);
    } else {
        Serial.println("Default Vref: 1100mV");
    }
}

void setupRTC()
{
    rtc.begin(Wire);
    //Check if the RTC clock matches, if not, use compile time
    rtc.check();

    RTC_Date datetime = rtc.getDateTime();
    hh = datetime.hour;
    mm = datetime.minute;
    ss = datetime.second;
}

void setFloatBytes(float f, uint8_t *p_bin){
  uint8_t *p_ptr = (uint8_t*)&f;

  p_bin[0] = p_ptr[3];
  p_bin[1] = p_ptr[2];
  p_bin[2] = p_ptr[1];
  p_bin[3] = p_ptr[0];
}

String getVoltage()
{
    uint16_t v = analogRead(BATT_ADC_PIN);
    float battery_voltage = ((float)v / 4095.0) * 2.0 * 3.3 * (vref / 1000.0);
    return String(battery_voltage) + "V";
}

void RTC_Show()
{
    if (initial || targetTime < millis()) {
        RTC_Date datetime = rtc.getDateTime();
        hh = datetime.hour;
        mm = datetime.minute;
        ss = datetime.second;
        // Serial.printf("hh:%d mm:%d ss:%d\n", hh, mm, ss);
        targetTime = millis() + 1000;
        if (ss == 0 || initial) {
            if( initial ){
               tft.fillScreen(TFT_BLACK);
               omm = 99;
            }
            initial = false;
            tft.setTextColor(TFT_GREEN, TFT_BLACK);
            tft.setCursor (8, 60);
            tft.print(__DATE__); // This uses the standard ADAFruit small font
        }

        tft.setTextColor(TFT_BLUE, TFT_BLACK);
        tft.drawCentreString(getVoltage(), 120, 60, 1); // Next size up font 2

        // Update digital time
        uint8_t xpos = 6;
        uint8_t ypos = 0;
        if (omm != mm) { // Only redraw every minute to minimise flicker
            // Uncomment ONE of the next 2 lines, using the ghost image demonstrates text overlay as time is drawn over it
            tft.setTextColor(0x39C4, TFT_BLACK);  // Leave a 7 segment ghost image, comment out next line!
            //tft.setTextColor(TFT_BLACK, TFT_BLACK); // Set font colour to black to wipe image
            // Font 7 is to show a pseudo 7 segment display.
            // Font 7 only contains characters [space] 0 1 2 3 4 5 6 7 8 9 0 : .
            tft.drawString("88:88", xpos, ypos, 7); // Overwrite the text to clear it
            tft.setTextColor(0xFBE0, TFT_BLACK); // Orange
            omm = mm;

            if (hh < 10) xpos += tft.drawChar('0', xpos, ypos, 7);
            xpos += tft.drawNumber(hh, xpos, ypos, 7);
            xcolon = xpos;
            xpos += tft.drawChar(':', xpos, ypos, 7);
            if (mm < 10) xpos += tft.drawChar('0', xpos, ypos, 7);
            tft.drawNumber(mm, xpos, ypos, 7);
        }

        if (ss % 2) { // Flash the colon
            tft.setTextColor(0x39C4, TFT_BLACK);
            xpos += tft.drawChar(':', xcolon, ypos, 7);
            tft.setTextColor(0xFBE0, TFT_BLACK);
        } else {
            tft.drawChar(':', xcolon, ypos, 7);
        }
    }
}

void COMPASS_Show(){
    readMPU9250();

    tft.fillScreen(TFT_BLACK);

    float center = 66 / 2;
    tft.drawCircle(center, center, center, TFT_WHITE);

    float scaler = sqrt(IMU.mx * IMU.mx + IMU.my * IMU.my);
    float x = IMU.mx / scaler * 0.9;
    float y = IMU.my / scaler * 0.9;
    tft.fillTriangle( center + x * center, center - y * center,
                      center - y * center / 10, center - x * center / 10,
                      center + y * center / 10, center + x * center / 10, TFT_RED  );

    x = -x;
    y = -y;
    tft.fillTriangle( center + x * center, center - y * center,
                      center - y * center / 10, center - x * center / 10,
                      center + y * center / 10, center + x * center / 10, TFT_BLUE  );

    delay(200);
}

void IMU_Show()
{
    tft.setTextColor(TFT_GREEN, TFT_BLACK);
    tft.fillScreen(TFT_BLACK);
    tft.setTextDatum(TL_DATUM);
    readMPU9250();

    snprintf(buff, sizeof(buff), "--  ACC  GYR   MAG");
    tft.drawString(buff, 0, 0);
    snprintf(buff, sizeof(buff), "x %.2f  %.2f  %.2f", (int)1000 * IMU.ax, IMU.gx, IMU.mx);
    tft.drawString(buff, 0, 16);
    snprintf(buff, sizeof(buff), "y %.2f  %.2f  %.2f", (int)1000 * IMU.ay, IMU.gy, IMU.my);
    tft.drawString(buff, 0, 32);
    snprintf(buff, sizeof(buff), "z %.2f  %.2f  %.2f", (int)1000 * IMU.az, IMU.gz, IMU.mz);
    tft.drawString(buff, 0, 48);

    delay(200);
}

void goto_sleep(void){
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_GREEN, TFT_BLACK);
  tft.setTextDatum(MC_DATUM);
  tft.drawString("Press again to wake up",  tft.width() / 2, tft.height() / 2 );
  IMU.setSleepEnabled(true);
  Serial.println("Go to Sleep");
  delay(3000);
  initial = true;
  func_select = 0;
  tft.fillScreen(TFT_BLACK);
  tft.writecommand(ST7735_SLPIN);
  tft.writecommand(ST7735_DISPOFF);
  esp_sleep_enable_ext1_wakeup(GPIO_SEL_33, ESP_EXT1_WAKEUP_ANY_HIGH);
  esp_deep_sleep_start();
}

void setup(void){
    Serial.begin(9600);

    tft.init();
    tft.setRotation(1);
    tft.setSwapBytes(true);

    Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
    Wire.setClock(400000);

    setupRTC();
    setupMPU9250();
    setupADC();

    tft.fillScreen(TFT_BLACK);
    tft.setTextColor(TFT_YELLOW, TFT_BLACK); // Note: the new fonts do not draw the background colour

    targetTime = millis() + 1000;

    pinMode(TP_PIN_PIN, INPUT);
    //! Must be set to pull-up output mode in order to wake up in deep sleep mode
    pinMode(TP_PWR_PIN, PULLUP);
    digitalWrite(TP_PWR_PIN, HIGH);

    pinMode(LED_PIN, OUTPUT);

    pinMode(CHARGE_PIN, INPUT_PULLUP);
    attachInterrupt(CHARGE_PIN, [] {
        charge_indication = true;
    }, CHANGE);

    if (digitalRead(CHARGE_PIN) == LOW) {
        charge_indication = true;
    }
}

void loop()
{
    if (charge_indication) {
        charge_indication = false;
        if (digitalRead(CHARGE_PIN) == LOW) {
            tft.pushImage(140, 55, 16, 16, charge);
        } else {
            tft.fillRect(140, 55, 16, 16, TFT_BLACK);
        }
    }

    if (digitalRead(TP_PIN_PIN) == HIGH) {
        if (!pressed) {
            initial = true;
            func_select++;
            if( func_select > 2 )
              func_select = 0;
            if( func_select == 2)
              digitalWrite(LED_PIN, HIGH);
            else
              digitalWrite(LED_PIN, LOW);
            pressed = true;
            pressedTime = millis();
        } else {
            if ( (millis() - pressedTime) > PRESSED_HOLD_TIME) {
              pressedTime = 0;
              goto_sleep();
            }
        }
    } else {
      if( pressed ){
        pressed = false;
        releasedTime = millis();
      }else{
        if (releasedTime == 0 ){
          releasedTime = millis();
        }else if( (millis() - releasedTime) > RELEASED_HOLD_TIME){
          releasedTime = 0;
          goto_sleep();
        }
      }
    }

    switch (func_select) {
    case 0:
        RTC_Show();
        break;
    case 1:
        COMPASS_Show();
        break;
    case 2:
        IMU_Show();
        break;
    default:
        break;
    }
}

（BLEペリフェラル有効版）
　参考：Androidをペンタブレットにする(3/3)

#include <pcf8563.h>
#include <TFT_eSPI.h> // Graphics and font library for ST7735 driver chip
#include <SPI.h>
#include <Wire.h>
#include "sensor.h"
#include "esp_adc_cal.h"
#include "charge.h"

//#define ENABLE_WIFI_BLE_DEVICE

#ifdef ENABLE_WIFI_BLE_DEVICE
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include "BLE2902.h"
#endif

#define TP_PIN_PIN          33
#define I2C_SDA_PIN         21
#define I2C_SCL_PIN         22
#define IMU_INT_PIN         38
#define RTC_INT_PIN         34
#define BATT_ADC_PIN        35
#define VBUS_PIN            36
#define TP_PWR_PIN          25
#define LED_PIN             4
#define CHARGE_PIN          32

extern MPU9250 IMU;

TFT_eSPI tft = TFT_eSPI();  // Invoke library, pins defined in User_Setup.h
PCF8563_Class rtc;

char buff[256];
bool rtcIrq = false;
bool initial = true;

uint8_t func_select = 0;
uint8_t omm = 99;
uint8_t xcolon = 0;
uint32_t targetTime = 0;       // for next 1 second timeout
uint32_t colour = 0;
int vref = 1100;

bool pressed = false;
uint32_t pressedTime = 0;
uint32_t releasedTime = 0;
bool charge_indication = false;

uint8_t hh, mm, ss;

#define PRESSED_HOLD_TIME   3000
#define RELEASED_HOLD_TIME  10000

#ifdef ENABLE_WIFI_BLE_DEVICE

bool isGyroscope = false;
bool isAccelerometer = false;
bool isMagnetic = false;

void processCommaind(void);
void sendBuffer(uint8_t *p_value, uint16_t len);

#define CMD_SENSOR_MASK   0x0e

#define RSP_ACK           0x00
#define RSP_MAGNETIC      0x12
#define RSP_GYROSCOPE     0x14
#define RSP_ACCELEROMETER 0x15

#endif

void scanI2Cdevice(void)
{
    uint8_t err, addr;
    int nDevices = 0;
    for (addr = 1; addr < 127; addr++) {
        Wire.beginTransmission(addr);
        err = Wire.endTransmission();
        if (err == 0) {
            Serial.print("I2C device found at address 0x");
            if (addr < 16)
                Serial.print("0");
            Serial.print(addr, HEX);
            Serial.println(" !");
            nDevices++;
        } else if (err == 4) {
            Serial.print("Unknow error at address 0x");
            if (addr < 16)
                Serial.print("0");
            Serial.println(addr, HEX);
        }
    }
    if (nDevices == 0)
        Serial.println("No I2C devices found\n");
    else
        Serial.println("Done\n");
}

void drawProgressBar(uint16_t x0, uint16_t y0, uint16_t w, uint16_t h, uint8_t percentage, uint16_t frameColor, uint16_t barColor)
{
    if (percentage == 0) {
        tft.fillRoundRect(x0, y0, w, h, 3, TFT_BLACK);
    }
    uint8_t margin = 2;
    uint16_t barHeight = h - 2 * margin;
    uint16_t barWidth = w - 2 * margin;
    tft.drawRoundRect(x0, y0, w, h, 3, frameColor);
    tft.fillRect(x0 + margin, y0 + margin, barWidth * percentage / 100.0, barHeight, barColor);
}

void setupADC()
{
    esp_adc_cal_characteristics_t adc_chars;
    esp_adc_cal_value_t val_type = esp_adc_cal_characterize((adc_unit_t)ADC_UNIT_1, (adc_atten_t)ADC1_CHANNEL_6, (adc_bits_width_t)ADC_WIDTH_BIT_12, 1100, &adc_chars);
    //Check type of calibration value used to characterize ADC
    if (val_type == ESP_ADC_CAL_VAL_EFUSE_VREF) {
        Serial.printf("eFuse Vref:%u mV", adc_chars.vref);
        vref = adc_chars.vref;
    } else if (val_type == ESP_ADC_CAL_VAL_EFUSE_TP) {
        Serial.printf("Two Point --> coeff_a:%umV coeff_b:%umV\n", adc_chars.coeff_a, adc_chars.coeff_b);
    } else {
        Serial.println("Default Vref: 1100mV");
    }
}

void setupRTC()
{
    rtc.begin(Wire);
    //Check if the RTC clock matches, if not, use compile time
    rtc.check();

    RTC_Date datetime = rtc.getDateTime();
    hh = datetime.hour;
    mm = datetime.minute;
    ss = datetime.second;
}

void setFloatBytes(float f, uint8_t *p_bin){
  uint8_t *p_ptr = (uint8_t*)&f;

  p_bin[0] = p_ptr[3];
  p_bin[1] = p_ptr[2];
  p_bin[2] = p_ptr[1];
  p_bin[3] = p_ptr[0];
}

String getVoltage()
{
    uint16_t v = analogRead(BATT_ADC_PIN);
    float battery_voltage = ((float)v / 4095.0) * 2.0 * 3.3 * (vref / 1000.0);
    return String(battery_voltage) + "V";
}

void RTC_Show()
{
    if (initial || targetTime < millis()) {
        RTC_Date datetime = rtc.getDateTime();
        hh = datetime.hour;
        mm = datetime.minute;
        ss = datetime.second;
        // Serial.printf("hh:%d mm:%d ss:%d\n", hh, mm, ss);
        targetTime = millis() + 1000;
        if (ss == 0 || initial) {
            if( initial ){
               tft.fillScreen(TFT_BLACK);
               omm = 99;
            }
            initial = false;
            tft.setTextColor(TFT_GREEN, TFT_BLACK);
            tft.setCursor (8, 60);
            tft.print(__DATE__); // This uses the standard ADAFruit small font
        }

        tft.setTextColor(TFT_BLUE, TFT_BLACK);
        tft.drawCentreString(getVoltage(), 120, 60, 1); // Next size up font 2

        // Update digital time
        uint8_t xpos = 6;
        uint8_t ypos = 0;
        if (omm != mm) { // Only redraw every minute to minimise flicker
            // Uncomment ONE of the next 2 lines, using the ghost image demonstrates text overlay as time is drawn over it
            tft.setTextColor(0x39C4, TFT_BLACK);  // Leave a 7 segment ghost image, comment out next line!
            //tft.setTextColor(TFT_BLACK, TFT_BLACK); // Set font colour to black to wipe image
            // Font 7 is to show a pseudo 7 segment display.
            // Font 7 only contains characters [space] 0 1 2 3 4 5 6 7 8 9 0 : .
            tft.drawString("88:88", xpos, ypos, 7); // Overwrite the text to clear it
            tft.setTextColor(0xFBE0, TFT_BLACK); // Orange
            omm = mm;

            if (hh < 10) xpos += tft.drawChar('0', xpos, ypos, 7);
            xpos += tft.drawNumber(hh, xpos, ypos, 7);
            xcolon = xpos;
            xpos += tft.drawChar(':', xpos, ypos, 7);
            if (mm < 10) xpos += tft.drawChar('0', xpos, ypos, 7);
            tft.drawNumber(mm, xpos, ypos, 7);
        }

        if (ss % 2) { // Flash the colon
            tft.setTextColor(0x39C4, TFT_BLACK);
            xpos += tft.drawChar(':', xcolon, ypos, 7);
            tft.setTextColor(0xFBE0, TFT_BLACK);
        } else {
            tft.drawChar(':', xcolon, ypos, 7);
        }
    }
}

void COMPASS_Show(){
    readMPU9250();

    tft.fillScreen(TFT_BLACK);

    float center = 66 / 2;
    tft.drawCircle(center, center, center, TFT_WHITE);

    float scaler = sqrt(IMU.mx * IMU.mx + IMU.my * IMU.my);
    float x = IMU.mx / scaler * 0.9;
    float y = IMU.my / scaler * 0.9;
    tft.fillTriangle( center + x * center, center - y * center,
                      center - y * center / 10, center - x * center / 10,
                      center + y * center / 10, center + x * center / 10, TFT_RED  );

    x = -x;
    y = -y;
    tft.fillTriangle( center + x * center, center - y * center,
                      center - y * center / 10, center - x * center / 10,
                      center + y * center / 10, center + x * center / 10, TFT_BLUE  );

    delay(200);
}

void IMU_Show()
{
    tft.setTextColor(TFT_GREEN, TFT_BLACK);
    tft.fillScreen(TFT_BLACK);
    tft.setTextDatum(TL_DATUM);
    readMPU9250();

    snprintf(buff, sizeof(buff), "--  ACC  GYR   MAG");
    tft.drawString(buff, 0, 0);
    snprintf(buff, sizeof(buff), "x %.2f  %.2f  %.2f", (int)1000 * IMU.ax, IMU.gx, IMU.mx);
    tft.drawString(buff, 0, 16);
    snprintf(buff, sizeof(buff), "y %.2f  %.2f  %.2f", (int)1000 * IMU.ay, IMU.gy, IMU.my);
    tft.drawString(buff, 0, 32);
    snprintf(buff, sizeof(buff), "z %.2f  %.2f  %.2f", (int)1000 * IMU.az, IMU.gz, IMU.mz);
    tft.drawString(buff, 0, 48);

#ifdef ENABLE_WIFI_BLE_DEVICE

    uint8_t sensor_buffer[13];

    if( isAccelerometer ){
      sensor_buffer[0] = RSP_ACCELEROMETER;
      setFloatBytes(IMU.ax, &sensor_buffer[1]);
      setFloatBytes(IMU.ay, &sensor_buffer[5]);
      setFloatBytes(IMU.az, &sensor_buffer[9]);
      sendBuffer(sensor_buffer, sizeof(sensor_buffer));
    }
    if( isGyroscope ){
      sensor_buffer[0] = RSP_GYROSCOPE;
      setFloatBytes(IMU.gx, &sensor_buffer[1]);
      setFloatBytes(IMU.gy, &sensor_buffer[5]);
      setFloatBytes(IMU.gz, &sensor_buffer[9]);
      sendBuffer(sensor_buffer, sizeof(sensor_buffer));
    }
    if( isMagnetic ){
      sensor_buffer[0] = RSP_MAGNETIC;
      setFloatBytes(IMU.mx, &sensor_buffer[1]);
      setFloatBytes(IMU.my, &sensor_buffer[5]);
      setFloatBytes(IMU.mz, &sensor_buffer[9]);
      sendBuffer(sensor_buffer, sizeof(sensor_buffer));
    }

#endif

    delay(200);
}

#ifdef ENABLE_WIFI_BLE_DEVICE

#define UUID_SERVICE "08030900-7d3b-4ebf-94e9-18abc4cebede"
#define UUID_WRITE "08030901-7d3b-4ebf-94e9-18abc4cebede"
#define UUID_READ "08030902-7d3b-4ebf-94e9-18abc4cebede"
#define UUID_NOTIFY "08030903-7d3b-4ebf-94e9-18abc4cebede"

#define CAP_MAGNETIC      0x00000001
#define CAP_GYROSCOPE     0x00000002
#define CAP_ACCELEROMETER 0x00000004

bool connected = false;

BLECharacteristic *pCharacteristic_write;
BLECharacteristic *pCharacteristic_read;
BLECharacteristic *pCharacteristic_notify;

class MyCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer){
    connected = true;
    Serial.println("Connected\n");
  }

  void onDisconnect(BLEServer* pServer){
    connected = false;
    isGyroscope = false;
    isAccelerometer = false;
    isMagnetic = false;

    BLE2902* desc = (BLE2902*)pCharacteristic_notify->getDescriptorByUUID(BLEUUID((uint16_t)0x2902));
    desc->setNotifications(false);

    Serial.println("Disconnected\n");
  }
};

unsigned short recv_len = 0;
unsigned short expected_len = 0;
unsigned char expected_slot = 0;
unsigned char recv_buffer[1024];

class MyCharacteristicCallbacks : public BLECharacteristicCallbacks{
  void onWrite(BLECharacteristic* pCharacteristic){
    Serial.println("onWrite");
    uint8_t* value = pCharacteristic->getData();
    std::string str = pCharacteristic->getValue(); 

    if( expected_len > 0 && value[0] != expected_slot )
        expected_len = 0;

    if( expected_len == 0 ){
      if( value[0] != 0x83 )
          return;
      recv_len = 0;
      expected_len = (value[1] << 8) | value[2];
      memmove(&recv_buffer[recv_len], &value[3], str.length() - 3);
      recv_len += str.length() - 3;
      expected_slot = 0;
      if( recv_len < expected_len )
        return;
    }else{
      memmove(&recv_buffer[recv_len], &value[1], str.length() - 1);
      recv_len += str.length() - 1;
      expected_slot++;
      if( recv_len < expected_len )
        return;
    }
    expected_len = 0;

    processCommaind();
  }

  void onStatus(BLECharacteristic* pCharacteristic, Status s, uint32_t code){
    Serial.println("onStatus : ");
    Serial.print(s);
    Serial.print(" ");
    Serial.println(code);
  }
};

void processCommaind(void){
  switch(recv_buffer[0]){
    case CMD_SENSOR_MASK:{
      isMagnetic = (recv_buffer[1] & 0x01);
      isGyroscope = (recv_buffer[1] & 0x02);
      isAccelerometer = (recv_buffer[1] & 0x04);
      break;
    }
  }
  uint8_t ack = RSP_ACK;
  sendBuffer(&ack, 1);
}

#define UUID_VALUE_SIZE 20
uint8_t value_write[UUID_VALUE_SIZE];
uint32_t capability = CAP_MAGNETIC | CAP_GYROSCOPE | CAP_ACCELEROMETER;
uint8_t value_read[] = { (uint8_t)((UUID_VALUE_SIZE >> 8) & 0xff), (uint8_t)(UUID_VALUE_SIZE & 0xff),
                          (uint8_t)((capability >> 24) & 0xff), (uint8_t)((capability >> 16) & 0xff), (uint8_t)((capability >> 8) & 0xff), (uint8_t)(capability & 0xff) };

void taskServer(void*) {
  BLEDevice::init("T-Wristband");

  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyCallbacks());

  BLEService *pService = pServer->createService(UUID_SERVICE);

  pCharacteristic_write = pService->createCharacteristic( UUID_WRITE, BLECharacteristic::PROPERTY_WRITE );
  pCharacteristic_write->setAccessPermissions(ESP_GATT_PERM_WRITE);
  pCharacteristic_write->setValue(value_write, sizeof(value_write));
  pCharacteristic_write->setCallbacks(new MyCharacteristicCallbacks());

  pCharacteristic_read = pService->createCharacteristic( UUID_READ, BLECharacteristic::PROPERTY_READ );
  pCharacteristic_read->setAccessPermissions(ESP_GATT_PERM_READ);
  pCharacteristic_read->setValue(value_read, sizeof(value_read));

  pCharacteristic_notify = pService->createCharacteristic( UUID_NOTIFY, BLECharacteristic::PROPERTY_NOTIFY );
  pCharacteristic_notify->addDescriptor(new BLE2902());

  pService->start();

  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->addServiceUUID(UUID_SERVICE);
  pAdvertising->start();

  vTaskDelay(portMAX_DELAY); //delay(portMAX_DELAY);
}

void sendBuffer(uint8_t *p_value, uint16_t len){
  BLE2902* desc = (BLE2902*)pCharacteristic_notify->getDescriptorByUUID(BLEUUID((uint16_t)0x2902));
  if( !desc->getNotifications() )
    return;

  int offset = 0;
  int slot = 0;
  int packet_size = 0;
  do{
    if( offset == 0){
      value_write[0] = 0x83;
      value_write[1] = (len >> 8) & 0xff;
      value_write[2] = len & 0xff;
      packet_size = len - offset;
      if( packet_size > (UUID_VALUE_SIZE - 3) )
        packet_size = UUID_VALUE_SIZE - 3;
      memmove(&value_write[3], &p_value[offset], packet_size);

      offset += packet_size;
      packet_size += 3;

    }else{
      value_write[0] = slot++;
      packet_size = len - offset;
      if( packet_size > (UUID_VALUE_SIZE - 1) )
        packet_size = UUID_VALUE_SIZE - 1;
      memmove(&value_write[1], &p_value[offset], packet_size);

      offset += packet_size;
      packet_size += 1;
    }

    pCharacteristic_notify->setValue(value_write, packet_size);
    pCharacteristic_notify->notify();

  }while(packet_size >= UUID_VALUE_SIZE);  
}

#endif

void goto_sleep(void){
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_GREEN, TFT_BLACK);
  tft.setTextDatum(MC_DATUM);
  tft.drawString("Press again to wake up",  tft.width() / 2, tft.height() / 2 );
  IMU.setSleepEnabled(true);
  Serial.println("Go to Sleep");
  delay(3000);
  initial = true;
  func_select = 0;
  tft.fillScreen(TFT_BLACK);
  tft.writecommand(ST7735_SLPIN);
  tft.writecommand(ST7735_DISPOFF);
  esp_sleep_enable_ext1_wakeup(GPIO_SEL_33, ESP_EXT1_WAKEUP_ANY_HIGH);
  esp_deep_sleep_start();
}

void setup(void){
    Serial.begin(9600);

    tft.init();
    tft.setRotation(1);
    tft.setSwapBytes(true);

    Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
    Wire.setClock(400000);

    setupRTC();
    setupMPU9250();
    setupADC();

    tft.fillScreen(TFT_BLACK);
    tft.setTextColor(TFT_YELLOW, TFT_BLACK); // Note: the new fonts do not draw the background colour

    targetTime = millis() + 1000;

    pinMode(TP_PIN_PIN, INPUT);
    //! Must be set to pull-up output mode in order to wake up in deep sleep mode
    pinMode(TP_PWR_PIN, PULLUP);
    digitalWrite(TP_PWR_PIN, HIGH);

    pinMode(LED_PIN, OUTPUT);

    pinMode(CHARGE_PIN, INPUT_PULLUP);
    attachInterrupt(CHARGE_PIN, [] {
        charge_indication = true;
    }, CHANGE);

    if (digitalRead(CHARGE_PIN) == LOW) {
        charge_indication = true;
    }

#ifdef ENABLE_WIFI_BLE_DEVICE
    xTaskCreate(taskServer, "server", 20000, NULL, 5, NULL);
#endif
}

void loop()
{
    if (charge_indication) {
        charge_indication = false;
        if (digitalRead(CHARGE_PIN) == LOW) {
            tft.pushImage(140, 55, 16, 16, charge);
        } else {
            tft.fillRect(140, 55, 16, 16, TFT_BLACK);
        }
    }

    if (digitalRead(TP_PIN_PIN) == HIGH) {
        if (!pressed) {
            initial = true;
            func_select++;
            if( func_select > 2 )
              func_select = 0;
            if( func_select == 2)
              digitalWrite(LED_PIN, HIGH);
            else
              digitalWrite(LED_PIN, LOW);
            pressed = true;
            pressedTime = millis();
        } else {
            if ( (millis() - pressedTime) > PRESSED_HOLD_TIME) {
              pressedTime = 0;
              goto_sleep();
            }
        }
    } else {
      if( pressed ){
        pressed = false;
        releasedTime = millis();
      }else{
        if (releasedTime == 0 ){
          releasedTime = millis();
        }else if( (millis() - releasedTime) > RELEASED_HOLD_TIME){
          releasedTime = 0;
          goto_sleep();
        }
      }
    }

    switch (func_select) {
    case 0:
        RTC_Show();
        break;
    case 1:
        COMPASS_Show();
        break;
    case 2:
        IMU_Show();
        break;
    default:
        break;
    }
}

以上