Initial commit

.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

.vscode/extensions.json

@@ -0,0 +1,7 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ]
+}

include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

platformio.ini

@@ -0,0 +1,27 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[platformio]
+default_envs = m5atom-matrix
+
+[common]
+upload_port = COM6
+
+[env:m5atom-matrix]
+platform = espressif32
+board = pico32
+framework = arduino
+monitor_speed = 115200
+upload_speed = 115200
+lib_deps = fastled/FastLED@^3.4.0
+
+[env:serial]
+upload_protocol = esptool
+lib_deps = fastled/FastLED@^3.4.0

src/M5Atom.cpp

@@ -0,0 +1,51 @@
+// Copyright (c) M5Stack. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full license information.
+
+#include "M5Atom.h"
+
+M5Atom::M5Atom()
+{
+
+}
+
+M5Atom::~M5Atom()
+{
+
+}
+
+void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
+{
+	if( _isInited ) return;
+
+	_isInited = true;
+
+	if( I2CEnable )
+	{
+		Wire.begin(25,21,10000);
+	}
+	if( SerialEnable )
+	{
+		Serial.begin(115200);
+		Serial.flush();
+		delay(50);
+		Serial.println("M5Atom initializing...OK");
+	}
+
+	if( DisplayEnable )
+	{
+        dis.begin();
+		dis.setTaskName("LEDs");
+		dis.setTaskPriority(2);
+		dis.start();
+	}
+
+
+
+}
+
+void M5Atom::update()
+{
+	M5.Btn.read();
+}
+
+M5Atom M5;

src/M5Atom.h

@@ -0,0 +1,85 @@
+// 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 drive for M5Stack core.
+ *
+ * \par Method List:
+ *
+ *  System:
+        M5.begin();
+        M5.update();
+
+    LED:
+
+    void animation(uint8_t *buffptr, uint8_t amspeed, uint8_t ammode, int64_t amcount = -1);
+    void displaybuff(uint8_t *buffptr, int8_t offsetx = 0, int8_t offsety = 0);
+    void setBrightness(uint8_t brightness);
+    void drawpix(uint8_t xpos, uint8_t ypos, CRGB Color);
+    void drawpix(uint8_t Number, CRGB Color);
+    void clear();
+
+    Button:
+        M5.Btn.read();
+        M5.Btn.isPressed();
+        M5.Btn.isReleased();
+        M5.Btn.wasPressed();
+        M5.Btn.wasReleased();
+        M5.Btn.wasreleasedFor()
+        M5.Btn.pressedFor(uint32_t ms);
+        M5.Btn.releasedFor(uint32_t ms);
+        M5.Btn.lastChange();
+ */
+// #define ESP32
+
+#ifndef _M5ATOM_H_
+#define _M5ATOM_H_
+
+#if defined(ESP32)
+
+#define FASTLED_INTERNAL
+
+#include <Arduino.h>
+#include <Wire.h>
+#include <FastLED.h>
+
+#include "utility/MPU6886.h"
+#include "utility/Button.h"
+#include "utility/LED_DisPlay.h"
+
+#define FASTLED_INTERNAL
+class M5Atom
+{
+private:
+	bool _isInited = false;
+	/* data */
+public:
+	M5Atom(/* args */);
+	~M5Atom();
+
+	MPU6886 IMU;
+    LED_DisPlay dis;
+
+	Button Btn = Button(39, true, 10);
+
+	void begin(bool SerialEnable = true, bool I2CEnable = true, bool DisplayEnable = false);
+	void update();
+};
+
+extern M5Atom M5;
+
+
+#else
+#error “This library only supports boards with ESP32 processor.”
+#endif
+#endif

src/MCUInfo.h

@@ -0,0 +1,9 @@
+#pragma once
+struct MCUInfo
+{
+  static constexpr float maxTicks= 60.f;
+  static constexpr float periodIntervalInMs = 2000.f;
+  static constexpr float tickIntervalInMs = periodIntervalInMs/maxTicks;
+
+
+};

