Browse Source

Initial commit

pull/14/head
Dennis Bücker 2 months ago
commit
641cb5a105
24 changed files with 2079 additions and 0 deletions
  1. +5
    -0
      .gitignore
  2. +7
    -0
      .vscode/extensions.json
  3. +39
    -0
      include/README
  4. +46
    -0
      lib/README
  5. +27
    -0
      platformio.ini
  6. +51
    -0
      src/M5Atom.cpp
  7. +85
    -0
      src/M5Atom.h
  8. +9
    -0
      src/MCUInfo.h
  9. +137
    -0
      src/SimonSays.cpp
  10. +32
    -0
      src/SimonSays.h
  11. +61
    -0
      src/main.cpp
  12. +132
    -0
      src/utility/Button.cpp
  13. +45
    -0
      src/utility/Button.h
  14. +203
    -0
      src/utility/LED_DisPlay.cpp
  15. +67
    -0
      src/utility/LED_DisPlay.h
  16. +218
    -0
      src/utility/M5Timer.cpp
  17. +120
    -0
      src/utility/M5Timer.h
  18. +278
    -0
      src/utility/MPU6886.cpp
  19. +100
    -0
      src/utility/MPU6886.h
  20. +256
    -0
      src/utility/MahonyAHRS.cpp
  21. +33
    -0
      src/utility/MahonyAHRS.h
  22. +78
    -0
      src/utility/Task.cpp
  23. +39
    -0
      src/utility/Task.h
  24. +11
    -0
      test/README

+ 5
- 0
.gitignore View File

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

+ 7
- 0
.vscode/extensions.json View File

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

+ 39
- 0
include/README View File

@ -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

+ 46
- 0
lib/README View File

@ -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

+ 27
- 0
platformio.ini View File

@ -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

+ 51
- 0
src/M5Atom.cpp View File

@ -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;

+ 85
- 0
src/M5Atom.h View File

@ -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

+ 9
- 0
src/MCUInfo.h View File

@ -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;
};

+ 137
- 0
src/SimonSays.cpp View File

@ -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
};

+ 32
- 0
src/SimonSays.h View File

@ -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();
};

+ 61
- 0
src/main.cpp View File

@ -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);
}

+ 132
- 0
src/utility/Button.cpp View File

@ -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;
}

+ 45
- 0
src/utility/Button.h View File

@ -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

+ 203
- 0
src/utility/LED_DisPlay.cpp View File

@ -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);
}

+ 67
- 0
src/utility/LED_DisPlay.h View File

@ -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

+ 218
- 0
src/utility/M5Timer.cpp View File

@ -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;
}

+ 120
- 0
src/utility/M5Timer.h View File

@ -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

+ 278
- 0
src/utility/MPU6886.cpp View File

@ -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;
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, &regdata);
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, &regdata);
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;
}

+ 100
- 0
src/utility/MPU6886.h View File

@ -0,0 +1,100 @@
/*
Note: The MPU6886 is an I2C sensor and uses the Arduino Wire library.
Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or
a 3.3 V Teensy 3.1. We have disabled the internal pull-ups used by the Wire
library in the Wire.h/twi.c utility file. We are also using the 400 kHz fast
I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
*/
#ifndef _MPU6886_H_
#define _MPU6886_H_
#include <Wire.h>
#include <Arduino.h>
#include "MahonyAHRS.h"
#define MPU6886_ADDRESS 0x68
#define MPU6886_WHOAMI 0x75
#define MPU6886_ACCEL_INTEL_CTRL 0x69
#define MPU6886_SMPLRT_DIV 0x19
#define MPU6886_INT_PIN_CFG 0x37
#define MPU6886_INT_ENABLE 0x38
#define MPU6886_ACCEL_XOUT_H 0x3B
#define MPU6886_ACCEL_XOUT_L 0x3C
#define MPU6886_ACCEL_YOUT_H 0x3D
#define MPU6886_ACCEL_YOUT_L 0x3E
#define MPU6886_ACCEL_ZOUT_H 0x3F
#define MPU6886_ACCEL_ZOUT_L 0x40
#define MPU6886_TEMP_OUT_H 0x41
#define MPU6886_TEMP_OUT_L 0x42
#define MPU6886_GYRO_XOUT_H 0x43
#define MPU6886_GYRO_XOUT_L 0x44
#define MPU6886_GYRO_YOUT_H 0x45
#define MPU6886_GYRO_YOUT_L 0x46
#define MPU6886_GYRO_ZOUT_H 0x47
#define MPU6886_GYRO_ZOUT_L 0x48
#define MPU6886_USER_CTRL 0x6A
#define MPU6886_PWR_MGMT_1 0x6B
#define MPU6886_PWR_MGMT_2 0x6C
#define MPU6886_CONFIG 0x1A
#define MPU6886_GYRO_CONFIG 0x1B
#define MPU6886_ACCEL_CONFIG 0x1C
#define MPU6886_ACCEL_CONFIG2 0x1D
#define MPU6886_FIFO_EN 0x23
//#define G (9.8)
#define RtA 57.324841
#define AtR 0.0174533
#define Gyro_Gr 0.0010653
class MPU6886 {
public:
enum Ascale {
AFS_2G = 0,
AFS_4G,
AFS_8G,
AFS_16G
};
enum Gscale {
GFS_250DPS = 0,
GFS_500DPS,
GFS_1000DPS,
GFS_2000DPS
};
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