├── .gitignore ├── .vscode └── extensions.json ├── src ├── input │ ├── ButtonData.h │ ├── ButtonCheck.h │ └── ButtonCheck.cpp ├── imu │ ├── mahony │ │ ├── MahonyAHRS.h │ │ └── MahonyAHRS.cpp │ ├── ImuData.h │ ├── ImuReader.h │ ├── AverageCalc.cpp │ ├── AverageCalc.h │ └── ImuReader.cpp ├── session │ ├── SessionData.h │ ├── SessionDefine.h │ └── SessionHeader.h ├── prefs │ ├── Settings.h │ └── Settings.cpp ├── device_name │ ├── DeviceName.h │ └── DeviceName.cpp └── main.cpp ├── test └── README ├── platformio.ini ├── LICENSE ├── lib └── README ├── include └── README ├── README.md └── HtmlViewer └── index.html /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .pioenvs 3 | .piolibdeps 4 | .vscode/.browse.c_cpp.db* 5 | .vscode/c_cpp_properties.json 6 | .vscode/launch.json 7 | .vscode/ipch/* -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "platformio.platformio-ide" 6 | ] 7 | } 8 | -------------------------------------------------------------------------------- /src/input/ButtonData.h: -------------------------------------------------------------------------------- 1 | #ifndef __INPUT_BUTTON_DATA_H__ 2 | #define __INPUT_BUTTON_DATA_H__ 3 | 4 | #include 5 | 6 | namespace input { 7 | 8 | static const int ButtonDataLen = 5; 9 | 10 | struct ButtonData { 11 | public: 12 | uint32_t timestamp; 13 | uint8_t btnBits; 14 | 15 | explicit ButtonData() : timestamp(0), btnBits(0) { } 16 | }; 17 | 18 | } // input 19 | 20 | #endif // __INPUT_BUTTON_DATA_H__ 21 | -------------------------------------------------------------------------------- /test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /src/imu/mahony/MahonyAHRS.h: -------------------------------------------------------------------------------- 1 | #ifndef __IMU_MAHONY_AHRS_H__ 2 | #define __IMU_MAHONY_AHRS_H__ 3 | 4 | #ifdef imu 5 | #undef imu 6 | #endif 7 | 8 | namespace imu { 9 | namespace mahony { 10 | 11 | class MahonyAHRS { 12 | public: 13 | void UpdateQuaternion( 14 | float gx, float gy, float gz, 15 | float ax, float ay, float az, 16 | float& q0, float& q1, float& q2, float& q3); 17 | 18 | void QuaternionToEuler( 19 | float q0, float q1, float q2, float q3, 20 | float& pitch, float& roll, float& yaw); 21 | }; 22 | 23 | } // mahony 24 | } // imu 25 | 26 | #endif // __IMU_MAHONY_AHRS_H__ 27 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:m5stick-c] 12 | platform = espressif32 13 | board = m5stick-c 14 | framework = arduino 15 | monitor_speed = 115200 16 | upload_speed = 115200 17 | lib_deps = 18 | m5stack/M5GFX@^0.0.9 19 | m5stack/M5Unified@^0.0.1 20 | -------------------------------------------------------------------------------- /src/imu/ImuData.h: -------------------------------------------------------------------------------- 1 | #ifndef __IMU_IMU_DATA_H__ 2 | #define __IMU_IMU_DATA_H__ 3 | 4 | #include 5 | #include 6 | 7 | namespace imu { 8 | 9 | static const int ImuXyz = 3; 10 | static const int ImuWxyz = 4; 11 | static const int ImuDataLen = 44;//4 + (4*3) + (4*3) + (4*4); 12 | 13 | struct ImuData { 14 | public: 15 | uint32_t timestamp; 16 | float acc[ImuXyz]; 17 | float gyro[ImuXyz]; 18 | float quat[ImuWxyz]; 19 | 20 | explicit ImuData() : timestamp(0) { 21 | memset(this, 0, ImuDataLen); 22 | quat[0] = 1.0F; 23 | } 24 | }; 25 | 26 | } // imu 27 | 28 | #endif // __IMU_IMU_DATA_H__ 29 | -------------------------------------------------------------------------------- /src/imu/ImuReader.h: -------------------------------------------------------------------------------- 1 | #ifndef __IMU_IMU_READER_H__ 2 | #define __IMU_IMU_READER_H__ 3 | 4 | #include "utility/IMU_Class.hpp" 5 | #include "mahony/MahonyAHRS.h" 6 | #include "ImuData.h" 7 | 8 | namespace imu { 9 | 10 | class ImuReader { 11 | public: 12 | explicit ImuReader(m5::IMU_Class& m5); 13 | bool initialize(); 14 | bool writeGyroOffset(float x, float y, float z); 15 | bool update(); 16 | bool read(ImuData& outImuData) const; 17 | private: 18 | m5::IMU_Class& m5Imu; 19 | mahony::MahonyAHRS ahrs; 20 | ImuData imuData; 21 | uint32_t lastUpdated; 22 | float gyroOffsets[ImuXyz]; 23 | }; 24 | 25 | } // imu 26 | 27 | #endif // __IMU_IMU_READER_H__ 28 | -------------------------------------------------------------------------------- /src/session/SessionData.h: -------------------------------------------------------------------------------- 1 | #ifndef __SESSION_SESSION_DATA_H__ 2 | #define __SESSION_SESSION_DATA_H__ 3 | 4 | #include 5 | #include 6 | #include "SessionHeader.h" 7 | 8 | namespace session { 9 | 10 | struct SessionData { 11 | public: 12 | SessionHeader header; 13 | uint8_t data[data_length::max] = {0}; 14 | 15 | explicit SessionData(DataDefine define) : header(define) { 16 | } 17 | void write(const uint8_t* data, uint16_t len) { 18 | memcpy(this->data, data, len); 19 | } 20 | uint32_t length() const { return data_length::header + header.dataLength; } 21 | }; 22 | 23 | } // session 24 | 25 | #endif // __SESSION_SESSION_DATA_H__ 26 | -------------------------------------------------------------------------------- /src/imu/AverageCalc.cpp: -------------------------------------------------------------------------------- 1 | #include "AverageCalc.h" 2 | 3 | namespace imu { 4 | 5 | AverageCalc::AverageCalc() : cnt(0) { 6 | memset(data, 0, sizeof(float) * DataMaxCount); 7 | } 8 | 9 | AverageCalc::~AverageCalc() { } 10 | 11 | bool AverageCalc::push(float data) { 12 | if (cnt >= DataMaxCount) { 13 | return false; 14 | } 15 | this->data[cnt] = data; 16 | cnt++; 17 | return true; 18 | } 19 | 20 | float AverageCalc::average() const { 21 | float sum = 0; 22 | for (int i = 0; i < cnt; i++) { 23 | sum += data[i]; 24 | } 25 | return sum / (float)cnt; 26 | } 27 | 28 | } // imu 29 | -------------------------------------------------------------------------------- /src/prefs/Settings.h: -------------------------------------------------------------------------------- 1 | #ifndef __PREFS_SETTINGS_H__ 2 | #define __PREFS_SETTINGS_H__ 3 | 4 | #include 5 | 6 | namespace prefs { 7 | 8 | static const char* PrefNameSpaceKey = "axis_orange"; 9 | static const char* PrefDataKey_gyroOffsetX = "gyro_offset_x"; 10 | static const char* PrefDataKey_gyroOffsetY = "gyro_offset_y"; 11 | static const char* PrefDataKey_gyroOffsetZ = "gyro_offset_z"; 12 | 13 | class Settings { 14 | public: 15 | explicit Settings(); 16 | ~Settings(); 17 | void begin(); 18 | void clear(); 19 | void finish(); 20 | void writeGyroOffset(const float* gyroOffset); 21 | bool readGyroOffset(float* gyroOffset); 22 | private: 23 | Preferences preferences; 24 | }; // ButtonCheck 25 | 26 | } // prefs 27 | 28 | #endif // __PREFS_SETTINGS_H__ 29 | -------------------------------------------------------------------------------- /src/session/SessionDefine.h: -------------------------------------------------------------------------------- 1 | #ifndef __SESSION_SESSION_DEFINE_H__ 2 | #define __SESSION_SESSION_DEFINE_H__ 3 | 4 | #include 5 | 6 | namespace session { 7 | 8 | enum DataDefine { 9 | DataDefineUnknown = 0, 10 | DataDefineImu = 1, 11 | DataDefineButton = 2 12 | }; 13 | 14 | namespace data_type { 15 | // send to client 16 | static const uint16_t imu = 0x0001; 17 | static const uint16_t button = 0x0002; 18 | // request form client 19 | static const uint16_t installGyroOffset = 0x8001; 20 | } 21 | 22 | namespace data_length { 23 | static const uint16_t max = 64; 24 | // send to client 25 | static const uint16_t header = 4; 26 | static const uint16_t imu = 44; 27 | static const uint16_t button = 5; 28 | // request form client 29 | static const uint16_t installGyroOffset = 0; 30 | } 31 | 32 | } // session 33 | 34 | #endif // __SESSION_SESSION_DEFINE_H__ 35 | -------------------------------------------------------------------------------- /src/session/SessionHeader.h: -------------------------------------------------------------------------------- 1 | #ifndef __SESSION_SESSION_HEADER_H__ 2 | #define __SESSION_SESSION_HEADER_H__ 3 | 4 | #include 5 | #include "SessionDefine.h" 6 | 7 | namespace session { 8 | 9 | struct SessionHeader { 10 | public: 11 | uint16_t dataType; 12 | uint16_t dataLength; 13 | 14 | explicit SessionHeader(DataDefine define) { 15 | switch (define) { 16 | case DataDefineImu: 17 | dataType = data_type::imu; 18 | dataLength = data_length::imu; 19 | break; 20 | case DataDefineButton: 21 | dataType = data_type::button; 22 | dataLength = data_length::button; 23 | break; 24 | default: 25 | dataType = 0; 26 | dataLength = 0; 27 | break; 28 | } 29 | } 30 | }; 31 | 32 | } // session 33 | 34 | #endif // __SESSION_SESSION_HEADER_H__ 35 | -------------------------------------------------------------------------------- /src/input/ButtonCheck.h: -------------------------------------------------------------------------------- 1 | #ifndef __INPUT_BUTTON_CHECK_H 2 | #define __INPUT_BUTTON_CHECK_H 3 | 4 | #include 5 | #include 6 | 7 | namespace input { 8 | enum Btn { BtnA = 0x01, BtnB = 0x02, BtnC = 0x04 }; 9 | enum BtnState { BtnStateRelease = 0, BtnStatePress = 1, }; 10 | 11 | #define INPUT_BTN_NUM 3 12 | static const Btn AllBtns[INPUT_BTN_NUM] = { BtnA, BtnB, BtnC }; 13 | 14 | class ButtonCheck { 15 | public: 16 | explicit ButtonCheck(); 17 | ~ButtonCheck(); 18 | bool containsUpdate(m5::M5Unified& runningDevice, uint8_t& outBtnBits); 19 | private: 20 | uint8_t updateFlag; 21 | BtnState getBtnState(Btn of) const; 22 | uint8_t getCurrentBits() const; 23 | bool isButtonStateChanged(Btn of, BtnState ithink, m5::M5Unified& device); 24 | BtnState getCurrentDeviceBtnState(Btn of, m5::M5Unified& device) const; 25 | std::map buttonStateMap; 26 | }; // ButtonCheck 27 | 28 | } // input 29 | 30 | #endif // __INPUT_BUTTON_CHECK_H 31 | -------------------------------------------------------------------------------- /src/device_name/DeviceName.h: -------------------------------------------------------------------------------- 1 | #ifndef __DEVICE_NAME_DEVICE_NAME_H__ 2 | #define __DEVICE_NAME_DEVICE_NAME_H__ 3 | 4 | #include 5 | 6 | namespace device_name { 7 | 8 | #define DEVICE_NAME_MAX_LEN 32 9 | #define DEVICE_NAME_PREFIX "AxisOrange" 10 | #define DEVICE_NAME_UNKNOWN "???" 11 | #define DEVICE_NAME_M5STACK "M5Stack" 12 | #define DEVICE_NAME_M5STACK_CORE2 "M5StackCore2" 13 | #define DEVICE_NAME_M5STICK_C "M5StickC" 14 | #define DEVICE_NAME_M5STICK_CPLUS "M5StickCPlus" 15 | #define DEVICE_NAME_M5STACK_COREINK "M5StackCoreInk" 16 | #define DEVICE_NAME_M5PAPER "M5Paper" 17 | #define DEVICE_NAME_M5TOUGH "M5Tough" 18 | #define DEVICE_NAME_M5ATOM "M5ATOM" 19 | #define DEVICE_NAME_M5TIMER_CAM "M5TimerCam" 20 | 21 | class DeviceName { 22 | public: 23 | explicit DeviceName(const char* prefix); 24 | const char* getName(m5gfx::board_t board); 25 | private: 26 | const char* prefixName; 27 | char name[DEVICE_NAME_MAX_LEN] = {0}; 28 | void concatDeviceName(const char* deviceName); 29 | }; 30 | 31 | } // device_name 32 | 33 | #endif // __DEVICE_NAME_DEVICE_NAME_H__ 34 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Nakano Yosuke 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/prefs/Settings.cpp: -------------------------------------------------------------------------------- 1 | #include "Settings.h" 2 | 3 | namespace prefs { 4 | 5 | Settings::Settings() { } 6 | Settings::~Settings() { } 7 | 8 | void Settings::begin() { 9 | preferences.begin(PrefNameSpaceKey, false); 10 | } 11 | 12 | void Settings::clear() { 13 | preferences.clear(); 14 | } 15 | 16 | void Settings::finish() { 17 | preferences.end(); 18 | } 19 | 20 | void Settings::writeGyroOffset(const float* gyroOffset) { 21 | preferences.putFloat(PrefDataKey_gyroOffsetX, gyroOffset[0]); 22 | preferences.putFloat(PrefDataKey_gyroOffsetY, gyroOffset[1]); 23 | preferences.putFloat(PrefDataKey_gyroOffsetZ, gyroOffset[2]); 24 | } 25 | 26 | bool Settings::readGyroOffset(float* gyroOffset) { 27 | float x = preferences.getFloat(PrefDataKey_gyroOffsetX, 0.0F); 28 | float y = preferences.getFloat(PrefDataKey_gyroOffsetY, 0.0F); 29 | float z = preferences.getFloat(PrefDataKey_gyroOffsetZ, 0.0F); 30 | gyroOffset[0] = x; 31 | gyroOffset[1] = y; 32 | gyroOffset[2] = z; 33 | return x != 0.0F || y != 0.0F || z != 0.0F; 34 | } 35 | 36 | } // prefs 37 | -------------------------------------------------------------------------------- /lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /src/imu/AverageCalc.h: -------------------------------------------------------------------------------- 1 | #ifndef __IMU_AVERAGE_CALC_H__ 2 | #define __IMU_AVERAGE_CALC_H__ 3 | 4 | #include 5 | 6 | namespace imu { 7 | 8 | static const int DataMaxCount = 1000; 9 | 10 | class AverageCalc { 11 | public: 12 | explicit AverageCalc(); 13 | ~AverageCalc(); 14 | bool push(float data); 15 | float average() const; 16 | int count() const { return cnt; } 17 | void reset() { cnt = 0; } 18 | private: 19 | float data[DataMaxCount]; 20 | int cnt; 21 | }; 22 | 23 | class AverageCalcXYZ { 24 | public: 25 | explicit AverageCalcXYZ() { } 26 | ~AverageCalcXYZ() { } 27 | bool push(float x, float y, float z) { return aveX.push(x) && aveY.push(y) && aveZ.push(z); } 28 | float averageX() const { return aveX.average(); } 29 | float averageY() const { return aveY.average(); } 30 | float averageZ() const { return aveZ.average(); } 31 | int countX() const { return aveX.count(); } 32 | int countY() const { return aveY.count(); } 33 | int countZ() const { return aveZ.count(); } 34 | void reset() { aveX.reset(); aveY.reset(); aveZ.reset(); } 35 | private: 36 | AverageCalc aveX; 37 | AverageCalc aveY; 38 | AverageCalc aveZ; 39 | }; 40 | 41 | } // imu 42 | 43 | #endif // __IMU_AVERAGE_CALC_H__ 44 | -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /src/imu/ImuReader.cpp: -------------------------------------------------------------------------------- 1 | #include "ImuReader.h" 2 | 3 | namespace imu { 4 | ImuReader::ImuReader(m5::IMU_Class& m5) : m5Imu(m5), ahrs(), imuData() { 5 | memset(gyroOffsets, 0, sizeof(float) * ImuXyz); 6 | } 7 | 8 | bool ImuReader::initialize() { 9 | return m5Imu.begin(); 10 | } 11 | 12 | bool ImuReader::writeGyroOffset(float x, float y, float z) { 13 | gyroOffsets[0] = x; 14 | gyroOffsets[1] = y; 15 | gyroOffsets[2] = z; 16 | return true; 17 | } 18 | 19 | bool ImuReader::update() { 20 | float& ax = imuData.acc[0]; 21 | float& ay = imuData.acc[1]; 22 | float& az = imuData.acc[2]; 23 | float& gx = imuData.gyro[0]; 24 | float& gy = imuData.gyro[1]; 25 | float& gz = imuData.gyro[2]; 26 | float& qw = imuData.quat[0]; 27 | float& qx = imuData.quat[1]; 28 | float& qy = imuData.quat[2]; 29 | float& qz = imuData.quat[3]; 30 | 31 | m5Imu.getAccel(&ax, &ay, &az); 32 | m5Imu.getGyro(&gx, &gy, &gz); 33 | 34 | gx -= gyroOffsets[0]; 35 | gy -= gyroOffsets[1]; 36 | gz -= gyroOffsets[2]; 37 | 38 | ahrs.UpdateQuaternion( 39 | gx * DEG_TO_RAD, gy * DEG_TO_RAD, gz * DEG_TO_RAD, 40 | ax, ay, az, 41 | qw, qx, qy, qz); 42 | imuData.timestamp = millis(); 43 | lastUpdated = imuData.timestamp; 44 | return true; 45 | } 46 | 47 | bool ImuReader::read(ImuData& outImuData) const { 48 | if (lastUpdated == outImuData.timestamp) { 49 | return false; // not updated 50 | } 51 | memcpy(&outImuData, &imuData, ImuDataLen); 52 | return true; 53 | } 54 | 55 | } // imu 56 | -------------------------------------------------------------------------------- /src/device_name/DeviceName.cpp: -------------------------------------------------------------------------------- 1 | #include "DeviceName.h" 2 | 3 | namespace device_name { 4 | DeviceName::DeviceName(const char* prefix) : prefixName(prefix) { 5 | } 6 | 7 | const char* DeviceName::getName(m5gfx::board_t board) { 8 | switch (board) { 9 | case m5gfx::board_t::board_M5Stack: 10 | concatDeviceName(DEVICE_NAME_UNKNOWN); 11 | break; 12 | case m5gfx::board_t::board_M5StackCore2: 13 | concatDeviceName(DEVICE_NAME_M5STACK_CORE2); 14 | break; 15 | case m5gfx::board_t::board_M5StickC: 16 | concatDeviceName(DEVICE_NAME_M5STICK_C); 17 | break; 18 | case m5gfx::board_t::board_M5StickCPlus: 19 | concatDeviceName(DEVICE_NAME_M5STICK_CPLUS); 20 | break; 21 | case m5gfx::board_t::board_M5StackCoreInk: 22 | concatDeviceName(DEVICE_NAME_M5STACK_COREINK); 23 | break; 24 | case m5gfx::board_t::board_M5Paper: 25 | concatDeviceName(DEVICE_NAME_M5PAPER); 26 | break; 27 | case m5gfx::board_t::board_M5Tough: 28 | concatDeviceName(DEVICE_NAME_M5TOUGH); 29 | break; 30 | case m5gfx::board_t::board_M5ATOM: 31 | concatDeviceName(DEVICE_NAME_M5ATOM); 32 | break; 33 | case m5gfx::board_t::board_M5TimerCam: 34 | concatDeviceName(DEVICE_NAME_M5TIMER_CAM); 35 | break; 36 | case m5gfx::board_t::board_unknown: 37 | default: 38 | concatDeviceName(DEVICE_NAME_UNKNOWN); 39 | break; 40 | } 41 | return name; 42 | } 43 | 44 | void DeviceName::concatDeviceName(const char* deviceName) { 45 | snprintf(name, DEVICE_NAME_MAX_LEN, "%s(%s)", DEVICE_NAME_PREFIX, deviceName); 46 | } 47 | } // device_name 48 | -------------------------------------------------------------------------------- /src/input/ButtonCheck.cpp: -------------------------------------------------------------------------------- 1 | #include "ButtonCheck.h" 2 | 3 | namespace input { 4 | ButtonCheck::ButtonCheck() : updateFlag(0) { 5 | for(int i = 0; i < INPUT_BTN_NUM; i++) { 6 | buttonStateMap.insert(std::make_pair(AllBtns[i], BtnStateRelease)); 7 | } 8 | } 9 | 10 | ButtonCheck::~ButtonCheck() { } 11 | 12 | bool ButtonCheck::containsUpdate(m5::M5Unified& runningDevice, uint8_t& outBtnBits) { 13 | updateFlag = 0; 14 | for(int i = 0; i < INPUT_BTN_NUM; i++) { 15 | const Btn btn = AllBtns[i]; 16 | const BtnState ithink = getBtnState(btn); 17 | if (isButtonStateChanged(btn, ithink, runningDevice)) { 18 | updateFlag |= btn; 19 | } 20 | } 21 | bool hasUpdate = updateFlag != 0; 22 | if (hasUpdate) { 23 | outBtnBits = getCurrentBits(); 24 | } 25 | return hasUpdate; 26 | } 27 | 28 | uint8_t ButtonCheck::getCurrentBits() const { 29 | return ( 30 | BtnC * getBtnState(BtnC) + 31 | BtnB * getBtnState(BtnB) + 32 | BtnA * getBtnState(BtnA) 33 | ); 34 | } 35 | 36 | BtnState ButtonCheck::getBtnState(Btn of) const { 37 | const auto itr = buttonStateMap.find(of); 38 | if (itr == buttonStateMap.end()) { 39 | return BtnStateRelease; /// not found 40 | } 41 | return itr->second; 42 | } 43 | 44 | bool ButtonCheck::isButtonStateChanged(Btn of, BtnState ithink, m5::M5Unified& device) { 45 | BtnState current = getCurrentDeviceBtnState(of, device); 46 | if (ithink == current) { 47 | return false; // not changed 48 | } 49 | buttonStateMap[of] = current; 50 | return true; 51 | } 52 | 53 | BtnState ButtonCheck::getCurrentDeviceBtnState(Btn of, m5::M5Unified& device) const { 54 | switch (of) { 55 | case BtnA: return (device.BtnA.isPressed() == 0) ? BtnStateRelease : BtnStatePress; 56 | case BtnB: return (device.BtnB.isPressed() == 0) ? BtnStateRelease : BtnStatePress; 57 | case BtnC: return (device.BtnC.isPressed() == 0) ? BtnStateRelease : BtnStatePress; 58 | } 59 | return BtnStateRelease; 60 | } 61 | } // input 62 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | AxisOrange 2 | === 3 | 4 | # Overview 5 | 6 | 7 | M5Stack series firmware to send 6DOF sensor(SH200Q or MPU6886) data and quaternion by Bluetooth serial port protocol(SPP). 8 | 9 | 10 | 11 | This is demo. M5StickC calucutates and sends quaternion to PC by SPP. 12 | PC receives quaternion via serial port. 13 | 14 | # Usage 15 | AxisOrange supports [M5Unified](https://github.com/m5stack/M5Unified) devices. like 16 | 17 | * M5Stack Gray (without MPU9250 model) 18 | * M5Stack Core2 19 | * M5StickC 20 | * M5StickCPlus 21 | 22 | ## Simple viewer 23 | 24 | 25 | This is AxisOrange data viewer by web serial port. 26 | 27 | 0. Build and write AxisOrange. 28 | 1. Paring AxisOrange with your PC. 29 | 2. Open `HtmlViewer/index.html`. Reccomend: Google Chrome. 30 | 3. Push [list serial device] and select AxisOrange Port. May be named `ESP32SPP`. 31 | 4. AxisOrange can calculate and save gyro zero point error. push [gyro-offset install] 32 | 5. You can copy raw output of AxisOrange as csv text. 33 | 34 | # Features 35 | 36 | ## Sensor Data 37 | 38 | AxisOrange sends raw value of 6DOF sensor. 39 | 40 | * Acc(xyz) [G] 41 | * Gyro(xyz) [deg/s] 42 | 43 | ## Quaternion 44 | 45 | AxisOrange calucutates quaternion by acc/gyro and sends it. 46 | 47 | * Quaternion(xyzw) 48 | 49 | ## Button 50 | 51 | AxisOrange sends button A/B(/C) push/release event trigger. 52 | 53 | * Button A Push/Release event 54 | * Button B Push/Release event 55 | * Button C Push/Release event (if 3rd button is installed) 56 | 57 | ## Install 58 | This project use [PlatformIO](https://platformio.org/). 59 | 60 | ## Library 61 | * M5Unified and M5GFX 62 | - https://github.com/m5stack/M5Unified 63 | - https://github.com/m5stack/M5GFX 64 | - MIT 65 | * BluetoothSerial 66 | - https://github.com/espressif/arduino-esp32/tree/master/libraries/BluetoothSerial 67 | - http://www.apache.org/licenses/LICENSE-2.0 68 | * Mahony filter 69 | - https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 70 | 71 | ## License 72 | MIT 73 | 74 | ## Author 75 | [@naninunenoy](https://github.com/naninunenoy) 76 | -------------------------------------------------------------------------------- /src/imu/mahony/MahonyAHRS.cpp: -------------------------------------------------------------------------------- 1 | //===================================================================================================== 2 | // MahonyAHRS.c 3 | //===================================================================================================== 4 | // 5 | // Madgwick's implementation of Mayhony's AHRS algorithm. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 12 | //===================================================================================================== 13 | // from https://github.com/m5stack/M5StickC/blob/master/src/utility/MahonyAHRS.cpp 14 | 15 | #include 16 | #include 17 | #include "MahonyAHRS.h" 18 | 19 | #define sampleFreq 200.0f // sample frequency in Hz 20 | #define twoKpDef (2.0f * 1.0f) // 2 * proportional gain 21 | #define twoKiDef (2.0f * 0.0f) // 2 * integral gain 22 | 23 | namespace imu { 24 | namespace mahony { 25 | 26 | volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp) 27 | volatile float twoKi = twoKiDef; // 2 * integral gain (Ki) 28 | //volatile float q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; // quaternion of sensor frame relative to auxiliary frame 29 | volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki 30 | 31 | void MahonyAHRS::UpdateQuaternion(float gx, float gy, float gz, float ax, float ay, float az, float& q0, float& q1, float& q2, float& q3) { 32 | float recipNorm; 33 | float halfvx, halfvy, halfvz; 34 | float halfex, halfey, halfez; 35 | float qa, qb, qc; 36 | 37 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 38 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 39 | 40 | // Normalise accelerometer measurement 41 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 42 | ax *= recipNorm; 43 | ay *= recipNorm; 44 | az *= recipNorm; 45 | 46 | // Estimated direction of gravity and vector perpendicular to magnetic flux 47 | halfvx = q1 * q3 - q0 * q2; 48 | halfvy = q0 * q1 + q2 * q3; 49 | halfvz = q0 * q0 - 0.5f + q3 * q3; 50 | 51 | // Error is sum of cross product between estimated and measured direction of gravity 52 | halfex = (ay * halfvz - az * halfvy); 53 | halfey = (az * halfvx - ax * halfvz); 54 | halfez = (ax * halfvy - ay * halfvx); 55 | 56 | // Compute and apply integral feedback if enabled 57 | if(twoKi > 0.0f) { 58 | integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki 59 | integralFBy += twoKi * halfey * (1.0f / sampleFreq); 60 | integralFBz += twoKi * halfez * (1.0f / sampleFreq); 61 | gx += integralFBx; // apply integral feedback 62 | gy += integralFBy; 63 | gz += integralFBz; 64 | } 65 | else { 66 | integralFBx = 0.0f; // prevent integral windup 67 | integralFBy = 0.0f; 68 | integralFBz = 0.0f; 69 | } 70 | 71 | // Apply proportional feedback 72 | gx += twoKp * halfex; 73 | gy += twoKp * halfey; 74 | gz += twoKp * halfez; 75 | } 76 | 77 | // Integrate rate of change of quaternion 78 | gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors 79 | gy *= (0.5f * (1.0f / sampleFreq)); 80 | gz *= (0.5f * (1.0f / sampleFreq)); 81 | qa = q0; 82 | qb = q1; 83 | qc = q2; 84 | q0 += (-qb * gx - qc * gy - q3 * gz); 85 | q1 += (qa * gx + qc * gz - q3 * gy); 86 | q2 += (qa * gy - qb * gz + q3 * gx); 87 | q3 += (qa * gz + qb * gy - qc * gx); 88 | 89 | // Normalise quaternion 90 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 91 | q0 *= recipNorm; 92 | q1 *= recipNorm; 93 | q2 *= recipNorm; 94 | q3 *= recipNorm; 95 | } 96 | 97 | void MahonyAHRS::QuaternionToEuler(float q0, float q1, float q2, float q3, float& pitch, float& roll, float& yaw) { 98 | pitch = asin(-2 * q1 * q3 + 2 * q0* q2); // pitch 99 | roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); // roll 100 | yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3); //yaw 101 | 102 | pitch *= RAD_TO_DEG; 103 | yaw *= RAD_TO_DEG; 104 | // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is 105 | // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 106 | // - http://www.ngdc.noaa.gov/geomag-web/#declination 107 | yaw -= 8.5; 108 | roll *= RAD_TO_DEG; 109 | } 110 | 111 | //--------------------------------------------------------------------------------------------------- 112 | // Fast inverse square-root 113 | // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root 114 | 115 | float invSqrt(float x) { 116 | float halfx = 0.5f * x; 117 | float y = x; 118 | #pragma GCC diagnostic ignored "-Wstrict-aliasing" 119 | long i = *(long*)&y; 120 | i = 0x5f3759df - (i>>1); 121 | y = *(float*)&i; 122 | #pragma GCC diagnostic warning "-Wstrict-aliasing" 123 | y = y * (1.5f - (halfx * y * y)); 124 | return y; 125 | } 126 | 127 | } // mahony 128 | } // imu 129 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "imu/ImuReader.h" 4 | #include "imu/AverageCalc.h" 5 | #include "input/ButtonCheck.h" 6 | #include "input/ButtonData.h" 7 | #include "session/SessionData.h" 8 | #include "prefs/Settings.h" 9 | #include "device_name/DeviceName.h" 10 | 11 | #define TASK_DEFAULT_CORE_ID 1 12 | #define TASK_STACK_DEPTH 4096UL 13 | #define TASK_NAME_IMU "IMUTask" 14 | #define TASK_NAME_WRITE_SESSION "WriteSessionTask" 15 | #define TASK_NAME_READ_SESSION "ReadSessionTask" 16 | #define TASK_NAME_BUTTON "ButtonTask" 17 | #define TASK_SLEEP_IMU 5 // = 1000[ms] / 200[Hz] 18 | #define TASK_SLEEP_WRITE_SESSION 40 // = 1000[ms] / 25[Hz] 19 | #define TASK_SLEEP_READ_SESSION 100 // = 1000[ms] / 10[Hz] 20 | #define TASK_SLEEP_BUTTON 1 // = 1000[ms] / 1000[Hz] 21 | #define MUTEX_DEFAULT_WAIT 1000UL 22 | 23 | static void ImuLoop(void* arg); 24 | static void WriteSessionLoop(void* arg); 25 | static void ReadSessionLoop(void* arg); 26 | static void ButtonLoop(void* arg); 27 | 28 | imu::ImuReader* imuReader; 29 | BluetoothSerial btSpp; 30 | input::ButtonCheck button; 31 | 32 | imu::ImuData imuData; 33 | input::ButtonData btnData; 34 | bool hasButtonUpdate = false; 35 | static SemaphoreHandle_t imuDataMutex = NULL; 36 | static SemaphoreHandle_t btnDataMutex = NULL; 37 | 38 | uint8_t readBuffer[session::data_length::max] = {0}; 39 | bool gyroOffsetInstalled = true; 40 | imu::AverageCalcXYZ gyroAve; 41 | prefs::Settings settingPref; 42 | device_name::DeviceName deviceName("AxisOrange"); 43 | 44 | void UpdateLcd() { 45 | M5.Lcd.setCursor(40, 0); 46 | if (gyroOffsetInstalled) { 47 | M5.Lcd.fillScreen(BLACK); 48 | M5.Lcd.println("AxisOrange"); 49 | } else { 50 | M5.Lcd.fillScreen(GREEN); 51 | M5.Lcd.println("GyroOffset"); 52 | } 53 | } 54 | 55 | void setup() { 56 | M5.begin(); 57 | Serial.begin(115200); 58 | // read settings 59 | float gyroOffset[3] = { 0.0F }; 60 | settingPref.begin(); 61 | //settingPref.clear(); // to reinstall gyro offset by only m5stickc remove commentout 62 | settingPref.readGyroOffset(gyroOffset); 63 | settingPref.finish(); 64 | // lcd 65 | M5.Lcd.setRotation(3); 66 | M5.Lcd.setTextSize(1); 67 | UpdateLcd(); 68 | // imu 69 | imuReader = new imu::ImuReader(M5.Imu); 70 | imuReader->initialize(); 71 | if (gyroOffsetInstalled) { 72 | imuReader->writeGyroOffset(gyroOffset[0], gyroOffset[1], gyroOffset[2]); 73 | } 74 | // bluetooth serial 75 | btSpp.begin(deviceName.getName(M5.getBoard())); 76 | // task 77 | imuDataMutex = xSemaphoreCreateMutex(); 78 | btnDataMutex = xSemaphoreCreateMutex(); 79 | xTaskCreatePinnedToCore(ImuLoop, TASK_NAME_IMU, TASK_STACK_DEPTH, 80 | NULL, 2, NULL, TASK_DEFAULT_CORE_ID); 81 | xTaskCreatePinnedToCore(WriteSessionLoop, TASK_NAME_WRITE_SESSION, TASK_STACK_DEPTH, 82 | NULL, 1, NULL, TASK_DEFAULT_CORE_ID); 83 | xTaskCreatePinnedToCore(ReadSessionLoop, TASK_NAME_READ_SESSION, TASK_STACK_DEPTH, 84 | NULL, 1, NULL, TASK_DEFAULT_CORE_ID); 85 | xTaskCreatePinnedToCore(ButtonLoop, TASK_NAME_BUTTON, TASK_STACK_DEPTH, 86 | NULL, 1, NULL, TASK_DEFAULT_CORE_ID); 87 | } 88 | 89 | void loop() { /* Do Nothing */ } 90 | 91 | static void ImuLoop(void* arg) { 92 | while (1) { 93 | uint32_t entryTime = millis(); 94 | if (xSemaphoreTake(imuDataMutex, MUTEX_DEFAULT_WAIT) == pdTRUE) { 95 | imuReader->update(); 96 | imuReader->read(imuData); 97 | if (!gyroOffsetInstalled) { 98 | if (!gyroAve.push(imuData.gyro[0], imuData.gyro[1], imuData.gyro[2])) { 99 | float x = gyroAve.averageX(); 100 | float y = gyroAve.averageY(); 101 | float z = gyroAve.averageZ(); 102 | // set offset 103 | imuReader->writeGyroOffset(x, y, z); 104 | // save offset 105 | float offset[] = {x, y, z}; 106 | settingPref.begin(); 107 | settingPref.writeGyroOffset(offset); 108 | settingPref.finish(); 109 | gyroOffsetInstalled = true; 110 | gyroAve.reset(); 111 | UpdateLcd(); 112 | } 113 | } 114 | } 115 | xSemaphoreGive(imuDataMutex); 116 | // idle 117 | int32_t sleep = TASK_SLEEP_IMU - (millis() - entryTime); 118 | vTaskDelay((sleep > 0) ? sleep : 0); 119 | } 120 | } 121 | 122 | static void WriteSessionLoop(void* arg) { 123 | static session::SessionData imuSessionData(session::DataDefineImu); 124 | static session::SessionData btnSessionData(session::DataDefineButton); 125 | while (1) { 126 | uint32_t entryTime = millis(); 127 | // imu 128 | if (gyroOffsetInstalled) { 129 | if (xSemaphoreTake(imuDataMutex, MUTEX_DEFAULT_WAIT) == pdTRUE) { 130 | imuSessionData.write((uint8_t*)&imuData, imu::ImuDataLen); 131 | btSpp.write((uint8_t*)&imuSessionData, imuSessionData.length()); 132 | } 133 | xSemaphoreGive(imuDataMutex); 134 | } 135 | // button 136 | if (xSemaphoreTake(btnDataMutex, MUTEX_DEFAULT_WAIT) == pdTRUE) { 137 | if (hasButtonUpdate) { 138 | btnSessionData.write((uint8_t*)&btnData, input::ButtonDataLen); 139 | btSpp.write((uint8_t*)&btnSessionData, btnSessionData.length()); 140 | hasButtonUpdate = false; 141 | } 142 | xSemaphoreGive(btnDataMutex); 143 | } 144 | // idle 145 | int32_t sleep = TASK_SLEEP_WRITE_SESSION - (millis() - entryTime); 146 | vTaskDelay((sleep > 0) ? sleep : 0); 147 | } 148 | } 149 | 150 | static void ReadSessionLoop(void* arg) { 151 | while (1) { 152 | uint32_t entryTime = millis(); 153 | //read 154 | size_t len = btSpp.readBytes(readBuffer, session::data_length::header); 155 | if (len == (size_t)session::data_length::header) { 156 | uint16_t dataId = (uint16_t)((readBuffer[1] << 8) | readBuffer[0]); 157 | if (dataId == session::data_type::installGyroOffset && gyroOffsetInstalled) { 158 | gyroOffsetInstalled = false; 159 | imuReader->writeGyroOffset(0.0F, 0.0F, 0.0F); 160 | UpdateLcd(); 161 | } 162 | } 163 | // idle 164 | int32_t sleep = TASK_SLEEP_READ_SESSION - (millis() - entryTime); 165 | vTaskDelay((sleep > 0) ? sleep : 0); 166 | } 167 | } 168 | 169 | static void ButtonLoop(void* arg) { 170 | uint8_t btnFlag = 0; 171 | while (1) { 172 | uint32_t entryTime = millis(); 173 | M5.update(); 174 | if (button.containsUpdate(M5, btnFlag)) { 175 | for (int i = 0; i < INPUT_BTN_NUM; i++) { 176 | if (xSemaphoreTake(btnDataMutex, MUTEX_DEFAULT_WAIT) == pdTRUE) { 177 | btnData.timestamp = millis(); 178 | btnData.btnBits = btnFlag; 179 | hasButtonUpdate = true; 180 | } 181 | xSemaphoreGive(btnDataMutex); 182 | } 183 | } 184 | // idle 185 | int32_t sleep = TASK_SLEEP_BUTTON - (millis() - entryTime); 186 | vTaskDelay((sleep > 0) ? sleep : 0); 187 | } 188 | } 189 | -------------------------------------------------------------------------------- /HtmlViewer/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Axis Orange Viewer 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 |

Axis Orange Viewer

17 | 18 |
19 | 20 | 22 | 23 |
24 | 25 | 26 |
27 | 28 | 29 | 215 | 216 | 217 | --------------------------------------------------------------------------------