src/SimonSays.cpp

@@ -0,0 +1,137 @@
+#include "SimonSays.h"
+
+/* Some Logic to get the Variable type with serial print
+// Generic catch-all implementation.
+template <typename T_ty>
+struct TypeInfo
+{
+    static const char *name;
+};
+template <typename T_ty>
+const char *TypeInfo<T_ty>::name = "unknown";
+
+// Handy macro to make querying stuff easier.
+#define TYPE_NAME(var) TypeInfo<typeof(var)>::name
+
+// Handy macro to make defining stuff easier.
+#define MAKE_TYPE_INFO(type) template <> \
+                             const char *TypeInfo<type>::name = #type;
+
+// Type-specific implementations.
+MAKE_TYPE_INFO(int);
+MAKE_TYPE_INFO(float);
+MAKE_TYPE_INFO(short);
+MAKE_TYPE_INFO(CRGB);
+
+USAGE:
+Serial.println(TYPE_NAME(variable));
+*/
+
+SimonSays::SimonSays(M5Atom &_m5device):
+m5device(_m5device)
+{
+
+};
+
+SimonSays::~SimonSays(){
+
+};
+
+void SimonSays::begin(uint8_t _currentLevel, uint8_t _numberColors)
+{
+    currentLevel = _currentLevel;
+    numberColors = _numberColors;
+
+    baseColors[0] = CRGB::DarkGreen;
+    baseColors[1] = CRGB::DarkBlue;
+    baseColors[2] = CRGB::DarkOrange;
+    baseColors[3] = CRGB::Gold;
+    baseColors[4] = CRGB::DarkViolet;
+    baseColors[5] = CRGB(100,100,100);
+    baseColors[6] = CRGB::Maroon;
+    baseColors[7] = CRGB::DarkTurquoise;
+    baseColors[8] = CRGB::DeepPink;
+
+    highlightColors[0] = CRGB::Green;
+    highlightColors[1] = CRGB::Blue;
+    highlightColors[2] = CRGB::Orange;
+    highlightColors[3] = CRGB::Yellow;
+    highlightColors[4] = CRGB::Violet;
+    highlightColors[5] = CRGB::White;
+    highlightColors[6] = CRGB::Brown;
+    highlightColors[7] = CRGB::Turquoise;
+    highlightColors[8] = CRGB::Pink;
+
+    // Copy   colors from src to dest in length 9
+    memmove(&currentColors, &baseColors, 9 * sizeof(CRGB));
+};
+
+void SimonSays::advanceHighlightColor()
+{
+    currentColors[currentColorId]=baseColors[currentColorId];
+    currentColorId++;
+    if (currentColorId >= numberColors)
+    {
+        currentColorId=0;
+    };
+    currentColors[currentColorId]=highlightColors[currentColorId];
+
+    // Serial.print("current Color: ");
+    // Serial.println(currentColorId);
+    return;
+};
+
+void SimonSays::checkBtnPress()
+{
+    if (m5device.Btn.wasPressed()) {
+        Serial.println("Btn pressed");
+    }
+
+};
+
+uint8_t SimonSays::getBuff()
+{
+    // uint8_t buff[2 + 5 * 5 * 3];
+
+    // buff[0] = 0x05;
+    // buff[1] = 0x05;
+    // for (int i = 0; i < 25; i++)
+    // {
+    //     buff[2 + i * 3 + 0] = 0xcc;
+    //     buff[2 + i * 3 + 1] = 0xee;
+    //     buff[2 + i * 3 + 2] = 0xff;
+    // }
+    uint8_t a=1;
+
+    return a;
+};
+
+void SimonSays::update()
+{
+    checkBtnPress();
+    
+    advanceHighlightColor();
+
+    // determine pattern von number color
+
+    switch (numberColors) {
+        case 2:
+            for (int i=0;i<=25;i++){
+                if (i%5==0 || (i-1)%5==0) {
+                    M5.dis.drawpix(i, currentColors[0]);
+                }
+                else if ((i-3)%5==0 || (i-4)%5==0) {
+                     M5.dis.drawpix(i,currentColors[1]);
+                }
+                else {
+                     M5.dis.drawpix(i,CRGB::Black);
+                }
+            };
+            break;
+        default:
+            break;
+    };
+
+    // draw pattern
+
+};

