├── types.cpp ├── .gitignore ├── assets ├── 01.jpg ├── 02.jpg ├── 03.jpg └── 04.jpg ├── .vscode ├── arduino.json ├── c_cpp_properties.json └── settings.json ├── math.h ├── device_node.cpp ├── oled_display.h ├── device_node.h ├── QmuTactile.h ├── sbus.h ├── math.cpp ├── SSD1306.h ├── types.h ├── QmuTactile.cpp ├── oled_display.cpp ├── README.md ├── SSD1306Wire.h ├── SSD1306Spi.h ├── SSD1306Brzo.h ├── sbus.cpp ├── OLEDDisplay.h ├── OLEDDisplayUi.h ├── OLEDDisplayUi.cpp ├── motion-controller.ino ├── LICENSE └── OLEDDisplay.cpp /types.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ -------------------------------------------------------------------------------- /assets/01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DzikuVx/DiyMotionController/HEAD/assets/01.jpg -------------------------------------------------------------------------------- /assets/02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DzikuVx/DiyMotionController/HEAD/assets/02.jpg -------------------------------------------------------------------------------- /assets/03.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DzikuVx/DiyMotionController/HEAD/assets/03.jpg -------------------------------------------------------------------------------- /assets/04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DzikuVx/DiyMotionController/HEAD/assets/04.jpg -------------------------------------------------------------------------------- /.vscode/arduino.json: -------------------------------------------------------------------------------- 1 | { 2 | "sketch": "motion-controller.ino", 3 | "board": "espressif:esp32:esp32", 4 | "configuration": "PSRAM=disabled,PartitionScheme=default,CPUFreq=240,FlashMode=qio,FlashFreq=80,FlashSize=4M,UploadSpeed=921600,DebugLevel=none", 5 | "output": "./../build", 6 | "port": "COM8" 7 | } -------------------------------------------------------------------------------- /math.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_H 2 | #define MATH_H 3 | 4 | #include "Arduino.h" 5 | 6 | typedef struct stdev_s 7 | { 8 | float m_oldM, m_newM, m_oldS, m_newS; 9 | int m_n; 10 | } stdev_t; 11 | 12 | void devClear(stdev_t *dev); 13 | void devPush(stdev_t *dev, float x); 14 | float devVariance(stdev_t *dev); 15 | float devStandardDeviation(stdev_t *dev); 16 | float applyDeadband(float value, float deadband); 17 | float fconstrainf(float amt, float low, float high); 18 | float fscalef(float x, float srcMin, float srcMax, float destMin, float destMax); 19 | 20 | #endif -------------------------------------------------------------------------------- /device_node.cpp: -------------------------------------------------------------------------------- 1 | #include "device_node.h" 2 | 3 | DeviceNode::DeviceNode(void) { 4 | } 5 | 6 | void DeviceNode::begin(void) { 7 | 8 | } 9 | 10 | deviceMode_e DeviceNode::getDeviceMode(void) { 11 | return _currentDeviceMode; 12 | } 13 | 14 | void DeviceNode::setDeviceMode(deviceMode_e mode) { 15 | _previousDeviceMode = _currentDeviceMode; 16 | _currentDeviceMode = mode; 17 | } 18 | 19 | bool DeviceNode::getActionEnabled(void) { 20 | return _actionEnabled; 21 | } 22 | 23 | void DeviceNode::setActionEnabled(bool enabled) { 24 | _previousActionEnabled = _actionEnabled; 25 | _actionEnabled = enabled; 26 | } -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Win32", 5 | "includePath": [ 6 | "C:\\Users\\pspyc\\Documents\\Arduino\\libraries\\Adafruit_MPU6050", 7 | "C:\\Users\\pspyc\\Documents\\Arduino\\libraries\\**", 8 | "C:\\Program Files (x86)\\Arduino\\libraries\\**", 9 | "C:\\Users\\pspyc\\Documents\\Arduino\\hardware\\espressif\\esp32\\**" 10 | ], 11 | "forcedInclude": [], 12 | "defines": [ 13 | "USBCON" 14 | ], 15 | "intelliSenseMode": "windows-msvc-x64", 16 | "cStandard": "c17", 17 | "cppStandard": "c++17" 18 | } 19 | ], 20 | "version": 4 21 | } -------------------------------------------------------------------------------- /oled_display.h: -------------------------------------------------------------------------------- 1 | #ifndef OLED_DISPLAY 2 | #define OLED_DISPLAY 3 | 4 | #include "SSD1306.h" 5 | #include "math.h" 6 | #include "types.h" 7 | #include "device_node.h" 8 | 9 | extern imuData_t imu; 10 | extern thumb_joystick_t thumbJoystick; 11 | extern DeviceNode device; 12 | 13 | enum txOledPages { 14 | OLED_PAGE_NONE, 15 | OLED_PAGE_BEACON_STATUS, 16 | }; 17 | 18 | #define OLED_COL_COUNT 64 19 | #define OLED_DISPLAY_PAGE_COUNT 2 20 | 21 | class OledDisplay { 22 | public: 23 | OledDisplay(SSD1306 *display); 24 | void init(); 25 | void loop(); 26 | void setPage(uint8_t page); 27 | private: 28 | SSD1306 *_display; 29 | void renderPageStatus(); 30 | void page(); 31 | uint8_t _page = OLED_PAGE_NONE; 32 | bool _forceDisplay = false; 33 | }; 34 | 35 | 36 | #endif -------------------------------------------------------------------------------- /device_node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef DEVICE_NODE_H 4 | #define DEVICE_NODE_H 5 | 6 | #include 7 | #include "math.h" 8 | #include "types.h" 9 | 10 | extern dataOutput_t output; 11 | 12 | enum deviceMode_e { 13 | DEVICE_MODE_MOTION_CONTROLLER = 0, 14 | DEVICE_MODE_LAST 15 | }; 16 | 17 | class DeviceNode { 18 | public: 19 | DeviceNode(void); 20 | void begin(void); 21 | deviceMode_e getDeviceMode(void); 22 | void setDeviceMode(deviceMode_e mode); 23 | bool getActionEnabled(void); 24 | void setActionEnabled(bool enabled); 25 | private: 26 | deviceMode_e _currentDeviceMode = DEVICE_MODE_MOTION_CONTROLLER; 27 | deviceMode_e _previousDeviceMode = DEVICE_MODE_MOTION_CONTROLLER; 28 | bool _actionEnabled = false; 29 | bool _previousActionEnabled = false; 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /QmuTactile.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef QMU_TACTILE_H 4 | #define QMU_TACTILE_H 5 | 6 | #include "Arduino.h" 7 | 8 | enum tactileStateFlags 9 | { 10 | TACTILE_STATE_NONE, 11 | TACTILE_STATE_SHORT_PRESS, 12 | TACTILE_STATE_LONG_PRESS, 13 | TACTILE_STATE_PRESSING 14 | }; 15 | 16 | enum tactileFlagsFlags_e 17 | { 18 | TACTILE_FLAG_NONE = 0, // 0 19 | TACTILE_FLAG_PRESSED = 1 << 0, // 1 20 | TACTILE_FLAG_EDGE_PRESSED = 1 << 1, // 2 21 | TACTILE_FLAG_EDGE_NOT_PRESSED = 1 << 2, // 4 22 | }; 23 | 24 | #define TACTILE_MIN_PRESS_TIME 50 25 | #define TACTILE_LONG_PRESS_TIME 1000 26 | #define TACTILE_PRESSING_TIME 400 27 | 28 | class QmuTactile 29 | { 30 | public: 31 | QmuTactile(uint8_t pin); 32 | void loop(void); 33 | void start(void); 34 | tactileStateFlags getState(void); 35 | uint8_t getFlags(void); 36 | bool checkFlag(tactileFlagsFlags_e flag); 37 | 38 | private: 39 | uint8_t _pin; 40 | uint8_t _previousPinState = HIGH; 41 | uint32_t _pressMillis = 0; 42 | tactileStateFlags _state = TACTILE_STATE_NONE; 43 | uint32_t _nextPressingEvent = 0; 44 | uint8_t _flags; 45 | }; 46 | 47 | #endif -------------------------------------------------------------------------------- /sbus.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef SBUS_INPUT 3 | #define SBUS_INPUT 4 | 5 | #include "Arduino.h" 6 | 7 | #define SBUS_PACKET_LENGTH 25 8 | 9 | enum sbusProtocolStates { 10 | SBUS_DECODING_STATE_IDLE, 11 | SBUS_DECODING_STATE_IN_PROGRESS 12 | }; 13 | 14 | class TxInput 15 | { 16 | public: 17 | virtual ~TxInput() {} 18 | virtual void start(void) {}; 19 | virtual void stop(void) {}; 20 | virtual bool isReceiving(void) { return false; }; 21 | virtual void loop(void) {}; 22 | }; 23 | 24 | class SbusInput : public TxInput 25 | { 26 | public: 27 | SbusInput(HardwareSerial &serial); 28 | void start(void); 29 | void restart(void); 30 | void loop(void); 31 | bool isReceiving(void); 32 | void recoverStuckFrames(void); 33 | void (* setRcChannelCallback)(uint8_t channel, int value, int offset); 34 | private: 35 | HardwareSerial &_serial; 36 | uint32_t _frameDecodingStartedAt = 0; 37 | uint32_t _frameDecodingEndedAt = 0 ; 38 | uint8_t _protocolState = SBUS_DECODING_STATE_IDLE; 39 | void sbusRead(void); 40 | void sbusToChannels(byte buffer[]); 41 | }; 42 | 43 | void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe, int (* rcChannelGetCallback)(uint8_t)); 44 | 45 | #endif 46 | 47 | -------------------------------------------------------------------------------- /math.cpp: -------------------------------------------------------------------------------- 1 | #include "math.h" 2 | 3 | void devClear(stdev_t *dev) 4 | { 5 | dev->m_n = 0; 6 | } 7 | 8 | void devPush(stdev_t *dev, float x) 9 | { 10 | dev->m_n++; 11 | if (dev->m_n == 1) { 12 | dev->m_oldM = dev->m_newM = x; 13 | dev->m_oldS = 0.0f; 14 | } else { 15 | dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n; 16 | dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM); 17 | dev->m_oldM = dev->m_newM; 18 | dev->m_oldS = dev->m_newS; 19 | } 20 | } 21 | 22 | float devVariance(stdev_t *dev) 23 | { 24 | return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f); 25 | } 26 | 27 | float devStandardDeviation(stdev_t *dev) 28 | { 29 | return sqrtf(devVariance(dev)); 30 | } 31 | 32 | float applyDeadband(float value, float deadband) 33 | { 34 | if (fabsf(value) < deadband) { 35 | value = 0; 36 | } else if (value > 0) { 37 | value -= deadband; 38 | } else if (value < 0) { 39 | value += deadband; 40 | } 41 | return value; 42 | } 43 | 44 | float fconstrainf(float amt, float low, float high) 45 | { 46 | if (amt < low) 47 | return low; 48 | else if (amt > high) 49 | return high; 50 | else 51 | return amt; 52 | } 53 | 54 | float fscalef(float x, float srcMin, float srcMax, float destMin, float destMax) { 55 | const float a = (destMax - destMin) * (x - srcMin); 56 | const float b = srcMax - srcMin; 57 | return ((a / b) + destMin); 58 | } -------------------------------------------------------------------------------- /SSD1306.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 by Daniel Eichhorn 5 | * Copyright (c) 2016 by Fabrice Weinberg 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | * 25 | * Credits for parts of this code go to Mike Rankin. Thank you so much for sharing! 26 | */ 27 | 28 | #ifndef SSD1306_h 29 | #define SSD1306_h 30 | #include "SSD1306Wire.h" 31 | 32 | // For legacy support make SSD1306 an alias for SSD1306 33 | typedef SSD1306Wire SSD1306; 34 | 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef TYPES_H 4 | #define TYPES_H 5 | 6 | #define AXIS_COUNT 3 7 | #define SBUS_CHANNEL_COUNT 16 8 | #define DEFAULT_CHANNEL_VALUE 1500 9 | #define THROTTLE_BUTTON_STEP 100 10 | 11 | #include 12 | 13 | typedef struct 14 | { 15 | float x, y, z; 16 | } axis_t; 17 | 18 | typedef enum 19 | { 20 | AXIS_X = 0, 21 | AXIS_Y = 1, 22 | AXIS_Z = 2 23 | } axis_definition_e; 24 | 25 | typedef enum 26 | { 27 | ROLL = 0, 28 | PITCH = 1, 29 | THROTTLE = 2, 30 | YAW = 3 31 | } channel_functions_e; 32 | 33 | enum calibrationState_e 34 | { 35 | CALIBARTION_NOT_DONE, 36 | CALIBRATION_IN_PROGRESS, 37 | CALIBRATION_DONE 38 | }; 39 | 40 | struct gyroCalibration_t 41 | { 42 | stdev_t deviation[AXIS_COUNT]; 43 | float accumulatedValue[AXIS_COUNT]; 44 | float zero[AXIS_COUNT]; 45 | uint32_t sampleCount; 46 | uint8_t state = CALIBARTION_NOT_DONE; 47 | }; 48 | 49 | typedef struct 50 | { 51 | axis_t gyro; // in DPS 52 | axis_t accAngle; // in degrees 53 | axis_t gyroNormalized; // in dps * dT 54 | axis_t angle; // angle from complimentary filter 55 | 56 | gyroCalibration_t gyroCalibration; 57 | 58 | } imuData_t; 59 | 60 | typedef struct 61 | { 62 | int raw[2]; 63 | int min[2]; 64 | int max[2]; 65 | int zeroed[2]; 66 | float position[2]; // this one is -1.0f to 1.0f 67 | gyroCalibration_t calibration; 68 | } thumb_joystick_t; 69 | 70 | typedef struct 71 | { 72 | uint16_t channels[SBUS_CHANNEL_COUNT]; 73 | 74 | } dataOutput_t; 75 | 76 | 77 | #endif -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "cmath": "cpp", 4 | "algorithm": "cpp", 5 | "array": "cpp", 6 | "*.tcc": "cpp", 7 | "cctype": "cpp", 8 | "cerrno": "cpp", 9 | "cfloat": "cpp", 10 | "climits": "cpp", 11 | "clocale": "cpp", 12 | "cstdarg": "cpp", 13 | "cstdbool": "cpp", 14 | "cstddef": "cpp", 15 | "cstdint": "cpp", 16 | "cstdio": "cpp", 17 | "cstdlib": "cpp", 18 | "cstring": "cpp", 19 | "ctime": "cpp", 20 | "cwchar": "cpp", 21 | "cwctype": "cpp", 22 | "deque": "cpp", 23 | "map": "cpp", 24 | "string": "cpp", 25 | "unordered_map": "cpp", 26 | "unordered_set": "cpp", 27 | "vector": "cpp", 28 | "exception": "cpp", 29 | "functional": "cpp", 30 | "system_error": "cpp", 31 | "tuple": "cpp", 32 | "type_traits": "cpp", 33 | "iterator": "cpp", 34 | "memory": "cpp", 35 | "numeric": "cpp", 36 | "random": "cpp", 37 | "fstream": "cpp", 38 | "initializer_list": "cpp", 39 | "iomanip": "cpp", 40 | "ios": "cpp", 41 | "iosfwd": "cpp", 42 | "istream": "cpp", 43 | "limits": "cpp", 44 | "locale": "cpp", 45 | "new": "cpp", 46 | "ostream": "cpp", 47 | "queue": "cpp", 48 | "sstream": "cpp", 49 | "stdexcept": "cpp", 50 | "streambuf": "cpp", 51 | "cinttypes": "cpp", 52 | "utility": "cpp", 53 | "typeinfo": "cpp" 54 | } 55 | } -------------------------------------------------------------------------------- /QmuTactile.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "QmuTactile.h" 3 | 4 | QmuTactile::QmuTactile(uint8_t pin) { 5 | _pin = pin; 6 | } 7 | 8 | void QmuTactile::loop(void) { 9 | 10 | uint8_t pinState = digitalRead(_pin); 11 | _state = TACTILE_STATE_NONE; 12 | _flags = TACTILE_FLAG_NONE; 13 | 14 | //Press moment 15 | if (pinState == LOW && _previousPinState == HIGH) { 16 | _pressMillis = millis(); 17 | _nextPressingEvent = 0; 18 | } 19 | 20 | const uint32_t buttonTime = abs(millis() - _pressMillis); 21 | 22 | //Pressing 23 | if (pinState == LOW && buttonTime > TACTILE_PRESSING_TIME) { 24 | if (millis() > _nextPressingEvent) { 25 | _state = TACTILE_STATE_PRESSING; 26 | _nextPressingEvent = millis() + 500; 27 | } 28 | } 29 | 30 | //Release moment 31 | if (pinState == HIGH && _previousPinState == LOW) { 32 | 33 | if (buttonTime > TACTILE_LONG_PRESS_TIME) { 34 | _state = TACTILE_STATE_LONG_PRESS; 35 | } else if (buttonTime > TACTILE_MIN_PRESS_TIME) { 36 | _state = TACTILE_STATE_SHORT_PRESS; 37 | } 38 | 39 | } 40 | 41 | // _previousPosition = _position; 42 | if (pinState == LOW) { 43 | _flags |= TACTILE_FLAG_PRESSED; 44 | } 45 | if (pinState == LOW && _previousPinState == HIGH) { 46 | _flags |= TACTILE_FLAG_EDGE_PRESSED; 47 | } 48 | if (pinState == HIGH && _previousPinState == LOW) { 49 | _flags |= TACTILE_FLAG_EDGE_NOT_PRESSED; 50 | } 51 | 52 | _previousPinState = pinState; 53 | } 54 | 55 | void QmuTactile::start(void) { 56 | pinMode(_pin, INPUT_PULLUP); 57 | } 58 | 59 | tactileStateFlags QmuTactile::getState(void) { 60 | return _state; 61 | } 62 | 63 | uint8_t QmuTactile::getFlags(void) { 64 | return _flags; 65 | } 66 | 67 | bool QmuTactile::checkFlag(tactileFlagsFlags_e flag) { 68 | return (_flags & flag); 69 | } -------------------------------------------------------------------------------- /oled_display.cpp: -------------------------------------------------------------------------------- 1 | #include "oled_display.h" 2 | #include "Arduino.h" 3 | #include "math.h" 4 | 5 | OledDisplay::OledDisplay(SSD1306 *display) { 6 | _display = display; 7 | } 8 | 9 | void OledDisplay::init() { 10 | _display->init(); 11 | _display->flipScreenVertically(); 12 | _display->setFont(ArialMT_Plain_10); 13 | } 14 | 15 | void OledDisplay::loop() { 16 | page(); 17 | } 18 | 19 | void OledDisplay::setPage(uint8_t page) { 20 | _page = page; 21 | } 22 | 23 | void OledDisplay::page() { 24 | 25 | static uint32_t lastUpdate = 0; 26 | 27 | _forceDisplay = false; 28 | switch (_page) { 29 | 30 | case OLED_PAGE_BEACON_STATUS: 31 | renderPageStatus(); 32 | break; 33 | } 34 | 35 | lastUpdate = millis(); 36 | } 37 | 38 | void OledDisplay::renderPageStatus() { 39 | 40 | _display->clear(); 41 | 42 | String val; 43 | 44 | _display->setFont(ArialMT_Plain_10); 45 | 46 | //Gyro calibration 47 | if (imu.gyroCalibration.state == CALIBRATION_DONE) { 48 | val = "Gyro: OK"; 49 | } else { 50 | val = "Gyro: Cal"; 51 | } 52 | _display->drawString(0, 0, val); 53 | 54 | //Gyro calibration 55 | if ( 56 | thumbJoystick.calibration.state == CALIBRATION_DONE && 57 | thumbJoystick.max[AXIS_X] > 750 && 58 | thumbJoystick.max[AXIS_Y] > 750 && 59 | thumbJoystick.min[AXIS_X] < -750 && 60 | thumbJoystick.min[AXIS_Y] < -750 61 | ) { 62 | val = "Stick: OK"; 63 | } else { 64 | val = "Stick: Cal"; 65 | } 66 | _display->drawString(64, 0, val); 67 | 68 | _display->setFont(ArialMT_Plain_24); 69 | if (device.getActionEnabled()) { 70 | _display->drawString(46, 16, "Hot"); 71 | 72 | _display->setFont(ArialMT_Plain_10); 73 | _display->drawString(0, 42, "R:" + String(fscalef(output.channels[ROLL] - 1500, -500, 500, -100, 100), 0) + "%"); 74 | _display->drawString(0, 52, "T:" + String(fscalef(output.channels[THROTTLE] - 1500, -500, 500, -100, 100), 0) + "%"); 75 | _display->drawString(64, 42, "P:" + String(fscalef(output.channels[PITCH] - 1500, -500, 500, -100, 100), 0) + "%"); 76 | _display->drawString(64, 52, "Y:" + String(fscalef(output.channels[YAW] - 1500, -500, 500, -100, 100), 0) + "%"); 77 | 78 | } else { 79 | _display->drawString(36, 16, "Safe"); 80 | } 81 | 82 | _display->display(); 83 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DiyMotionController 2 | 3 | [![Presetation video](assets/04.jpg)](https://youtu.be/aKAv_YmEXCA) 4 | 5 | **DIY Motion Controller** is a gesture control controller that works with any drone or RC airplane. It acts as an OpenTX trainer slave connected to the OpenTX radio like a Radiomaster TX16S or any other with SBUS trainer input. 6 | 7 | Drone or airplane should be capable of Position Hold and Altitude Hold. INAV or Ardupilot recommended. 8 | 9 | ![DIY Motion Controller](assets/01.jpg) 10 | 11 | # More info 12 | 13 | More info about the project can be found here: 14 | 15 | * [QuadMeUp Discord Server](https://discord.gg/5YebHyzDwC) 16 | * [RCGroups](https://www.rcgroups.com/forums/showthread.php?3857149-DIY-Motion-Controller-gesture-control-for-drones) 17 | * [QuadMeUp](https://quadmeup.com/tag/diy-motion-controller/) 18 | 19 | # Short manual for OpenTX, Radiomaster TX16S and INAV 20 | 21 | ## OpenTX setup with SBUS Trainer input 22 | 23 | 1. Configure OpenTX to use Serial Port 1 as a SBUS Trainer (System -> Hardware) 24 | 2. Enable Trainer input for your model (Model -> Model Setup -> Trainer Mode Master/Serial) 25 | 3. Configure one of the radio switches to enable `Trainer Sticks` (Model -> Special Functions) 26 | 4. Connect **DIY Motion Controller** to Serial Port 1. Radio RX pin to ESP32 `SERIAL1_TX` pin (14 default) 27 | 5. To enable gesture control, switch configured in point 3 has to be enabled! 28 | 29 | ## PPM Trainer input 30 | 31 | In case of radios that does not have user accessible serial port, PPM mode can be used. 32 | 33 | To enable PPM output on `pin 14`, uncomment `#define TRAINER_MODE_PPM` and comment out `#define TRAINER_MODE_SBUS` 34 | 35 | ``` 36 | //#define TRAINER_MODE_SBUS 37 | #define TRAINER_MODE_PPM 38 | ``` 39 | 40 | ## INAV Setup 41 | 42 | INAV craft should be capable of performing Position Hold and Altitude Hold. Craft should be stationary when Motion Controller is enabled. 43 | 44 | ## Motion Controller Setup 45 | 46 | ### Power Up 47 | 48 | 1. After powering up, controller should be stationary for gyro to calibrate. Calibration is done when OLED says `Gyro: OK` 49 | 1. Joystick should be calibrated by moving the stick to top/down/left/right positions after each power up 50 | 51 | ### Operations 52 | 53 | 1. Gesture Control is activated after a long press (1s) of a trigger button. OLED will display `HOT` 54 | 1. Enable OpenTX trainer mode with a switch 55 | 1. Tilting DIY Motion Controller left and right will roll the craft left and right 56 | 1. Tilting DIY Motion Controller forward and back will move the craft forward and back 57 | 1. Throttle control is assigned to the thumb joystick UP/DOWN 58 | 1. Yaw control is assigned to the thumb joystick LEFT/RIGHT 59 | 1. Gesture YAW control is enabled only when THUMB trigger is pressed 60 | 61 | # Parts 62 | 63 | * [ESP32 LORA32 V2](http://bit.ly/3vh5kmn) - future wireless version will require 2 boards. Other ESP32 with OLED will do, but you will have to adjust pin mapping 64 | * [MPU6050 gyro board](http://bit.ly/3byWMiU) 65 | * [10mm push button](http://bit.ly/30L4X5P) 66 | * [7mm push button](http://bit.ly/3vh5BWr) 67 | * [Analog joystick](http://bit.ly/2POPZt3) - current 3D printed model supports this kind of joystick. Future version might require different joystick 68 | * [AWG30 wires](http://bit.ly/35KMXc2) 69 | * [Optional 18650 holder](http://bit.ly/3cplTny) - for future wireless version 70 | 71 | [STL for the controller available here](http://bit.ly/2OkMKcy). Print with PLA/PET-G/ABS. Design has integrated supports, print without adding supports in 72 | slicing software. 73 | 74 | # Wiring 75 | 76 | | ESP32 PIN | Accessory PIN | 77 | |---- |---- | 78 | | 0 | MPU6050 SDA | 79 | | 23 | MPU6050 SCL | 80 | | 4 | Thumb trigger | 81 | | 15 | Index finger trigger | 82 | | 13 | Thumb joystick X Axis | 83 | | 33 | Thumb joystick Y Axis | 84 | | 32 | Thumb hoystick press button | 85 | 86 | Both Thumb and Index Finger Triggers should be wired to GND (ESP32 Pullups are enabled on those inputs) 87 | 88 | # Dependencies 89 | 90 | To complile and flash ESP32, you will need the latest Arduino and [Arduino ESP32 core](https://github.com/espressif/arduino-esp32) 91 | 92 | Following Arduino libraries are required: 93 | 94 | * [Adafruit_MPU6050](https://github.com/adafruit/Adafruit_MPU6050) 95 | * [Adafruit Unified Sensor Driver](https://github.com/adafruit/Adafruit_Sensor) 96 | 97 | ![DIY Motion Controller](assets/02.jpg) 98 | ![DIY Motion Controller](assets/03.jpg) -------------------------------------------------------------------------------- /SSD1306Wire.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 by Daniel Eichhorn 5 | * Copyright (c) 2016 by Fabrice Weinberg 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | * 25 | * Credits for parts of this code go to Mike Rankin. Thank you so much for sharing! 26 | */ 27 | 28 | #ifndef SSD1306Wire_h 29 | #define SSD1306Wire_h 30 | 31 | #include "OLEDDisplay.h" 32 | #include 33 | 34 | class SSD1306Wire : public OLEDDisplay { 35 | private: 36 | uint8_t _address; 37 | uint8_t _sda; 38 | uint8_t _scl; 39 | TwoWire *_wire; 40 | 41 | public: 42 | SSD1306Wire(uint8_t _address, TwoWire *_wire) { 43 | this->_address = _address; 44 | this->_wire = _wire; 45 | } 46 | 47 | bool connect() { 48 | return true; 49 | } 50 | 51 | void display(void) { 52 | #ifdef OLEDDISPLAY_DOUBLE_BUFFER 53 | uint8_t minBoundY = ~0; 54 | uint8_t maxBoundY = 0; 55 | 56 | uint8_t minBoundX = ~0; 57 | uint8_t maxBoundX = 0; 58 | uint8_t x, y; 59 | 60 | // Calculate the Y bounding box of changes 61 | // and copy buffer[pos] to buffer_back[pos]; 62 | for (y = 0; y < (DISPLAY_HEIGHT / 8); y++) { 63 | for (x = 0; x < DISPLAY_WIDTH; x++) { 64 | uint16_t pos = x + y * DISPLAY_WIDTH; 65 | if (buffer[pos] != buffer_back[pos]) { 66 | minBoundY = _min(minBoundY, y); 67 | maxBoundY = _max(maxBoundY, y); 68 | minBoundX = _min(minBoundX, x); 69 | maxBoundX = _max(maxBoundX, x); 70 | } 71 | buffer_back[pos] = buffer[pos]; 72 | } 73 | // yield(); 74 | } 75 | 76 | // If the minBoundY wasn't updated 77 | // we can savely assume that buffer_back[pos] == buffer[pos] 78 | // holdes true for all values of pos 79 | if (minBoundY == ~0) return; 80 | 81 | sendCommand(COLUMNADDR); 82 | sendCommand(minBoundX); 83 | sendCommand(maxBoundX); 84 | 85 | sendCommand(PAGEADDR); 86 | sendCommand(minBoundY); 87 | sendCommand(maxBoundY); 88 | 89 | byte k = 0; 90 | for (y = minBoundY; y <= maxBoundY; y++) { 91 | for (x = minBoundX; x <= maxBoundX; x++) { 92 | if (k == 0) { 93 | _wire->beginTransmission(_address); 94 | _wire->write(0x40); 95 | } 96 | _wire->write(buffer[x + y * DISPLAY_WIDTH]); 97 | k++; 98 | if (k == 16) { 99 | _wire->endTransmission(); 100 | k = 0; 101 | } 102 | } 103 | // yield(); 104 | } 105 | 106 | if (k != 0) { 107 | _wire->endTransmission(); 108 | } 109 | #else 110 | 111 | sendCommand(COLUMNADDR); 112 | sendCommand(0x0); 113 | sendCommand(0x7F); 114 | 115 | sendCommand(PAGEADDR); 116 | sendCommand(0x0); 117 | sendCommand(0x7); 118 | 119 | for (uint16_t i=0; i < DISPLAY_BUFFER_SIZE; i++) { 120 | _wire->beginTransmission(this->_address); 121 | _wire->write(0x40); 122 | for (uint8_t x = 0; x < 16; x++) { 123 | _wire->write(buffer[i]); 124 | i++; 125 | } 126 | i--; 127 | _wire->endTransmission(); 128 | } 129 | #endif 130 | } 131 | 132 | private: 133 | inline void sendCommand(uint8_t command) __attribute__((always_inline)){ 134 | _wire->beginTransmission(_address); 135 | _wire->write(0x80); 136 | _wire->write(command); 137 | _wire->endTransmission(); 138 | } 139 | 140 | 141 | }; 142 | 143 | #endif 144 | -------------------------------------------------------------------------------- /SSD1306Spi.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 by Daniel Eichhorn 5 | * Copyright (c) 2016 by Fabrice Weinberg 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | * 25 | * Credits for parts of this code go to Mike Rankin. Thank you so much for sharing! 26 | */ 27 | 28 | #ifndef SSD1306Spi_h 29 | #define SSD1306Spi_h 30 | 31 | #include "OLEDDisplay.h" 32 | #include 33 | 34 | #if F_CPU == 160000000L 35 | #define BRZO_I2C_SPEED 1000 36 | #else 37 | #define BRZO_I2C_SPEED 800 38 | #endif 39 | 40 | class SSD1306Spi : public OLEDDisplay { 41 | private: 42 | uint8_t _rst; 43 | uint8_t _dc; 44 | uint8_t _cs; 45 | 46 | public: 47 | SSD1306Spi(uint8_t _rst, uint8_t _dc, uint8_t _cs) { 48 | this->_rst = _rst; 49 | this->_dc = _dc; 50 | this->_cs = _cs; 51 | } 52 | 53 | bool connect(){ 54 | pinMode(_dc, OUTPUT); 55 | pinMode(_cs, OUTPUT); 56 | pinMode(_rst, OUTPUT); 57 | 58 | SPI.begin (); 59 | SPI.setClockDivider (SPI_CLOCK_DIV2); 60 | 61 | // Pulse Reset low for 10ms 62 | digitalWrite(_rst, HIGH); 63 | delay(1); 64 | digitalWrite(_rst, LOW); 65 | delay(10); 66 | digitalWrite(_rst, HIGH); 67 | return true; 68 | } 69 | 70 | void display(void) { 71 | #ifdef OLEDDISPLAY_DOUBLE_BUFFER 72 | uint8_t minBoundY = ~0; 73 | uint8_t maxBoundY = 0; 74 | 75 | uint8_t minBoundX = ~0; 76 | uint8_t maxBoundX = 0; 77 | 78 | uint8_t x, y; 79 | 80 | // Calculate the Y bounding box of changes 81 | // and copy buffer[pos] to buffer_back[pos]; 82 | for (y = 0; y < (DISPLAY_HEIGHT / 8); y++) { 83 | for (x = 0; x < DISPLAY_WIDTH; x++) { 84 | uint16_t pos = x + y * DISPLAY_WIDTH; 85 | if (buffer[pos] != buffer_back[pos]) { 86 | minBoundY = _min(minBoundY, y); 87 | maxBoundY = _max(maxBoundY, y); 88 | minBoundX = _min(minBoundX, x); 89 | maxBoundX = _max(maxBoundX, x); 90 | } 91 | buffer_back[pos] = buffer[pos]; 92 | } 93 | } 94 | 95 | // If the minBoundY wasn't updated 96 | // we can savely assume that buffer_back[pos] == buffer[pos] 97 | // holdes true for all values of pos 98 | if (minBoundY == ~0) return; 99 | 100 | sendCommand(COLUMNADDR); 101 | sendCommand(minBoundX); 102 | sendCommand(maxBoundX); 103 | 104 | sendCommand(PAGEADDR); 105 | sendCommand(minBoundY); 106 | sendCommand(maxBoundY); 107 | 108 | digitalWrite(_cs, HIGH); 109 | digitalWrite(_dc, HIGH); // data mode 110 | digitalWrite(_cs, LOW); 111 | for (y = minBoundY; y <= maxBoundY; y++) { 112 | for (x = minBoundX; x <= maxBoundX; x++) { 113 | SPI.transfer(buffer[x + y * DISPLAY_WIDTH]); 114 | } 115 | } 116 | digitalWrite(_cs, HIGH); 117 | #else 118 | // No double buffering 119 | sendCommand(COLUMNADDR); 120 | sendCommand(0x0); 121 | sendCommand(0x7F); 122 | 123 | sendCommand(PAGEADDR); 124 | sendCommand(0x0); 125 | sendCommand(0x7); 126 | 127 | digitalWrite(_cs, HIGH); 128 | digitalWrite(_dc, HIGH); // data mode 129 | digitalWrite(_cs, LOW); 130 | for (uint16_t i=0; i 33 | 34 | #if F_CPU == 160000000L 35 | #define BRZO_I2C_SPEED 1000 36 | #else 37 | #define BRZO_I2C_SPEED 800 38 | #endif 39 | 40 | class SSD1306Brzo : public OLEDDisplay { 41 | private: 42 | uint8_t _address; 43 | uint8_t _sda; 44 | uint8_t _scl; 45 | 46 | public: 47 | SSD1306Brzo(uint8_t _address, uint8_t _sda, uint8_t _scl) { 48 | this->_address = _address; 49 | this->_sda = _sda; 50 | this->_scl = _scl; 51 | } 52 | 53 | bool connect(){ 54 | brzo_i2c_setup(_sda, _scl, 0); 55 | return true; 56 | } 57 | 58 | void display(void) { 59 | #ifdef OLEDDISPLAY_DOUBLE_BUFFER 60 | uint8_t minBoundY = ~0; 61 | uint8_t maxBoundY = 0; 62 | 63 | uint8_t minBoundX = ~0; 64 | uint8_t maxBoundX = 0; 65 | 66 | uint8_t x, y; 67 | 68 | // Calculate the Y bounding box of changes 69 | // and copy buffer[pos] to buffer_back[pos]; 70 | for (y = 0; y < (DISPLAY_HEIGHT / 8); y++) { 71 | for (x = 0; x < DISPLAY_WIDTH; x++) { 72 | uint16_t pos = x + y * DISPLAY_WIDTH; 73 | if (buffer[pos] != buffer_back[pos]) { 74 | minBoundY = _min(minBoundY, y); 75 | maxBoundY = _max(maxBoundY, y); 76 | minBoundX = _min(minBoundX, x); 77 | maxBoundX = _max(maxBoundX, x); 78 | } 79 | buffer_back[pos] = buffer[pos]; 80 | } 81 | } 82 | 83 | // If the minBoundY wasn't updated 84 | // we can savely assume that buffer_back[pos] == buffer[pos] 85 | // holdes true for all values of pos 86 | if (minBoundY == ~0) return; 87 | 88 | sendCommand(COLUMNADDR); 89 | sendCommand(minBoundX); 90 | sendCommand(maxBoundX); 91 | 92 | sendCommand(PAGEADDR); 93 | sendCommand(minBoundY); 94 | sendCommand(maxBoundY); 95 | 96 | byte k = 0; 97 | uint8_t sendBuffer[17]; 98 | sendBuffer[0] = 0x40; 99 | brzo_i2c_start_transaction(this->_address, BRZO_I2C_SPEED); 100 | for (y = minBoundY; y <= maxBoundY; y++) { 101 | for (x = minBoundX; x <= maxBoundX; x++) { 102 | k++; 103 | sendBuffer[k] = buffer[x + y * DISPLAY_WIDTH]; 104 | if (k == 16) { 105 | brzo_i2c_write(sendBuffer, 17, true); 106 | k = 0; 107 | } 108 | } 109 | } 110 | brzo_i2c_write(sendBuffer, k + 1, true); 111 | brzo_i2c_end_transaction(); 112 | #else 113 | // No double buffering 114 | sendCommand(COLUMNADDR); 115 | sendCommand(0x0); 116 | sendCommand(0x7F); 117 | 118 | sendCommand(PAGEADDR); 119 | sendCommand(0x0); 120 | sendCommand(0x7); 121 | 122 | uint8_t sendBuffer[17]; 123 | sendBuffer[0] = 0x40; 124 | brzo_i2c_start_transaction(this->_address, BRZO_I2C_SPEED); 125 | for (uint16_t i=0; i>8 | (output[1] & 0x07FF)<<3); 56 | packet[3] = (uint8_t) ((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6); 57 | packet[4] = (uint8_t) ((output[2] & 0x07FF)>>2); 58 | packet[5] = (uint8_t) ((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1); 59 | packet[6] = (uint8_t) ((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4); 60 | packet[7] = (uint8_t) ((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7); 61 | packet[8] = (uint8_t) ((output[5] & 0x07FF)>>1); 62 | packet[9] = (uint8_t) ((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2); 63 | packet[10] = (uint8_t) ((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5); 64 | packet[11] = (uint8_t) ((output[7] & 0x07FF)>>3); 65 | packet[12] = (uint8_t) ((output[8] & 0x07FF)); 66 | packet[13] = (uint8_t) ((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3); 67 | packet[14] = (uint8_t) ((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6); 68 | packet[15] = (uint8_t) ((output[10] & 0x07FF)>>2); 69 | packet[16] = (uint8_t) ((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1); 70 | packet[17] = (uint8_t) ((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4); 71 | packet[18] = (uint8_t) ((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7); 72 | packet[19] = (uint8_t) ((output[13] & 0x07FF)>>1); 73 | packet[20] = (uint8_t) ((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2); 74 | packet[21] = (uint8_t) ((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5); 75 | packet[22] = (uint8_t) ((output[15] & 0x07FF)>>3); 76 | 77 | packet[23] = stateByte; //Flags byte 78 | packet[24] = SBUS_FRAME_FOOTER; //Footer 79 | } 80 | 81 | SbusInput::SbusInput(HardwareSerial &serial) : _serial(serial) 82 | { 83 | } 84 | 85 | void SbusInput::loop(void) 86 | { 87 | if (_serial.available()) { 88 | sbusRead(); 89 | } 90 | } 91 | 92 | void SbusInput::start(void) 93 | { 94 | #ifdef ARDUINO_ESP32_DEV 95 | _serial.begin(100000, SERIAL_8N2, 14, 25, false); 96 | #else 97 | _serial.begin(100000, SERIAL_8N2); 98 | #endif 99 | } 100 | 101 | void SbusInput::restart(void) 102 | { 103 | _serial.end(); 104 | start(); 105 | } 106 | 107 | /* 108 | * We prevent from frame decoding being stuck somewhere 109 | */ 110 | void SbusInput::recoverStuckFrames(void) 111 | { 112 | if ( 113 | _protocolState == SBUS_DECODING_STATE_IN_PROGRESS && 114 | millis() - _frameDecodingStartedAt > 6 115 | ) { 116 | _protocolState = SBUS_DECODING_STATE_IDLE; 117 | _serial.flush(); 118 | } 119 | } 120 | 121 | void SbusInput::sbusToChannels(byte buffer[]) { 122 | 123 | setRcChannelCallback(0, mapSbusToChannel((buffer[1] | buffer[2]<<8) & 0x07FF), 0); 124 | setRcChannelCallback(1, mapSbusToChannel((buffer[2]>>3 | buffer[3]<<5) & 0x07FF), 0); 125 | setRcChannelCallback(2, mapSbusToChannel((buffer[3]>>6 | buffer[4]<<2 | buffer[5]<<10) & 0x07FF), 0); 126 | setRcChannelCallback(3, mapSbusToChannel((buffer[5]>>1 | buffer[6]<<7) & 0x07FF), 0); 127 | setRcChannelCallback(4, mapSbusToChannel((buffer[6]>>4 | buffer[7]<<4) & 0x07FF), 0); 128 | setRcChannelCallback(5, mapSbusToChannel((buffer[7]>>7 | buffer[8]<<1 |buffer[9]<<9) & 0x07FF), 0); 129 | setRcChannelCallback(6, mapSbusToChannel((buffer[9]>>2 | buffer[10]<<6) & 0x07FF), 0); 130 | setRcChannelCallback(7, mapSbusToChannel((buffer[10]>>5 | buffer[11]<<3) & 0x07FF), 0); 131 | setRcChannelCallback(8, mapSbusToChannel((buffer[12] | buffer[13]<<8) & 0x07FF), 0); 132 | setRcChannelCallback(9, mapSbusToChannel((buffer[13]>>3 | buffer[14]<<5) & 0x07FF), 0); 133 | 134 | //We use only 10 channels, so the reset can be just ignored 135 | } 136 | 137 | void SbusInput::sbusRead() { 138 | static byte buffer[25]; 139 | static byte buffer_index = 0; 140 | 141 | while (_serial.available()) { 142 | byte rx = _serial.read(); 143 | 144 | if (_protocolState == SBUS_DECODING_STATE_IDLE) { 145 | //We allow next frame to start only when previous frame eneded some time ago 146 | if ( 147 | rx == SBUS_FRAME_HEADER && 148 | millis() - _frameDecodingEndedAt > 5 149 | ) { 150 | //Header is correct 151 | _frameDecodingStartedAt = millis(); 152 | buffer_index = 0; 153 | _protocolState = SBUS_DECODING_STATE_IN_PROGRESS; 154 | } 155 | } 156 | 157 | buffer[buffer_index] = rx; 158 | buffer_index++; 159 | 160 | if ( 161 | _protocolState == SBUS_DECODING_STATE_IN_PROGRESS && 162 | buffer_index == 25 && 163 | buffer[24] == SBUS_FRAME_FOOTER 164 | ) { 165 | //We have full frame now 166 | _frameDecodingEndedAt = millis(); 167 | sbusToChannels(buffer); 168 | _protocolState = SBUS_DECODING_STATE_IDLE; 169 | } 170 | } 171 | } 172 | 173 | bool SbusInput::isReceiving() { 174 | return !(millis() - _frameDecodingEndedAt > SBUS_IS_RECEIVING_THRESHOLD); 175 | } -------------------------------------------------------------------------------- /OLEDDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 by Daniel Eichhorn 5 | * Copyright (c) 2016 by Fabrice Weinberg 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | * 25 | * Credits for parts of this code go to Mike Rankin. Thank you so much for sharing! 26 | */ 27 | 28 | #ifndef OLEDDISPLAY_h 29 | #define OLEDDISPLAY_h 30 | 31 | #include 32 | #include "OLEDDisplayFonts.h" 33 | 34 | //#define DEBUG_OLEDDISPLAY(...) Serial.printf( __VA_ARGS__ ) 35 | 36 | #ifndef DEBUG_OLEDDISPLAY 37 | #define DEBUG_OLEDDISPLAY(...) 38 | #endif 39 | 40 | // Use DOUBLE BUFFERING by default 41 | #ifndef OLEDDISPLAY_REDUCE_MEMORY 42 | #define OLEDDISPLAY_DOUBLE_BUFFER 43 | #endif 44 | 45 | 46 | // Display settings 47 | #define DISPLAY_WIDTH 128 48 | #define DISPLAY_HEIGHT 64 49 | #define DISPLAY_BUFFER_SIZE 1024 50 | 51 | // Header Values 52 | #define JUMPTABLE_BYTES 4 53 | 54 | #define JUMPTABLE_LSB 1 55 | #define JUMPTABLE_SIZE 2 56 | #define JUMPTABLE_WIDTH 3 57 | #define JUMPTABLE_START 4 58 | 59 | #define WIDTH_POS 0 60 | #define HEIGHT_POS 1 61 | #define FIRST_CHAR_POS 2 62 | #define CHAR_NUM_POS 3 63 | 64 | 65 | // Display commands 66 | #define CHARGEPUMP 0x8D 67 | #define COLUMNADDR 0x21 68 | #define COMSCANDEC 0xC8 69 | #define COMSCANINC 0xC0 70 | #define DISPLAYALLON 0xA5 71 | #define DISPLAYALLON_RESUME 0xA4 72 | #define DISPLAYOFF 0xAE 73 | #define DISPLAYON 0xAF 74 | #define EXTERNALVCC 0x1 75 | #define INVERTDISPLAY 0xA7 76 | #define MEMORYMODE 0x20 77 | #define NORMALDISPLAY 0xA6 78 | #define PAGEADDR 0x22 79 | #define SEGREMAP 0xA0 80 | #define SETCOMPINS 0xDA 81 | #define SETCONTRAST 0x81 82 | #define SETDISPLAYCLOCKDIV 0xD5 83 | #define SETDISPLAYOFFSET 0xD3 84 | #define SETHIGHCOLUMN 0x10 85 | #define SETLOWCOLUMN 0x00 86 | #define SETMULTIPLEX 0xA8 87 | #define SETPRECHARGE 0xD9 88 | #define SETSEGMENTREMAP 0xA1 89 | #define SETSTARTLINE 0x40 90 | #define SETVCOMDETECT 0xDB 91 | #define SWITCHCAPVCC 0x2 92 | 93 | #ifndef _swap_int16_t 94 | #define _swap_int16_t(a, b) { int16_t t = a; a = b; b = t; } 95 | #endif 96 | 97 | enum OLEDDISPLAY_COLOR { 98 | BLACK = 0, 99 | WHITE = 1, 100 | INVERSE = 2 101 | }; 102 | 103 | enum OLEDDISPLAY_TEXT_ALIGNMENT { 104 | TEXT_ALIGN_LEFT = 0, 105 | TEXT_ALIGN_RIGHT = 1, 106 | TEXT_ALIGN_CENTER = 2, 107 | TEXT_ALIGN_CENTER_BOTH = 3 108 | }; 109 | 110 | 111 | class OLEDDisplay : public Print { 112 | public: 113 | // Initialize the display 114 | bool init(); 115 | 116 | // Free the memory used by the display 117 | void end(); 118 | 119 | // Cycle through the initialization 120 | void resetDisplay(void); 121 | 122 | /* Drawing functions */ 123 | // Sets the color of all pixel operations 124 | void setColor(OLEDDISPLAY_COLOR color); 125 | 126 | // Draw a pixel at given position 127 | void setPixel(int16_t x, int16_t y); 128 | 129 | // Draw a line from position 0 to position 1 130 | void drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1); 131 | 132 | // Draw the border of a rectangle at the given location 133 | void drawRect(int16_t x, int16_t y, int16_t width, int16_t height); 134 | 135 | // Fill the rectangle 136 | void fillRect(int16_t x, int16_t y, int16_t width, int16_t height); 137 | 138 | // Draw the border of a circle 139 | void drawCircle(int16_t x, int16_t y, int16_t radius); 140 | 141 | // Draw all Quadrants specified in the quads bit mask 142 | void drawCircleQuads(int16_t x0, int16_t y0, int16_t radius, uint8_t quads); 143 | 144 | // Fill circle 145 | void fillCircle(int16_t x, int16_t y, int16_t radius); 146 | 147 | // Draw a line horizontally 148 | void drawHorizontalLine(int16_t x, int16_t y, int16_t length); 149 | 150 | // Draw a lin vertically 151 | void drawVerticalLine(int16_t x, int16_t y, int16_t length); 152 | 153 | // Draws a rounded progress bar with the outer dimensions given by width and height. Progress is 154 | // a unsigned byte value between 0 and 100 155 | void drawProgressBar(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint8_t progress); 156 | 157 | // Draw a bitmap in the internal image format 158 | void drawFastImage(int16_t x, int16_t y, int16_t width, int16_t height, const char *image); 159 | 160 | // Draw a XBM 161 | void drawXbm(int16_t x, int16_t y, int16_t width, int16_t height, const char *xbm); 162 | 163 | /* Text functions */ 164 | 165 | // Draws a string at the given location 166 | void drawString(int16_t x, int16_t y, String text); 167 | 168 | // Draws a String with a maximum width at the given location. 169 | // If the given String is wider than the specified width 170 | // The text will be wrapped to the next line at a space or dash 171 | void drawStringMaxWidth(int16_t x, int16_t y, uint16_t maxLineWidth, String text); 172 | 173 | // Returns the width of the const char* with the current 174 | // font settings 175 | uint16_t getStringWidth(const char* text, uint16_t length); 176 | 177 | // Convencience method for the const char version 178 | uint16_t getStringWidth(String text); 179 | 180 | // Specifies relative to which anchor point 181 | // the text is rendered. Available constants: 182 | // TEXT_ALIGN_LEFT, TEXT_ALIGN_CENTER, TEXT_ALIGN_RIGHT, TEXT_ALIGN_CENTER_BOTH 183 | void setTextAlignment(OLEDDISPLAY_TEXT_ALIGNMENT textAlignment); 184 | 185 | // Sets the current font. Available default fonts 186 | // ArialMT_Plain_10, ArialMT_Plain_16, ArialMT_Plain_24 187 | void setFont(const char *fontData); 188 | 189 | /* Display functions */ 190 | 191 | // Turn the display on 192 | void displayOn(void); 193 | 194 | // Turn the display offs 195 | void displayOff(void); 196 | 197 | // Inverted display mode 198 | void invertDisplay(void); 199 | 200 | // Normal display mode 201 | void normalDisplay(void); 202 | 203 | // Set display contrast 204 | void setContrast(char contrast); 205 | 206 | // Turn the display upside down 207 | void flipScreenVertically(); 208 | 209 | // Write the buffer to the display memory 210 | virtual void display(void) = 0; 211 | 212 | // Clear the local pixel buffer 213 | void clear(void); 214 | 215 | // Log buffer implementation 216 | 217 | // This will define the lines and characters you can 218 | // print to the screen. When you exeed the buffer size (lines * chars) 219 | // the output may be truncated due to the size constraint. 220 | bool setLogBuffer(uint16_t lines, uint16_t chars); 221 | 222 | // Draw the log buffer at position (x, y) 223 | void drawLogBuffer(uint16_t x, uint16_t y); 224 | 225 | // Implementent needed function to be compatible with Print class 226 | size_t write(uint8_t c); 227 | size_t write(const char* s); 228 | 229 | uint8_t *buffer; 230 | 231 | #ifdef OLEDDISPLAY_DOUBLE_BUFFER 232 | uint8_t *buffer_back; 233 | #endif 234 | 235 | protected: 236 | 237 | OLEDDISPLAY_TEXT_ALIGNMENT textAlignment = TEXT_ALIGN_LEFT; 238 | OLEDDISPLAY_COLOR color = WHITE; 239 | 240 | const char *fontData = ArialMT_Plain_10; 241 | 242 | // State values for logBuffer 243 | uint16_t logBufferSize = 0; 244 | uint16_t logBufferFilled = 0; 245 | uint16_t logBufferLine = 0; 246 | uint16_t logBufferMaxLines = 0; 247 | char *logBuffer = NULL; 248 | 249 | // Send a command to the display (low level function) 250 | virtual void sendCommand(uint8_t com) {}; 251 | 252 | // Connect to the display 253 | virtual bool connect() {}; 254 | 255 | // Send all the init commands 256 | void sendInitCommands(); 257 | 258 | // converts utf8 characters to extended ascii 259 | static char* utf8ascii(String s); 260 | static byte utf8ascii(byte ascii); 261 | 262 | void inline drawInternal(int16_t xMove, int16_t yMove, int16_t width, int16_t height, const char *data, uint16_t offset, uint16_t bytesInData) __attribute__((always_inline)); 263 | 264 | void drawStringInternal(int16_t xMove, int16_t yMove, char* text, uint16_t textLength, uint16_t textWidth); 265 | 266 | }; 267 | 268 | #endif 269 | -------------------------------------------------------------------------------- /OLEDDisplayUi.h: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 by Daniel Eichhorn 5 | * Copyright (c) 2016 by Fabrice Weinberg 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | * 25 | */ 26 | 27 | #ifndef OLEDDISPLAYUI_h 28 | #define OLEDDISPLAYUI_h 29 | 30 | #include 31 | #include "OLEDDisplay.h" 32 | 33 | //#define DEBUG_OLEDDISPLAYUI(...) Serial.printf( __VA_ARGS__ ) 34 | 35 | #ifndef DEBUG_OLEDDISPLAYUI 36 | #define DEBUG_OLEDDISPLAYUI(...) 37 | #endif 38 | 39 | enum AnimationDirection { 40 | SLIDE_UP, 41 | SLIDE_DOWN, 42 | SLIDE_LEFT, 43 | SLIDE_RIGHT 44 | }; 45 | 46 | enum IndicatorPosition { 47 | TOP, 48 | RIGHT, 49 | BOTTOM, 50 | LEFT 51 | }; 52 | 53 | enum IndicatorDirection { 54 | LEFT_RIGHT, 55 | RIGHT_LEFT 56 | }; 57 | 58 | enum FrameState { 59 | IN_TRANSITION, 60 | FIXED 61 | }; 62 | 63 | 64 | const char ANIMATION_activeSymbol[] PROGMEM = { 65 | 0x00, 0x18, 0x3c, 0x7e, 0x7e, 0x3c, 0x18, 0x00 66 | }; 67 | 68 | const char ANIMATION_inactiveSymbol[] PROGMEM = { 69 | 0x00, 0x0, 0x0, 0x18, 0x18, 0x0, 0x0, 0x00 70 | }; 71 | 72 | 73 | // Structure of the UiState 74 | struct OLEDDisplayUiState { 75 | uint64_t lastUpdate = 0; 76 | uint16_t ticksSinceLastStateSwitch = 0; 77 | 78 | FrameState frameState = FIXED; 79 | uint8_t currentFrame = 0; 80 | 81 | bool isIndicatorDrawen = true; 82 | 83 | // Normal = 1, Inverse = -1; 84 | int8_t frameTransitionDirection = 1; 85 | 86 | bool manuelControll = false; 87 | 88 | // Custom data that can be used by the user 89 | void* userData = NULL; 90 | }; 91 | 92 | struct LoadingStage { 93 | const char* process; 94 | void (*callback)(); 95 | }; 96 | 97 | typedef void (*FrameCallback)(OLEDDisplay *display, OLEDDisplayUiState* state, int16_t x, int16_t y); 98 | typedef void (*OverlayCallback)(OLEDDisplay *display, OLEDDisplayUiState* state); 99 | typedef void (*LoadingDrawFunction)(OLEDDisplay *display, LoadingStage* stage, uint8_t progress); 100 | 101 | class OLEDDisplayUi { 102 | private: 103 | OLEDDisplay *display; 104 | 105 | // Symbols for the Indicator 106 | IndicatorPosition indicatorPosition = BOTTOM; 107 | IndicatorDirection indicatorDirection = LEFT_RIGHT; 108 | 109 | const char* activeSymbol = ANIMATION_activeSymbol; 110 | const char* inactiveSymbol = ANIMATION_inactiveSymbol; 111 | 112 | bool shouldDrawIndicators = true; 113 | 114 | // Values for the Frames 115 | AnimationDirection frameAnimationDirection = SLIDE_RIGHT; 116 | 117 | int8_t lastTransitionDirection = 1; 118 | 119 | uint16_t ticksPerFrame = 151; // ~ 5000ms at 30 FPS 120 | uint16_t ticksPerTransition = 15; // ~ 500ms at 30 FPS 121 | 122 | bool autoTransition = true; 123 | 124 | FrameCallback* frameFunctions; 125 | uint8_t frameCount = 0; 126 | 127 | // Internally used to transition to a specific frame 128 | int8_t nextFrameNumber = -1; 129 | 130 | // Values for Overlays 131 | OverlayCallback* overlayFunctions; 132 | uint8_t overlayCount = 0; 133 | 134 | // Will the Indicator be drawen 135 | // 3 Not drawn in both frames 136 | // 2 Drawn this frame but not next 137 | // 1 Not drown this frame but next 138 | // 0 Not known yet 139 | uint8_t indicatorDrawState = 1; 140 | 141 | // Loading screen 142 | LoadingDrawFunction loadingDrawFunction = [](OLEDDisplay *display, LoadingStage* stage, uint8_t progress) { 143 | display->setTextAlignment(TEXT_ALIGN_CENTER); 144 | display->setFont(ArialMT_Plain_10); 145 | display->drawString(64, 18, stage->process); 146 | display->drawProgressBar(4, 32, 120, 8, progress); 147 | }; 148 | 149 | // UI State 150 | OLEDDisplayUiState state; 151 | 152 | // Bookeeping for update 153 | uint8_t updateInterval = 33; 154 | 155 | uint8_t getNextFrameNumber(); 156 | void drawIndicator(); 157 | void drawFrame(); 158 | void drawOverlays(); 159 | void tick(); 160 | void resetState(); 161 | 162 | public: 163 | 164 | OLEDDisplayUi(OLEDDisplay *display); 165 | 166 | /** 167 | * Initialise the display 168 | */ 169 | void init(); 170 | 171 | /** 172 | * Configure the internal used target FPS 173 | */ 174 | void setTargetFPS(uint8_t fps); 175 | 176 | // Automatic Controll 177 | /** 178 | * Enable automatic transition to next frame after the some time can be configured with `setTimePerFrame` and `setTimePerTransition`. 179 | */ 180 | void enableAutoTransition(); 181 | 182 | /** 183 | * Disable automatic transition to next frame. 184 | */ 185 | void disableAutoTransition(); 186 | 187 | /** 188 | * Set the direction if the automatic transitioning 189 | */ 190 | void setAutoTransitionForwards(); 191 | void setAutoTransitionBackwards(); 192 | 193 | /** 194 | * Set the approx. time a frame is displayed 195 | */ 196 | void setTimePerFrame(uint16_t time); 197 | 198 | /** 199 | * Set the approx. time a transition will take 200 | */ 201 | void setTimePerTransition(uint16_t time); 202 | 203 | // Customize indicator position and style 204 | 205 | /** 206 | * Draw the indicator. 207 | * This is the defaut state for all frames if 208 | * the indicator was hidden on the previous frame 209 | * it will be slided in. 210 | */ 211 | void enableIndicator(); 212 | 213 | /** 214 | * Don't draw the indicator. 215 | * This will slide out the indicator 216 | * when transitioning to the next frame. 217 | */ 218 | void disableIndicator(); 219 | 220 | /** 221 | * Enable drawing of indicators 222 | */ 223 | void enableAllIndicators(); 224 | 225 | /** 226 | * Disable draw of indicators. 227 | */ 228 | void disableAllIndicators(); 229 | 230 | /** 231 | * Set the position of the indicator bar. 232 | */ 233 | void setIndicatorPosition(IndicatorPosition pos); 234 | 235 | /** 236 | * Set the direction of the indicator bar. Defining the order of frames ASCENDING / DESCENDING 237 | */ 238 | void setIndicatorDirection(IndicatorDirection dir); 239 | 240 | /** 241 | * Set the symbol to indicate an active frame in the indicator bar. 242 | */ 243 | void setActiveSymbol(const char* symbol); 244 | 245 | /** 246 | * Set the symbol to indicate an inactive frame in the indicator bar. 247 | */ 248 | void setInactiveSymbol(const char* symbol); 249 | 250 | 251 | // Frame settings 252 | 253 | /** 254 | * Configure what animation is used to transition from one frame to another 255 | */ 256 | void setFrameAnimation(AnimationDirection dir); 257 | 258 | /** 259 | * Add frame drawing functions 260 | */ 261 | void setFrames(FrameCallback* frameFunctions, uint8_t frameCount); 262 | 263 | // Overlay 264 | 265 | /** 266 | * Add overlays drawing functions that are draw independent of the Frames 267 | */ 268 | void setOverlays(OverlayCallback* overlayFunctions, uint8_t overlayCount); 269 | 270 | 271 | // Loading animation 272 | /** 273 | * Set the function that will draw each step 274 | * in the loading animation 275 | */ 276 | void setLoadingDrawFunction(LoadingDrawFunction loadingFunction); 277 | 278 | 279 | /** 280 | * Run the loading process 281 | */ 282 | void runLoadingProcess(LoadingStage* stages, uint8_t stagesCount); 283 | 284 | 285 | // Manual Control 286 | void nextFrame(); 287 | void previousFrame(); 288 | 289 | /** 290 | * Switch without transition to frame `frame`. 291 | */ 292 | void switchToFrame(uint8_t frame); 293 | 294 | /** 295 | * Transition to frame `frame`, when the `frame` number is bigger than the current 296 | * frame the forward animation will be used, otherwise the backwards animation is used. 297 | */ 298 | void transitionToFrame(uint8_t frame); 299 | 300 | // State Info 301 | OLEDDisplayUiState* getUiState(); 302 | 303 | int8_t update(); 304 | }; 305 | #endif 306 | -------------------------------------------------------------------------------- /OLEDDisplayUi.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 by Daniel Eichhorn 5 | * Copyright (c) 2016 by Fabrice Weinberg 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | * 25 | */ 26 | 27 | #include "OLEDDisplayUi.h" 28 | 29 | OLEDDisplayUi::OLEDDisplayUi(OLEDDisplay *display) { 30 | this->display = display; 31 | } 32 | 33 | void OLEDDisplayUi::init() { 34 | this->display->init(); 35 | } 36 | 37 | void OLEDDisplayUi::setTargetFPS(uint8_t fps){ 38 | float oldInterval = this->updateInterval; 39 | this->updateInterval = ((float) 1.0 / (float) fps) * 1000; 40 | 41 | // Calculate new ticksPerFrame 42 | float changeRatio = oldInterval / (float) this->updateInterval; 43 | this->ticksPerFrame *= changeRatio; 44 | this->ticksPerTransition *= changeRatio; 45 | } 46 | 47 | // -/------ Automatic controll ------\- 48 | 49 | void OLEDDisplayUi::enableAutoTransition(){ 50 | this->autoTransition = true; 51 | } 52 | void OLEDDisplayUi::disableAutoTransition(){ 53 | this->autoTransition = false; 54 | } 55 | void OLEDDisplayUi::setAutoTransitionForwards(){ 56 | this->state.frameTransitionDirection = 1; 57 | this->lastTransitionDirection = 1; 58 | } 59 | void OLEDDisplayUi::setAutoTransitionBackwards(){ 60 | this->state.frameTransitionDirection = -1; 61 | this->lastTransitionDirection = -1; 62 | } 63 | void OLEDDisplayUi::setTimePerFrame(uint16_t time){ 64 | this->ticksPerFrame = (int) ( (float) time / (float) updateInterval); 65 | } 66 | void OLEDDisplayUi::setTimePerTransition(uint16_t time){ 67 | this->ticksPerTransition = (int) ( (float) time / (float) updateInterval); 68 | } 69 | 70 | // -/------ Customize indicator position and style -------\- 71 | void OLEDDisplayUi::enableIndicator(){ 72 | this->state.isIndicatorDrawen = true; 73 | } 74 | 75 | void OLEDDisplayUi::disableIndicator(){ 76 | this->state.isIndicatorDrawen = false; 77 | } 78 | 79 | void OLEDDisplayUi::enableAllIndicators(){ 80 | this->shouldDrawIndicators = true; 81 | } 82 | 83 | void OLEDDisplayUi::disableAllIndicators(){ 84 | this->shouldDrawIndicators = false; 85 | } 86 | 87 | void OLEDDisplayUi::setIndicatorPosition(IndicatorPosition pos) { 88 | this->indicatorPosition = pos; 89 | } 90 | void OLEDDisplayUi::setIndicatorDirection(IndicatorDirection dir) { 91 | this->indicatorDirection = dir; 92 | } 93 | void OLEDDisplayUi::setActiveSymbol(const char* symbol) { 94 | this->activeSymbol = symbol; 95 | } 96 | void OLEDDisplayUi::setInactiveSymbol(const char* symbol) { 97 | this->inactiveSymbol = symbol; 98 | } 99 | 100 | 101 | // -/----- Frame settings -----\- 102 | void OLEDDisplayUi::setFrameAnimation(AnimationDirection dir) { 103 | this->frameAnimationDirection = dir; 104 | } 105 | void OLEDDisplayUi::setFrames(FrameCallback* frameFunctions, uint8_t frameCount) { 106 | this->frameFunctions = frameFunctions; 107 | this->frameCount = frameCount; 108 | this->resetState(); 109 | } 110 | 111 | // -/----- Overlays ------\- 112 | void OLEDDisplayUi::setOverlays(OverlayCallback* overlayFunctions, uint8_t overlayCount){ 113 | this->overlayFunctions = overlayFunctions; 114 | this->overlayCount = overlayCount; 115 | } 116 | 117 | // -/----- Loading Process -----\- 118 | 119 | void OLEDDisplayUi::setLoadingDrawFunction(LoadingDrawFunction loadingDrawFunction) { 120 | this->loadingDrawFunction = loadingDrawFunction; 121 | } 122 | 123 | void OLEDDisplayUi::runLoadingProcess(LoadingStage* stages, uint8_t stagesCount) { 124 | uint8_t progress = 0; 125 | uint8_t increment = 100 / stagesCount; 126 | 127 | for (uint8_t i = 0; i < stagesCount; i++) { 128 | display->clear(); 129 | this->loadingDrawFunction(this->display, &stages[i], progress); 130 | display->display(); 131 | 132 | stages[i].callback(); 133 | 134 | progress += increment; 135 | } 136 | 137 | display->clear(); 138 | this->loadingDrawFunction(this->display, &stages[stagesCount-1], progress); 139 | display->display(); 140 | 141 | delay(150); 142 | } 143 | 144 | // -/----- Manuel control -----\- 145 | void OLEDDisplayUi::nextFrame() { 146 | if (this->state.frameState != IN_TRANSITION) { 147 | this->state.manuelControll = true; 148 | this->state.frameState = IN_TRANSITION; 149 | this->state.ticksSinceLastStateSwitch = 0; 150 | this->lastTransitionDirection = this->state.frameTransitionDirection; 151 | this->state.frameTransitionDirection = 1; 152 | } 153 | } 154 | void OLEDDisplayUi::previousFrame() { 155 | if (this->state.frameState != IN_TRANSITION) { 156 | this->state.manuelControll = true; 157 | this->state.frameState = IN_TRANSITION; 158 | this->state.ticksSinceLastStateSwitch = 0; 159 | this->lastTransitionDirection = this->state.frameTransitionDirection; 160 | this->state.frameTransitionDirection = -1; 161 | } 162 | } 163 | 164 | void OLEDDisplayUi::switchToFrame(uint8_t frame) { 165 | if (frame >= this->frameCount) return; 166 | this->state.ticksSinceLastStateSwitch = 0; 167 | if (frame == this->state.currentFrame) return; 168 | this->state.frameState = FIXED; 169 | this->state.currentFrame = frame; 170 | this->state.isIndicatorDrawen = true; 171 | } 172 | 173 | void OLEDDisplayUi::transitionToFrame(uint8_t frame) { 174 | if (frame >= this->frameCount) return; 175 | this->state.ticksSinceLastStateSwitch = 0; 176 | if (frame == this->state.currentFrame) return; 177 | this->nextFrameNumber = frame; 178 | this->lastTransitionDirection = this->state.frameTransitionDirection; 179 | this->state.manuelControll = true; 180 | this->state.frameState = IN_TRANSITION; 181 | this->state.frameTransitionDirection = frame < this->state.currentFrame ? -1 : 1; 182 | } 183 | 184 | 185 | // -/----- State information -----\- 186 | OLEDDisplayUiState* OLEDDisplayUi::getUiState(){ 187 | return &this->state; 188 | } 189 | 190 | 191 | int8_t OLEDDisplayUi::update(){ 192 | long frameStart = millis(); 193 | int8_t timeBudget = this->updateInterval - (frameStart - this->state.lastUpdate); 194 | if ( timeBudget <= 0) { 195 | // Implement frame skipping to ensure time budget is keept 196 | if (this->autoTransition && this->state.lastUpdate != 0) this->state.ticksSinceLastStateSwitch += ceil(-timeBudget / this->updateInterval); 197 | 198 | this->state.lastUpdate = frameStart; 199 | this->tick(); 200 | } 201 | return this->updateInterval - (millis() - frameStart); 202 | } 203 | 204 | 205 | void OLEDDisplayUi::tick() { 206 | this->state.ticksSinceLastStateSwitch++; 207 | 208 | switch (this->state.frameState) { 209 | case IN_TRANSITION: 210 | if (this->state.ticksSinceLastStateSwitch >= this->ticksPerTransition){ 211 | this->state.frameState = FIXED; 212 | this->state.currentFrame = getNextFrameNumber(); 213 | this->state.ticksSinceLastStateSwitch = 0; 214 | this->nextFrameNumber = -1; 215 | } 216 | break; 217 | case FIXED: 218 | // Revert manuelControll 219 | if (this->state.manuelControll) { 220 | this->state.frameTransitionDirection = this->lastTransitionDirection; 221 | this->state.manuelControll = false; 222 | } 223 | if (this->state.ticksSinceLastStateSwitch >= this->ticksPerFrame){ 224 | if (this->autoTransition){ 225 | this->state.frameState = IN_TRANSITION; 226 | } 227 | this->state.ticksSinceLastStateSwitch = 0; 228 | } 229 | break; 230 | } 231 | 232 | this->display->clear(); 233 | this->drawFrame(); 234 | if (shouldDrawIndicators) { 235 | this->drawIndicator(); 236 | } 237 | this->drawOverlays(); 238 | this->display->display(); 239 | } 240 | 241 | void OLEDDisplayUi::resetState() { 242 | this->state.lastUpdate = 0; 243 | this->state.ticksSinceLastStateSwitch = 0; 244 | this->state.frameState = FIXED; 245 | this->state.currentFrame = 0; 246 | this->state.isIndicatorDrawen = true; 247 | } 248 | 249 | void OLEDDisplayUi::drawFrame(){ 250 | switch (this->state.frameState){ 251 | case IN_TRANSITION: { 252 | float progress = (float) this->state.ticksSinceLastStateSwitch / (float) this->ticksPerTransition; 253 | int16_t x, y, x1, y1; 254 | switch(this->frameAnimationDirection){ 255 | case SLIDE_LEFT: 256 | x = -128 * progress; 257 | y = 0; 258 | x1 = x + 128; 259 | y1 = 0; 260 | break; 261 | case SLIDE_RIGHT: 262 | x = 128 * progress; 263 | y = 0; 264 | x1 = x - 128; 265 | y1 = 0; 266 | break; 267 | case SLIDE_UP: 268 | x = 0; 269 | y = -64 * progress; 270 | x1 = 0; 271 | y1 = y + 64; 272 | break; 273 | case SLIDE_DOWN: 274 | x = 0; 275 | y = 64 * progress; 276 | x1 = 0; 277 | y1 = y - 64; 278 | break; 279 | } 280 | 281 | // Invert animation if direction is reversed. 282 | int8_t dir = this->state.frameTransitionDirection >= 0 ? 1 : -1; 283 | x *= dir; y *= dir; x1 *= dir; y1 *= dir; 284 | 285 | bool drawenCurrentFrame; 286 | 287 | 288 | // Prope each frameFunction for the indicator Drawen state 289 | this->enableIndicator(); 290 | (this->frameFunctions[this->state.currentFrame])(this->display, &this->state, x, y); 291 | drawenCurrentFrame = this->state.isIndicatorDrawen; 292 | 293 | this->enableIndicator(); 294 | (this->frameFunctions[this->getNextFrameNumber()])(this->display, &this->state, x1, y1); 295 | 296 | // Build up the indicatorDrawState 297 | if (drawenCurrentFrame && !this->state.isIndicatorDrawen) { 298 | // Drawen now but not next 299 | this->indicatorDrawState = 2; 300 | } else if (!drawenCurrentFrame && this->state.isIndicatorDrawen) { 301 | // Not drawen now but next 302 | this->indicatorDrawState = 1; 303 | } else if (!drawenCurrentFrame && !this->state.isIndicatorDrawen) { 304 | // Not drawen in both frames 305 | this->indicatorDrawState = 3; 306 | } 307 | 308 | // If the indicator isn't draw in the current frame 309 | // reflect it in state.isIndicatorDrawen 310 | if (!drawenCurrentFrame) this->state.isIndicatorDrawen = false; 311 | 312 | break; 313 | } 314 | case FIXED: 315 | // Always assume that the indicator is drawn! 316 | // And set indicatorDrawState to "not known yet" 317 | this->indicatorDrawState = 0; 318 | this->enableIndicator(); 319 | (this->frameFunctions[this->state.currentFrame])(this->display, &this->state, 0, 0); 320 | break; 321 | } 322 | } 323 | 324 | void OLEDDisplayUi::drawIndicator() { 325 | 326 | // Only draw if the indicator is invisible 327 | // for both frames or 328 | // the indiactor is shown and we are IN_TRANSITION 329 | if (this->indicatorDrawState == 3 || (!this->state.isIndicatorDrawen && this->state.frameState != IN_TRANSITION)) { 330 | return; 331 | } 332 | 333 | uint8_t posOfHighlightFrame; 334 | float indicatorFadeProgress = 0; 335 | 336 | // if the indicator needs to be slided in we want to 337 | // highlight the next frame in the transition 338 | uint8_t frameToHighlight = this->indicatorDrawState == 1 ? this->getNextFrameNumber() : this->state.currentFrame; 339 | 340 | // Calculate the frame that needs to be highlighted 341 | // based on the Direction the indiactor is drawn 342 | switch (this->indicatorDirection){ 343 | case LEFT_RIGHT: 344 | posOfHighlightFrame = frameToHighlight; 345 | break; 346 | case RIGHT_LEFT: 347 | posOfHighlightFrame = this->frameCount - frameToHighlight; 348 | break; 349 | } 350 | 351 | switch (this->indicatorDrawState) { 352 | case 1: // Indicator was not drawn in this frame but will be in next 353 | // Slide IN 354 | indicatorFadeProgress = 1 - ((float) this->state.ticksSinceLastStateSwitch / (float) this->ticksPerTransition); 355 | break; 356 | case 2: // Indicator was drawn in this frame but not in next 357 | // Slide OUT 358 | indicatorFadeProgress = ((float) this->state.ticksSinceLastStateSwitch / (float) this->ticksPerTransition); 359 | break; 360 | } 361 | 362 | uint16_t frameStartPos = (12 * frameCount / 2); 363 | const char *image; 364 | uint16_t x,y; 365 | for (byte i = 0; i < this->frameCount; i++) { 366 | 367 | switch (this->indicatorPosition){ 368 | case TOP: 369 | y = 0 - (8 * indicatorFadeProgress); 370 | x = 64 - frameStartPos + 12 * i; 371 | break; 372 | case BOTTOM: 373 | y = 56 + (8 * indicatorFadeProgress); 374 | x = 64 - frameStartPos + 12 * i; 375 | break; 376 | case RIGHT: 377 | x = 120 + (8 * indicatorFadeProgress); 378 | y = 32 - frameStartPos + 2 + 12 * i; 379 | break; 380 | case LEFT: 381 | x = 0 - (8 * indicatorFadeProgress); 382 | y = 32 - frameStartPos + 2 + 12 * i; 383 | break; 384 | } 385 | 386 | if (posOfHighlightFrame == i) { 387 | image = this->activeSymbol; 388 | } else { 389 | image = this->inactiveSymbol; 390 | } 391 | 392 | this->display->drawFastImage(x, y, 8, 8, image); 393 | } 394 | } 395 | 396 | void OLEDDisplayUi::drawOverlays() { 397 | for (uint8_t i=0;ioverlayCount;i++){ 398 | (this->overlayFunctions[i])(this->display, &this->state); 399 | } 400 | } 401 | 402 | uint8_t OLEDDisplayUi::getNextFrameNumber(){ 403 | if (this->nextFrameNumber != -1) return this->nextFrameNumber; 404 | return (this->state.currentFrame + this->frameCount + this->state.frameTransitionDirection) % this->frameCount; 405 | } 406 | -------------------------------------------------------------------------------- /motion-controller.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "QmuTactile.h" 6 | #include "sbus.h" 7 | #include "math.h" 8 | #include "types.h" 9 | #include "SSD1306.h" 10 | #include "oled_display.h" 11 | #include "device_node.h" 12 | 13 | /* 14 | * Choose Trainer output type. Uncommend correcty line 15 | */ 16 | #define TRAINER_MODE_SBUS 17 | // #define TRAINER_MODE_PPM 18 | 19 | #define MPU6050_UPDATE_TASK_MS 25 20 | #define OUTPUT_UPDATE_TASK_MS 20 21 | #define SERIAL_TASK_MS 50 22 | #define SERIAL1_RX 25 23 | #define SERIAL1_TX 14 24 | #define I2C1_SDA_PIN 21 25 | #define I2C1_SCL_PIN 22 26 | 27 | #define I2C2_SDA_PIN 0 28 | #define I2C2_SCL_PIN 23 29 | 30 | TwoWire I2C1 = TwoWire(0); //OLED bus 31 | TwoWire I2C2 = TwoWire(1); //Gyro bus 32 | 33 | #define PIN_BUTTON_THUMB 4 34 | #define PIN_GPS_RX 2 35 | #define PIN_BUTTON_TRIGGER 15 36 | 37 | #define PIN_THUMB_JOYSTICK_X 13 38 | #define PIN_THUMB_JOYSTICK_Y 33 39 | #define PIN_THUMB_JOYSTICK_SW 32 40 | 41 | Adafruit_MPU6050 mpu; 42 | sensors_event_t acc, gyro, temp; 43 | 44 | uint32_t nextSerialTaskMs = 0; 45 | 46 | imuData_t imu; 47 | dataOutput_t output; 48 | thumb_joystick_t thumbJoystick; 49 | DeviceNode device; 50 | 51 | #ifdef TRAINER_MODE_SBUS 52 | #define SBUS_UPDATE_TASK_MS 15 53 | uint8_t sbusPacket[SBUS_PACKET_LENGTH] = {0}; 54 | HardwareSerial sbusSerial(1); 55 | uint32_t nextSbusTaskMs = 0; 56 | #endif 57 | 58 | #ifdef TRAINER_MODE_PPM 59 | 60 | #define PPM_FRAME_LENGTH 22500 61 | #define PPM_PULSE_LENGTH 300 62 | #define PPM_CHANNELS 8 63 | 64 | hw_timer_t * timer = NULL; 65 | portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; 66 | 67 | enum ppmState_e { 68 | PPM_STATE_IDLE, 69 | PPM_STATE_PULSE, 70 | PPM_STATE_FILL, 71 | PPM_STATE_SYNC 72 | }; 73 | 74 | void IRAM_ATTR onPpmTimer() { 75 | 76 | static uint8_t ppmState = PPM_STATE_IDLE; 77 | static uint8_t ppmChannel = 0; 78 | static uint8_t ppmOutput = LOW; 79 | static int usedFrameLength = 0; 80 | int currentChannelValue; 81 | 82 | portENTER_CRITICAL(&timerMux); 83 | 84 | if (ppmState == PPM_STATE_IDLE) { 85 | ppmState = PPM_STATE_PULSE; 86 | ppmChannel = 0; 87 | usedFrameLength = 0; 88 | } 89 | 90 | if (ppmState == PPM_STATE_PULSE) { 91 | ppmOutput = HIGH; 92 | usedFrameLength += PPM_PULSE_LENGTH; 93 | ppmState = PPM_STATE_FILL; 94 | 95 | timerAlarmWrite(timer, PPM_PULSE_LENGTH, true); 96 | } else if (ppmState == PPM_STATE_FILL) { 97 | ppmOutput = LOW; 98 | currentChannelValue = getRcChannel_wrapper(ppmChannel); 99 | 100 | ppmChannel++; 101 | ppmState = PPM_STATE_PULSE; 102 | 103 | if (ppmChannel > PPM_CHANNELS) { 104 | ppmChannel = 0; 105 | timerAlarmWrite(timer, PPM_FRAME_LENGTH - usedFrameLength, true); 106 | usedFrameLength = 0; 107 | } else { 108 | usedFrameLength += currentChannelValue - PPM_PULSE_LENGTH; 109 | timerAlarmWrite(timer, currentChannelValue - PPM_PULSE_LENGTH, true); 110 | } 111 | } 112 | portEXIT_CRITICAL(&timerMux); 113 | digitalWrite(SERIAL1_TX, ppmOutput); 114 | } 115 | 116 | #endif 117 | 118 | TaskHandle_t i2cResourceTask; 119 | TaskHandle_t ioTask; 120 | TaskHandle_t oledTask; 121 | 122 | SSD1306Wire display(0x3c, &I2C1); 123 | OledDisplay oledDisplay(&display); 124 | 125 | QmuTactile buttonTrigger(PIN_BUTTON_TRIGGER); 126 | QmuTactile buttonThumb(PIN_BUTTON_THUMB); 127 | QmuTactile buttonJoystick(PIN_THUMB_JOYSTICK_SW); 128 | 129 | void sensorCalibrate(struct gyroCalibration_t *cal, float sampleX, float sampleY, float sampleZ, const float dev) 130 | { 131 | if (cal->state == CALIBARTION_NOT_DONE) 132 | { 133 | cal->state = CALIBRATION_IN_PROGRESS; 134 | cal->sampleCount = 0; 135 | devClear(&cal->deviation[AXIS_X]); 136 | devClear(&cal->deviation[AXIS_Y]); 137 | devClear(&cal->deviation[AXIS_Z]); 138 | cal->accumulatedValue[AXIS_X] = 0; 139 | cal->accumulatedValue[AXIS_Y] = 0; 140 | cal->accumulatedValue[AXIS_Z] = 0; 141 | } 142 | if (cal->state == CALIBRATION_IN_PROGRESS) 143 | { 144 | cal->sampleCount++; 145 | devPush(&cal->deviation[AXIS_X], sampleX); 146 | devPush(&cal->deviation[AXIS_Y], sampleY); 147 | devPush(&cal->deviation[AXIS_Z], sampleZ); 148 | cal->accumulatedValue[AXIS_X] += sampleX; 149 | cal->accumulatedValue[AXIS_Y] += sampleY; 150 | cal->accumulatedValue[AXIS_Z] += sampleZ; 151 | 152 | if (cal->sampleCount == 40) 153 | { 154 | 155 | if ( 156 | devStandardDeviation(&cal->deviation[AXIS_X]) > dev || 157 | devStandardDeviation(&cal->deviation[AXIS_Y]) > dev || 158 | devStandardDeviation(&cal->deviation[AXIS_Z]) > dev) 159 | { 160 | cal->sampleCount = 0; 161 | devClear(&cal->deviation[AXIS_X]); 162 | devClear(&cal->deviation[AXIS_Y]); 163 | devClear(&cal->deviation[AXIS_Z]); 164 | cal->accumulatedValue[AXIS_X] = 0; 165 | cal->accumulatedValue[AXIS_Y] = 0; 166 | cal->accumulatedValue[AXIS_Z] = 0; 167 | } 168 | else 169 | { 170 | cal->zero[AXIS_X] = cal->accumulatedValue[AXIS_X] / cal->sampleCount; 171 | cal->zero[AXIS_Y] = cal->accumulatedValue[AXIS_Y] / cal->sampleCount; 172 | cal->zero[AXIS_Z] = cal->accumulatedValue[AXIS_Z] / cal->sampleCount; 173 | cal->state = CALIBRATION_DONE; 174 | } 175 | } 176 | } 177 | } 178 | 179 | void setup() 180 | { 181 | Serial.begin(115200); 182 | 183 | #ifdef TRAINER_MODE_PPM 184 | pinMode(SERIAL1_TX, OUTPUT); 185 | timer = timerBegin(0, 80, true); 186 | timerAttachInterrupt(timer, &onPpmTimer, true); 187 | timerAlarmWrite(timer, 12000, true); 188 | timerAlarmEnable(timer); 189 | #endif 190 | 191 | #ifdef TRAINER_MODE_SBUS 192 | sbusSerial.begin(100000, SERIAL_8E2, SERIAL1_RX, SERIAL1_TX, false, 100UL); 193 | #endif 194 | 195 | I2C1.begin(I2C1_SDA_PIN, I2C1_SCL_PIN, 50000); 196 | I2C2.begin(I2C2_SDA_PIN, I2C2_SCL_PIN, 100000); 197 | 198 | if (!mpu.begin(0x68, &I2C2, 0)) 199 | { 200 | Serial.println("MPU6050 init fail"); 201 | while (1) 202 | { 203 | delay(10); 204 | } 205 | } 206 | Serial.println("MPU6050 init success"); 207 | 208 | mpu.setAccelerometerRange(MPU6050_RANGE_2_G); 209 | mpu.setGyroRange(MPU6050_RANGE_250_DEG); 210 | mpu.setFilterBandwidth(MPU6050_BAND_5_HZ); 211 | 212 | delay(50); 213 | 214 | oledDisplay.init(); 215 | oledDisplay.setPage(OLED_PAGE_BEACON_STATUS); 216 | 217 | delay(50); 218 | 219 | buttonTrigger.start(); 220 | buttonThumb.start(); 221 | buttonJoystick.start(); 222 | 223 | xTaskCreatePinnedToCore( 224 | i2cResourceTaskHandler, /* Function to implement the task */ 225 | "imuTask", /* Name of the task */ 226 | 10000, /* Stack size in words */ 227 | NULL, /* Task input parameter */ 228 | 0, /* Priority of the task */ 229 | &i2cResourceTask, /* Task handle. */ 230 | 0); 231 | 232 | xTaskCreatePinnedToCore( 233 | ioTaskHandler, /* Function to implement the task */ 234 | "outputTask", /* Name of the task */ 235 | 10000, /* Stack size in words */ 236 | NULL, /* Task input parameter */ 237 | 0, /* Priority of the task */ 238 | &ioTask, /* Task handle. */ 239 | 0); 240 | 241 | xTaskCreatePinnedToCore( 242 | oledTaskHandler, /* Function to implement the task */ 243 | "oledTask", /* Name of the task */ 244 | 10000, /* Stack size in words */ 245 | NULL, /* Task input parameter */ 246 | 0, /* Priority of the task */ 247 | &oledTask, /* Task handle. */ 248 | 0); 249 | } 250 | 251 | //I do not get function pointers to object methods, no way... 252 | int getRcChannel_wrapper(uint8_t channel) 253 | { 254 | if (channel >= 0 && channel < SBUS_CHANNEL_COUNT) 255 | { 256 | return output.channels[channel]; 257 | } 258 | else 259 | { 260 | return DEFAULT_CHANNEL_VALUE; 261 | } 262 | } 263 | 264 | void processJoystickAxis(uint8_t axis, uint8_t pin) 265 | { 266 | thumbJoystick.raw[axis] = analogRead(pin); 267 | thumbJoystick.zeroed[axis] = thumbJoystick.calibration.zero[axis] - thumbJoystick.raw[axis]; 268 | 269 | if (thumbJoystick.calibration.state == CALIBRATION_DONE) 270 | { 271 | 272 | if (thumbJoystick.zeroed[axis] > thumbJoystick.max[axis]) 273 | { 274 | thumbJoystick.max[axis] = thumbJoystick.zeroed[axis]; 275 | } 276 | 277 | if (thumbJoystick.zeroed[axis] < thumbJoystick.min[axis]) 278 | { 279 | thumbJoystick.min[axis] = thumbJoystick.zeroed[axis]; 280 | } 281 | 282 | if (thumbJoystick.zeroed[axis] > 0) 283 | { 284 | thumbJoystick.position[axis] = fscalef(thumbJoystick.zeroed[axis], 0, thumbJoystick.max[axis], 0.0f, 1.0f); 285 | } 286 | else 287 | { 288 | thumbJoystick.position[axis] = fscalef(thumbJoystick.zeroed[axis], thumbJoystick.min[axis], 0, -1.0f, 0.0f); 289 | } 290 | } 291 | } 292 | 293 | void outputSubtask() 294 | { 295 | // Reset Z on each trigger press 296 | if (buttonTrigger.checkFlag(TACTILE_FLAG_EDGE_PRESSED)) 297 | { 298 | imu.angle.z = 0; 299 | } 300 | 301 | // Z updates from gyro happens only when THUMB button is pressed 302 | if (!buttonThumb.checkFlag(TACTILE_FLAG_PRESSED)) { 303 | imu.angle.z = 0; 304 | } 305 | 306 | for (uint8_t i = 0; i < SBUS_CHANNEL_COUNT; i++) 307 | { 308 | output.channels[i] = DEFAULT_CHANNEL_VALUE; 309 | } 310 | 311 | if (buttonTrigger.getState() == TACTILE_STATE_LONG_PRESS) 312 | { 313 | device.setActionEnabled(!device.getActionEnabled()); 314 | } 315 | 316 | if ( 317 | isnan(imu.angle.x) || 318 | isnan(imu.angle.y) 319 | ) { 320 | //TODO Data is broken, time to react or just do nothing 321 | device.setActionEnabled(false); 322 | } 323 | 324 | if (device.getActionEnabled()) 325 | { 326 | output.channels[ROLL] = DEFAULT_CHANNEL_VALUE + angleToRcChannel(imu.angle.x); 327 | output.channels[PITCH] = DEFAULT_CHANNEL_VALUE + angleToRcChannel(imu.angle.y); 328 | output.channels[YAW] = DEFAULT_CHANNEL_VALUE - angleToRcChannel(imu.angle.z) + joystickToRcChannel(thumbJoystick.position[AXIS_X]); 329 | output.channels[THROTTLE] = DEFAULT_CHANNEL_VALUE + joystickToRcChannel(thumbJoystick.position[AXIS_Y]); 330 | 331 | for (uint8_t i = 0; i < SBUS_CHANNEL_COUNT; i++) { 332 | output.channels[i] = constrain(output.channels[i], 1000, 2000); 333 | } 334 | 335 | } 336 | } 337 | 338 | void oledTaskHandler(void *pvParameters) 339 | { 340 | portTickType xLastWakeTime; 341 | const portTickType xPeriod = 200 / portTICK_PERIOD_MS; 342 | xLastWakeTime = xTaskGetTickCount(); 343 | 344 | /* 345 | * MPU6050 and OLED share the same I2C bus 346 | * To simplify the implementation and do not have to resolve resource conflicts, 347 | * both tasks are called in one thread pinned to the same core 348 | */ 349 | for (;;) 350 | { 351 | /* 352 | * Process OLED display 353 | */ 354 | oledDisplay.loop(); 355 | 356 | // Put task to sleep 357 | vTaskDelayUntil(&xLastWakeTime, xPeriod); //There is a conflict on a I2C due to too much load. Have to put to sleep for a period of time instead 358 | } 359 | 360 | vTaskDelete(NULL); 361 | } 362 | 363 | void ioTaskHandler(void *pvParameters) 364 | { 365 | portTickType xLastWakeTime; 366 | const portTickType xPeriod = OUTPUT_UPDATE_TASK_MS / portTICK_PERIOD_MS; 367 | xLastWakeTime = xTaskGetTickCount(); 368 | 369 | for (;;) 370 | { 371 | buttonTrigger.loop(); 372 | buttonJoystick.loop(); 373 | buttonThumb.loop(); 374 | 375 | /* 376 | * Joystick handling 377 | */ 378 | processJoystickAxis(AXIS_X, PIN_THUMB_JOYSTICK_X); 379 | processJoystickAxis(AXIS_Y, PIN_THUMB_JOYSTICK_Y); 380 | sensorCalibrate(&thumbJoystick.calibration, thumbJoystick.raw[AXIS_X], thumbJoystick.raw[AXIS_Y], 0, 3.0f); 381 | 382 | outputSubtask(); 383 | 384 | // Put task to sleep 385 | vTaskDelayUntil(&xLastWakeTime, xPeriod); 386 | } 387 | 388 | vTaskDelete(NULL); 389 | } 390 | 391 | void imuSubtask() 392 | { 393 | static uint32_t prevMicros = 0; 394 | float dT = (micros() - prevMicros) * 0.000001f; 395 | prevMicros = micros(); 396 | 397 | if (prevMicros > 0) 398 | { 399 | 400 | mpu.getEvent(&acc, &gyro, &temp); 401 | 402 | imu.accAngle.x = (atan(acc.acceleration.y / sqrt(pow(acc.acceleration.x, 2) + pow(acc.acceleration.z, 2))) * 180 / PI); 403 | imu.accAngle.y = (atan(-1 * acc.acceleration.x / sqrt(pow(acc.acceleration.y, 2) + pow(acc.acceleration.z, 2))) * 180 / PI); 404 | 405 | imu.gyro.x = (gyro.gyro.x * SENSORS_RADS_TO_DPS) - imu.gyroCalibration.zero[AXIS_X]; 406 | imu.gyro.y = (gyro.gyro.y * SENSORS_RADS_TO_DPS) - imu.gyroCalibration.zero[AXIS_Y]; 407 | imu.gyro.z = (gyro.gyro.z * SENSORS_RADS_TO_DPS) - imu.gyroCalibration.zero[AXIS_Z]; 408 | 409 | imu.gyroNormalized.x = imu.gyro.x * dT; 410 | imu.gyroNormalized.y = imu.gyro.y * dT; 411 | imu.gyroNormalized.z = imu.gyro.z * dT; 412 | 413 | imu.angle.x = (0.95 * (imu.angle.x + imu.gyroNormalized.x)) + (0.05 * imu.accAngle.x); 414 | imu.angle.y = (0.95 * (imu.angle.y + imu.gyroNormalized.y)) + (0.05 * imu.accAngle.y); 415 | imu.angle.z = imu.angle.z + imu.gyroNormalized.z; 416 | 417 | /* 418 | * Calibration Routine 419 | */ 420 | sensorCalibrate(&imu.gyroCalibration, imu.gyro.x, imu.gyro.y, imu.gyro.z, 3.0f); 421 | } 422 | } 423 | 424 | void i2cResourceTaskHandler(void *pvParameters) 425 | { 426 | portTickType xLastWakeTime; 427 | const portTickType xPeriod = MPU6050_UPDATE_TASK_MS / portTICK_PERIOD_MS; 428 | xLastWakeTime = xTaskGetTickCount(); 429 | 430 | /* 431 | * MPU6050 and OLED share the same I2C bus 432 | * To simplify the implementation and do not have to resolve resource conflicts, 433 | * both tasks are called in one thread pinned to the same core 434 | */ 435 | for (;;) 436 | { 437 | /* 438 | * Read gyro 439 | */ 440 | imuSubtask(); 441 | 442 | /* 443 | * Process OLED display 444 | */ 445 | // oledDisplay.loop(); 446 | 447 | // Put task to sleep 448 | vTaskDelayUntil(&xLastWakeTime, xPeriod); 449 | } 450 | 451 | vTaskDelete(NULL); 452 | } 453 | 454 | int angleToRcChannel(float angle) 455 | { 456 | const float value = fconstrainf(applyDeadband(angle, 5.0f), -45.0f, 45.0f); //5 deg deadband 457 | return (int)fscalef(value, -45.0f, 45.0f, -500, 500); 458 | } 459 | 460 | int joystickToRcChannel(float angle) 461 | { 462 | const float value = fconstrainf(applyDeadband(angle, 0.02f), -1.0f, 1.0f); 463 | return (int)fscalef(value, -1.0f, 1.0f, -200, 200); 464 | } 465 | 466 | void loop() 467 | { 468 | /* 469 | * Send Trainer data in SBUS stream 470 | */ 471 | #ifdef TRAINER_MODE_SBUS 472 | if (millis() > nextSbusTaskMs) 473 | { 474 | sbusPreparePacket(sbusPacket, false, false, getRcChannel_wrapper); 475 | sbusSerial.write(sbusPacket, SBUS_PACKET_LENGTH); 476 | 477 | nextSbusTaskMs = millis() + SBUS_UPDATE_TASK_MS; 478 | } 479 | #endif 480 | 481 | if (millis() > nextSerialTaskMs) 482 | { 483 | // Serial.println(String(imu.angle.z, 2) + " " + String(imu.gyro.z, 2)); 484 | // Serial.println(String(imu.angle.x, 1) + " " + String(imu.angle.y, 1) + " " + String(imu.gyro.z, 1)); 485 | // Serial.println("Zero: " + String(imu.gyroCalibration.zero[AXIS_X], 2) + " " + String(imu.gyroCalibration.zero[AXIS_Y], 2) + " " + String(imu.gyroCalibration.zero[AXIS_Z], 2)); 486 | // Serial.println("Gyro: " + String(imu.gyro.x, 2) + " " + String(imu.gyro.y, 2) + " " + String(imu.gyro.z, 2)); 487 | // Serial.println(String(devStandardDeviation(&imu.gyroCalDevX), 1) + " " + String(devStandardDeviation(&imu.gyroCalDevY), 1) + " " + String(devStandardDeviation(&imu.gyroCalDevZ), 1)); 488 | // Serial.println(String(output.channels[ROLL]) + " " + String(output.channels[PITCH]) + " " + String(output.channels[THROTTLE]) + " " + String(output.channels[YAW])); 489 | // Serial.println("Zero: " + String(thumbJoystick.calibration.zero[AXIS_X], 2) + " " + String(thumbJoystick.calibration.zero[AXIS_Y], 2)); 490 | // Serial.println(String(thumbJoystick.raw[AXIS_X]) + " " + String(thumbJoystick.raw[AXIS_Y]) + " " + digitalRead(PIN_THUMB_JOYSTICK_SW)); 491 | // Serial.println(String(thumbJoystick.zeroed[AXIS_X]) + " " + String(thumbJoystick.max[AXIS_X]) + " " + String(thumbJoystick.min[AXIS_X])); 492 | // Serial.println(String(thumbJoystick.position[AXIS_X], 2) + " " + String(thumbJoystick.position[AXIS_Y], 2)); 493 | 494 | nextSerialTaskMs = millis() + SERIAL_TASK_MS; 495 | } 496 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Mozilla Public License Version 2.0 2 | ================================== 3 | 4 | 1. Definitions 5 | -------------- 6 | 7 | 1.1. "Contributor" 8 | means each individual or legal entity that creates, contributes to 9 | the creation of, or owns Covered Software. 10 | 11 | 1.2. "Contributor Version" 12 | means the combination of the Contributions of others (if any) used 13 | by a Contributor and that particular Contributor's Contribution. 14 | 15 | 1.3. "Contribution" 16 | means Covered Software of a particular Contributor. 17 | 18 | 1.4. "Covered Software" 19 | means Source Code Form to which the initial Contributor has attached 20 | the notice in Exhibit A, the Executable Form of such Source Code 21 | Form, and Modifications of such Source Code Form, in each case 22 | including portions thereof. 23 | 24 | 1.5. "Incompatible With Secondary Licenses" 25 | means 26 | 27 | (a) that the initial Contributor has attached the notice described 28 | in Exhibit B to the Covered Software; or 29 | 30 | (b) that the Covered Software was made available under the terms of 31 | version 1.1 or earlier of the License, but not also under the 32 | terms of a Secondary License. 33 | 34 | 1.6. "Executable Form" 35 | means any form of the work other than Source Code Form. 36 | 37 | 1.7. "Larger Work" 38 | means a work that combines Covered Software with other material, in 39 | a separate file or files, that is not Covered Software. 40 | 41 | 1.8. "License" 42 | means this document. 43 | 44 | 1.9. "Licensable" 45 | means having the right to grant, to the maximum extent possible, 46 | whether at the time of the initial grant or subsequently, any and 47 | all of the rights conveyed by this License. 48 | 49 | 1.10. "Modifications" 50 | means any of the following: 51 | 52 | (a) any file in Source Code Form that results from an addition to, 53 | deletion from, or modification of the contents of Covered 54 | Software; or 55 | 56 | (b) any new file in Source Code Form that contains any Covered 57 | Software. 58 | 59 | 1.11. "Patent Claims" of a Contributor 60 | means any patent claim(s), including without limitation, method, 61 | process, and apparatus claims, in any patent Licensable by such 62 | Contributor that would be infringed, but for the grant of the 63 | License, by the making, using, selling, offering for sale, having 64 | made, import, or transfer of either its Contributions or its 65 | Contributor Version. 66 | 67 | 1.12. "Secondary License" 68 | means either the GNU General Public License, Version 2.0, the GNU 69 | Lesser General Public License, Version 2.1, the GNU Affero General 70 | Public License, Version 3.0, or any later versions of those 71 | licenses. 72 | 73 | 1.13. "Source Code Form" 74 | means the form of the work preferred for making modifications. 75 | 76 | 1.14. "You" (or "Your") 77 | means an individual or a legal entity exercising rights under this 78 | License. For legal entities, "You" includes any entity that 79 | controls, is controlled by, or is under common control with You. For 80 | purposes of this definition, "control" means (a) the power, direct 81 | or indirect, to cause the direction or management of such entity, 82 | whether by contract or otherwise, or (b) ownership of more than 83 | fifty percent (50%) of the outstanding shares or beneficial 84 | ownership of such entity. 85 | 86 | 2. License Grants and Conditions 87 | -------------------------------- 88 | 89 | 2.1. Grants 90 | 91 | Each Contributor hereby grants You a world-wide, royalty-free, 92 | non-exclusive license: 93 | 94 | (a) under intellectual property rights (other than patent or trademark) 95 | Licensable by such Contributor to use, reproduce, make available, 96 | modify, display, perform, distribute, and otherwise exploit its 97 | Contributions, either on an unmodified basis, with Modifications, or 98 | as part of a Larger Work; and 99 | 100 | (b) under Patent Claims of such Contributor to make, use, sell, offer 101 | for sale, have made, import, and otherwise transfer either its 102 | Contributions or its Contributor Version. 103 | 104 | 2.2. Effective Date 105 | 106 | The licenses granted in Section 2.1 with respect to any Contribution 107 | become effective for each Contribution on the date the Contributor first 108 | distributes such Contribution. 109 | 110 | 2.3. Limitations on Grant Scope 111 | 112 | The licenses granted in this Section 2 are the only rights granted under 113 | this License. No additional rights or licenses will be implied from the 114 | distribution or licensing of Covered Software under this License. 115 | Notwithstanding Section 2.1(b) above, no patent license is granted by a 116 | Contributor: 117 | 118 | (a) for any code that a Contributor has removed from Covered Software; 119 | or 120 | 121 | (b) for infringements caused by: (i) Your and any other third party's 122 | modifications of Covered Software, or (ii) the combination of its 123 | Contributions with other software (except as part of its Contributor 124 | Version); or 125 | 126 | (c) under Patent Claims infringed by Covered Software in the absence of 127 | its Contributions. 128 | 129 | This License does not grant any rights in the trademarks, service marks, 130 | or logos of any Contributor (except as may be necessary to comply with 131 | the notice requirements in Section 3.4). 132 | 133 | 2.4. Subsequent Licenses 134 | 135 | No Contributor makes additional grants as a result of Your choice to 136 | distribute the Covered Software under a subsequent version of this 137 | License (see Section 10.2) or under the terms of a Secondary License (if 138 | permitted under the terms of Section 3.3). 139 | 140 | 2.5. Representation 141 | 142 | Each Contributor represents that the Contributor believes its 143 | Contributions are its original creation(s) or it has sufficient rights 144 | to grant the rights to its Contributions conveyed by this License. 145 | 146 | 2.6. Fair Use 147 | 148 | This License is not intended to limit any rights You have under 149 | applicable copyright doctrines of fair use, fair dealing, or other 150 | equivalents. 151 | 152 | 2.7. Conditions 153 | 154 | Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted 155 | in Section 2.1. 156 | 157 | 3. Responsibilities 158 | ------------------- 159 | 160 | 3.1. Distribution of Source Form 161 | 162 | All distribution of Covered Software in Source Code Form, including any 163 | Modifications that You create or to which You contribute, must be under 164 | the terms of this License. You must inform recipients that the Source 165 | Code Form of the Covered Software is governed by the terms of this 166 | License, and how they can obtain a copy of this License. You may not 167 | attempt to alter or restrict the recipients' rights in the Source Code 168 | Form. 169 | 170 | 3.2. Distribution of Executable Form 171 | 172 | If You distribute Covered Software in Executable Form then: 173 | 174 | (a) such Covered Software must also be made available in Source Code 175 | Form, as described in Section 3.1, and You must inform recipients of 176 | the Executable Form how they can obtain a copy of such Source Code 177 | Form by reasonable means in a timely manner, at a charge no more 178 | than the cost of distribution to the recipient; and 179 | 180 | (b) You may distribute such Executable Form under the terms of this 181 | License, or sublicense it under different terms, provided that the 182 | license for the Executable Form does not attempt to limit or alter 183 | the recipients' rights in the Source Code Form under this License. 184 | 185 | 3.3. Distribution of a Larger Work 186 | 187 | You may create and distribute a Larger Work under terms of Your choice, 188 | provided that You also comply with the requirements of this License for 189 | the Covered Software. If the Larger Work is a combination of Covered 190 | Software with a work governed by one or more Secondary Licenses, and the 191 | Covered Software is not Incompatible With Secondary Licenses, this 192 | License permits You to additionally distribute such Covered Software 193 | under the terms of such Secondary License(s), so that the recipient of 194 | the Larger Work may, at their option, further distribute the Covered 195 | Software under the terms of either this License or such Secondary 196 | License(s). 197 | 198 | 3.4. Notices 199 | 200 | You may not remove or alter the substance of any license notices 201 | (including copyright notices, patent notices, disclaimers of warranty, 202 | or limitations of liability) contained within the Source Code Form of 203 | the Covered Software, except that You may alter any license notices to 204 | the extent required to remedy known factual inaccuracies. 205 | 206 | 3.5. Application of Additional Terms 207 | 208 | You may choose to offer, and to charge a fee for, warranty, support, 209 | indemnity or liability obligations to one or more recipients of Covered 210 | Software. However, You may do so only on Your own behalf, and not on 211 | behalf of any Contributor. You must make it absolutely clear that any 212 | such warranty, support, indemnity, or liability obligation is offered by 213 | You alone, and You hereby agree to indemnify every Contributor for any 214 | liability incurred by such Contributor as a result of warranty, support, 215 | indemnity or liability terms You offer. You may include additional 216 | disclaimers of warranty and limitations of liability specific to any 217 | jurisdiction. 218 | 219 | 4. Inability to Comply Due to Statute or Regulation 220 | --------------------------------------------------- 221 | 222 | If it is impossible for You to comply with any of the terms of this 223 | License with respect to some or all of the Covered Software due to 224 | statute, judicial order, or regulation then You must: (a) comply with 225 | the terms of this License to the maximum extent possible; and (b) 226 | describe the limitations and the code they affect. Such description must 227 | be placed in a text file included with all distributions of the Covered 228 | Software under this License. Except to the extent prohibited by statute 229 | or regulation, such description must be sufficiently detailed for a 230 | recipient of ordinary skill to be able to understand it. 231 | 232 | 5. Termination 233 | -------------- 234 | 235 | 5.1. The rights granted under this License will terminate automatically 236 | if You fail to comply with any of its terms. However, if You become 237 | compliant, then the rights granted under this License from a particular 238 | Contributor are reinstated (a) provisionally, unless and until such 239 | Contributor explicitly and finally terminates Your grants, and (b) on an 240 | ongoing basis, if such Contributor fails to notify You of the 241 | non-compliance by some reasonable means prior to 60 days after You have 242 | come back into compliance. Moreover, Your grants from a particular 243 | Contributor are reinstated on an ongoing basis if such Contributor 244 | notifies You of the non-compliance by some reasonable means, this is the 245 | first time You have received notice of non-compliance with this License 246 | from such Contributor, and You become compliant prior to 30 days after 247 | Your receipt of the notice. 248 | 249 | 5.2. If You initiate litigation against any entity by asserting a patent 250 | infringement claim (excluding declaratory judgment actions, 251 | counter-claims, and cross-claims) alleging that a Contributor Version 252 | directly or indirectly infringes any patent, then the rights granted to 253 | You by any and all Contributors for the Covered Software under Section 254 | 2.1 of this License shall terminate. 255 | 256 | 5.3. In the event of termination under Sections 5.1 or 5.2 above, all 257 | end user license agreements (excluding distributors and resellers) which 258 | have been validly granted by You or Your distributors under this License 259 | prior to termination shall survive termination. 260 | 261 | ************************************************************************ 262 | * * 263 | * 6. Disclaimer of Warranty * 264 | * ------------------------- * 265 | * * 266 | * Covered Software is provided under this License on an "as is" * 267 | * basis, without warranty of any kind, either expressed, implied, or * 268 | * statutory, including, without limitation, warranties that the * 269 | * Covered Software is free of defects, merchantable, fit for a * 270 | * particular purpose or non-infringing. The entire risk as to the * 271 | * quality and performance of the Covered Software is with You. * 272 | * Should any Covered Software prove defective in any respect, You * 273 | * (not any Contributor) assume the cost of any necessary servicing, * 274 | * repair, or correction. This disclaimer of warranty constitutes an * 275 | * essential part of this License. No use of any Covered Software is * 276 | * authorized under this License except under this disclaimer. * 277 | * * 278 | ************************************************************************ 279 | 280 | ************************************************************************ 281 | * * 282 | * 7. Limitation of Liability * 283 | * -------------------------- * 284 | * * 285 | * Under no circumstances and under no legal theory, whether tort * 286 | * (including negligence), contract, or otherwise, shall any * 287 | * Contributor, or anyone who distributes Covered Software as * 288 | * permitted above, be liable to You for any direct, indirect, * 289 | * special, incidental, or consequential damages of any character * 290 | * including, without limitation, damages for lost profits, loss of * 291 | * goodwill, work stoppage, computer failure or malfunction, or any * 292 | * and all other commercial damages or losses, even if such party * 293 | * shall have been informed of the possibility of such damages. This * 294 | * limitation of liability shall not apply to liability for death or * 295 | * personal injury resulting from such party's negligence to the * 296 | * extent applicable law prohibits such limitation. Some * 297 | * jurisdictions do not allow the exclusion or limitation of * 298 | * incidental or consequential damages, so this exclusion and * 299 | * limitation may not apply to You. * 300 | * * 301 | ************************************************************************ 302 | 303 | 8. Litigation 304 | ------------- 305 | 306 | Any litigation relating to this License may be brought only in the 307 | courts of a jurisdiction where the defendant maintains its principal 308 | place of business and such litigation shall be governed by laws of that 309 | jurisdiction, without reference to its conflict-of-law provisions. 310 | Nothing in this Section shall prevent a party's ability to bring 311 | cross-claims or counter-claims. 312 | 313 | 9. Miscellaneous 314 | ---------------- 315 | 316 | This License represents the complete agreement concerning the subject 317 | matter hereof. If any provision of this License is held to be 318 | unenforceable, such provision shall be reformed only to the extent 319 | necessary to make it enforceable. Any law or regulation which provides 320 | that the language of a contract shall be construed against the drafter 321 | shall not be used to construe this License against a Contributor. 322 | 323 | 10. Versions of the License 324 | --------------------------- 325 | 326 | 10.1. New Versions 327 | 328 | Mozilla Foundation is the license steward. Except as provided in Section 329 | 10.3, no one other than the license steward has the right to modify or 330 | publish new versions of this License. Each version will be given a 331 | distinguishing version number. 332 | 333 | 10.2. Effect of New Versions 334 | 335 | You may distribute the Covered Software under the terms of the version 336 | of the License under which You originally received the Covered Software, 337 | or under the terms of any subsequent version published by the license 338 | steward. 339 | 340 | 10.3. Modified Versions 341 | 342 | If you create software not governed by this License, and you want to 343 | create a new license for such software, you may create and use a 344 | modified version of this License if you rename the license and remove 345 | any references to the name of the license steward (except to note that 346 | such modified license differs from this License). 347 | 348 | 10.4. Distributing Source Code Form that is Incompatible With Secondary 349 | Licenses 350 | 351 | If You choose to distribute Source Code Form that is Incompatible With 352 | Secondary Licenses under the terms of this version of the License, the 353 | notice described in Exhibit B of this License must be attached. 354 | 355 | Exhibit A - Source Code Form License Notice 356 | ------------------------------------------- 357 | 358 | This Source Code Form is subject to the terms of the Mozilla Public 359 | License, v. 2.0. If a copy of the MPL was not distributed with this 360 | file, You can obtain one at http://mozilla.org/MPL/2.0/. 361 | 362 | If it is not possible or desirable to put the notice in a particular 363 | file, then You may include the notice in a location (such as a LICENSE 364 | file in a relevant directory) where a recipient would be likely to look 365 | for such a notice. 366 | 367 | You may add additional accurate notices of copyright ownership. 368 | 369 | Exhibit B - "Incompatible With Secondary Licenses" Notice 370 | --------------------------------------------------------- 371 | 372 | This Source Code Form is "Incompatible With Secondary Licenses", as 373 | defined by the Mozilla Public License, v. 2.0. 374 | -------------------------------------------------------------------------------- /OLEDDisplay.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016 by Daniel Eichhorn 5 | * Copyright (c) 2016 by Fabrice Weinberg 6 | * 7 | * Permission is hereby granted, free of charge, to any person obtaining a copy 8 | * of this software and associated documentation files (the "Software"), to deal 9 | * in the Software without restriction, including without limitation the rights 10 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | * copies of the Software, and to permit persons to whom the Software is 12 | * furnished to do so, subject to the following conditions: 13 | * 14 | * The above copyright notice and this permission notice shall be included in all 15 | * copies or substantial portions of the Software. 16 | * 17 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | * 25 | * Credits for parts of this code go to Mike Rankin. Thank you so much for sharing! 26 | */ 27 | 28 | #include "OLEDDisplay.h" 29 | 30 | bool OLEDDisplay::init() { 31 | if (!this->connect()) { 32 | DEBUG_OLEDDISPLAY("[OLEDDISPLAY][init] Can't establish connection to display\n"); 33 | return false; 34 | } 35 | this->buffer = (uint8_t*) malloc(sizeof(uint8_t) * DISPLAY_BUFFER_SIZE); 36 | if(!this->buffer) { 37 | DEBUG_OLEDDISPLAY("[OLEDDISPLAY][init] Not enough memory to create display\n"); 38 | return false; 39 | } 40 | 41 | #ifdef OLEDDISPLAY_DOUBLE_BUFFER 42 | this->buffer_back = (uint8_t*) malloc(sizeof(uint8_t) * DISPLAY_BUFFER_SIZE); 43 | if(!this->buffer_back) { 44 | DEBUG_OLEDDISPLAY("[OLEDDISPLAY][init] Not enough memory to create back buffer\n"); 45 | free(this->buffer); 46 | return false; 47 | } 48 | #endif 49 | 50 | sendInitCommands(); 51 | resetDisplay(); 52 | 53 | return true; 54 | } 55 | 56 | void OLEDDisplay::end() { 57 | if (this->buffer) free(this->buffer); 58 | #ifdef OLEDDISPLAY_DOUBLE_BUFFER 59 | if (this->buffer_back) free(this->buffer_back); 60 | #endif 61 | } 62 | 63 | void OLEDDisplay::resetDisplay(void) { 64 | clear(); 65 | #ifdef OLEDDISPLAY_DOUBLE_BUFFER 66 | memset(buffer_back, 1, DISPLAY_BUFFER_SIZE); 67 | #endif 68 | display(); 69 | } 70 | 71 | void OLEDDisplay::setColor(OLEDDISPLAY_COLOR color) { 72 | this->color = color; 73 | } 74 | 75 | void OLEDDisplay::setPixel(int16_t x, int16_t y) { 76 | if (x >= 0 && x < 128 && y >= 0 && y < 64) { 77 | switch (color) { 78 | case WHITE: buffer[x + (y / 8) * DISPLAY_WIDTH] |= (1 << (y & 7)); break; 79 | case BLACK: buffer[x + (y / 8) * DISPLAY_WIDTH] &= ~(1 << (y & 7)); break; 80 | case INVERSE: buffer[x + (y / 8) * DISPLAY_WIDTH] ^= (1 << (y & 7)); break; 81 | } 82 | } 83 | } 84 | 85 | // Bresenham's algorithm - thx wikipedia and Adafruit_GFX 86 | void OLEDDisplay::drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1) { 87 | int16_t steep = abs(y1 - y0) > abs(x1 - x0); 88 | if (steep) { 89 | _swap_int16_t(x0, y0); 90 | _swap_int16_t(x1, y1); 91 | } 92 | 93 | if (x0 > x1) { 94 | _swap_int16_t(x0, x1); 95 | _swap_int16_t(y0, y1); 96 | } 97 | 98 | int16_t dx, dy; 99 | dx = x1 - x0; 100 | dy = abs(y1 - y0); 101 | 102 | int16_t err = dx / 2; 103 | int16_t ystep; 104 | 105 | if (y0 < y1) { 106 | ystep = 1; 107 | } else { 108 | ystep = -1; 109 | } 110 | 111 | for (; x0<=x1; x0++) { 112 | if (steep) { 113 | setPixel(y0, x0); 114 | } else { 115 | setPixel(x0, y0); 116 | } 117 | err -= dy; 118 | if (err < 0) { 119 | y0 += ystep; 120 | err += dx; 121 | } 122 | } 123 | } 124 | 125 | void OLEDDisplay::drawRect(int16_t x, int16_t y, int16_t width, int16_t height) { 126 | drawHorizontalLine(x, y, width); 127 | drawVerticalLine(x, y, height); 128 | drawVerticalLine(x + width - 1, y, height); 129 | drawHorizontalLine(x, y + height - 1, width); 130 | } 131 | 132 | void OLEDDisplay::fillRect(int16_t xMove, int16_t yMove, int16_t width, int16_t height) { 133 | for (int16_t x = xMove; x < xMove + width; x++) { 134 | drawVerticalLine(x, yMove, height); 135 | } 136 | } 137 | 138 | void OLEDDisplay::drawCircle(int16_t x0, int16_t y0, int16_t radius) { 139 | int16_t x = 0, y = radius; 140 | int16_t dp = 1 - radius; 141 | do { 142 | if (dp < 0) 143 | dp = dp + 2 * (++x) + 3; 144 | else 145 | dp = dp + 2 * (++x) - 2 * (--y) + 5; 146 | 147 | setPixel(x0 + x, y0 + y); //For the 8 octants 148 | setPixel(x0 - x, y0 + y); 149 | setPixel(x0 + x, y0 - y); 150 | setPixel(x0 - x, y0 - y); 151 | setPixel(x0 + y, y0 + x); 152 | setPixel(x0 - y, y0 + x); 153 | setPixel(x0 + y, y0 - x); 154 | setPixel(x0 - y, y0 - x); 155 | 156 | } while (x < y); 157 | 158 | setPixel(x0 + radius, y0); 159 | setPixel(x0, y0 + radius); 160 | setPixel(x0 - radius, y0); 161 | setPixel(x0, y0 - radius); 162 | } 163 | 164 | void OLEDDisplay::drawCircleQuads(int16_t x0, int16_t y0, int16_t radius, uint8_t quads) { 165 | int16_t x = 0, y = radius; 166 | int16_t dp = 1 - radius; 167 | while (x < y) { 168 | if (dp < 0) 169 | dp = dp + 2 * (++x) + 3; 170 | else 171 | dp = dp + 2 * (++x) - 2 * (--y) + 5; 172 | if (quads & 0x1) { 173 | setPixel(x0 + x, y0 - y); 174 | setPixel(x0 + y, y0 - x); 175 | } 176 | if (quads & 0x2) { 177 | setPixel(x0 - y, y0 - x); 178 | setPixel(x0 - x, y0 - y); 179 | } 180 | if (quads & 0x4) { 181 | setPixel(x0 - y, y0 + x); 182 | setPixel(x0 - x, y0 + y); 183 | } 184 | if (quads & 0x8) { 185 | setPixel(x0 + x, y0 + y); 186 | setPixel(x0 + y, y0 + x); 187 | } 188 | } 189 | if (quads & 0x1 && quads & 0x8) { 190 | setPixel(x0 + radius, y0); 191 | } 192 | if (quads & 0x4 && quads & 0x8) { 193 | setPixel(x0, y0 + radius); 194 | } 195 | if (quads & 0x2 && quads & 0x4) { 196 | setPixel(x0 - radius, y0); 197 | } 198 | if (quads & 0x1 && quads & 0x2) { 199 | setPixel(x0, y0 - radius); 200 | } 201 | } 202 | 203 | 204 | void OLEDDisplay::fillCircle(int16_t x0, int16_t y0, int16_t radius) { 205 | int16_t x = 0, y = radius; 206 | int16_t dp = 1 - radius; 207 | do { 208 | if (dp < 0) 209 | dp = dp + 2 * (++x) + 3; 210 | else 211 | dp = dp + 2 * (++x) - 2 * (--y) + 5; 212 | 213 | drawHorizontalLine(x0 - x, y0 - y, 2*x); 214 | drawHorizontalLine(x0 - x, y0 + y, 2*x); 215 | drawHorizontalLine(x0 - y, y0 - x, 2*y); 216 | drawHorizontalLine(x0 - y, y0 + x, 2*y); 217 | 218 | 219 | } while (x < y); 220 | drawHorizontalLine(x0 - radius, y0, 2 * radius); 221 | 222 | } 223 | 224 | void OLEDDisplay::drawHorizontalLine(int16_t x, int16_t y, int16_t length) { 225 | if (y < 0 || y >= DISPLAY_HEIGHT) { return; } 226 | 227 | if (x < 0) { 228 | length += x; 229 | x = 0; 230 | } 231 | 232 | if ( (x + length) > DISPLAY_WIDTH) { 233 | length = (DISPLAY_WIDTH - x); 234 | } 235 | 236 | if (length <= 0) { return; } 237 | 238 | uint8_t * bufferPtr = buffer; 239 | bufferPtr += (y >> 3) * DISPLAY_WIDTH; 240 | bufferPtr += x; 241 | 242 | uint8_t drawBit = 1 << (y & 7); 243 | 244 | switch (color) { 245 | case WHITE: while (length--) { 246 | *bufferPtr++ |= drawBit; 247 | }; break; 248 | case BLACK: drawBit = ~drawBit; while (length--) { 249 | *bufferPtr++ &= drawBit; 250 | }; break; 251 | case INVERSE: while (length--) { 252 | *bufferPtr++ ^= drawBit; 253 | }; break; 254 | } 255 | } 256 | 257 | void OLEDDisplay::drawVerticalLine(int16_t x, int16_t y, int16_t length) { 258 | if (x < 0 || x >= DISPLAY_WIDTH) return; 259 | 260 | if (y < 0) { 261 | length += y; 262 | y = 0; 263 | } 264 | 265 | if ( (y + length) > DISPLAY_HEIGHT) { 266 | length = (DISPLAY_HEIGHT - y); 267 | } 268 | 269 | if (length <= 0) return; 270 | 271 | 272 | uint8_t yOffset = y & 7; 273 | uint8_t drawBit; 274 | uint8_t *bufferPtr = buffer; 275 | 276 | bufferPtr += (y >> 3) * DISPLAY_WIDTH; 277 | bufferPtr += x; 278 | 279 | if (yOffset) { 280 | yOffset = 8 - yOffset; 281 | drawBit = ~(0xFF >> (yOffset)); 282 | 283 | if (length < yOffset) { 284 | drawBit &= (0xFF >> (yOffset - length)); 285 | } 286 | 287 | switch (color) { 288 | case WHITE: *bufferPtr |= drawBit; break; 289 | case BLACK: *bufferPtr &= ~drawBit; break; 290 | case INVERSE: *bufferPtr ^= drawBit; break; 291 | } 292 | 293 | if (length < yOffset) return; 294 | 295 | length -= yOffset; 296 | bufferPtr += DISPLAY_WIDTH; 297 | } 298 | 299 | if (length >= 8) { 300 | switch (color) { 301 | case WHITE: 302 | case BLACK: 303 | drawBit = (color == WHITE) ? 0xFF : 0x00; 304 | do { 305 | *bufferPtr = drawBit; 306 | bufferPtr += DISPLAY_WIDTH; 307 | length -= 8; 308 | } while (length >= 8); 309 | break; 310 | case INVERSE: 311 | do { 312 | *bufferPtr = ~(*bufferPtr); 313 | bufferPtr += DISPLAY_WIDTH; 314 | length -= 8; 315 | } while (length >= 8); 316 | break; 317 | } 318 | } 319 | 320 | if (length > 0) { 321 | drawBit = (1 << (length & 7)) - 1; 322 | switch (color) { 323 | case WHITE: *bufferPtr |= drawBit; break; 324 | case BLACK: *bufferPtr &= ~drawBit; break; 325 | case INVERSE: *bufferPtr ^= drawBit; break; 326 | } 327 | } 328 | } 329 | 330 | void OLEDDisplay::drawProgressBar(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint8_t progress) { 331 | uint16_t radius = height / 2; 332 | uint16_t xRadius = x + radius; 333 | uint16_t yRadius = y + radius; 334 | uint16_t doubleRadius = 2 * radius; 335 | uint16_t innerRadius = radius - 2; 336 | 337 | setColor(WHITE); 338 | drawCircleQuads(xRadius, yRadius, radius, 0b00000110); 339 | drawHorizontalLine(xRadius, y, width - doubleRadius + 1); 340 | drawHorizontalLine(xRadius, y + height, width - doubleRadius + 1); 341 | drawCircleQuads(x + width - radius, yRadius, radius, 0b00001001); 342 | 343 | uint16_t maxProgressWidth = (width - doubleRadius - 1) * progress / 100; 344 | 345 | fillCircle(xRadius, yRadius, innerRadius); 346 | fillRect(xRadius + 1, y + 2, maxProgressWidth, height - 3); 347 | fillCircle(xRadius + maxProgressWidth, yRadius, innerRadius); 348 | } 349 | 350 | void OLEDDisplay::drawFastImage(int16_t xMove, int16_t yMove, int16_t width, int16_t height, const char *image) { 351 | drawInternal(xMove, yMove, width, height, image, 0, 0); 352 | } 353 | 354 | void OLEDDisplay::drawXbm(int16_t xMove, int16_t yMove, int16_t width, int16_t height, const char *xbm) { 355 | int16_t widthInXbm = (width + 7) / 8; 356 | uint8_t data; 357 | 358 | for(int16_t y = 0; y < height; y++) { 359 | for(int16_t x = 0; x < width; x++ ) { 360 | if (x & 7) { 361 | data >>= 1; // Move a bit 362 | } else { // Read new data every 8 bit 363 | data = pgm_read_byte(xbm + (x / 8) + y * widthInXbm); 364 | } 365 | // if there is a bit draw it 366 | if (data & 0x01) { 367 | setPixel(xMove + x, yMove + y); 368 | } 369 | } 370 | } 371 | } 372 | 373 | void OLEDDisplay::drawStringInternal(int16_t xMove, int16_t yMove, char* text, uint16_t textLength, uint16_t textWidth) { 374 | uint8_t textHeight = pgm_read_byte(fontData + HEIGHT_POS); 375 | uint8_t firstChar = pgm_read_byte(fontData + FIRST_CHAR_POS); 376 | uint16_t sizeOfJumpTable = pgm_read_byte(fontData + CHAR_NUM_POS) * JUMPTABLE_BYTES; 377 | 378 | uint8_t cursorX = 0; 379 | uint8_t cursorY = 0; 380 | 381 | switch (textAlignment) { 382 | case TEXT_ALIGN_CENTER_BOTH: 383 | yMove -= textHeight >> 1; 384 | // Fallthrough 385 | case TEXT_ALIGN_CENTER: 386 | xMove -= textWidth >> 1; // divide by 2 387 | break; 388 | case TEXT_ALIGN_RIGHT: 389 | xMove -= textWidth; 390 | break; 391 | } 392 | 393 | // Don't draw anything if it is not on the screen. 394 | if (xMove + textWidth < 0 || xMove > DISPLAY_WIDTH ) {return;} 395 | if (yMove + textHeight < 0 || yMove > DISPLAY_HEIGHT) {return;} 396 | 397 | for (uint16_t j = 0; j < textLength; j++) { 398 | int16_t xPos = xMove + cursorX; 399 | int16_t yPos = yMove + cursorY; 400 | 401 | byte code = text[j]; 402 | if (code >= firstChar) { 403 | byte charCode = code - firstChar; 404 | 405 | // 4 Bytes per char code 406 | byte msbJumpToChar = pgm_read_byte( fontData + JUMPTABLE_START + charCode * JUMPTABLE_BYTES ); // MSB \ JumpAddress 407 | byte lsbJumpToChar = pgm_read_byte( fontData + JUMPTABLE_START + charCode * JUMPTABLE_BYTES + JUMPTABLE_LSB); // LSB / 408 | byte charByteSize = pgm_read_byte( fontData + JUMPTABLE_START + charCode * JUMPTABLE_BYTES + JUMPTABLE_SIZE); // Size 409 | byte currentCharWidth = pgm_read_byte( fontData + JUMPTABLE_START + charCode * JUMPTABLE_BYTES + JUMPTABLE_WIDTH); // Width 410 | 411 | // Test if the char is drawable 412 | if (!(msbJumpToChar == 255 && lsbJumpToChar == 255)) { 413 | // Get the position of the char data 414 | uint16_t charDataPosition = JUMPTABLE_START + sizeOfJumpTable + ((msbJumpToChar << 8) + lsbJumpToChar); 415 | drawInternal(xPos, yPos, currentCharWidth, textHeight, fontData, charDataPosition, charByteSize); 416 | } 417 | 418 | cursorX += currentCharWidth; 419 | } 420 | } 421 | } 422 | 423 | 424 | void OLEDDisplay::drawString(int16_t xMove, int16_t yMove, String strUser) { 425 | uint16_t lineHeight = pgm_read_byte(fontData + HEIGHT_POS); 426 | 427 | // char* text must be freed! 428 | char* text = utf8ascii(strUser); 429 | 430 | uint16_t yOffset = 0; 431 | // If the string should be centered vertically too 432 | // we need to now how heigh the string is. 433 | if (textAlignment == TEXT_ALIGN_CENTER_BOTH) { 434 | uint16_t lb = 0; 435 | // Find number of linebreaks in text 436 | for (uint16_t i=0;text[i] != 0; i++) { 437 | lb += (text[i] == 10); 438 | } 439 | // Calculate center 440 | yOffset = (lb * lineHeight) / 2; 441 | } 442 | 443 | uint16_t line = 0; 444 | char* textPart = strtok(text,"\n"); 445 | while (textPart != NULL) { 446 | uint16_t length = strlen(textPart); 447 | drawStringInternal(xMove, yMove - yOffset + (line++) * lineHeight, textPart, length, getStringWidth(textPart, length)); 448 | textPart = strtok(NULL, "\n"); 449 | } 450 | free(text); 451 | } 452 | 453 | void OLEDDisplay::drawStringMaxWidth(int16_t xMove, int16_t yMove, uint16_t maxLineWidth, String strUser) { 454 | uint16_t firstChar = pgm_read_byte(fontData + FIRST_CHAR_POS); 455 | uint16_t lineHeight = pgm_read_byte(fontData + HEIGHT_POS); 456 | 457 | char* text = utf8ascii(strUser); 458 | 459 | uint16_t length = strlen(text); 460 | uint16_t lastDrawnPos = 0; 461 | uint16_t lineNumber = 0; 462 | uint16_t strWidth = 0; 463 | 464 | uint16_t preferredBreakpoint = 0; 465 | uint16_t widthAtBreakpoint = 0; 466 | 467 | for (uint16_t i = 0; i < length; i++) { 468 | strWidth += pgm_read_byte(fontData + JUMPTABLE_START + (text[i] - firstChar) * JUMPTABLE_BYTES + JUMPTABLE_WIDTH); 469 | 470 | // Always try to break on a space or dash 471 | if (text[i] == ' ' || text[i]== '-') { 472 | preferredBreakpoint = i; 473 | widthAtBreakpoint = strWidth; 474 | } 475 | 476 | if (strWidth >= maxLineWidth) { 477 | if (preferredBreakpoint == 0) { 478 | preferredBreakpoint = i; 479 | widthAtBreakpoint = strWidth; 480 | } 481 | drawStringInternal(xMove, yMove + (lineNumber++) * lineHeight , &text[lastDrawnPos], preferredBreakpoint - lastDrawnPos, widthAtBreakpoint); 482 | lastDrawnPos = preferredBreakpoint + 1; 483 | // It is possible that we did not draw all letters to i so we need 484 | // to account for the width of the chars from `i - preferredBreakpoint` 485 | // by calculating the width we did not draw yet. 486 | strWidth = strWidth - widthAtBreakpoint; 487 | preferredBreakpoint = 0; 488 | } 489 | } 490 | 491 | // Draw last part if needed 492 | if (lastDrawnPos < length) { 493 | drawStringInternal(xMove, yMove + lineNumber * lineHeight , &text[lastDrawnPos], length - lastDrawnPos, getStringWidth(&text[lastDrawnPos], length - lastDrawnPos)); 494 | } 495 | 496 | free(text); 497 | } 498 | 499 | uint16_t OLEDDisplay::getStringWidth(const char* text, uint16_t length) { 500 | uint16_t firstChar = pgm_read_byte(fontData + FIRST_CHAR_POS); 501 | 502 | uint16_t stringWidth = 0; 503 | uint16_t maxWidth = 0; 504 | 505 | while (length--) { 506 | stringWidth += pgm_read_byte(fontData + JUMPTABLE_START + (text[length] - firstChar) * JUMPTABLE_BYTES + JUMPTABLE_WIDTH); 507 | if (text[length] == 10) { 508 | maxWidth = max(maxWidth, stringWidth); 509 | stringWidth = 0; 510 | } 511 | } 512 | 513 | return max(maxWidth, stringWidth); 514 | } 515 | 516 | uint16_t OLEDDisplay::getStringWidth(String strUser) { 517 | char* text = utf8ascii(strUser); 518 | uint16_t length = strlen(text); 519 | uint16_t width = getStringWidth(text, length); 520 | free(text); 521 | return width; 522 | } 523 | 524 | void OLEDDisplay::setTextAlignment(OLEDDISPLAY_TEXT_ALIGNMENT textAlignment) { 525 | this->textAlignment = textAlignment; 526 | } 527 | 528 | void OLEDDisplay::setFont(const char *fontData) { 529 | this->fontData = fontData; 530 | } 531 | 532 | void OLEDDisplay::displayOn(void) { 533 | sendCommand(DISPLAYON); 534 | } 535 | 536 | void OLEDDisplay::displayOff(void) { 537 | sendCommand(DISPLAYOFF); 538 | } 539 | 540 | void OLEDDisplay::invertDisplay(void) { 541 | sendCommand(INVERTDISPLAY); 542 | } 543 | 544 | void OLEDDisplay::normalDisplay(void) { 545 | sendCommand(NORMALDISPLAY); 546 | } 547 | 548 | void OLEDDisplay::setContrast(char contrast) { 549 | sendCommand(SETCONTRAST); 550 | sendCommand(contrast); 551 | } 552 | 553 | void OLEDDisplay::flipScreenVertically() { 554 | sendCommand(SEGREMAP | 0x01); 555 | sendCommand(COMSCANDEC); //Rotate screen 180 Deg 556 | } 557 | 558 | void OLEDDisplay::clear(void) { 559 | memset(buffer, 0, DISPLAY_BUFFER_SIZE); 560 | } 561 | 562 | void OLEDDisplay::drawLogBuffer(uint16_t xMove, uint16_t yMove) { 563 | uint16_t lineHeight = pgm_read_byte(fontData + HEIGHT_POS); 564 | // Always align left 565 | setTextAlignment(TEXT_ALIGN_LEFT); 566 | 567 | // State values 568 | uint16_t length = 0; 569 | uint16_t line = 0; 570 | uint16_t lastPos = 0; 571 | 572 | for (uint16_t i=0;ilogBufferFilled;i++){ 573 | // Everytime we have a \n print 574 | if (this->logBuffer[i] == 10) { 575 | length++; 576 | // Draw string on line `line` from lastPos to length 577 | // Passing 0 as the lenght because we are in TEXT_ALIGN_LEFT 578 | drawStringInternal(xMove, yMove + (line++) * lineHeight, &this->logBuffer[lastPos], length, 0); 579 | // Remember last pos 580 | lastPos = i; 581 | // Reset length 582 | length = 0; 583 | } else { 584 | // Count chars until next linebreak 585 | length++; 586 | } 587 | } 588 | // Draw the remaining string 589 | if (length > 0) { 590 | drawStringInternal(xMove, yMove + line * lineHeight, &this->logBuffer[lastPos], length, 0); 591 | } 592 | } 593 | 594 | bool OLEDDisplay::setLogBuffer(uint16_t lines, uint16_t chars){ 595 | if (logBuffer != NULL) free(logBuffer); 596 | uint16_t size = lines * chars; 597 | if (size > 0) { 598 | this->logBufferLine = 0; // Lines printed 599 | this->logBufferMaxLines = lines; // Lines max printable 600 | this->logBufferSize = size; // Total number of characters the buffer can hold 601 | this->logBuffer = (char *) malloc(size * sizeof(uint8_t)); 602 | if(!this->logBuffer) { 603 | DEBUG_OLEDDISPLAY("[OLEDDISPLAY][setLogBuffer] Not enough memory to create log buffer\n"); 604 | return false; 605 | } 606 | } 607 | return true; 608 | } 609 | 610 | size_t OLEDDisplay::write(uint8_t c) { 611 | if (this->logBufferSize > 0) { 612 | // Don't waste space on \r\n line endings, dropping \r 613 | if (c == 13) return 1; 614 | 615 | bool maxLineNotReached = this->logBufferLine < this->logBufferMaxLines; 616 | bool bufferNotFull = this->logBufferFilled < this->logBufferSize; 617 | 618 | // Can we write to the buffer? 619 | if (bufferNotFull && maxLineNotReached) { 620 | this->logBuffer[logBufferFilled] = utf8ascii(c); 621 | this->logBufferFilled++; 622 | // Keep track of lines written 623 | if (c == 10) this->logBufferLine++; 624 | } else { 625 | // Max line number is reached 626 | if (!maxLineNotReached) this->logBufferLine--; 627 | 628 | // Find the end of the first line 629 | uint16_t firstLineEnd = 0; 630 | for (uint16_t i=0;ilogBufferFilled;i++) { 631 | if (this->logBuffer[i] == 10){ 632 | // Include last char too 633 | firstLineEnd = i + 1; 634 | break; 635 | } 636 | } 637 | // If there was a line ending 638 | if (firstLineEnd > 0) { 639 | // Calculate the new logBufferFilled value 640 | this->logBufferFilled = logBufferFilled - firstLineEnd; 641 | // Now we move the lines infront of the buffer 642 | memcpy(this->logBuffer, &this->logBuffer[firstLineEnd], logBufferFilled); 643 | } else { 644 | // Let's reuse the buffer if it was full 645 | if (!bufferNotFull) { 646 | this->logBufferFilled = 0; 647 | }// else { 648 | // Nothing to do here 649 | //} 650 | } 651 | write(c); 652 | } 653 | } 654 | // We are always writing all uint8_t to the buffer 655 | return 1; 656 | } 657 | 658 | size_t OLEDDisplay::write(const char* str) { 659 | if (str == NULL) return 0; 660 | size_t length = strlen(str); 661 | for (size_t i = 0; i < length; i++) { 662 | write(str[i]); 663 | } 664 | return length; 665 | } 666 | 667 | // Private functions 668 | void OLEDDisplay::sendInitCommands(void) { 669 | sendCommand(DISPLAYOFF); 670 | sendCommand(SETDISPLAYCLOCKDIV); 671 | sendCommand(0xF0); // Increase speed of the display max ~96Hz 672 | sendCommand(SETMULTIPLEX); 673 | sendCommand(0x3F); 674 | sendCommand(SETDISPLAYOFFSET); 675 | sendCommand(0x00); 676 | sendCommand(SETSTARTLINE); 677 | sendCommand(CHARGEPUMP); 678 | sendCommand(0x14); 679 | sendCommand(MEMORYMODE); 680 | sendCommand(0x00); 681 | sendCommand(SEGREMAP); 682 | sendCommand(COMSCANINC); 683 | sendCommand(SETCOMPINS); 684 | sendCommand(0x12); 685 | sendCommand(SETCONTRAST); 686 | sendCommand(0xCF); 687 | sendCommand(SETPRECHARGE); 688 | sendCommand(0xF1); 689 | sendCommand(DISPLAYALLON_RESUME); 690 | sendCommand(NORMALDISPLAY); 691 | sendCommand(0x2e); // stop scroll 692 | sendCommand(DISPLAYON); 693 | } 694 | 695 | void inline OLEDDisplay::drawInternal(int16_t xMove, int16_t yMove, int16_t width, int16_t height, const char *data, uint16_t offset, uint16_t bytesInData) { 696 | if (width < 0 || height < 0) return; 697 | if (yMove + height < 0 || yMove > DISPLAY_HEIGHT) return; 698 | if (xMove + width < 0 || xMove > DISPLAY_WIDTH) return; 699 | 700 | uint8_t rasterHeight = 1 + ((height - 1) >> 3); // fast ceil(height / 8.0) 701 | int8_t yOffset = yMove & 7; 702 | 703 | bytesInData = bytesInData == 0 ? width * rasterHeight : bytesInData; 704 | 705 | int16_t initYMove = yMove; 706 | int8_t initYOffset = yOffset; 707 | 708 | 709 | for (uint16_t i = 0; i < bytesInData; i++) { 710 | 711 | // Reset if next horizontal drawing phase is started. 712 | if ( i % rasterHeight == 0) { 713 | yMove = initYMove; 714 | yOffset = initYOffset; 715 | } 716 | 717 | byte currentByte = pgm_read_byte(data + offset + i); 718 | 719 | int16_t xPos = xMove + (i / rasterHeight); 720 | int16_t yPos = ((yMove >> 3) + (i % rasterHeight)) * DISPLAY_WIDTH; 721 | 722 | int16_t yScreenPos = yMove + yOffset; 723 | int16_t dataPos = xPos + yPos; 724 | 725 | if (dataPos >= 0 && dataPos < DISPLAY_BUFFER_SIZE && 726 | xPos >= 0 && xPos < DISPLAY_WIDTH ) { 727 | 728 | if (yOffset >= 0) { 729 | switch (this->color) { 730 | case WHITE: buffer[dataPos] |= currentByte << yOffset; break; 731 | case BLACK: buffer[dataPos] &= ~(currentByte << yOffset); break; 732 | case INVERSE: buffer[dataPos] ^= currentByte << yOffset; break; 733 | } 734 | if (dataPos < (DISPLAY_BUFFER_SIZE - DISPLAY_WIDTH)) { 735 | switch (this->color) { 736 | case WHITE: buffer[dataPos + DISPLAY_WIDTH] |= currentByte >> (8 - yOffset); break; 737 | case BLACK: buffer[dataPos + DISPLAY_WIDTH] &= ~(currentByte >> (8 - yOffset)); break; 738 | case INVERSE: buffer[dataPos + DISPLAY_WIDTH] ^= currentByte >> (8 - yOffset); break; 739 | } 740 | } 741 | } else { 742 | // Make new offset position 743 | yOffset = -yOffset; 744 | 745 | switch (this->color) { 746 | case WHITE: buffer[dataPos] |= currentByte >> yOffset; break; 747 | case BLACK: buffer[dataPos] &= ~(currentByte >> yOffset); break; 748 | case INVERSE: buffer[dataPos] ^= currentByte >> yOffset; break; 749 | } 750 | 751 | // Prepare for next iteration by moving one block up 752 | yMove -= 8; 753 | 754 | // and setting the new yOffset 755 | yOffset = 8 - yOffset; 756 | } 757 | 758 | } 759 | } 760 | } 761 | 762 | // Code form http://playground.arduino.cc/Main/Utf8ascii 763 | uint8_t OLEDDisplay::utf8ascii(byte ascii) { 764 | static uint8_t LASTCHAR; 765 | 766 | if ( ascii < 128 ) { // Standard ASCII-set 0..0x7F handling 767 | LASTCHAR = 0; 768 | return ascii; 769 | } 770 | 771 | uint8_t last = LASTCHAR; // get last char 772 | LASTCHAR = ascii; 773 | 774 | switch (last) { // conversion depnding on first UTF8-character 775 | case 0xC2: return (ascii); break; 776 | case 0xC3: return (ascii | 0xC0); break; 777 | case 0x82: if (ascii == 0xAC) return (0x80); // special case Euro-symbol 778 | } 779 | 780 | return 0; // otherwise: return zero, if character has to be ignored 781 | } 782 | 783 | // You need to free the char! 784 | char* OLEDDisplay::utf8ascii(String str) { 785 | uint16_t k = 0; 786 | uint16_t length = str.length() + 1; 787 | 788 | // Copy the string into a char array 789 | char* s = (char*) malloc(length * sizeof(char)); 790 | if(!s) { 791 | DEBUG_OLEDDISPLAY("[OLEDDISPLAY][utf8ascii] Can't allocate another char array. Drop support for UTF-8.\n"); 792 | return (char*) str.c_str(); 793 | } 794 | str.toCharArray(s, length); 795 | 796 | length--; 797 | 798 | for (uint16_t i=0; i < length; i++) { 799 | char c = utf8ascii(s[i]); 800 | if (c!=0) { 801 | s[k++]=c; 802 | } 803 | } 804 | 805 | s[k]=0; 806 | 807 | // This will leak 's' be sure to free it in the calling function. 808 | return s; 809 | } 810 | --------------------------------------------------------------------------------