src/SimonSays.h

@@ -0,0 +1,32 @@
+#include <Arduino.h>
+#include "M5Atom.h"
+
+#include <FastLED.h>
+
+class SimonSays 
+{
+    public:
+        SimonSays(M5Atom & m5device);
+        ~SimonSays();
+
+        M5Atom & m5device;
+
+    private:
+        // char *colors[] = {'0xcc0000','0x00cc00'};
+        // void _setDisplay();
+        CRGB currentColors[9];
+        CRGB baseColors[9];
+        CRGB highlightColors[9];
+
+    public:
+        int numberColors = 2;
+        int currentLevel = 0;
+        int currentColorId = 0;
+
+
+        void begin(uint8_t currentLevel = 0, uint8_t numberColors = 2);
+        void update();
+        void advanceHighlightColor();
+        void checkBtnPress();
+        uint8_t getBuff();
+};

src/main.cpp

@@ -0,0 +1,61 @@
+/****************************************************************
+ * 
+ * This Example is used to test button
+ * 
+ * Arduino tools Setting 
+ * -board : M5StickC
+ * -Upload Speed: 115200 / 750000 / 1500000
+ * 
+****************************************************************/
+
+#include "M5Atom.h"
+#include "SimonSays.h"
+#include "MCU.h"
+
+uint8_t DisBuff[2 + 5 * 5 * 3];
+
+void setBuff(uint8_t Rdata, uint8_t Gdata, uint8_t Bdata)
+{
+  DisBuff[0] = 0x05;
+  DisBuff[1] = 0x05;
+  for (int i = 0; i < 25; i++)
+  {
+    DisBuff[2 + i * 3 + 0] = Rdata;
+    DisBuff[2 + i * 3 + 1] = Gdata;
+    DisBuff[2 + i * 3 + 2] = Bdata;
+  }
+}
+
+SimonSays Simon(M5);
+
+uint32_t timeOfLastTick = 0;
+
+void setup()
+{
+  M5.begin(true, false, true);
+  delay(10);
+
+  Simon.begin();
+
+  // Initialize ticks
+  timeOfLastTick = millis();
+}
+
+uint8_t FSM = 0;
+
+void loop()
+{
+
+  // Update in tick rate.
+  unsigned long time = millis();
+  if (time - timeOfLastTick >= MCU_Info::tick_interval_in_ms)
+  {
+    timeOfLastTick = time;
+
+    Simon.update();
+    M5.update();
+  }
+
+  //delay(1000);
+
+}

src/utility/Button.cpp

@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------*
+ * 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::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;
+}

src/utility/Button.h

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------*
+ * Arduino Button Library v1.0                                          *
+ * Jack Christensen Mar 2012                                            *
+ *                                                                      *
+ * This work is licensed under the Creative Commons Attribution-        *
+ * ShareAlike 3.0 Unported License. To view a copy of this license,     *
+ * visit http://creativecommons.org/licenses/by-sa/3.0/ or send a       *
+ * letter to Creative Commons, 171 Second Street, Suite 300,            *
+ * San Francisco, California, 94105, USA.                               *
+ *----------------------------------------------------------------------*/
+#ifndef Button_h
+#define Button_h
+// #if ARDUINO >= 100
+#include <Arduino.h>
+// #else
+// #include <WProgram.h>
+// #endif
+class Button {
+  public:
+    Button(uint8_t pin, uint8_t invert, uint32_t dbTime);
+    uint8_t read();
+    uint8_t isPressed();
+    uint8_t isReleased();
+    uint8_t wasPressed();
+    uint8_t wasReleased();
+    uint8_t pressedFor(uint32_t ms);
+    uint8_t 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 _dbTime;       //debounce time
+    uint32_t _pressTime;    //press time
+    uint32_t _hold_time;    //hold time call wasreleasefor
+};
+#endif

src/utility/LED_DisPlay.cpp

@@ -0,0 +1,203 @@
+#include "LED_DisPlay.h"
+
+LED_DisPlay::LED_DisPlay()
+{
+    
+}
+
+LED_DisPlay::~LED_DisPlay()
+{
+
+}
+
+void LED_DisPlay::begin(uint8_t LEDNumbre)
+{
+    FastLED.addLeds<WS2812, DATA_PIN, GRB>(_ledbuff, LEDNumbre);
+    _xSemaphore = xSemaphoreCreateMutex();
+    _numberled = LEDNumbre;
+}
+
+void LED_DisPlay::run(void *data)
+{
+    data = nullptr;
+
+    for (int num = 0; num < NUM_LEDS; num++)
+    {
+        _ledbuff[num] = 0x000000;
+    }
+    FastLED.show();
+    FastLED.setBrightness(20);
+
+    while (1)
+    {
+        xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+        if (_mode == kAnmiation_run)
+        {
+            if ((_am_mode & kMoveRight) || (_am_mode & kMoveLeft))
+            {
+                if (_am_mode & kMoveRight)
+                {
+                    _count_x++;
+                }
+                else
+                {
+                    _count_x--;
+                }
+            }
+            if ((_am_mode & kMoveTop) || (_am_mode & kMoveButtom))
+            {
+                if (_am_mode & kMoveTop)
+                {
+                    _count_y--;
+                }
+                else
+                {
+                    _count_y++;
+                }
+            }
+            if ((_am_count != -1) && (_am_count != 0))
+            {
+                _am_count--;
+                if (_am_count == 0)
+                {
+                    _mode = kAnmiation_stop;
+                }
+            }
+            _displaybuff(_am_buffptr, _count_x, _count_y);
+            FastLED.show();
+            delay(_am_speed);
+
+        }
+        else if( _mode == kAnmiation_frush )
+        {
+            _mode = kAnmiation_stop;
+            FastLED.show();
+        }
+        xSemaphoreGive(_xSemaphore);
+        delay(10);
+    }
+}
+
+void LED_DisPlay::_displaybuff(uint8_t *buffptr, int8_t offsetx, int8_t offsety)
+{
+    uint16_t xsize = 0, ysize = 0;
+    xsize = buffptr[0];
+    ysize = buffptr[1];
+
+    offsetx = offsetx % xsize;
+    offsety = offsety % ysize;
+
+    int8_t setdatax = (offsetx < 0) ? (-offsetx) : (xsize - offsetx);
+    int8_t setdatay = (offsety < 0) ? (-offsety) : (ysize - offsety);
+    for (int x = 0; x < 5; x++)
+    {
+        for (int y = 0; y < 5; y++)
+        {
+            _ledbuff[x + y * 5].raw[1] = buffptr[2 + ((setdatax + x) % xsize + ((setdatay + y) % ysize) * xsize) * 3 + 0];
+            _ledbuff[x + y * 5].raw[0] = buffptr[2 + ((setdatax + x) % xsize + ((setdatay + y) % ysize) * xsize) * 3 + 1];
+            _ledbuff[x + y * 5].raw[2] = buffptr[2 + ((setdatax + x) % xsize + ((setdatay + y) % ysize) * xsize) * 3 + 2];
+        }
+    }
+    FastLED.setBrightness(Brightness);
+}
+
+void LED_DisPlay::animation(uint8_t *buffptr, uint8_t amspeed, uint8_t ammode, int64_t amcount)
+{
+    xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+    if (_mode == kAnmiation_run)
+    {
+        _mode = kAnmiation_stop;
+    }
+    _am_buffptr = buffptr;
+    _am_speed = amspeed;
+    _am_mode = ammode;
+    _am_count = amcount;
+    _count_x = _count_y = 0;
+    _mode = kAnmiation_run;
+    xSemaphoreGive(_xSemaphore);
+}
+
+void LED_DisPlay::displaybuff(uint8_t *buffptr, int8_t offsetx, int8_t offsety)
+{
+    uint16_t xsize = 0, ysize = 0;
+    xsize = buffptr[0];
+    ysize = buffptr[1];
+
+    offsetx = offsetx % xsize;
+    offsety = offsety % ysize;
+
+    int8_t setdatax = (offsetx < 0) ? (-offsetx) : (xsize - offsetx);
+    int8_t setdatay = (offsety < 0) ? (-offsety) : (ysize - offsety);
+    xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+
+    for (int x = 0; x < 5; x++)
+    {
+        for (int y = 0; y < 5; y++)
+        {
+            _ledbuff[x + y * 5].raw[1] = buffptr[2 + ((setdatax + x) % xsize + ((setdatay + y) % ysize) * xsize) * 3 + 0];
+            _ledbuff[x + y * 5].raw[0] = buffptr[2 + ((setdatax + x) % xsize + ((setdatay + y) % ysize) * xsize) * 3 + 1];
+            _ledbuff[x + y * 5].raw[2] = buffptr[2 + ((setdatax + x) % xsize + ((setdatay + y) % ysize) * xsize) * 3 + 2];
+        }
+    }
+
+    _mode = kAnmiation_frush;
+
+    xSemaphoreGive(_xSemaphore);
+    FastLED.setBrightness(Brightness);
+}
+
+void LED_DisPlay::setBrightness(uint8_t brightness)
+{
+    xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+    brightness = ( brightness > 100 ) ? 100 :  brightness;
+    brightness = ( 40 * brightness / 100 );
+    Brightness = brightness;
+    FastLED.setBrightness(Brightness);
+    xSemaphoreGive(_xSemaphore);
+}
+
+void LED_DisPlay::drawpix(uint8_t xpos, uint8_t ypos, CRGB Color)
+{
+    if ((xpos >= 5) || (ypos >= 5))
+    {
+        return;
+    }
+    xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+    _ledbuff[xpos + ypos * 5] = Color;
+    _mode = kAnmiation_frush;
+    xSemaphoreGive(_xSemaphore);
+}
+
+void LED_DisPlay::drawpix(uint8_t Number, CRGB Color)
+{
+    if (Number >= NUM_LEDS)
+    {
+        return;
+    }
+    xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+    _ledbuff[Number] = Color;
+    _mode = kAnmiation_frush;
+    xSemaphoreGive(_xSemaphore);
+}
+
+void LED_DisPlay::fillpix(CRGB Color)
+{
+    xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+    for (int i = 0; i < NUM_LEDS; i++)
+    {
+        _ledbuff[i] = Color;
+    }
+    _mode = kAnmiation_frush;
+    xSemaphoreGive(_xSemaphore);
+}
+
+void LED_DisPlay::clear()
+{
+    xSemaphoreTake(_xSemaphore, portMAX_DELAY);
+    for (int8_t i = 0; i < NUM_LEDS; i++)
+    {
+        _ledbuff[i] = 0;
+    }
+    _mode = kAnmiation_frush;
+    xSemaphoreGive(_xSemaphore);
+}

src/utility/LED_DisPlay.h

@@ -0,0 +1,67 @@
+#ifndef _LED_DISPLAY_H_
+#define _LED_DISPLAY_H_
+
+#include <FastLED.h>
+#include "utility/Task.h"
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+
+#define NUM_LEDS 25
+#define DATA_PIN 27
+
+class LED_DisPlay : public Task
+{
+private:
+    CRGB _ledbuff[NUM_LEDS];
+    uint8_t _numberled;
+
+    uint8_t _mode;
+    uint16_t _am_speed;
+    uint8_t _am_mode;
+    int32_t _count_x, _count_y;
+    int32_t _am_count = -1;
+    uint8_t *_am_buffptr;
+
+    SemaphoreHandle_t _xSemaphore = NULL;
+
+public:
+    enum
+    {
+        kStatic = 0,
+        kAnmiation_run,
+        kAnmiation_stop,
+        kAnmiation_frush,
+    } Dismode;
+    enum
+    {
+        kMoveRight = 0x01,
+        kMoveLeft = 0x02,
+        kMoveTop = 0x04,
+        kMoveButtom = 0x08,
+    } Am_mode;
+
+    uint8_t Brightness = 40;
+
+    /* data */
+public:
+    LED_DisPlay();
+    ~LED_DisPlay();
+
+    void begin(uint8_t LEDNumbre = NUM_LEDS);
+    void run(void *data);
+
+    void animation(uint8_t *buffptr, uint8_t amspeed, uint8_t ammode, int64_t amcount = -1);
+    void displaybuff(uint8_t *buffptr, int8_t offsetx = 0, int8_t offsety = 0);
+    void MoveDisPlayBuff(int8_t offsetx = 0, int8_t offsety = 0);
+
+    void setBrightness(uint8_t brightness);
+    void drawpix(uint8_t xpos, uint8_t ypos, CRGB Color);
+    void drawpix(uint8_t Number, CRGB Color);
+    void fillpix(CRGB Color);
+    void clear();
+
+private:
+    void _displaybuff(uint8_t *buffptr, int8_t offsetx = 0, int8_t offsety = 0);
+};
+
+#endif

src/utility/M5Timer.cpp

@@ -0,0 +1,218 @@
+/*
+ * 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;
+}

src/utility/M5Timer.h

@@ -0,0 +1,120 @@
+/*
+ * M5Timer.h
+ *
+ * M5Timer - A timer library for Arduino.
+ * Author: mromani@ottotecnica.com
+ * Copyright (c) 2010 OTTOTECNICA Italy
+ *
+ * This library is free software; you can redistribute it
+ * and/or modify it under the terms of the GNU Lesser
+ * General Public License as published by the Free Software
+ * Foundation; either version 2.1 of the License, or (at
+ * your option) any later version.
+ *
+ * This library is distributed in the hope that it will
+ * be useful, but WITHOUT ANY WARRANTY; without even the
+ * implied warranty of MERCHANTABILITY or FITNESS FOR A
+ * PARTICULAR PURPOSE.  See the GNU Lesser General Public
+ * License for more details.
+ *
+ * You should have received a copy of the GNU Lesser
+ * General Public License along with this library; if not,
+ * write to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+
+#ifndef M5Timer_H
+  #define M5Timer_H
+
+  #include <functional>
+  #include <Arduino.h>
+
+  typedef std::function<void(void)> timer_callback;
+
+  class M5Timer {
+    public:
+      // maximum number of timers
+      const static int MAX_TIMERS = 10;
+
+      // setTimer() constants
+      const static int RUN_FOREVER = 0;
+      const static int RUN_ONCE = 1;
+
+      // constructor
+      M5Timer();
+
+      // this function must be called inside loop()
+      void run();
+
+      // call function f every d milliseconds
+      int setInterval(long d, timer_callback f);
+
+      // call function f once after d milliseconds
+      int setTimeout(long d, timer_callback f);
+
+      // call function f every d milliseconds for n times
+      int setTimer(long d, timer_callback f, int n);
+
+      // destroy the specified timer
+      void deleteTimer(int numTimer);
+
+      // restart the specified timer
+      void restartTimer(int numTimer);
+
+      // returns true if the specified timer is enabled
+      boolean isEnabled(int numTimer);
+
+      // enables the specified timer
+      void enable(int numTimer);
+
+      // disables the specified timer
+      void disable(int numTimer);
+
+      // enables the specified timer if it's currently disabled,
+      // and vice-versa
+      void toggle(int numTimer);
+
+      // returns the number of used timers
+      int getNumTimers();
+
+      // returns the number of available timers
+      int getNumAvailableTimers() { return MAX_TIMERS - numTimers; };
+
+    private:
+      // deferred call constants
+      const static int DEFCALL_DONTRUN = 0;   // don't call the callback function
+      const static int DEFCALL_RUNONLY = 1;   // call the callback function but don't delete the timer
+      const static int DEFCALL_RUNANDDEL = 2;  // call the callback function and delete the timer
+
+      // find the first available slot
+      int findFirstFreeSlot();
+
+      // value returned by the millis() function
+      // in the previous run() call
+      unsigned long prev_millis[MAX_TIMERS];
+
+      // pointers to the callback functions
+      timer_callback callbacks[MAX_TIMERS];
+
+      // delay values
+      long delays[MAX_TIMERS];
+
+      // number of runs to be executed for each timer
+      int maxNumRuns[MAX_TIMERS];
+
+      // number of executed runs for each timer
+      int numRuns[MAX_TIMERS];
+
+      // which timers are enabled
+      boolean enabled[MAX_TIMERS];
+
+      // deferred function call (sort of) - N.B.: this array is only used in run()
+      int toBeCalled[MAX_TIMERS];
+
+      // actual number of timers in use
+      int numTimers;
+  };
+
+#endif

src/utility/MPU6886.cpp

@@ -0,0 +1,278 @@
+#include "MPU6886.h"
+#include <math.h>
+#include <Arduino.h>
+
+MPU6886::MPU6886(){
+
+}
+
+void MPU6886::I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr, uint8_t number_Bytes, uint8_t *read_Buffer){
+    
+  Wire1.beginTransmission(driver_Addr);
+  Wire1.write(start_Addr);  
+  Wire1.endTransmission(false);
+  uint8_t i = 0;
+  Wire1.requestFrom(driver_Addr,number_Bytes);
+  
+  //! Put read results in the Rx buffer
+  while (Wire1.available()) {
+    read_Buffer[i++] = Wire1.read();
+  }        
+}
+
+void MPU6886::I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr, uint8_t number_Bytes, uint8_t *write_Buffer){
+
+  Wire1.beginTransmission(driver_Addr);
+  Wire1.write(start_Addr);
+  Wire1.write(*write_Buffer);
+  Wire1.endTransmission();
+}
+
+int MPU6886::Init(void){
+  unsigned char tempdata[1];
+  unsigned char regdata;
+  
+  Wire1.begin(25,21,100000);
+  
+  I2C_Read_NBytes(MPU6886_ADDRESS, MPU6886_WHOAMI, 1, tempdata);
+  Serial.printf("%02X\r\n",tempdata[0]);
+  if(tempdata[0] != 0x19)
+    return -1;
+  delay(1);
+  
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+  delay(10);
+
+  regdata = (0x01<<7);
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+  delay(10);
+
+  regdata = (0x01<<0);
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_PWR_MGMT_1, 1, &regdata);
+  delay(10);
+
+  regdata = 0x10;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x18;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_GYRO_CONFIG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x01;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_CONFIG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x05;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_SMPLRT_DIV, 1,&regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, &regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_ACCEL_CONFIG2, 1, &regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_USER_CTRL, 1, &regdata);
+  delay(1);
+
+  regdata = 0x00;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_FIFO_EN, 1, &regdata);
+  delay(1);
+
+  regdata = 0x22;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_PIN_CFG, 1, &regdata);
+  delay(1);
+
+  regdata = 0x01;
+  I2C_Write_NBytes(MPU6886_ADDRESS, MPU6886_INT_ENABLE, 1, &regdata);
+
+  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::getAttitude(double *pitch, double *roll)
+{
+    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);
+
+    if ((accX < 1) && (accX > -1))
+    {
+        *pitch = asin(-accX) * 57.295;
+    }
+    if (accZ != 0)
+    {
+        *roll = atan(accY / accZ) * 57.295;
+    }
+
+    ( *pitch ) = _alpha * ( *pitch ) + (1 - _alpha) * _last_theta;
+    ( *roll ) = _alpha * ( *roll ) + (1 - _alpha) * _last_phi;
+}
+
+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;