diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 654cfb38..6a72c065 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -3,7 +3,10 @@ name: CI on: pull_request: branches: - - '*' + - '**' + push: + branches: + - '**' jobs: unit-tests: diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 00000000..a7271db8 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,101 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project Overview + +OpenAstroTracker-Firmware is embedded C++ firmware for the OpenAstroTracker automated astronomy mount. It supports multiple hardware platforms (AVR ATmega2560 and ESP32) with various stepper motor drivers, display types, and accessories. + +## Build System + +PlatformIO is the build system. The project uses the Arduino framework. + +### Common Commands + +```shell +# Build for a specific board environment +pio run -e ramps +pio run -e esp32 +pio run -e mksgenlv21 +pio run -e mksgenlv2 +pio run -e mksgenlv1 +pio run -e oaeboardv1 + +# Upload firmware +pio run -e ramps -t upload + +# Run unit tests (native platform) +pio test -e native + +# Run matrix build (tests many configuration combinations across boards) +python matrix_build.py -b ramps # single board +python matrix_build.py # all boards + +# Format code (clang-format v12 required, enforced by CI) +bash -c 'shopt -s nullglob globstar;GLOBIGNORE=./src/libs/TimerInterrupt/*; for i in ./{.,src/**,unit_tests,boards/**}/*.{c,cpp,h,hpp}; do clang-format -i $i; done' + +# Debug (ATmega2560 only, uses avr-stub) +pio run -e ramps -t clean && piodebuggdb -e ramps + +# Generate Meade command Wiki docs +python scripts/MeadeCommandParser.py +``` + +### Build Notes + +- `-Werror` is enabled: all warnings are errors +- Uses ETL (Embedded Template Library) in non-STL mode (`-D ETL_NO_STL`), not the C++ standard library +- Optimization: `-O2` (production), `-Og` (debug) +- Monitor baud rate: 19200 +- `src/libs/TimerInterrupt/` is excluded from formatting checks +- CI runs on every PR: build all boards, clang-format check, unit tests + +## Architecture + +### Configuration Hierarchy (evaluated in this order) + +1. **`Configuration_local.hpp`** - User hardware config (not tracked by git). Generate at https://config.openastrotech.com/ +2. **`Configuration_local_.hpp`** - Board-specific local config variant +3. **`Configuration.hpp`** - Default values for anything not set in local config +4. **`Configuration_adv.hpp`** - Advanced settings (motor steps, speed, microstepping, TMC driver params) +5. **`Constants.hpp`** - System constants (board IDs, feature flag values). **Do not modify for user configuration.** + +### Core Modules + +- **`Mount` (src/Mount.hpp/cpp)** - Central controller. Manages stepper motors (RA, DEC, AZ, ALT, Focus), coordinate calculations, slewing, tracking, parking, and guiding. This is by far the largest module (~150KB source). +- **`MeadeCommandProcessor` (src/MeadeCommandProcessor.hpp/cpp)** - Serial command interface implementing the Meade LX200 protocol. Handles all external communication. +- **`EPROMStore` (src/EPROMStore.hpp/cpp)** - Platform-agnostic non-volatile storage abstraction (coordinates, calibration, motor config). Uses magic markers `0xCE`/`0xCF` for validation. +- **`b_setup.hpp`** - Arduino `setup()` implementation and hardware initialization. +- **`a_inits.hpp`** - Global variable initialization. + +### Entry Point + +`OpenAstroTracker-Firmware.ino` is the Arduino sketch entry point. It delegates to `src/a_inits.hpp` (globals) and `src/b_setup.hpp` (setup/loop). + +### Stepper Control + +Two modes depending on platform: + +- **AVR (NEW_STEPPER_LIB)**: Interrupt-driven via Timer 3 (RA) and Timer 4 (DEC) at 2 kHz. Uses `InterruptAccelStepper` library. Configured via `StepperConfiguration.hpp`. +- **ESP32**: FreeRTOS task on Core 0 at 1 kHz (`stepperControlTask`). Core 1 runs `loop()` for serial/UI. + +Both call `Mount::interruptLoop()` for step generation. + +### Board Pin Definitions + +Board-specific pin mappings live in `boards//pins_.h`. Each board has its own directory. + +### Display & UI + +Optional LCD menu system (`LcdMenu`, `LcdButtons`) supporting multiple display types: direct LCD keypad, I2C LCD (MCP23008/MCP23017), and I2C SSD1306 OLED with joystick. An optional separate 128x64 info display shows real-time status. + +## Key Conventions + +- Primary branch: `develop` +- PRs must pass CI (build all boards + clang-format + unit tests) +- PR comments are resolved by OAT developers, not PR authors +- clang-format v12 is the enforced formatter (column limit: 140, Allman-style braces, 4-space indent, no tabs) +- Pointer alignment: right (`int *ptr`) +- Supported driver types: `DRIVER_TYPE_A4988_GENERIC`, `DRIVER_TYPE_TMC2209_STANDALONE`, `DRIVER_TYPE_TMC2209_UART` +- Supported stepper types: `STEPPER_TYPE_NONE`, `STEPPER_TYPE_ENABLED` +- Debug logging uses bit-flag levels (INFO, SERIAL, WIFI, MOUNT, MEADE, STEPPERS, EEPROM, GYRO, GPS, FOCUS, etc.) diff --git a/src/DayTime.cpp b/src/DayTime.cpp index 05a24e9a..1197f256 100644 --- a/src/DayTime.cpp +++ b/src/DayTime.cpp @@ -238,11 +238,11 @@ const char *DayTime::ToString() const *p++ = '0' + (secs / 10); } - *p++ = '0' + (secs % 10); - *p++ = ' '; - *p++ = '('; - strcpy(p, String(this->getTotalHours(), 5).c_str()); - strcat(p, ")"); + *p++ = '0' + (secs % 10); + size_t used = p - achBuf; + size_t remaining = sizeof(achBuf) - used; + String floatStr = String(this->getTotalHours(), 5); + snprintf(p, remaining, " (%s)", floatStr.c_str()); return achBuf; } void DayTime::printTwoDigits(char *achDegs, int num) const diff --git a/src/Declination.cpp b/src/Declination.cpp index e19ac817..3970f07f 100644 --- a/src/Declination.cpp +++ b/src/Declination.cpp @@ -84,14 +84,12 @@ const char *Declination::ToString() const { ToDisplayString('*', ':'); - char *p = achBufDeg + strlen(achBufDeg); - - *p++ = ' '; - *p++ = '('; - strcpy(p, String(inNorthernHemisphere ? 90 - fabsf(getTotalHours()) : -90 + fabsf(getTotalHours()), 4).c_str()); - strcat(p, ", "); - strcat(p, String(getTotalHours(), 4).c_str()); - strcat(p, ")"); + size_t used = strlen(achBufDeg); + size_t remaining = sizeof(achBufDeg) - used; + float displayVal = inNorthernHemisphere ? 90 - fabsf(getTotalHours()) : -90 + fabsf(getTotalHours()); + String valStr = String(displayVal, 4); + String hoursStr = String(getTotalHours(), 4); + snprintf(achBufDeg + used, remaining, " (%s, %s)", valStr.c_str(), hoursStr.c_str()); return achBufDeg; } diff --git a/src/EPROMStore.hpp b/src/EPROMStore.hpp index 506e987c..11acd189 100644 --- a/src/EPROMStore.hpp +++ b/src/EPROMStore.hpp @@ -168,7 +168,7 @@ class EEPROMStore LATITUDE_ADDR = 12, _LATITUDE_ADDR_1 = 13, // Int16 LONGITUDE_ADDR = 14, - _LONGITUDE_ADDR_1 = 13, // Int16 + _LONGITUDE_ADDR_1 = 15, // Int16 LCD_BRIGHTNESS_ADDR = 16, // Uint8 PITCH_OFFSET_ADDR = 17, _PITCH_OFFSET_ADDR_1 = 18, // Uint16 diff --git a/src/EndSwitches.hpp b/src/EndSwitches.hpp index 9af0d98c..b979ed64 100644 --- a/src/EndSwitches.hpp +++ b/src/EndSwitches.hpp @@ -27,10 +27,10 @@ enum EndSwitchState class EndSwitch { private: - EndSwitchState _state; + volatile EndSwitchState _state; Mount *_pMount; StepperAxis _axis; - long _posWhenTriggered; + volatile long _posWhenTriggered; int _activeState; int _inactiveState; int _dir; diff --git a/src/Gyro.cpp b/src/Gyro.cpp index b1b9ee2a..13a3edc9 100644 --- a/src/Gyro.cpp +++ b/src/Gyro.cpp @@ -16,6 +16,10 @@ POP_NO_WARNINGS * */ bool Gyro::isPresent(false); +float Gyro::_pitchEma(0); +float Gyro::_rollEma(0); +bool Gyro::_initialized(false); +unsigned long Gyro::_lastSampleTime(0); void Gyro::startup() /* Starts up the MPU-6050 device. @@ -48,12 +52,17 @@ void Gyro::startup() Wire.write(0); // Disable sleep, 8 MHz clock Wire.endTransmission(true); - // Execute 1 byte write to MPU6050_REG_PWR_MGMT_1 + // Execute 1 byte write to MPU6050_REG_CONFIG Wire.beginTransmission(MPU6050_I2C_ADDR); Wire.write(MPU6050_REG_CONFIG); Wire.write(6); // 5Hz bandwidth (lowest) for smoothing Wire.endTransmission(true); + _initialized = false; + _pitchEma = 0; + _rollEma = 0; + _lastSampleTime = 0; + LOG(DEBUG_INFO, "[GYRO]:: Started"); } @@ -66,45 +75,67 @@ void Gyro::shutdown() // Nothing to do } +void Gyro::collectSample() +/* Reads one accelerometer sample and updates the EMA state. +*/ +{ + // Execute 6 byte read from MPU6050_REG_ACCEL_XOUT_H + Wire.beginTransmission(MPU6050_I2C_ADDR); + Wire.write(MPU6050_REG_ACCEL_XOUT_H); + Wire.endTransmission(false); + Wire.requestFrom(MPU6050_I2C_ADDR, 6, 1); // Read 6 registers total, each axis value is stored in 2 registers + int16_t AcX = Wire.read() << 8 | Wire.read(); // X-axis value + int16_t AcY = Wire.read() << 8 | Wire.read(); // Y-axis value + int16_t AcZ = Wire.read() << 8 | Wire.read(); // Z-axis value + + // Calculating the Pitch angle (rotation around Y-axis) + float pitch = atan2f(-1.0f * AcX, sqrtf((float) AcY * AcY + (float) AcZ * AcZ)) * 180.0f / static_cast(PI); + // Calculating the Roll angle (rotation around X-axis) + float roll = atan2f(-1.0f * AcY, sqrtf((float) AcX * AcX + (float) AcZ * AcZ)) * 180.0f / static_cast(PI); + + if (!_initialized) + { + _pitchEma = pitch; + _rollEma = roll; + _initialized = true; + } + else + { + _pitchEma = ((1.0f - EMA_ALPHA) * _pitchEma) + (EMA_ALPHA * pitch); + _rollEma = ((1.0f - EMA_ALPHA) * _rollEma) + (EMA_ALPHA * roll); + } +} + angle_t Gyro::getCurrentAngles() /* Returns roll & tilt angles from MPU-6050 device in angle_t object in degrees. - If MPU-6050 is not found then returns {0,0}. + Non-blocking: collects one sample per call if at least SAMPLE_INTERVAL ms + have elapsed since the last sample. Returns the EMA-filtered angles. */ { - const int windowSize = 16; - // Read the accelerometer data - struct angle_t result; - result.pitchAngle = 0; - result.rollAngle = 0; + angle_t result; + if (!isPresent) - return result; // Gyro is not available + return result; - for (int i = 0; i < windowSize; i++) + unsigned long now = millis(); + if (now - _lastSampleTime >= SAMPLE_INTERVAL) { - // Execute 6 byte read from MPU6050_REG_WHO_AM_I - Wire.beginTransmission(MPU6050_I2C_ADDR); - Wire.write(MPU6050_REG_ACCEL_XOUT_H); - Wire.endTransmission(false); - Wire.requestFrom(MPU6050_I2C_ADDR, 6, 1); // Read 6 registers total, each axis value is stored in 2 registers - int16_t AcX = Wire.read() << 8 | Wire.read(); // X-axis value - int16_t AcY = Wire.read() << 8 | Wire.read(); // Y-axis value - int16_t AcZ = Wire.read() << 8 | Wire.read(); // Z-axis value - - // Calculating the Pitch angle (rotation around Y-axis) - result.pitchAngle += ((atanf(-1 * AcX / sqrtf(powf(AcY, 2) + powf(AcZ, 2))) * 180.0f / static_cast(PI)) * 2.0f) / 2.0f; - // Calculating the Roll angle (rotation around X-axis) - result.rollAngle += ((atanf(-1 * AcY / sqrtf(powf(AcX, 2) + powf(AcZ, 2))) * 180.0f / static_cast(PI)) * 2.0f) / 2.0f; - - delay(10); // Decorrelate measurements + _lastSampleTime = now; + collectSample(); } - result.pitchAngle /= windowSize; - result.rollAngle /= windowSize; + if (!_initialized) + return result; + + result.pitchAngle = _pitchEma; + result.rollAngle = _rollEma; + #if GYRO_AXIS_SWAP == 1 float temp = result.pitchAngle; result.pitchAngle = result.rollAngle; result.rollAngle = temp; #endif + return result; } diff --git a/src/Gyro.hpp b/src/Gyro.hpp index 563a21f5..76abc1eb 100644 --- a/src/Gyro.hpp +++ b/src/Gyro.hpp @@ -15,6 +15,8 @@ class Gyro static float getCurrentTemperature(); private: + static void collectSample(); + // MPU6050 constants enum { @@ -28,6 +30,13 @@ class Gyro MPU6050_REG_WHO_AM_I = 0x75 }; - static bool isPresent; // True if gyro correctly detected on startup + static constexpr float EMA_ALPHA = 0.1f; // Smoothing factor for exponential moving average + static const unsigned long SAMPLE_INTERVAL = 5; // ms between samples + + static bool isPresent; + static float _pitchEma; + static float _rollEma; + static bool _initialized; + static unsigned long _lastSampleTime; }; #endif diff --git a/src/MeadeCommandProcessor.cpp b/src/MeadeCommandProcessor.cpp index de66f0f3..04b04417 100644 --- a/src/MeadeCommandProcessor.cpp +++ b/src/MeadeCommandProcessor.cpp @@ -7,6 +7,9 @@ #include "WifiControl.hpp" #include "Gyro.hpp" +#include +#include + #if USE_GPS == 1 bool gpsAqcuisitionComplete(int &indicator); // defined in c72_menuHA_GPS.hpp #endif @@ -1211,6 +1214,29 @@ bool gpsAqcuisitionComplete(int &indicator); // defined in c72_menuHA_GPS.hpp ///////////////////////////////////////////////////////////////////////////////////////// MeadeCommandProcessor *MeadeCommandProcessor::_instance = nullptr; +char MeadeCommandProcessor::_responseBuffer[200] = {}; + +const char *MeadeCommandProcessor::copyToResponse(const char *src) +{ + strlcpy(_responseBuffer, src, sizeof(_responseBuffer)); + return _responseBuffer; +} + +const char *MeadeCommandProcessor::formatResponse(const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vsnprintf(_responseBuffer, sizeof(_responseBuffer), fmt, args); + va_end(args); + return _responseBuffer; +} + +const char *MeadeCommandProcessor::copyToResponse_P(const char *pgmSrc) +{ + strncpy_P(_responseBuffer, pgmSrc, sizeof(_responseBuffer) - 1); + _responseBuffer[sizeof(_responseBuffer) - 1] = '\0'; + return _responseBuffer; +} ///////////////////////////// // Create the processor @@ -1243,7 +1269,7 @@ MeadeCommandProcessor::MeadeCommandProcessor(Mount *mount, LcdMenu *lcdMenu) ///////////////////////////// // INIT ///////////////////////////// -String MeadeCommandProcessor::handleMeadeInit(String inCmd) +const char *MeadeCommandProcessor::handleMeadeInit(const String &inCmd) { inSerialControl = true; _lcdMenu->setCursor(0, 0); @@ -1256,8 +1282,12 @@ String MeadeCommandProcessor::handleMeadeInit(String inCmd) ///////////////////////////// // GET INFO ///////////////////////////// -String MeadeCommandProcessor::handleMeadeGetInfo(String inCmd) +const char *MeadeCommandProcessor::handleMeadeGetInfo(const String &inCmd) { + if (inCmd.length() < 1) + { + return ""; + } char cmdOne = inCmd[0]; char cmdTwo = (inCmd.length() > 1) ? inCmd[1] : '\0'; char achBuffer[20]; @@ -1267,7 +1297,7 @@ String MeadeCommandProcessor::handleMeadeGetInfo(String inCmd) case 'V': if (cmdTwo == 'N') // :GVN { - return String(VERSION) + "#"; + return formatResponse("%s#", VERSION); } else if (cmdTwo == 'P') // :GVP { @@ -1281,47 +1311,47 @@ String MeadeCommandProcessor::handleMeadeGetInfo(String inCmd) } break; - case 'r': // :Gr - return _mount->RAString(MEADE_STRING | TARGET_STRING); // returns trailing # + case 'r': // :Gr + return copyToResponse(_mount->RAString(MEADE_STRING | TARGET_STRING).c_str()); // returns trailing # - case 'd': // :Gd - return _mount->DECString(MEADE_STRING | TARGET_STRING); // returns trailing # + case 'd': // :Gd + return copyToResponse(_mount->DECString(MEADE_STRING | TARGET_STRING).c_str()); // returns trailing # - case 'R': // :GR - return _mount->RAString(MEADE_STRING | CURRENT_STRING); // returns trailing # + case 'R': // :GR + return copyToResponse(_mount->RAString(MEADE_STRING | CURRENT_STRING).c_str()); // returns trailing # - case 'D': // :GD - return _mount->DECString(MEADE_STRING | CURRENT_STRING); // returns trailing # + case 'D': // :GD + return copyToResponse(_mount->DECString(MEADE_STRING | CURRENT_STRING).c_str()); // returns trailing # case 'X': // :GX - return _mount->getStatusString() + "#"; + return formatResponse("%s#", _mount->getStatusString().c_str()); case 'I': { - String retVal = ""; + const char *val = ""; if (cmdTwo == 'S') // :GIS { - retVal = _mount->isSlewingRAorDEC() ? "1" : "0"; + val = _mount->isSlewingRAorDEC() ? "1" : "0"; } else if (cmdTwo == 'T') // :GIT { - retVal = _mount->isSlewingTRK() ? "1" : "0"; + val = _mount->isSlewingTRK() ? "1" : "0"; } else if (cmdTwo == 'G') // :GIG { - retVal = _mount->isGuiding() ? "1" : "0"; + val = _mount->isGuiding() ? "1" : "0"; } - return retVal + "#"; + return formatResponse("%s#", val); } case 't': // :Gt { _mount->latitude().formatString(achBuffer, "{d}*{m}#"); - return String(achBuffer); + return copyToResponse(achBuffer); } case 'g': // :Gg { _mount->longitude().formatStringForMeade(achBuffer); - return String(achBuffer) + "#"; + return formatResponse("%s#", achBuffer); } case 'c': // :Gc { @@ -1330,8 +1360,8 @@ String MeadeCommandProcessor::handleMeadeGetInfo(String inCmd) case 'G': // :GG { int offset = _mount->getLocalUtcOffset(); - sprintf(achBuffer, "%+03d#", -offset); - return String(achBuffer); + snprintf(achBuffer, sizeof(achBuffer), "%+03d#", -offset); + return copyToResponse(achBuffer); } case 'a': // :Ga { @@ -1341,19 +1371,19 @@ String MeadeCommandProcessor::handleMeadeGetInfo(String inCmd) time.addHours(-12); } time.formatString(achBuffer, "{d}:{m}:{s}#"); - return String(achBuffer + 1); + return copyToResponse(achBuffer + 1); } case 'L': // :GL { DayTime time = _mount->getLocalTime(); time.formatString(achBuffer, "{d}:{m}:{s}#"); - return String(achBuffer + 1); + return copyToResponse(achBuffer + 1); } case 'C': // :GC { LocalDate date = _mount->getLocalDate(); - sprintf(achBuffer, "%02d/%02d/%02d#", date.month, date.day, date.year % 100); - return String(achBuffer); + snprintf(achBuffer, sizeof(achBuffer), "%02d/%02d/%02d#", date.month, date.day, date.year % 100); + return copyToResponse(achBuffer); } case 'M': // :GM { @@ -1383,8 +1413,12 @@ String MeadeCommandProcessor::handleMeadeGetInfo(String inCmd) ///////////////////////////// // GPS CONTROL ///////////////////////////// -String MeadeCommandProcessor::handleMeadeGPSCommands(String inCmd) +const char *MeadeCommandProcessor::handleMeadeGPSCommands(const String &inCmd) { + if (inCmd.length() < 1) + { + return "0"; + } #if USE_GPS == 1 if (inCmd[0] == 'T') { @@ -1413,8 +1447,12 @@ String MeadeCommandProcessor::handleMeadeGPSCommands(String inCmd) ///////////////////////////// // SYNC CONTROL ///////////////////////////// -String MeadeCommandProcessor::handleMeadeSyncControl(String inCmd) +const char *MeadeCommandProcessor::handleMeadeSyncControl(const String &inCmd) { + if (inCmd.length() < 1) + { + return "FAIL#"; + } if (inCmd[0] == 'M') // :CM { _mount->syncPosition(_mount->targetRA(), _mount->targetDEC()); @@ -1427,8 +1465,12 @@ String MeadeCommandProcessor::handleMeadeSyncControl(String inCmd) ///////////////////////////// // SET INFO ///////////////////////////// -String MeadeCommandProcessor::handleMeadeSetInfo(String inCmd) +const char *MeadeCommandProcessor::handleMeadeSetInfo(const String &inCmd) { + if (inCmd.length() < 1) + { + return "0"; + } if ((inCmd[0] == 'd') && (inCmd.length() == 10)) { // Set DEC @@ -1545,10 +1587,10 @@ String MeadeCommandProcessor::handleMeadeSetInfo(String inCmd) /* From https://www.astro.louisville.edu/software/xmtel/archive/xmtel-indi-6.0/xmtel-6.0l/support/lx200/CommandSet.html : - SC: Calendar: If the date is valid 2 s are returned, each string is 31 bytes long. + SC: Calendar: If the date is valid 2 s are returned, each string is 31 bytes long. The first is: "Updating planetary data#" followed by a second string of 30 spaces terminated by '#' */ - return F("1Updating Planetary Data# #"); // + return copyToResponse_P(PSTR("1Updating Planetary Data# #")); } else { @@ -1559,8 +1601,12 @@ String MeadeCommandProcessor::handleMeadeSetInfo(String inCmd) ///////////////////////////// // MOVEMENT ///////////////////////////// -String MeadeCommandProcessor::handleMeadeMovement(String inCmd) +const char *MeadeCommandProcessor::handleMeadeMovement(const String &inCmd) { + if (inCmd.length() < 1) + { + return ""; + } LOG(DEBUG_MEADE, "[MEADE]: Process Move command: [%s]", inCmd.c_str()); if (inCmd[0] == 'S') // :MS# { @@ -1593,24 +1639,24 @@ String MeadeCommandProcessor::handleMeadeMovement(String inCmd) // Guide pulse // 012345678901 // :MGd0403 - if (inCmd.length() == 6) + if (inCmd.length() == 6 && isdigit(inCmd[2]) && isdigit(inCmd[3]) && isdigit(inCmd[4]) && isdigit(inCmd[5])) { byte direction = EAST; - inCmd.toLowerCase(); - if (inCmd[1] == 'n') + char dirChar = tolower(inCmd[1]); + if (dirChar == 'n') direction = NORTH; - else if (inCmd[1] == 's') + else if (dirChar == 's') direction = SOUTH; - else if (inCmd[1] == 'e') + else if (dirChar == 'e') direction = EAST; - else if (inCmd[1] == 'w') + else if (dirChar == 'w') direction = WEST; int duration = (inCmd[2] - '0') * 1000 + (inCmd[3] - '0') * 100 + (inCmd[4] - '0') * 10 + (inCmd[5] - '0'); _mount->guidePulse(direction, duration); return ""; } } - else if (inCmd[0] == 'A') + else if (inCmd[0] == 'A' && inCmd.length() > 1) { LOG(DEBUG_MEADE, "[MEADE]: Move Az/Alt"); @@ -1728,8 +1774,12 @@ String MeadeCommandProcessor::handleMeadeMovement(String inCmd) ///////////////////////////// // HOME ///////////////////////////// -String MeadeCommandProcessor::handleMeadeHome(String inCmd) +const char *MeadeCommandProcessor::handleMeadeHome(const String &inCmd) { + if (inCmd.length() < 1) + { + return ""; + } if (inCmd[0] == 'P') // :hP { // Park _mount->park(); @@ -1751,7 +1801,7 @@ String MeadeCommandProcessor::handleMeadeHome(String inCmd) return ""; } -String MeadeCommandProcessor::handleMeadeDistance(String inCmd) +const char *MeadeCommandProcessor::handleMeadeDistance(const String &inCmd) { if (_mount->isSlewingRAorDEC()) { @@ -1763,8 +1813,12 @@ String MeadeCommandProcessor::handleMeadeDistance(String inCmd) ///////////////////////////// // EXTRA COMMANDS ///////////////////////////// -String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) +const char *MeadeCommandProcessor::handleMeadeExtraCommands(const String &inCmd) { + if (inCmd.length() < 1) + { + return ""; + } #if SUPPORT_DRIFT_ALIGNMENT == 1 // :XDmmm if (inCmd[0] == 'D') // :XD @@ -1799,7 +1853,7 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) { // Get RA/DEC steps/deg, speedfactor if (inCmd[1] == 'R') // :XGR# { - return String(_mount->getStepsPerDegree(RA_STEPS), 1) + "#"; + return formatResponse("%s#", String(_mount->getStepsPerDegree(RA_STEPS), 1).c_str()); } else if (inCmd[1] == 'D') { @@ -1813,11 +1867,11 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) { if (inCmd[3] == 'L') // :XGDLL# { - return String(loLimit, 1) + "#"; + return formatResponse("%s#", String(loLimit, 1).c_str()); } else if (inCmd[3] == 'U') // :XGDLU# { - return String(hiLimit, 1) + "#"; + return formatResponse("%s#", String(hiLimit, 1).c_str()); } else { @@ -1826,7 +1880,7 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) } else // :XGDL# { - return String(loLimit, 1) + "|" + String(hiLimit, 1) + "#"; + return formatResponse("%s|%s#", String(loLimit, 1).c_str(), String(hiLimit, 1).c_str()); } } if (inCmd[2] == 'P') // :XGDP# @@ -1836,47 +1890,45 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) } else // :XGD# { - return String(_mount->getStepsPerDegree(DEC_STEPS), 1) + "#"; + return formatResponse("%s#", String(_mount->getStepsPerDegree(DEC_STEPS), 1).c_str()); } } else if (inCmd[1] == 'S') { if (inCmd.length() == 2) // :XGS# { - return String(_mount->getSpeedCalibration(), 5) + "#"; + return formatResponse("%s#", String(_mount->getSpeedCalibration(), 5).c_str()); } else if ((inCmd.length() == 3) && (inCmd[2] == 'T')) // :XGST# { - return String(_mount->checkRALimit(), 7) + "#"; + return formatResponse("%s#", String(_mount->checkRALimit(), 7).c_str()); } } else if (inCmd[1] == 'T') // :XGT# { - return String(_mount->getSpeed(TRACKING), 7) + "#"; + return formatResponse("%s#", String(_mount->getSpeed(TRACKING), 7).c_str()); } else if (inCmd[1] == 'B') // :XGB# { - return String(_mount->getBacklashCorrection()) + "#"; + return formatResponse("%s#", String(_mount->getBacklashCorrection()).c_str()); } else if ((inCmd[1] == 'A') && (inCmd.length() == 2)) // :XGA# { - return String(_mount->getStepsPerDegree(ALTITUDE_STEPS), 1) + "#"; + return formatResponse("%s#", String(_mount->getStepsPerDegree(ALTITUDE_STEPS), 1).c_str()); } else if ((inCmd[1] == 'Z') && (inCmd.length() == 2)) // :XGZ# { - return String(_mount->getStepsPerDegree(AZIMUTH_STEPS), 1) + "#"; + return formatResponse("%s#", String(_mount->getStepsPerDegree(AZIMUTH_STEPS), 1).c_str()); } else if ((inCmd[1] == 'A') && (inCmd.length() > 2) && (inCmd[2] == 'H')) // :XGAH# { - return _mount->getAutoHomingStates() + "#"; + return formatResponse("%s#", _mount->getAutoHomingStates().c_str()); } else if ((inCmd[1] == 'A') && (inCmd.length() > 2) && (inCmd[2] == 'A')) // :XGAA# { long azPos, altPos; _mount->getAZALTPositions(azPos, altPos); - char scratchBuffer[20]; - sprintf(scratchBuffer, "%ld|%ld#", azPos, altPos); - return String(scratchBuffer); + return formatResponse("%ld|%ld#", azPos, altPos); } else if (inCmd[1] == 'C') // :XGCn.nn*m.mm# { @@ -1888,22 +1940,20 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) float raCoord = coords.substring(0, star).toFloat(); float decCoord = coords.substring(star + 1).toFloat(); _mount->calculateStepperPositions(raCoord, decCoord, raPos, decPos); - char scratchBuffer[20]; - sprintf(scratchBuffer, "%ld|%ld#", raPos, decPos); - return String(scratchBuffer); + return formatResponse("%ld|%ld#", raPos, decPos); } } else if (inCmd[1] == 'M') { if ((inCmd.length() > 2) && (inCmd[2] == 'S')) // :XGMS# { - return _mount->getStepperInfo() + "#"; + return formatResponse("%s#", _mount->getStepperInfo().c_str()); } - return _mount->getMountHardwareInfo() + "#"; // :XGM# + return formatResponse("%s#", _mount->getMountHardwareInfo().c_str()); // :XGM# } else if (inCmd[1] == 'O') // :XGO# { - return getLogBuffer(); + return copyToResponse(getLogBuffer().c_str()); } else if (inCmd[1] == 'H') // :XGH# { @@ -1913,17 +1963,17 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) if (inCmd[2] == 'R') // :XGHR# { LOG(DEBUG_MEADE, "[MEADE]: XGHR -> %s", inCmd.c_str()); - return String(_mount->getHomingOffset(StepperAxis::RA_STEPS)) + "#"; + return formatResponse("%ld#", _mount->getHomingOffset(StepperAxis::RA_STEPS)); } else if (inCmd[2] == 'D') // :XGHD# { LOG(DEBUG_MEADE, "[MEADE]: XGHD -> %s", inCmd.c_str()); - return String(_mount->getHomingOffset(StepperAxis::DEC_STEPS)) + "#"; + return formatResponse("%ld#", _mount->getHomingOffset(StepperAxis::DEC_STEPS)); } else if (inCmd[2] == 'S') // :XGHS# { LOG(DEBUG_MEADE, "[MEADE]: XGHS -> %s", inCmd.c_str()); - return String(inNorthernHemisphere ? "N#" : "S#"); + return inNorthernHemisphere ? "N#" : "S#"; } LOG(DEBUG_MEADE, "[MEADE]: XGH? -> %s", inCmd.c_str()); @@ -1931,23 +1981,19 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) } else { - char scratchBuffer[10]; DayTime ha = _mount->calculateHa(); - sprintf(scratchBuffer, "%02d%02d%02d#", ha.getHours(), ha.getMinutes(), ha.getSeconds()); - return String(scratchBuffer); + return formatResponse("%02d%02d%02d#", ha.getHours(), ha.getMinutes(), ha.getSeconds()); } } else if (inCmd[1] == 'L') // :XGL# { - char scratchBuffer[10]; DayTime lst = _mount->calculateLst(); - sprintf(scratchBuffer, "%02d%02d%02d#", lst.getHours(), lst.getMinutes(), lst.getSeconds()); - return String(scratchBuffer); + return formatResponse("%02d%02d%02d#", lst.getHours(), lst.getMinutes(), lst.getSeconds()); } else if (inCmd[1] == 'N') // :XGN# { #if (WIFI_ENABLED == 1) - return wifiControl.getStatus() + "#"; + return formatResponse("%s#", wifiControl.getStatus().c_str()); #endif return "0,#"; @@ -2063,17 +2109,18 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) { // get values if (inCmd[2] == 'R') // :XLGR { // get Calibration/Reference values - return String(_mount->getPitchCalibrationAngle(), 4) + "," + String(_mount->getRollCalibrationAngle(), 4) + "#"; + return formatResponse( + "%s,%s#", String(_mount->getPitchCalibrationAngle(), 4).c_str(), String(_mount->getRollCalibrationAngle(), 4).c_str()); } else if (inCmd[2] == 'C') // :XLGC { // Get current values auto angles = Gyro::getCurrentAngles(); - return String(angles.pitchAngle, 4) + "," + String(angles.rollAngle, 4) + "#"; + return formatResponse("%s,%s#", String(angles.pitchAngle, 4).c_str(), String(angles.rollAngle, 4).c_str()); } else if (inCmd[2] == 'T') // :XLGT { // Get current temp float temp = Gyro::getCurrentTemperature(); - return String(temp, 1) + "#"; + return formatResponse("%s#", String(temp, 1).c_str()); } } else if (inCmd[1] == 'S') @@ -2081,35 +2128,35 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) if (inCmd[2] == 'P') // :XLSP { // get Calibration/Reference values _mount->setPitchCalibrationAngle(inCmd.substring(3).toFloat()); - return String("1#"); + return "1#"; } else if (inCmd[2] == 'R') // :XLSR { _mount->setRollCalibrationAngle(inCmd.substring(3).toFloat()); - return String("1#"); + return "1#"; } } else if (inCmd[1] == '1') // :XL1 { // Turn on Gyro Gyro::startup(); - return String("1#"); + return "1#"; } else if (inCmd[1] == '0') // :XL0 { // Turn off Gyro Gyro::shutdown(); - return String("1#"); + return "1#"; } else { - return "Unknown Level command: X" + inCmd; + return formatResponse("Unknown Level command: X%s", inCmd.c_str()); } #endif - return String("0#"); + return "0#"; } else if ((inCmd[0] == 'F') && (inCmd[1] == 'R')) { _mount->clearConfiguration(); // :XFR - return String("1#"); + return "1#"; } return ""; @@ -2118,7 +2165,7 @@ String MeadeCommandProcessor::handleMeadeExtraCommands(String inCmd) ///////////////////////////// // QUIT ///////////////////////////// -String MeadeCommandProcessor::handleMeadeQuit(String inCmd) +const char *MeadeCommandProcessor::handleMeadeQuit(const String &inCmd) { // :Q# stops a motors - remains in Control mode // :Qq# command does not stop motors, but quits Control mode @@ -2162,8 +2209,12 @@ String MeadeCommandProcessor::handleMeadeQuit(String inCmd) ///////////////////////////// // Set Slew Rates ///////////////////////////// -String MeadeCommandProcessor::handleMeadeSetSlewRate(String inCmd) +const char *MeadeCommandProcessor::handleMeadeSetSlewRate(const String &inCmd) { + if (inCmd.length() < 1) + { + return ""; + } switch (inCmd[0]) { case 'S': @@ -2187,8 +2238,12 @@ String MeadeCommandProcessor::handleMeadeSetSlewRate(String inCmd) ///////////////////////////// // FOCUS COMMANDS ///////////////////////////// -String MeadeCommandProcessor::handleMeadeFocusCommands(String inCmd) +const char *MeadeCommandProcessor::handleMeadeFocusCommands(const String &inCmd) { + if (inCmd.length() < 1) + { + return ""; + } #if (FOCUS_STEPPER_TYPE != STEPPER_TYPE_NONE) if (inCmd[0] == '+') // :F+ { @@ -2226,7 +2281,7 @@ String MeadeCommandProcessor::handleMeadeFocusCommands(String inCmd) { LOG(DEBUG_MEADE, "[MEADE]: Focus get stepperPosition"); long focusPos = _mount->focusGetStepperPosition(); - return String(focusPos) + "#"; + return formatResponse("%ld#", focusPos); } else if (inCmd[0] == 'P') // :FPnnn { @@ -2259,8 +2314,12 @@ String MeadeCommandProcessor::handleMeadeFocusCommands(String inCmd) return ""; } -String MeadeCommandProcessor::processCommand(String inCmd) +const char *MeadeCommandProcessor::processCommand(String inCmd) { + if (inCmd.length() < 2) + { + return ""; + } if (inCmd[0] == ':') { LOG(DEBUG_MEADE, "[MEADE]: Received command '%s'", inCmd.c_str()); @@ -2272,6 +2331,10 @@ String MeadeCommandProcessor::processCommand(String inCmd) inCmd.remove(spacePos, 1); } + if (inCmd.length() < 2) + { + return ""; + } LOG(DEBUG_MEADE, "[MEADE]: Processing command '%s'", inCmd.c_str()); char command = inCmd[1]; inCmd = inCmd.substring(2); diff --git a/src/MeadeCommandProcessor.hpp b/src/MeadeCommandProcessor.hpp index b8861f17..e50653d1 100644 --- a/src/MeadeCommandProcessor.hpp +++ b/src/MeadeCommandProcessor.hpp @@ -9,23 +9,29 @@ class MeadeCommandProcessor public: static MeadeCommandProcessor *createProcessor(Mount *mount, LcdMenu *lcdMenu); static MeadeCommandProcessor *instance(); - String processCommand(String incmd); + const char *processCommand(String inCmd); private: MeadeCommandProcessor(Mount *mount, LcdMenu *lcdMenu); - String handleMeadeSetInfo(String inCmd); - String handleMeadeMovement(String inCmd); - String handleMeadeGetInfo(String inCmd); - String handleMeadeGPSCommands(String inCmd); - String handleMeadeSyncControl(String inCmd); - String handleMeadeHome(String inCmd); - String handleMeadeInit(String inCmd); - String handleMeadeQuit(String inCmd); - String handleMeadeDistance(String inCmd); - String handleMeadeSetSlewRate(String inCmd); - String handleMeadeExtraCommands(String inCmd); - String handleMeadeFocusCommands(String inCmd); + const char *handleMeadeSetInfo(const String &inCmd); + const char *handleMeadeMovement(const String &inCmd); + const char *handleMeadeGetInfo(const String &inCmd); + const char *handleMeadeGPSCommands(const String &inCmd); + const char *handleMeadeSyncControl(const String &inCmd); + const char *handleMeadeHome(const String &inCmd); + const char *handleMeadeInit(const String &inCmd); + const char *handleMeadeQuit(const String &inCmd); + const char *handleMeadeDistance(const String &inCmd); + const char *handleMeadeSetSlewRate(const String &inCmd); + const char *handleMeadeExtraCommands(const String &inCmd); + const char *handleMeadeFocusCommands(const String &inCmd); + + static const char *copyToResponse(const char *src); + static const char *formatResponse(const char *fmt, ...); + static const char *copyToResponse_P(const char *pgmSrc); + Mount *_mount; LcdMenu *_lcdMenu; static MeadeCommandProcessor *_instance; + static char _responseBuffer[200]; }; diff --git a/src/Mount.cpp b/src/Mount.cpp index 28ccce26..22911649 100644 --- a/src/Mount.cpp +++ b/src/Mount.cpp @@ -1522,7 +1522,7 @@ void Mount::startSlewingToTarget() LOG(DEBUG_STEPPERS, "[STEPPERS]: startSlewingToTarget: TRK stopped at %lms", _trackerStoppedAt); } - _mountStatus |= STATUS_SLEWING | STATUS_SLEWING_TO_TARGET; + setStatusFlag(STATUS_SLEWING | STATUS_SLEWING_TO_TARGET); #if DEC_DRIVER_TYPE == DRIVER_TYPE_TMC2209_UART // Since normal state for DEC is guide microstepping, switch to slew microstepping here. LOG(DEBUG_STEPPERS, "[STEPPERS]: startSlewingToTarget: Switching DEC driver to microsteps(%d)", DEC_SLEW_MICROSTEPPING); @@ -1586,7 +1586,7 @@ void Mount::startSlewingToHome() LOG(DEBUG_STEPPERS, "[STEPPERS]: startSlewingToHome: TRK stopped at %lms", _trackerStoppedAt); } - _mountStatus |= STATUS_SLEWING | STATUS_SLEWING_TO_TARGET; + setStatusFlag(STATUS_SLEWING | STATUS_SLEWING_TO_TARGET); #if DEC_DRIVER_TYPE == DRIVER_TYPE_TMC2209_UART // Since normal state for DEC is guide microstepping, switch to slew microstepping here. LOG(DEBUG_STEPPERS, "[STEPPERS]: startSlewingToHome: Switching DEC driver to microsteps(%d)", DEC_SLEW_MICROSTEPPING); @@ -1615,7 +1615,7 @@ void Mount::stopGuiding(bool ra, bool dec) LOG(DEBUG_STEPPERS | DEBUG_GUIDE, "[GUIDE]: stopGuide: TRK stop guide at : %l", _stepperTRK->currentPosition()); _stepperTRK->setSpeed(_trackingSpeed); LOG(DEBUG_STEPPERS | DEBUG_GUIDE, "[GUIDE]: stopGuide: TRK speed set to : %f", _trackingSpeed); - _mountStatus &= ~STATUS_GUIDE_PULSE_RA; + clearStatusFlag(STATUS_GUIDE_PULSE_RA); } if (dec && (_mountStatus & STATUS_GUIDE_PULSE_DEC)) @@ -1633,14 +1633,14 @@ void Mount::stopGuiding(bool ra, bool dec) } #endif - _mountStatus &= ~STATUS_GUIDE_PULSE_DEC; + clearStatusFlag(STATUS_GUIDE_PULSE_DEC); } //disable pulse state if no direction is active if ((_mountStatus & STATUS_GUIDE_PULSE_DIR) == 0) { LOG(DEBUG_STEPPERS | DEBUG_GUIDE, "[GUIDE]: Clear guiding state"); - _mountStatus &= ~STATUS_GUIDE_PULSE_MASK; + clearStatusFlag(STATUS_GUIDE_PULSE_MASK); } else { @@ -1714,7 +1714,7 @@ void Mount::guidePulse(byte direction, int duration) "[GUIDE]: guidePulse: DEC guide speed : %f", DEC_PULSE_MULTIPLIER * decGuidingSpeed); _stepperGUIDE->setSpeed(DEC_PULSE_MULTIPLIER * decGuidingSpeed); - _mountStatus |= STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_DEC; + setStatusFlag(STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_DEC); _guideDecEndTime = millis() + duration; } break; @@ -1735,7 +1735,7 @@ void Mount::guidePulse(byte direction, int duration) "[GUIDE]: guidePulse: DEC guide speed : %f", -DEC_PULSE_MULTIPLIER * decGuidingSpeed); _stepperGUIDE->setSpeed(-DEC_PULSE_MULTIPLIER * decGuidingSpeed); - _mountStatus |= STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_DEC; + setStatusFlag(STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_DEC); _guideDecEndTime = millis() + duration; } break; @@ -1751,7 +1751,7 @@ void Mount::guidePulse(byte direction, int duration) (RA_PULSE_MULTIPLIER * raGuidingSpeed), RA_PULSE_MULTIPLIER); _stepperTRK->setSpeed(RA_PULSE_MULTIPLIER * raGuidingSpeed); // Faster than siderael - _mountStatus |= STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_RA; + setStatusFlag(STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_RA); _guideRaEndTime = millis() + duration; break; @@ -1766,7 +1766,7 @@ void Mount::guidePulse(byte direction, int duration) (2.0 - RA_PULSE_MULTIPLIER * raGuidingSpeed), (2.0 - RA_PULSE_MULTIPLIER)); _stepperTRK->setSpeed(raGuidingSpeed * (2.0f - RA_PULSE_MULTIPLIER)); // Slower than siderael - _mountStatus |= STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_RA; + setStatusFlag(STATUS_GUIDE_PULSE | STATUS_GUIDE_PULSE_RA); _guideRaEndTime = millis() + duration; break; } @@ -1853,7 +1853,7 @@ void Mount::setManualSlewMode(bool state) stopSlewing(ALL_DIRECTIONS); stopSlewing(TRACKING); waitUntilStopped(ALL_DIRECTIONS); - _mountStatus |= STATUS_SLEWING | STATUS_SLEWING_MANUAL; + setStatusFlag(STATUS_SLEWING | STATUS_SLEWING_MANUAL); #if RA_DRIVER_TYPE == DRIVER_TYPE_TMC2209_UART LOG(DEBUG_STEPPERS, "[STEPPERS]: setManualSlewMode: Switching RA driver to microsteps(%d)", RA_SLEW_MICROSTEPPING); _driverRA->microsteps(RA_SLEW_MICROSTEPPING == 1 ? 0 : RA_SLEW_MICROSTEPPING); @@ -1861,7 +1861,7 @@ void Mount::setManualSlewMode(bool state) } else { - _mountStatus &= ~STATUS_SLEWING_MANUAL; + clearStatusFlag(STATUS_SLEWING_MANUAL); stopSlewing(ALL_DIRECTIONS); waitUntilStopped(ALL_DIRECTIONS); LOG(DEBUG_STEPPERS, "[STEPPERS]: setManualSlewMode: Set RA speed/accel: %l / %l", _maxRASpeed, _maxRAAcceleration); @@ -1986,7 +1986,7 @@ void Mount::park() stopSlewing(ALL_DIRECTIONS | TRACKING); waitUntilStopped(ALL_DIRECTIONS); startSlewingToHome(); - _mountStatus |= STATUS_PARKING; + setStatusFlag(STATUS_PARKING); } bool Mount::isAxisRunning(StepperAxis axis) @@ -2279,14 +2279,25 @@ void Mount::setTrackingStepperPos(long stepPos) _stepperTRK->setCurrentPosition(stepPos); } +void Mount::setStatus(int state) +{ + noInterrupts(); + _mountStatus = state; + interrupts(); +} + void Mount::setStatusFlag(int flag) { + noInterrupts(); _mountStatus |= flag; + interrupts(); } void Mount::clearStatusFlag(int flag) { + noInterrupts(); _mountStatus &= ~flag; + interrupts(); } ///////////////////////////////// @@ -2518,7 +2529,7 @@ void Mount::startSlewing(int direction) _stepperTRK->setSpeed(_trackingSpeed); // Turn on tracking - _mountStatus |= STATUS_TRACKING; + setStatusFlag(STATUS_TRACKING); } else { @@ -2563,7 +2574,7 @@ void Mount::startSlewing(int direction) } _stepperDEC->moveTo(targetLocation); - _mountStatus |= STATUS_SLEWING; + setStatusFlag(STATUS_SLEWING); } if (direction & SOUTH) @@ -2583,10 +2594,10 @@ void Mount::startSlewing(int direction) } _stepperDEC->moveTo(targetLocation); - _mountStatus |= STATUS_SLEWING; + setStatusFlag(STATUS_SLEWING); } - const float trackedHours = (_stepperTRK->currentPosition() / _trackingSpeed) / 3600.0F; // steps / steps/s / 3600 = hours + const float trackedHours = getTrackedHours(); if (direction & EAST) { // We need to subtract the distance tracked from the physical RA home coordinate @@ -2596,7 +2607,7 @@ void Mount::startSlewing(int direction) -sign * targetEastPos, trackedHours); _stepperRA->moveTo(-sign * targetEastPos); - _mountStatus |= STATUS_SLEWING; + setStatusFlag(STATUS_SLEWING); } if (direction & WEST) { @@ -2607,7 +2618,7 @@ void Mount::startSlewing(int direction) sign * targetWestPos, trackedHours); _stepperRA->moveTo(sign * targetWestPos); - _mountStatus |= STATUS_SLEWING; + setStatusFlag(STATUS_SLEWING); } } } @@ -2624,7 +2635,7 @@ void Mount::stopSlewing(int direction) if (direction & TRACKING) { // Turn off tracking - _mountStatus &= ~STATUS_TRACKING; + clearStatusFlag(STATUS_TRACKING); LOG(DEBUG_STEPPERS, "[STEPPERS]: stopSlewing: TRK stepper stop()"); _stepperTRK->stop(); @@ -2642,7 +2653,7 @@ void Mount::stopSlewing(int direction) _stepperRA->stop(); if (isFindingHome()) { - _mountStatus &= ~STATUS_FINDING_HOME; + clearStatusFlag(STATUS_FINDING_HOME); } } } @@ -3150,7 +3161,7 @@ void Mount::loop() // // Arrived at target after Slew! // - _mountStatus &= ~(STATUS_SLEWING | STATUS_SLEWING_TO_TARGET | STATUS_SLEWING_MANUAL); + clearStatusFlag(STATUS_SLEWING | STATUS_SLEWING_TO_TARGET | STATUS_SLEWING_MANUAL); if (_stepperWasRunning) { @@ -3253,7 +3264,7 @@ void Mount::loop() _driverDEC->microsteps(DEC_SLEW_MICROSTEPPING == 1 ? 0 : DEC_SLEW_MICROSTEPPING); #endif LOG(DEBUG_MOUNT | DEBUG_STEPPERS, "[MOUNT]: Loop: Was parking, so no tracking. Proceeding to park position..."); - _mountStatus &= ~STATUS_PARKING; + clearStatusFlag(STATUS_PARKING); _slewingToPark = true; _stepperRA->moveTo(-getHomingOffset(StepperAxis::RA_STEPS)); _stepperDEC->moveTo(-getHomingOffset(StepperAxis::DEC_STEPS)); @@ -3268,12 +3279,14 @@ void Mount::loop() if ((_stepperDEC->distanceToGo() != 0) || (_stepperRA->distanceToGo() != 0)) { LOG(DEBUG_MOUNT | DEBUG_STEPPERS, "[MOUNT]: Loop: Distance to Parking is non-zero, slewing to park position..."); - _mountStatus |= STATUS_PARKING_POS | STATUS_SLEWING; + setStatusFlag(STATUS_PARKING_POS | STATUS_SLEWING); } else { LOG(DEBUG_MOUNT | DEBUG_STEPPERS, "[MOUNT]: Loop: Already at Parking pos, so done."); - _mountStatus = STATUS_PARKED; + noInterrupts(); + setStatus(STATUS_PARKED); + interrupts(); } } else @@ -3291,7 +3304,9 @@ void Mount::loop() else if (_slewingToPark) { LOG(DEBUG_MOUNT | DEBUG_STEPPERS, "[MOUNT]: Loop: Arrived at park position..."); - _mountStatus = STATUS_PARKED; + noInterrupts(); + setStatus(STATUS_PARKED); + interrupts(); _slewingToPark = false; } _totalDECMove = _totalRAMove = 0; @@ -3581,7 +3596,7 @@ void Mount::calculateRAandDECSteppers(long &targetRASteps, long &targetDECSteps, LOG(DEBUG_COORD_CALC, "[MOUNT]: CalcSteppersIn: moveRA (target) is %f", moveRA); // Total hours of tracking-to-date - float trackedHours = (_stepperTRK->currentPosition() / _trackingSpeed) / 3600.0F; // steps / steps/s / 3600 = hours + float trackedHours = getTrackedHours(); LOG(DEBUG_COORD_CALC, "[MOUNT]: CalcSteppersIn: Tracked time is %l steps (%f h).", _stepperTRK->currentPosition(), trackedHours); // The current RA of the home position, taking tracking-to-date into account @@ -3792,7 +3807,7 @@ void Mount::moveStepperBy(StepperAxis direction, long steps) LOG(DEBUG_STEPPERS, "[STEPPERS]: moveStepperBy: Switching RA driver to microsteps(%d)", RA_SLEW_MICROSTEPPING); _driverRA->microsteps(RA_SLEW_MICROSTEPPING == 1 ? 0 : RA_SLEW_MICROSTEPPING); #endif - _mountStatus |= STATUS_SLEWING | STATUS_SLEWING_TO_TARGET; + setStatusFlag(STATUS_SLEWING | STATUS_SLEWING_TO_TARGET); _stepperWasRunning = true; moveSteppersTo(_stepperRA->currentPosition() + steps, 0, direction); _totalRAMove = 1.0f * _stepperRA->distanceToGo(); @@ -3800,7 +3815,7 @@ void Mount::moveStepperBy(StepperAxis direction, long steps) case DEC_STEPS: { - _mountStatus |= STATUS_SLEWING | STATUS_SLEWING_TO_TARGET; + setStatusFlag(STATUS_SLEWING | STATUS_SLEWING_TO_TARGET); #if DEC_DRIVER_TYPE == DRIVER_TYPE_TMC2209_UART // Since normal state for DEC is guide microstepping, switch to slew microstepping here. LOG(DEBUG_STEPPERS, "[STEPPERS]: moveStepperBy: Switching DEC driver to microsteps(%d)", DEC_SLEW_MICROSTEPPING); @@ -4269,6 +4284,11 @@ void Mount::testUART_vactual(TMC2209Stepper *driver, int _speed, int _duration) #endif #endif +float Mount::getTrackedHours() const +{ + return (_trackingSpeed > 0) ? ((_stepperTRK->currentPosition() / _trackingSpeed) / 3600.0F) : 0.0F; // steps / steps/s / 3600 = hours +} + ///////////////////////////////// // // checkRALimit @@ -4276,7 +4296,7 @@ void Mount::testUART_vactual(TMC2209Stepper *driver, int _speed, int _duration) ///////////////////////////////// float Mount::checkRALimit() { - const float trackedHours = (_stepperTRK->currentPosition() / _trackingSpeed) / 3600.0F; // steps / steps/s / 3600 = hours + const float trackedHours = getTrackedHours(); const float homeRA = _zeroPosRA.getTotalHours() + trackedHours; const float RALimit = RA_TRACKING_LIMIT; LOG(DEBUG_MOUNT_VERBOSE, diff --git a/src/Mount.hpp b/src/Mount.hpp index 0985a4d3..a6cdc2c5 100644 --- a/src/Mount.hpp +++ b/src/Mount.hpp @@ -264,6 +264,9 @@ class Mount void setSlewRate(int rate); int getSlewRate(); + // Get the number of hours we've been tracking + float getTrackedHours() const; + // Set the HA time (HA is derived from LST, the setter calculates and sets LST) void setHA(const DayTime &haTime); const DayTime HA() const; @@ -391,6 +394,7 @@ class Mount // Returns a comma-delimited string with all the mounts' information String getStatusString(); + void setStatus(int state); void setStatusFlag(int flag); void clearStatusFlag(int flag); diff --git a/src/WifiControl.cpp b/src/WifiControl.cpp index a3dedabb..e3c1dacb 100644 --- a/src/WifiControl.cpp +++ b/src/WifiControl.cpp @@ -129,10 +129,12 @@ void WifiControl::loop() LOG(DEBUG_WIFI, "[WIFI]: Connected status changed to %s", wifiStatus(_status).c_str()); if (_status == WL_CONNECTED) { + delete _tcpServer; _tcpServer = new WiFiServer(WIFI_PORT); _tcpServer->begin(); _tcpServer->setNoDelay(true); + delete _udp; _udp = new WiFiUDP(); _udp->begin(4031); @@ -189,12 +191,12 @@ void WifiControl::tcpLoop() { String cmd = client.readStringUntil('#'); LOG(DEBUG_WIFI, "[WIFITCP]: Query <-- %s#", cmd.c_str()); - String retVal = _cmdProcessor->processCommand(cmd); + const char *retVal = _cmdProcessor->processCommand(cmd); - if (retVal != "") + if (retVal[0] != '\0') { - client.write(retVal.c_str()); - LOG(DEBUG_WIFI, "[WIFITCP]: Reply --> %s", retVal.c_str()); + client.write(retVal); + LOG(DEBUG_WIFI, "[WIFITCP]: Reply --> %s", retVal); } else { @@ -226,8 +228,8 @@ void WifiControl::udpLoop() packetSize, _udp->remoteIP().toString().c_str(), _udp->remotePort()); - char incomingPacket[255]; - int len = _udp->read(incomingPacket, 255); + char incomingPacket[256]; + int len = _udp->read(incomingPacket, sizeof(incomingPacket) - 1); incomingPacket[len] = 0; LOG(DEBUG_WIFI, "[WIFIUDP]: Received: %s", incomingPacket); diff --git a/src/WifiControl.hpp b/src/WifiControl.hpp index c42a24d2..943aec2e 100644 --- a/src/WifiControl.hpp +++ b/src/WifiControl.hpp @@ -34,8 +34,8 @@ class WifiControl LcdMenu *_lcdMenu; MeadeCommandProcessor *_cmdProcessor; - WiFiServer *_tcpServer; - WiFiUDP *_udp; + WiFiServer *_tcpServer = nullptr; + WiFiUDP *_udp = nullptr; WiFiClient client; unsigned long _infraStart = 0; diff --git a/src/f_serial.hpp b/src/f_serial.hpp index ad3741a7..7f8458a1 100644 --- a/src/f_serial.hpp +++ b/src/f_serial.hpp @@ -132,10 +132,10 @@ void processSerialData() const String inCmd = String(buffer); LOG(DEBUG_SERIAL, "[SERIAL]: ReceivedCommand(%d chars): [%s]", inCmd.length(), inCmd.c_str()); - const String retVal = MeadeCommandProcessor::instance()->processCommand(inCmd); - if (retVal != "") + const char *retVal = MeadeCommandProcessor::instance()->processCommand(inCmd); + if (retVal[0] != '\0') { - LOG(DEBUG_SERIAL, "[SERIAL]: RepliedWith: [%s]", retVal.c_str()); + LOG(DEBUG_SERIAL, "[SERIAL]: RepliedWith: [%s]", retVal); // When not debugging, print the result to the serial port . // When debugging, only print the result to Serial if we're on seperate ports. #if (DEBUG_LEVEL == DEBUG_NONE) || (DEBUG_SEPARATE_SERIAL == 1) diff --git a/src/fonts128x64.h b/src/fonts128x64.h index bdefadfc..ded20191 100644 --- a/src/fonts128x64.h +++ b/src/fonts128x64.h @@ -4,442 +4,1887 @@ // clang-format off // Font generated or edited with the glyphEditor const uint8_t Bitmap3x5[] PROGMEM = { -0x03, // Width: 3 -0x05, // Height: 5 -0x20, // First char: 32 -0x3B, // Number of chars: 59 -// Jump Table: -0xFF, 0xFF, 0x00, 0x03, // 32 -0x00, 0x00, 0x02, 0x03, // 33 -0x00, 0x02, 0x03, 0x04, // 34 -0x00, 0x05, 0x03, 0x04, // 35 -0x00, 0x08, 0x03, 0x04, // 36 -0x00, 0x0B, 0x03, 0x04, // 37 -0x00, 0x0E, 0x03, 0x04, // 38 -0x00, 0x11, 0x02, 0x03, // 39 -0x00, 0x13, 0x03, 0x04, // 40 -0x00, 0x16, 0x02, 0x03, // 41 -0x00, 0x18, 0x03, 0x04, // 42 -0x00, 0x1B, 0x03, 0x04, // 43 -0x00, 0x1E, 0x02, 0x03, // 44 -0x00, 0x20, 0x03, 0x04, // 45 -0x00, 0x23, 0x02, 0x03, // 46 -0x00, 0x25, 0x03, 0x04, // 47 -0x00, 0x28, 0x03, 0x04, // 48 -0x00, 0x2B, 0x02, 0x03, // 49 -0x00, 0x2D, 0x03, 0x04, // 50 -0x00, 0x30, 0x03, 0x04, // 51 -0x00, 0x33, 0x03, 0x04, // 52 -0x00, 0x36, 0x03, 0x04, // 53 -0x00, 0x39, 0x03, 0x04, // 54 -0x00, 0x3C, 0x03, 0x04, // 55 -0x00, 0x3F, 0x03, 0x04, // 56 -0x00, 0x42, 0x03, 0x04, // 57 -0x00, 0x45, 0x02, 0x03, // 58 -0x00, 0x47, 0x02, 0x03, // 59 -0x00, 0x49, 0x03, 0x04, // 60 -0x00, 0x4C, 0x03, 0x04, // 61 -0x00, 0x4F, 0x03, 0x04, // 62 -0x00, 0x52, 0x03, 0x04, // 63 -0x00, 0x55, 0x03, 0x04, // 64 -0x00, 0x58, 0x03, 0x04, // 65 -0x00, 0x5B, 0x03, 0x04, // 66 -0x00, 0x5E, 0x03, 0x04, // 67 -0x00, 0x61, 0x03, 0x04, // 68 -0x00, 0x64, 0x03, 0x04, // 69 -0x00, 0x67, 0x03, 0x04, // 70 -0x00, 0x6A, 0x03, 0x04, // 71 -0x00, 0x6D, 0x03, 0x04, // 72 -0x00, 0x70, 0x02, 0x03, // 73 -0x00, 0x72, 0x03, 0x04, // 74 -0x00, 0x75, 0x03, 0x04, // 75 -0x00, 0x78, 0x03, 0x04, // 76 -0x00, 0x7B, 0x03, 0x04, // 77 -0x00, 0x7E, 0x03, 0x04, // 78 -0x00, 0x81, 0x03, 0x04, // 79 -0x00, 0x84, 0x03, 0x04, // 80 -0x00, 0x87, 0x03, 0x04, // 81 -0x00, 0x8A, 0x03, 0x04, // 82 -0x00, 0x8D, 0x03, 0x04, // 83 -0x00, 0x90, 0x03, 0x04, // 84 -0x00, 0x93, 0x03, 0x04, // 85 -0x00, 0x96, 0x03, 0x04, // 86 -0x00, 0x99, 0x03, 0x04, // 87 -0x00, 0x9C, 0x03, 0x04, // 88 -0x00, 0x9F, 0x03, 0x04, // 89 -0x00, 0xA2, 0x03, 0x04, // 90 -// Font Data: -0x00, 0x17, // 33 -0x03, 0x00, 0x03, // 34 -0x0E, 0x0A, 0x0E, // 35 -0x12, 0x1F, 0x09, // 36 -0x19, 0x04, 0x13, // 37 -0x1A, 0x15, 0x0E, // 38 -0x00, 0x03, // 39 -0x00, 0x0E, 0x11, // 40 -0x11, 0x0E, // 41 -0x0A, 0x04, 0x0A, // 42 -0x04, 0x0E, 0x04, // 43 -0x10, 0x08, // 44 -0x04, 0x04, 0x04, // 45 -0x00, 0x10, // 46 -0x18, 0x04, 0x03, // 47 -0x1F, 0x11, 0x1F, // 48 -0x02, 0x1F, // 49 -0x19, 0x15, 0x13, // 50 -0x11, 0x15, 0x1F, // 51 -0x07, 0x04, 0x1F, // 52 -0x17, 0x15, 0x1D, // 53 -0x1F, 0x15, 0x1D, // 54 -0x01, 0x1D, 0x03, // 55 -0x1F, 0x15, 0x1F, // 56 -0x17, 0x15, 0x1F, // 57 -0x00, 0x0A, // 58 -0x10, 0x0A, // 59 -0x04, 0x0A, 0x11, // 60 -0x0A, 0x0A, 0x0A, // 61 -0x11, 0x0A, 0x04, // 62 -0x01, 0x15, 0x02, // 63 -0x0E, 0x1F, 0x0E, // 64 -0x1E, 0x09, 0x1E, // 65 -0x1F, 0x15, 0x0A, // 66 -0x0E, 0x11, 0x11, // 67 -0x1F, 0x11, 0x0E, // 68 -0x1F, 0x15, 0x11, // 69 -0x1F, 0x05, 0x01, // 70 -0x0E, 0x11, 0x1D, // 71 -0x1F, 0x04, 0x1F, // 72 -0x00, 0x1F, // 73 -0x08, 0x10, 0x0F, // 74 -0x1F, 0x04, 0x1B, // 75 -0x1F, 0x10, 0x10, // 76 -0x1F, 0x02, 0x1F, // 77 -0x1F, 0x0E, 0x1F, // 78 -0x0E, 0x11, 0x0E, // 79 -0x1F, 0x05, 0x02, // 80 -0x0E, 0x19, 0x1E, // 81 -0x1F, 0x05, 0x1A, // 82 -0x12, 0x15, 0x09, // 83 -0x01, 0x1F, 0x01, // 84 -0x0F, 0x10, 0x0F, // 85 -0x07, 0x18, 0x07, // 86 -0x1F, 0x0C, 0x1F, // 87 -0x1B, 0x04, 0x1B, // 88 -0x03, 0x1C, 0x03, // 89 -0x19, 0x15, 0x13, // 90 + 0x03, // Width: 3 + 0x05, // Height: 5 + 0x20, // First char: 32 + 0x3B, // Number of chars: 59 + // Jump Table: + 0xFF, + 0xFF, + 0x00, + 0x03, // 32 + 0x00, + 0x00, + 0x02, + 0x03, // 33 + 0x00, + 0x02, + 0x03, + 0x04, // 34 + 0x00, + 0x05, + 0x03, + 0x04, // 35 + 0x00, + 0x08, + 0x03, + 0x04, // 36 + 0x00, + 0x0B, + 0x03, + 0x04, // 37 + 0x00, + 0x0E, + 0x03, + 0x04, // 38 + 0x00, + 0x11, + 0x02, + 0x03, // 39 + 0x00, + 0x13, + 0x03, + 0x04, // 40 + 0x00, + 0x16, + 0x02, + 0x03, // 41 + 0x00, + 0x18, + 0x03, + 0x04, // 42 + 0x00, + 0x1B, + 0x03, + 0x04, // 43 + 0x00, + 0x1E, + 0x02, + 0x03, // 44 + 0x00, + 0x20, + 0x03, + 0x04, // 45 + 0x00, + 0x23, + 0x02, + 0x03, // 46 + 0x00, + 0x25, + 0x03, + 0x04, // 47 + 0x00, + 0x28, + 0x03, + 0x04, // 48 + 0x00, + 0x2B, + 0x02, + 0x03, // 49 + 0x00, + 0x2D, + 0x03, + 0x04, // 50 + 0x00, + 0x30, + 0x03, + 0x04, // 51 + 0x00, + 0x33, + 0x03, + 0x04, // 52 + 0x00, + 0x36, + 0x03, + 0x04, // 53 + 0x00, + 0x39, + 0x03, + 0x04, // 54 + 0x00, + 0x3C, + 0x03, + 0x04, // 55 + 0x00, + 0x3F, + 0x03, + 0x04, // 56 + 0x00, + 0x42, + 0x03, + 0x04, // 57 + 0x00, + 0x45, + 0x02, + 0x03, // 58 + 0x00, + 0x47, + 0x02, + 0x03, // 59 + 0x00, + 0x49, + 0x03, + 0x04, // 60 + 0x00, + 0x4C, + 0x03, + 0x04, // 61 + 0x00, + 0x4F, + 0x03, + 0x04, // 62 + 0x00, + 0x52, + 0x03, + 0x04, // 63 + 0x00, + 0x55, + 0x03, + 0x04, // 64 + 0x00, + 0x58, + 0x03, + 0x04, // 65 + 0x00, + 0x5B, + 0x03, + 0x04, // 66 + 0x00, + 0x5E, + 0x03, + 0x04, // 67 + 0x00, + 0x61, + 0x03, + 0x04, // 68 + 0x00, + 0x64, + 0x03, + 0x04, // 69 + 0x00, + 0x67, + 0x03, + 0x04, // 70 + 0x00, + 0x6A, + 0x03, + 0x04, // 71 + 0x00, + 0x6D, + 0x03, + 0x04, // 72 + 0x00, + 0x70, + 0x02, + 0x03, // 73 + 0x00, + 0x72, + 0x03, + 0x04, // 74 + 0x00, + 0x75, + 0x03, + 0x04, // 75 + 0x00, + 0x78, + 0x03, + 0x04, // 76 + 0x00, + 0x7B, + 0x03, + 0x04, // 77 + 0x00, + 0x7E, + 0x03, + 0x04, // 78 + 0x00, + 0x81, + 0x03, + 0x04, // 79 + 0x00, + 0x84, + 0x03, + 0x04, // 80 + 0x00, + 0x87, + 0x03, + 0x04, // 81 + 0x00, + 0x8A, + 0x03, + 0x04, // 82 + 0x00, + 0x8D, + 0x03, + 0x04, // 83 + 0x00, + 0x90, + 0x03, + 0x04, // 84 + 0x00, + 0x93, + 0x03, + 0x04, // 85 + 0x00, + 0x96, + 0x03, + 0x04, // 86 + 0x00, + 0x99, + 0x03, + 0x04, // 87 + 0x00, + 0x9C, + 0x03, + 0x04, // 88 + 0x00, + 0x9F, + 0x03, + 0x04, // 89 + 0x00, + 0xA2, + 0x03, + 0x04, // 90 + // Font Data: + 0x00, + 0x17, // 33 + 0x03, + 0x00, + 0x03, // 34 + 0x0E, + 0x0A, + 0x0E, // 35 + 0x12, + 0x1F, + 0x09, // 36 + 0x19, + 0x04, + 0x13, // 37 + 0x1A, + 0x15, + 0x0E, // 38 + 0x00, + 0x03, // 39 + 0x00, + 0x0E, + 0x11, // 40 + 0x11, + 0x0E, // 41 + 0x0A, + 0x04, + 0x0A, // 42 + 0x04, + 0x0E, + 0x04, // 43 + 0x10, + 0x08, // 44 + 0x04, + 0x04, + 0x04, // 45 + 0x00, + 0x10, // 46 + 0x18, + 0x04, + 0x03, // 47 + 0x1F, + 0x11, + 0x1F, // 48 + 0x02, + 0x1F, // 49 + 0x19, + 0x15, + 0x13, // 50 + 0x11, + 0x15, + 0x1F, // 51 + 0x07, + 0x04, + 0x1F, // 52 + 0x17, + 0x15, + 0x1D, // 53 + 0x1F, + 0x15, + 0x1D, // 54 + 0x01, + 0x1D, + 0x03, // 55 + 0x1F, + 0x15, + 0x1F, // 56 + 0x17, + 0x15, + 0x1F, // 57 + 0x00, + 0x0A, // 58 + 0x10, + 0x0A, // 59 + 0x04, + 0x0A, + 0x11, // 60 + 0x0A, + 0x0A, + 0x0A, // 61 + 0x11, + 0x0A, + 0x04, // 62 + 0x01, + 0x15, + 0x02, // 63 + 0x0E, + 0x1F, + 0x0E, // 64 + 0x1E, + 0x09, + 0x1E, // 65 + 0x1F, + 0x15, + 0x0A, // 66 + 0x0E, + 0x11, + 0x11, // 67 + 0x1F, + 0x11, + 0x0E, // 68 + 0x1F, + 0x15, + 0x11, // 69 + 0x1F, + 0x05, + 0x01, // 70 + 0x0E, + 0x11, + 0x1D, // 71 + 0x1F, + 0x04, + 0x1F, // 72 + 0x00, + 0x1F, // 73 + 0x08, + 0x10, + 0x0F, // 74 + 0x1F, + 0x04, + 0x1B, // 75 + 0x1F, + 0x10, + 0x10, // 76 + 0x1F, + 0x02, + 0x1F, // 77 + 0x1F, + 0x0E, + 0x1F, // 78 + 0x0E, + 0x11, + 0x0E, // 79 + 0x1F, + 0x05, + 0x02, // 80 + 0x0E, + 0x19, + 0x1E, // 81 + 0x1F, + 0x05, + 0x1A, // 82 + 0x12, + 0x15, + 0x09, // 83 + 0x01, + 0x1F, + 0x01, // 84 + 0x0F, + 0x10, + 0x0F, // 85 + 0x07, + 0x18, + 0x07, // 86 + 0x1F, + 0x0C, + 0x1F, // 87 + 0x1B, + 0x04, + 0x1B, // 88 + 0x03, + 0x1C, + 0x03, // 89 + 0x19, + 0x15, + 0x13, // 90 }; // Font generated or edited with the glyphEditor const uint8_t Bitmap5x7[] PROGMEM = { -0x05, // Width: 5 -0x07, // Height: 7 -0x20, // First char: 32 -0x60, // Number of chars: 96 -// Jump Table: -0xFF, 0xFF, 0x00, 0x05, // 32 -0x00, 0x00, 0x03, 0x04, // 33 -0x00, 0x03, 0x04, 0x05, // 34 -0x00, 0x07, 0x05, 0x06, // 35 -0x00, 0x0C, 0x05, 0x06, // 36 -0x00, 0x11, 0x05, 0x06, // 37 -0x00, 0x16, 0x05, 0x06, // 38 -0x00, 0x1B, 0x03, 0x04, // 39 -0x00, 0x1E, 0x04, 0x05, // 40 -0x00, 0x22, 0x04, 0x05, // 41 -0x00, 0x26, 0x05, 0x06, // 42 -0x00, 0x2B, 0x05, 0x06, // 43 -0x00, 0x30, 0x03, 0x04, // 44 -0x00, 0x33, 0x05, 0x06, // 45 -0x00, 0x38, 0x03, 0x04, // 46 -0x00, 0x3B, 0x05, 0x06, // 47 -0x00, 0x40, 0x05, 0x06, // 48 -0x00, 0x45, 0x04, 0x05, // 49 -0x00, 0x49, 0x05, 0x06, // 50 -0x00, 0x4E, 0x05, 0x06, // 51 -0x00, 0x53, 0x05, 0x06, // 52 -0x00, 0x58, 0x05, 0x06, // 53 -0x00, 0x5D, 0x05, 0x06, // 54 -0x00, 0x62, 0x05, 0x06, // 55 -0x00, 0x67, 0x05, 0x06, // 56 -0x00, 0x6C, 0x05, 0x06, // 57 -0x00, 0x71, 0x03, 0x04, // 58 -0x00, 0x74, 0x03, 0x04, // 59 -0x00, 0x77, 0x05, 0x06, // 60 -0x00, 0x7C, 0x05, 0x06, // 61 -0x00, 0x81, 0x05, 0x06, // 62 -0x00, 0x86, 0x05, 0x06, // 63 -0x00, 0x8B, 0x05, 0x06, // 64 -0x00, 0x90, 0x05, 0x06, // 65 -0x00, 0x95, 0x05, 0x06, // 66 -0x00, 0x9A, 0x05, 0x06, // 67 -0x00, 0x9F, 0x05, 0x06, // 68 -0x00, 0xA4, 0x05, 0x06, // 69 -0x00, 0xA9, 0x05, 0x06, // 70 -0x00, 0xAE, 0x05, 0x06, // 71 -0x00, 0xB3, 0x05, 0x06, // 72 -0x00, 0xB8, 0x04, 0x05, // 73 -0x00, 0xBC, 0x05, 0x06, // 74 -0x00, 0xC1, 0x05, 0x06, // 75 -0x00, 0xC6, 0x05, 0x06, // 76 -0x00, 0xCB, 0x05, 0x06, // 77 -0x00, 0xD0, 0x05, 0x06, // 78 -0x00, 0xD5, 0x05, 0x06, // 79 -0x00, 0xDA, 0x05, 0x06, // 80 -0x00, 0xDF, 0x05, 0x06, // 81 -0x00, 0xE4, 0x05, 0x06, // 82 -0x00, 0xE9, 0x05, 0x06, // 83 -0x00, 0xEE, 0x05, 0x06, // 84 -0x00, 0xF3, 0x05, 0x06, // 85 -0x00, 0xF8, 0x05, 0x06, // 86 -0x00, 0xFD, 0x05, 0x06, // 87 -0x01, 0x02, 0x05, 0x06, // 88 -0x01, 0x07, 0x05, 0x06, // 89 -0x01, 0x0C, 0x05, 0x06, // 90 -0x01, 0x11, 0x04, 0x05, // 91 -0x01, 0x15, 0x05, 0x06, // 92 -0x01, 0x1A, 0x04, 0x05, // 93 -0x01, 0x1E, 0x05, 0x06, // 94 -0x01, 0x23, 0x05, 0x06, // 95 -0x01, 0x28, 0x03, 0x04, // 96 -0x01, 0x2B, 0x05, 0x06, // 97 -0x01, 0x30, 0x05, 0x06, // 98 -0x01, 0x35, 0x05, 0x06, // 99 -0x01, 0x3A, 0x05, 0x06, // 100 -0x01, 0x3F, 0x05, 0x06, // 101 -0x01, 0x44, 0x04, 0x05, // 102 -0x01, 0x48, 0x05, 0x06, // 103 -0x01, 0x4D, 0x04, 0x05, // 104 -0x01, 0x51, 0x03, 0x04, // 105 -0x01, 0x54, 0x03, 0x04, // 106 -0x01, 0x57, 0x04, 0x05, // 107 -0x01, 0x5B, 0x03, 0x04, // 108 -0x01, 0x5E, 0x05, 0x06, // 109 -0x01, 0x63, 0x04, 0x05, // 110 -0x01, 0x67, 0x05, 0x06, // 111 -0x01, 0x6C, 0x05, 0x06, // 112 -0x01, 0x71, 0x05, 0x06, // 113 -0x01, 0x76, 0x04, 0x05, // 114 -0x01, 0x7A, 0x05, 0x06, // 115 -0x01, 0x7F, 0x03, 0x04, // 116 -0x01, 0x82, 0x04, 0x05, // 117 -0x01, 0x86, 0x04, 0x05, // 118 -0x01, 0x8A, 0x05, 0x06, // 119 -0x01, 0x8F, 0x05, 0x06, // 120 -0x01, 0x94, 0x05, 0x06, // 121 -0x01, 0x99, 0x04, 0x05, // 122 -0x01, 0x9D, 0x04, 0x05, // 123 -0x01, 0xA1, 0x03, 0x04, // 124 -0x01, 0xA4, 0x05, 0x06, // 125 -0x01, 0xA9, 0x04, 0x05, // 126 -0x01, 0xAD, 0x05, 0x06, // 127 -// Font Data: -0x00, 0x00, 0x5F, // 33 -0x00, 0x03, 0x00, 0x03, // 34 -0x0A, 0x1F, 0x0A, 0x1F, 0x0A, // 35 -0x26, 0x49, 0x77, 0x49, 0x32, // 36 -0x43, 0x33, 0x08, 0x66, 0x61, // 37 -0x36, 0x49, 0x55, 0x22, 0x50, // 38 -0x00, 0x00, 0x03, // 39 -0x00, 0x1C, 0x22, 0x41, // 40 -0x00, 0x41, 0x22, 0x1C, // 41 -0x2A, 0x1C, 0x3E, 0x1C, 0x2A, // 42 -0x08, 0x08, 0x3E, 0x08, 0x08, // 43 -0x00, 0x00, 0x60, // 44 -0x08, 0x08, 0x08, 0x08, 0x08, // 45 -0x00, 0x00, 0x40, // 46 -0x40, 0x30, 0x08, 0x06, 0x01, // 47 -0x3E, 0x51, 0x49, 0x45, 0x3E, // 48 -0x00, 0x42, 0x7F, 0x40, // 49 -0x42, 0x61, 0x51, 0x49, 0x46, // 50 -0x22, 0x49, 0x49, 0x49, 0x36, // 51 -0x0F, 0x08, 0x08, 0x7F, 0x08, // 52 -0x2F, 0x49, 0x49, 0x49, 0x31, // 53 -0x3E, 0x49, 0x49, 0x49, 0x32, // 54 -0x61, 0x11, 0x09, 0x05, 0x03, // 55 -0x36, 0x49, 0x49, 0x49, 0x36, // 56 -0x26, 0x49, 0x49, 0x49, 0x3E, // 57 -0x00, 0x00, 0x14, // 58 -0x00, 0x40, 0x34, // 59 -0x08, 0x14, 0x14, 0x22, 0x22, // 60 -0x14, 0x14, 0x14, 0x14, 0x14, // 61 -0x22, 0x22, 0x14, 0x14, 0x08, // 62 -0x02, 0x01, 0x51, 0x09, 0x06, // 63 -0x3E, 0x49, 0x5D, 0x59, 0x5E, // 64 -0x7C, 0x0A, 0x09, 0x0A, 0x7C, // 65 -0x7F, 0x49, 0x49, 0x49, 0x36, // 66 -0x3E, 0x41, 0x41, 0x41, 0x22, // 67 -0x7F, 0x41, 0x41, 0x41, 0x3E, // 68 -0x7F, 0x49, 0x49, 0x49, 0x41, // 69 -0x7F, 0x09, 0x09, 0x09, 0x01, // 70 -0x3E, 0x41, 0x49, 0x49, 0x3A, // 71 -0x7F, 0x08, 0x08, 0x08, 0x7F, // 72 -0x00, 0x41, 0x7F, 0x41, // 73 -0x20, 0x40, 0x40, 0x40, 0x3F, // 74 -0x7F, 0x08, 0x14, 0x22, 0x41, // 75 -0x7F, 0x40, 0x40, 0x40, 0x40, // 76 -0x7F, 0x04, 0x08, 0x04, 0x7F, // 77 -0x7F, 0x04, 0x08, 0x10, 0x7F, // 78 -0x3E, 0x41, 0x41, 0x41, 0x3E, // 79 -0x7F, 0x09, 0x09, 0x09, 0x06, // 80 -0x3E, 0x41, 0x51, 0x61, 0x7E, // 81 -0x7F, 0x09, 0x09, 0x09, 0x76, // 82 -0x26, 0x49, 0x49, 0x49, 0x32, // 83 -0x01, 0x01, 0x7F, 0x01, 0x01, // 84 -0x3F, 0x40, 0x40, 0x40, 0x3F, // 85 -0x1F, 0x20, 0x40, 0x20, 0x1F, // 86 -0x3F, 0x40, 0x38, 0x40, 0x3F, // 87 -0x63, 0x14, 0x08, 0x14, 0x63, // 88 -0x03, 0x04, 0x78, 0x04, 0x03, // 89 -0x61, 0x51, 0x49, 0x45, 0x43, // 90 -0x00, 0x7F, 0x41, 0x41, // 91 -0x01, 0x06, 0x08, 0x30, 0x40, // 92 -0x00, 0x41, 0x41, 0x7F, // 93 -0x04, 0x02, 0x01, 0x02, 0x04, // 94 -0x40, 0x40, 0x40, 0x40, 0x40, // 95 -0x00, 0x03, 0x04, // 96 -0x30, 0x54, 0x54, 0x54, 0x78, // 97 -0x7E, 0x48, 0x48, 0x48, 0x30, // 98 -0x38, 0x44, 0x44, 0x44, 0x44, // 99 -0x30, 0x48, 0x48, 0x48, 0x7E, // 100 -0x38, 0x54, 0x54, 0x54, 0x18, // 101 -0x10, 0x7E, 0x11, 0x11, // 102 -0x08, 0x54, 0x54, 0x54, 0x38, // 103 -0x7E, 0x08, 0x08, 0x70, // 104 -0x00, 0x00, 0x7A, // 105 -0x00, 0x40, 0x7A, // 106 -0x7E, 0x10, 0x28, 0x40, // 107 -0x00, 0x7E, 0x40, // 108 -0x7C, 0x04, 0x7C, 0x04, 0x78, // 109 -0x7C, 0x04, 0x04, 0x78, // 110 -0x38, 0x44, 0x44, 0x44, 0x38, // 111 -0x7C, 0x24, 0x24, 0x24, 0x18, // 112 -0x18, 0x24, 0x24, 0x24, 0x7C, // 113 -0x78, 0x04, 0x04, 0x04, // 114 -0x48, 0x54, 0x54, 0x54, 0x24, // 115 -0x04, 0x7F, 0x44, // 116 -0x3C, 0x40, 0x40, 0x7C, // 117 -0x1C, 0x20, 0x40, 0x7C, // 118 -0x3C, 0x40, 0x70, 0x40, 0x7C, // 119 -0x44, 0x28, 0x10, 0x28, 0x44, // 120 -0x4C, 0x50, 0x20, 0x10, 0x0C, // 121 -0x64, 0x54, 0x54, 0x4C, // 122 -0x08, 0x36, 0x41, 0x41, // 123 -0x00, 0x00, 0x7F, // 124 -0x00, 0x41, 0x41, 0x36, 0x08, // 125 -0x02, 0x01, 0x02, 0x01, // 126 -0x7F, 0x7F, 0x7F, 0x7F, 0x7F, // 127 + 0x05, // Width: 5 + 0x07, // Height: 7 + 0x20, // First char: 32 + 0x60, // Number of chars: 96 + // Jump Table: + 0xFF, + 0xFF, + 0x00, + 0x05, // 32 + 0x00, + 0x00, + 0x03, + 0x04, // 33 + 0x00, + 0x03, + 0x04, + 0x05, // 34 + 0x00, + 0x07, + 0x05, + 0x06, // 35 + 0x00, + 0x0C, + 0x05, + 0x06, // 36 + 0x00, + 0x11, + 0x05, + 0x06, // 37 + 0x00, + 0x16, + 0x05, + 0x06, // 38 + 0x00, + 0x1B, + 0x03, + 0x04, // 39 + 0x00, + 0x1E, + 0x04, + 0x05, // 40 + 0x00, + 0x22, + 0x04, + 0x05, // 41 + 0x00, + 0x26, + 0x05, + 0x06, // 42 + 0x00, + 0x2B, + 0x05, + 0x06, // 43 + 0x00, + 0x30, + 0x03, + 0x04, // 44 + 0x00, + 0x33, + 0x05, + 0x06, // 45 + 0x00, + 0x38, + 0x03, + 0x04, // 46 + 0x00, + 0x3B, + 0x05, + 0x06, // 47 + 0x00, + 0x40, + 0x05, + 0x06, // 48 + 0x00, + 0x45, + 0x04, + 0x05, // 49 + 0x00, + 0x49, + 0x05, + 0x06, // 50 + 0x00, + 0x4E, + 0x05, + 0x06, // 51 + 0x00, + 0x53, + 0x05, + 0x06, // 52 + 0x00, + 0x58, + 0x05, + 0x06, // 53 + 0x00, + 0x5D, + 0x05, + 0x06, // 54 + 0x00, + 0x62, + 0x05, + 0x06, // 55 + 0x00, + 0x67, + 0x05, + 0x06, // 56 + 0x00, + 0x6C, + 0x05, + 0x06, // 57 + 0x00, + 0x71, + 0x03, + 0x04, // 58 + 0x00, + 0x74, + 0x03, + 0x04, // 59 + 0x00, + 0x77, + 0x05, + 0x06, // 60 + 0x00, + 0x7C, + 0x05, + 0x06, // 61 + 0x00, + 0x81, + 0x05, + 0x06, // 62 + 0x00, + 0x86, + 0x05, + 0x06, // 63 + 0x00, + 0x8B, + 0x05, + 0x06, // 64 + 0x00, + 0x90, + 0x05, + 0x06, // 65 + 0x00, + 0x95, + 0x05, + 0x06, // 66 + 0x00, + 0x9A, + 0x05, + 0x06, // 67 + 0x00, + 0x9F, + 0x05, + 0x06, // 68 + 0x00, + 0xA4, + 0x05, + 0x06, // 69 + 0x00, + 0xA9, + 0x05, + 0x06, // 70 + 0x00, + 0xAE, + 0x05, + 0x06, // 71 + 0x00, + 0xB3, + 0x05, + 0x06, // 72 + 0x00, + 0xB8, + 0x04, + 0x05, // 73 + 0x00, + 0xBC, + 0x05, + 0x06, // 74 + 0x00, + 0xC1, + 0x05, + 0x06, // 75 + 0x00, + 0xC6, + 0x05, + 0x06, // 76 + 0x00, + 0xCB, + 0x05, + 0x06, // 77 + 0x00, + 0xD0, + 0x05, + 0x06, // 78 + 0x00, + 0xD5, + 0x05, + 0x06, // 79 + 0x00, + 0xDA, + 0x05, + 0x06, // 80 + 0x00, + 0xDF, + 0x05, + 0x06, // 81 + 0x00, + 0xE4, + 0x05, + 0x06, // 82 + 0x00, + 0xE9, + 0x05, + 0x06, // 83 + 0x00, + 0xEE, + 0x05, + 0x06, // 84 + 0x00, + 0xF3, + 0x05, + 0x06, // 85 + 0x00, + 0xF8, + 0x05, + 0x06, // 86 + 0x00, + 0xFD, + 0x05, + 0x06, // 87 + 0x01, + 0x02, + 0x05, + 0x06, // 88 + 0x01, + 0x07, + 0x05, + 0x06, // 89 + 0x01, + 0x0C, + 0x05, + 0x06, // 90 + 0x01, + 0x11, + 0x04, + 0x05, // 91 + 0x01, + 0x15, + 0x05, + 0x06, // 92 + 0x01, + 0x1A, + 0x04, + 0x05, // 93 + 0x01, + 0x1E, + 0x05, + 0x06, // 94 + 0x01, + 0x23, + 0x05, + 0x06, // 95 + 0x01, + 0x28, + 0x03, + 0x04, // 96 + 0x01, + 0x2B, + 0x05, + 0x06, // 97 + 0x01, + 0x30, + 0x05, + 0x06, // 98 + 0x01, + 0x35, + 0x05, + 0x06, // 99 + 0x01, + 0x3A, + 0x05, + 0x06, // 100 + 0x01, + 0x3F, + 0x05, + 0x06, // 101 + 0x01, + 0x44, + 0x04, + 0x05, // 102 + 0x01, + 0x48, + 0x05, + 0x06, // 103 + 0x01, + 0x4D, + 0x04, + 0x05, // 104 + 0x01, + 0x51, + 0x03, + 0x04, // 105 + 0x01, + 0x54, + 0x03, + 0x04, // 106 + 0x01, + 0x57, + 0x04, + 0x05, // 107 + 0x01, + 0x5B, + 0x03, + 0x04, // 108 + 0x01, + 0x5E, + 0x05, + 0x06, // 109 + 0x01, + 0x63, + 0x04, + 0x05, // 110 + 0x01, + 0x67, + 0x05, + 0x06, // 111 + 0x01, + 0x6C, + 0x05, + 0x06, // 112 + 0x01, + 0x71, + 0x05, + 0x06, // 113 + 0x01, + 0x76, + 0x04, + 0x05, // 114 + 0x01, + 0x7A, + 0x05, + 0x06, // 115 + 0x01, + 0x7F, + 0x03, + 0x04, // 116 + 0x01, + 0x82, + 0x04, + 0x05, // 117 + 0x01, + 0x86, + 0x04, + 0x05, // 118 + 0x01, + 0x8A, + 0x05, + 0x06, // 119 + 0x01, + 0x8F, + 0x05, + 0x06, // 120 + 0x01, + 0x94, + 0x05, + 0x06, // 121 + 0x01, + 0x99, + 0x04, + 0x05, // 122 + 0x01, + 0x9D, + 0x04, + 0x05, // 123 + 0x01, + 0xA1, + 0x03, + 0x04, // 124 + 0x01, + 0xA4, + 0x05, + 0x06, // 125 + 0x01, + 0xA9, + 0x04, + 0x05, // 126 + 0x01, + 0xAD, + 0x05, + 0x06, // 127 + // Font Data: + 0x00, + 0x00, + 0x5F, // 33 + 0x00, + 0x03, + 0x00, + 0x03, // 34 + 0x0A, + 0x1F, + 0x0A, + 0x1F, + 0x0A, // 35 + 0x26, + 0x49, + 0x77, + 0x49, + 0x32, // 36 + 0x43, + 0x33, + 0x08, + 0x66, + 0x61, // 37 + 0x36, + 0x49, + 0x55, + 0x22, + 0x50, // 38 + 0x00, + 0x00, + 0x03, // 39 + 0x00, + 0x1C, + 0x22, + 0x41, // 40 + 0x00, + 0x41, + 0x22, + 0x1C, // 41 + 0x2A, + 0x1C, + 0x3E, + 0x1C, + 0x2A, // 42 + 0x08, + 0x08, + 0x3E, + 0x08, + 0x08, // 43 + 0x00, + 0x00, + 0x60, // 44 + 0x08, + 0x08, + 0x08, + 0x08, + 0x08, // 45 + 0x00, + 0x00, + 0x40, // 46 + 0x40, + 0x30, + 0x08, + 0x06, + 0x01, // 47 + 0x3E, + 0x51, + 0x49, + 0x45, + 0x3E, // 48 + 0x00, + 0x42, + 0x7F, + 0x40, // 49 + 0x42, + 0x61, + 0x51, + 0x49, + 0x46, // 50 + 0x22, + 0x49, + 0x49, + 0x49, + 0x36, // 51 + 0x0F, + 0x08, + 0x08, + 0x7F, + 0x08, // 52 + 0x2F, + 0x49, + 0x49, + 0x49, + 0x31, // 53 + 0x3E, + 0x49, + 0x49, + 0x49, + 0x32, // 54 + 0x61, + 0x11, + 0x09, + 0x05, + 0x03, // 55 + 0x36, + 0x49, + 0x49, + 0x49, + 0x36, // 56 + 0x26, + 0x49, + 0x49, + 0x49, + 0x3E, // 57 + 0x00, + 0x00, + 0x14, // 58 + 0x00, + 0x40, + 0x34, // 59 + 0x08, + 0x14, + 0x14, + 0x22, + 0x22, // 60 + 0x14, + 0x14, + 0x14, + 0x14, + 0x14, // 61 + 0x22, + 0x22, + 0x14, + 0x14, + 0x08, // 62 + 0x02, + 0x01, + 0x51, + 0x09, + 0x06, // 63 + 0x3E, + 0x49, + 0x5D, + 0x59, + 0x5E, // 64 + 0x7C, + 0x0A, + 0x09, + 0x0A, + 0x7C, // 65 + 0x7F, + 0x49, + 0x49, + 0x49, + 0x36, // 66 + 0x3E, + 0x41, + 0x41, + 0x41, + 0x22, // 67 + 0x7F, + 0x41, + 0x41, + 0x41, + 0x3E, // 68 + 0x7F, + 0x49, + 0x49, + 0x49, + 0x41, // 69 + 0x7F, + 0x09, + 0x09, + 0x09, + 0x01, // 70 + 0x3E, + 0x41, + 0x49, + 0x49, + 0x3A, // 71 + 0x7F, + 0x08, + 0x08, + 0x08, + 0x7F, // 72 + 0x00, + 0x41, + 0x7F, + 0x41, // 73 + 0x20, + 0x40, + 0x40, + 0x40, + 0x3F, // 74 + 0x7F, + 0x08, + 0x14, + 0x22, + 0x41, // 75 + 0x7F, + 0x40, + 0x40, + 0x40, + 0x40, // 76 + 0x7F, + 0x04, + 0x08, + 0x04, + 0x7F, // 77 + 0x7F, + 0x04, + 0x08, + 0x10, + 0x7F, // 78 + 0x3E, + 0x41, + 0x41, + 0x41, + 0x3E, // 79 + 0x7F, + 0x09, + 0x09, + 0x09, + 0x06, // 80 + 0x3E, + 0x41, + 0x51, + 0x61, + 0x7E, // 81 + 0x7F, + 0x09, + 0x09, + 0x09, + 0x76, // 82 + 0x26, + 0x49, + 0x49, + 0x49, + 0x32, // 83 + 0x01, + 0x01, + 0x7F, + 0x01, + 0x01, // 84 + 0x3F, + 0x40, + 0x40, + 0x40, + 0x3F, // 85 + 0x1F, + 0x20, + 0x40, + 0x20, + 0x1F, // 86 + 0x3F, + 0x40, + 0x38, + 0x40, + 0x3F, // 87 + 0x63, + 0x14, + 0x08, + 0x14, + 0x63, // 88 + 0x03, + 0x04, + 0x78, + 0x04, + 0x03, // 89 + 0x61, + 0x51, + 0x49, + 0x45, + 0x43, // 90 + 0x00, + 0x7F, + 0x41, + 0x41, // 91 + 0x01, + 0x06, + 0x08, + 0x30, + 0x40, // 92 + 0x00, + 0x41, + 0x41, + 0x7F, // 93 + 0x04, + 0x02, + 0x01, + 0x02, + 0x04, // 94 + 0x40, + 0x40, + 0x40, + 0x40, + 0x40, // 95 + 0x00, + 0x03, + 0x04, // 96 + 0x30, + 0x54, + 0x54, + 0x54, + 0x78, // 97 + 0x7E, + 0x48, + 0x48, + 0x48, + 0x30, // 98 + 0x38, + 0x44, + 0x44, + 0x44, + 0x44, // 99 + 0x30, + 0x48, + 0x48, + 0x48, + 0x7E, // 100 + 0x38, + 0x54, + 0x54, + 0x54, + 0x18, // 101 + 0x10, + 0x7E, + 0x11, + 0x11, // 102 + 0x08, + 0x54, + 0x54, + 0x54, + 0x38, // 103 + 0x7E, + 0x08, + 0x08, + 0x70, // 104 + 0x00, + 0x00, + 0x7A, // 105 + 0x00, + 0x40, + 0x7A, // 106 + 0x7E, + 0x10, + 0x28, + 0x40, // 107 + 0x00, + 0x7E, + 0x40, // 108 + 0x7C, + 0x04, + 0x7C, + 0x04, + 0x78, // 109 + 0x7C, + 0x04, + 0x04, + 0x78, // 110 + 0x38, + 0x44, + 0x44, + 0x44, + 0x38, // 111 + 0x7C, + 0x24, + 0x24, + 0x24, + 0x18, // 112 + 0x18, + 0x24, + 0x24, + 0x24, + 0x7C, // 113 + 0x78, + 0x04, + 0x04, + 0x04, // 114 + 0x48, + 0x54, + 0x54, + 0x54, + 0x24, // 115 + 0x04, + 0x7F, + 0x44, // 116 + 0x3C, + 0x40, + 0x40, + 0x7C, // 117 + 0x1C, + 0x20, + 0x40, + 0x7C, // 118 + 0x3C, + 0x40, + 0x70, + 0x40, + 0x7C, // 119 + 0x44, + 0x28, + 0x10, + 0x28, + 0x44, // 120 + 0x4C, + 0x50, + 0x20, + 0x10, + 0x0C, // 121 + 0x64, + 0x54, + 0x54, + 0x4C, // 122 + 0x08, + 0x36, + 0x41, + 0x41, // 123 + 0x00, + 0x00, + 0x7F, // 124 + 0x00, + 0x41, + 0x41, + 0x36, + 0x08, // 125 + 0x02, + 0x01, + 0x02, + 0x01, // 126 + 0x7F, + 0x7F, + 0x7F, + 0x7F, + 0x7F, // 127 }; // Font generated or edited with the glyphEditor const uint8_t Bitmap7x15[] PROGMEM = { - 0x07, // Width: 7 - 0x0F, // Height: 15 - 0x20, // First char: 32 - 0x1E, // Number of chars: 30 + 0x07, // Width: 7 + 0x0F, // Height: 15 + 0x20, // First char: 32 + 0x1E, // Number of chars: 30 // Jump Table: - 0xFF, 0xFF, 0x00, 0x07, // 32 - 0x00, 0x00, 0x0E, 0x08, // 33 - 0x00, 0x0E, 0x0B, 0x07, // 34 - 0x00, 0x19, 0x0E, 0x08, // 35 - 0x00, 0x27, 0x0E, 0x08, // 36 - 0x00, 0x35, 0x0C, 0x07, // 37 - 0x00, 0x41, 0x0E, 0x08, // 38 - 0x00, 0x4F, 0x03, 0x03, // 39 - 0x00, 0x52, 0x09, 0x06, // 40 - 0xFF, 0xFF, 0x00, 0x07, // 41 - 0xFF, 0xFF, 0x00, 0x07, // 42 - 0x00, 0x5B, 0x0B, 0x07, // 43 - 0xFF, 0xFF, 0x00, 0x07, // 44 - 0x00, 0x66, 0x0B, 0x07, // 45 - 0x00, 0x71, 0x06, 0x04, // 46 - 0xFF, 0xFF, 0x00, 0x07, // 47 - 0x00, 0x77, 0x0C, 0x07, // 48 - 0x00, 0x83, 0x0C, 0x07, // 49 - 0x00, 0x8F, 0x0C, 0x07, // 50 - 0x00, 0x9B, 0x0C, 0x07, // 51 - 0x00, 0xA7, 0x0C, 0x07, // 52 - 0x00, 0xB3, 0x0C, 0x07, // 53 - 0x00, 0xBF, 0x0C, 0x07, // 54 - 0x00, 0xCB, 0x0B, 0x07, // 55 - 0x00, 0xD6, 0x0C, 0x07, // 56 - 0x00, 0xE2, 0x0C, 0x07, // 57 - 0x00, 0xEE, 0x06, 0x04, // 58 - 0x00, 0xF4, 0x0A, 0x06, // 59 - 0x00, 0xFE, 0x08, 0x05, // 60 - 0x01, 0x06, 0x08, 0x05, // 61 + 0xFF, + 0xFF, + 0x00, + 0x07, // 32 + 0x00, + 0x00, + 0x0E, + 0x08, // 33 + 0x00, + 0x0E, + 0x0B, + 0x07, // 34 + 0x00, + 0x19, + 0x0E, + 0x08, // 35 + 0x00, + 0x27, + 0x0E, + 0x08, // 36 + 0x00, + 0x35, + 0x0C, + 0x07, // 37 + 0x00, + 0x41, + 0x0E, + 0x08, // 38 + 0x00, + 0x4F, + 0x03, + 0x03, // 39 + 0x00, + 0x52, + 0x09, + 0x06, // 40 + 0xFF, + 0xFF, + 0x00, + 0x07, // 41 + 0xFF, + 0xFF, + 0x00, + 0x07, // 42 + 0x00, + 0x5B, + 0x0B, + 0x07, // 43 + 0xFF, + 0xFF, + 0x00, + 0x07, // 44 + 0x00, + 0x66, + 0x0B, + 0x07, // 45 + 0x00, + 0x71, + 0x06, + 0x04, // 46 + 0xFF, + 0xFF, + 0x00, + 0x07, // 47 + 0x00, + 0x77, + 0x0C, + 0x07, // 48 + 0x00, + 0x83, + 0x0C, + 0x07, // 49 + 0x00, + 0x8F, + 0x0C, + 0x07, // 50 + 0x00, + 0x9B, + 0x0C, + 0x07, // 51 + 0x00, + 0xA7, + 0x0C, + 0x07, // 52 + 0x00, + 0xB3, + 0x0C, + 0x07, // 53 + 0x00, + 0xBF, + 0x0C, + 0x07, // 54 + 0x00, + 0xCB, + 0x0B, + 0x07, // 55 + 0x00, + 0xD6, + 0x0C, + 0x07, // 56 + 0x00, + 0xE2, + 0x0C, + 0x07, // 57 + 0x00, + 0xEE, + 0x06, + 0x04, // 58 + 0x00, + 0xF4, + 0x0A, + 0x06, // 59 + 0x00, + 0xFE, + 0x08, + 0x05, // 60 + 0x01, + 0x06, + 0x08, + 0x05, // 61 // Font Data: - 0xFF, 0x7F, 0xFF, 0x7F, 0x83, 0x01, 0x83, 0x01, 0x83, 0x07, 0xFF, 0x7F, 0xFE, 0x7C, // 33 - 0x0F, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x0F, // 34 - 0xFF, 0x7F, 0xFF, 0x7F, 0x03, 0x60, 0x03, 0x60, 0x07, 0x70, 0xFE, 0x3F, 0xFC, 0x1F, // 35 - 0xFF, 0x7F, 0xFF, 0x7F, 0xC3, 0x60, 0xC3, 0x60, 0x03, 0x60, 0x03, 0x60, 0x03, 0x60, // 36 - 0xF8, 0x0F, 0xFE, 0x3F, 0x0F, 0x78, 0x03, 0x60, 0x03, 0x60, 0x03, 0x60, // 37 - 0xFE, 0x7F, 0xFF, 0x7F, 0x83, 0x01, 0x83, 0x01, 0x83, 0x01, 0xFF, 0x7F, 0xFE, 0x7F, // 38 - 0x0F, 0x00, 0x0F, // 39 - 0x1E, 0x00, 0x33, 0x00, 0x21, 0x00, 0x33, 0x00, 0x1E, // 40 - 0xC0, 0x00, 0xC0, 0x00, 0xF0, 0x03, 0xF0, 0x03, 0xC0, 0x00, 0xC0, // 43 - 0xC0, 0x00, 0xC0, 0x00, 0xC0, 0x00, 0xC0, 0x00, 0xC0, 0x00, 0xC0, // 45 - 0x00, 0x00, 0x00, 0x60, 0x00, 0x60, // 46 - 0xFE, 0x3F, 0xFF, 0x7F, 0x03, 0x60, 0x03, 0x60, 0xFF, 0x7F, 0xFE, 0x3F, // 48 - 0x00, 0x60, 0x03, 0x60, 0xFF, 0x7F, 0xFF, 0x7F, 0x00, 0x60, 0x00, 0x60, // 49 - 0x03, 0x7E, 0x03, 0x7F, 0x83, 0x63, 0xE7, 0x61, 0xFE, 0x60, 0x3C, 0x60, // 50 - 0x03, 0x60, 0x03, 0x60, 0xC3, 0x60, 0xC7, 0x70, 0xFE, 0x3F, 0x3C, 0x1F, // 51 - 0xFF, 0x01, 0xFF, 0x01, 0x80, 0x01, 0x80, 0x01, 0xF8, 0x7F, 0xF8, 0x7F, // 52 - 0x7F, 0x60, 0x7F, 0x60, 0x63, 0x60, 0xE3, 0x70, 0xC3, 0x3F, 0x83, 0x1F, // 53 - 0xFE, 0x3F, 0xFF, 0x7F, 0x63, 0x60, 0x63, 0x60, 0xE3, 0x7F, 0xC0, 0x3F, // 54 - 0x03, 0x00, 0x03, 0x70, 0x03, 0x7F, 0xF3, 0x0F, 0xFF, 0x01, 0x1F, // 55 - 0x3E, 0x3F, 0xFF, 0x7F, 0xC3, 0x60, 0xC3, 0x60, 0xFF, 0x7F, 0x3E, 0x3F, // 56 - 0x7E, 0x00, 0xFF, 0x60, 0xC3, 0x60, 0xC3, 0x60, 0xFF, 0x7F, 0xFE, 0x3F, // 57 - 0x00, 0x00, 0x30, 0x03, 0x30, 0x03, // 58 - 0x00, 0x7F, 0x00, 0x01, 0x00, 0x7E, 0x00, 0x01, 0x00, 0x7E, // 59 - 0xF0, 0x7F, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7E, // 60 - 0x00, 0x23, 0x80, 0x44, 0x80, 0x44, 0x00, 0x39, // 61 + 0xFF, + 0x7F, + 0xFF, + 0x7F, + 0x83, + 0x01, + 0x83, + 0x01, + 0x83, + 0x07, + 0xFF, + 0x7F, + 0xFE, + 0x7C, // 33 + 0x0F, + 0x00, + 0x0F, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x0F, + 0x00, + 0x0F, // 34 + 0xFF, + 0x7F, + 0xFF, + 0x7F, + 0x03, + 0x60, + 0x03, + 0x60, + 0x07, + 0x70, + 0xFE, + 0x3F, + 0xFC, + 0x1F, // 35 + 0xFF, + 0x7F, + 0xFF, + 0x7F, + 0xC3, + 0x60, + 0xC3, + 0x60, + 0x03, + 0x60, + 0x03, + 0x60, + 0x03, + 0x60, // 36 + 0xF8, + 0x0F, + 0xFE, + 0x3F, + 0x0F, + 0x78, + 0x03, + 0x60, + 0x03, + 0x60, + 0x03, + 0x60, // 37 + 0xFE, + 0x7F, + 0xFF, + 0x7F, + 0x83, + 0x01, + 0x83, + 0x01, + 0x83, + 0x01, + 0xFF, + 0x7F, + 0xFE, + 0x7F, // 38 + 0x0F, + 0x00, + 0x0F, // 39 + 0x1E, + 0x00, + 0x33, + 0x00, + 0x21, + 0x00, + 0x33, + 0x00, + 0x1E, // 40 + 0xC0, + 0x00, + 0xC0, + 0x00, + 0xF0, + 0x03, + 0xF0, + 0x03, + 0xC0, + 0x00, + 0xC0, // 43 + 0xC0, + 0x00, + 0xC0, + 0x00, + 0xC0, + 0x00, + 0xC0, + 0x00, + 0xC0, + 0x00, + 0xC0, // 45 + 0x00, + 0x00, + 0x00, + 0x60, + 0x00, + 0x60, // 46 + 0xFE, + 0x3F, + 0xFF, + 0x7F, + 0x03, + 0x60, + 0x03, + 0x60, + 0xFF, + 0x7F, + 0xFE, + 0x3F, // 48 + 0x00, + 0x60, + 0x03, + 0x60, + 0xFF, + 0x7F, + 0xFF, + 0x7F, + 0x00, + 0x60, + 0x00, + 0x60, // 49 + 0x03, + 0x7E, + 0x03, + 0x7F, + 0x83, + 0x63, + 0xE7, + 0x61, + 0xFE, + 0x60, + 0x3C, + 0x60, // 50 + 0x03, + 0x60, + 0x03, + 0x60, + 0xC3, + 0x60, + 0xC7, + 0x70, + 0xFE, + 0x3F, + 0x3C, + 0x1F, // 51 + 0xFF, + 0x01, + 0xFF, + 0x01, + 0x80, + 0x01, + 0x80, + 0x01, + 0xF8, + 0x7F, + 0xF8, + 0x7F, // 52 + 0x7F, + 0x60, + 0x7F, + 0x60, + 0x63, + 0x60, + 0xE3, + 0x70, + 0xC3, + 0x3F, + 0x83, + 0x1F, // 53 + 0xFE, + 0x3F, + 0xFF, + 0x7F, + 0x63, + 0x60, + 0x63, + 0x60, + 0xE3, + 0x7F, + 0xC0, + 0x3F, // 54 + 0x03, + 0x00, + 0x03, + 0x70, + 0x03, + 0x7F, + 0xF3, + 0x0F, + 0xFF, + 0x01, + 0x1F, // 55 + 0x3E, + 0x3F, + 0xFF, + 0x7F, + 0xC3, + 0x60, + 0xC3, + 0x60, + 0xFF, + 0x7F, + 0x3E, + 0x3F, // 56 + 0x7E, + 0x00, + 0xFF, + 0x60, + 0xC3, + 0x60, + 0xC3, + 0x60, + 0xFF, + 0x7F, + 0xFE, + 0x3F, // 57 + 0x00, + 0x00, + 0x30, + 0x03, + 0x30, + 0x03, // 58 + 0x00, + 0x7F, + 0x00, + 0x01, + 0x00, + 0x7E, + 0x00, + 0x01, + 0x00, + 0x7E, // 59 + 0xF0, + 0x7F, + 0x00, + 0x01, + 0x00, + 0x01, + 0x00, + 0x7E, // 60 + 0x00, + 0x23, + 0x80, + 0x44, + 0x80, + 0x44, + 0x00, + 0x39, // 61 }; // Font generated or edited with the glyphEditor const uint8_t CommSymbols[] PROGMEM = { -0x0A, // Width: 10 -0x08, // Height: 8 -0x41, // First char: 65 -0x0D, // Number of chars: 13 -// Jump Table: -0x00, 0x00, 0x05, 0x06, // 65 -0x00, 0x05, 0x0A, 0x0B, // 66 -0x00, 0x0F, 0x05, 0x06, // 67 -0x00, 0x14, 0x05, 0x06, // 68 -0x00, 0x19, 0x06, 0x07, // 69 -0x00, 0x1F, 0x07, 0x08, // 70 -0x00, 0x26, 0x09, 0x0A, // 71 -0x00, 0x2F, 0x09, 0x0A, // 72 -0x00, 0x38, 0x09, 0x0A, // 73 -0x00, 0x41, 0x0A, 0x0B, // 74 -0x00, 0x4B, 0x07, 0x08, // 75 -0x00, 0x52, 0x07, 0x08, // 76 -0x00, 0x59, 0x05, 0x06, // 77 -// Font Data: -0x15, 0x05, 0x19, 0x02, 0x1C, // 65 -0x02, 0x07, 0x02, 0x0A, 0x0A, 0x0A, 0x0A, 0x08, 0x1C, 0x08, // 66 -0x00, 0x00, 0x08, 0x18, 0x08, // 67 -0x00, 0x00, 0x04, 0x14, 0x04, // 68 -0x00, 0x04, 0x02, 0x12, 0x02, 0x04, // 69 -0x02, 0x01, 0x01, 0x11, 0x01, 0x01, 0x02, // 70 -0x0E, 0x11, 0x15, 0x71, 0x45, 0x31, 0x15, 0x11, 0x0E, // 71 -0x10, 0x1F, 0x01, 0x1F, 0x10, 0x1F, 0x01, 0x1F, 0x10, // 72 -0x07, 0x03, 0x05, 0x08, 0x11, 0x02, 0x14, 0x18, 0x1C, // 73 -0x07, 0x03, 0x05, 0x08, 0x11, 0x22, 0x04, 0x28, 0x30, 0x38, // 74 -0x1C, 0x22, 0x41, 0x4F, 0x49, 0x2A, 0x1C, // 75 -0x0E, 0x1B, 0x0A, 0x1B, 0x0A, 0x1F, 0x0E, // 76 -0x0E, 0x11, 0x17, 0x15, 0x0E, // 77 + 0x0A, // Width: 10 + 0x08, // Height: 8 + 0x41, // First char: 65 + 0x0D, // Number of chars: 13 + // Jump Table: + 0x00, + 0x00, + 0x05, + 0x06, // 65 + 0x00, + 0x05, + 0x0A, + 0x0B, // 66 + 0x00, + 0x0F, + 0x05, + 0x06, // 67 + 0x00, + 0x14, + 0x05, + 0x06, // 68 + 0x00, + 0x19, + 0x06, + 0x07, // 69 + 0x00, + 0x1F, + 0x07, + 0x08, // 70 + 0x00, + 0x26, + 0x09, + 0x0A, // 71 + 0x00, + 0x2F, + 0x09, + 0x0A, // 72 + 0x00, + 0x38, + 0x09, + 0x0A, // 73 + 0x00, + 0x41, + 0x0A, + 0x0B, // 74 + 0x00, + 0x4B, + 0x07, + 0x08, // 75 + 0x00, + 0x52, + 0x07, + 0x08, // 76 + 0x00, + 0x59, + 0x05, + 0x06, // 77 + // Font Data: + 0x15, + 0x05, + 0x19, + 0x02, + 0x1C, // 65 + 0x02, + 0x07, + 0x02, + 0x0A, + 0x0A, + 0x0A, + 0x0A, + 0x08, + 0x1C, + 0x08, // 66 + 0x00, + 0x00, + 0x08, + 0x18, + 0x08, // 67 + 0x00, + 0x00, + 0x04, + 0x14, + 0x04, // 68 + 0x00, + 0x04, + 0x02, + 0x12, + 0x02, + 0x04, // 69 + 0x02, + 0x01, + 0x01, + 0x11, + 0x01, + 0x01, + 0x02, // 70 + 0x0E, + 0x11, + 0x15, + 0x71, + 0x45, + 0x31, + 0x15, + 0x11, + 0x0E, // 71 + 0x10, + 0x1F, + 0x01, + 0x1F, + 0x10, + 0x1F, + 0x01, + 0x1F, + 0x10, // 72 + 0x07, + 0x03, + 0x05, + 0x08, + 0x11, + 0x02, + 0x14, + 0x18, + 0x1C, // 73 + 0x07, + 0x03, + 0x05, + 0x08, + 0x11, + 0x22, + 0x04, + 0x28, + 0x30, + 0x38, // 74 + 0x1C, + 0x22, + 0x41, + 0x4F, + 0x49, + 0x2A, + 0x1C, // 75 + 0x0E, + 0x1B, + 0x0A, + 0x1B, + 0x0A, + 0x1F, + 0x0E, // 76 + 0x0E, + 0x11, + 0x17, + 0x15, + 0x0E, // 77 }; // Font generated or edited with the glyphEditor const uint8_t OATLogo[] PROGMEM = { -0x1A, // Width: 26 -0x11, // Height: 17 -0x21, // First char: 33 -0x01, // Number of chars: 1 -// Jump Table: -0x00, 0x00, 0x4D, 0x1B, // 33 -// Font Data: -0x00, 0x0E, 0x00, 0x80, 0x31, 0x00, 0xC0, 0x40, 0x00, 0x40, 0x00, 0x00, 0x20, 0x00, 0x00, 0x10, 0x01, 0x00, 0x10, 0x00, 0x00, 0x08, 0x01, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x20, 0x05, 0x00, 0x40, 0x03, 0x00, 0x80, 0x03, 0x00, 0xF5, 0x5F, 0x01, 0x80, 0x03, 0x00, 0x40, 0x05, 0x00, 0x40, 0x41, 0x00, 0x00, 0x41, 0x00, 0x00, 0x20, 0x00, 0x00, 0x21, 0x00, 0x00, 0x10, 0x00, 0x00, 0x11, 0x00, 0x00, 0x08, 0x00, 0x08, 0x0C, 0x00, 0x30, 0x06, 0x00, 0xC0, 0x01, // 33 + 0x1A, // Width: 26 + 0x11, // Height: 17 + 0x21, // First char: 33 + 0x01, // Number of chars: 1 + // Jump Table: + 0x00, + 0x00, + 0x4D, + 0x1B, // 33 + // Font Data: + 0x00, + 0x0E, + 0x00, + 0x80, + 0x31, + 0x00, + 0xC0, + 0x40, + 0x00, + 0x40, + 0x00, + 0x00, + 0x20, + 0x00, + 0x00, + 0x10, + 0x01, + 0x00, + 0x10, + 0x00, + 0x00, + 0x08, + 0x01, + 0x00, + 0x08, + 0x00, + 0x00, + 0x00, + 0x01, + 0x00, + 0x20, + 0x05, + 0x00, + 0x40, + 0x03, + 0x00, + 0x80, + 0x03, + 0x00, + 0xF5, + 0x5F, + 0x01, + 0x80, + 0x03, + 0x00, + 0x40, + 0x05, + 0x00, + 0x40, + 0x41, + 0x00, + 0x00, + 0x41, + 0x00, + 0x00, + 0x20, + 0x00, + 0x00, + 0x21, + 0x00, + 0x00, + 0x10, + 0x00, + 0x00, + 0x11, + 0x00, + 0x00, + 0x08, + 0x00, + 0x08, + 0x0C, + 0x00, + 0x30, + 0x06, + 0x00, + 0xC0, + 0x01, // 33 }; // clang-format on diff --git a/src/libs/TimerInterrupt/TimerInterrupt.cpp b/src/libs/TimerInterrupt/TimerInterrupt.cpp index 8ffe6ffa..8afab480 100644 --- a/src/libs/TimerInterrupt/TimerInterrupt.cpp +++ b/src/libs/TimerInterrupt/TimerInterrupt.cpp @@ -27,659 +27,658 @@ ****************************************************************************************************************************/ #if !defined(ESP32) -#define TIMER_INTERRUPT_DEBUG 0 -#include "TimerInterrupt.h" + #define TIMER_INTERRUPT_DEBUG 0 + #include "TimerInterrupt.h" -//#ifndef TIMER_INTERRUPT_DEBUG -#define TIMER_INTERRUPT_DEBUG 0 + //#ifndef TIMER_INTERRUPT_DEBUG + #define TIMER_INTERRUPT_DEBUG 0 //#endif void TimerInterrupt::init(int8_t timer) { - // Set timer specific stuff - // All timers in CTC mode - // 8 bit timers will require changing prescalar values, - // whereas 16 bit timers are set to either ck/1 or ck/64 prescalar - - //cli();//stop interrupts - noInterrupts(); - - switch (timer) - { -#if defined(TCCR1A) && defined(TCCR1B) && defined(WGM12) - case 1: - // 16 bit timer - TCCR1A = 0; - TCCR1B = 0; - // Page 172-173. ATmega 328/328P or Page 145-146 of ATmega 640/1280/2560 - // Mode 4 => Clear Timer on Compare match (CTC) using OCR1A for counter value - bitWrite(TCCR1B, WGM12, 1); - // No scaling now - bitWrite(TCCR1B, CS10, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T1"); -#endif - - break; -#endif - -#if defined(TCCR2A) && defined(TCCR2B) - case 2: - // 8 bit timer - TCCR2A = 0; - TCCR2B = 0; - // Page 205-206. ATmegal328, Page 184-185 ATmega 640/1280/2560 - // Mode 2 => Clear Timer on Compare match (CTC) using OCR2A for counter value - bitWrite(TCCR2A, WGM21, 1); - // No scaling now - bitWrite(TCCR2B, CS20, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T2"); -#endif - - break; -#endif - -#if defined(TCCR3A) && defined(TCCR3B) && defined(TIMSK3) - case 3: - // 16 bit timer - TCCR3A = 0; - TCCR3B = 0; - bitWrite(TCCR3B, WGM32, 1); - bitWrite(TCCR3B, CS30, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T3"); -#endif - - break; -#endif - -#if defined(TCCR4A) && defined(TCCR4B) && defined(TIMSK4) - case 4: - // 16 bit timer - TCCR4A = 0; - TCCR4B = 0; -#if defined(WGM42) - bitWrite(TCCR4B, WGM42, 1); -#elif defined(CS43) - // TODO this may not be correct - // atmega32u4 - bitWrite(TCCR4B, CS43, 1); -#endif - bitWrite(TCCR4B, CS40, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T4"); -#endif - - break; -#endif - -#if defined(TCCR5A) && defined(TCCR5B) && defined(TIMSK5) - case 5: - // 16 bit timer - TCCR5A = 0; - TCCR5B = 0; - bitWrite(TCCR5B, WGM52, 1); - bitWrite(TCCR5B, CS50, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T5"); -#endif - - break; -#endif - } - - _timer = timer; - - //sei();//enable interrupts - interrupts(); + // Set timer specific stuff + // All timers in CTC mode + // 8 bit timers will require changing prescalar values, + // whereas 16 bit timers are set to either ck/1 or ck/64 prescalar + //cli();//stop interrupts + noInterrupts(); + + switch (timer) + { + #if defined(TCCR1A) && defined(TCCR1B) && defined(WGM12) + case 1: + // 16 bit timer + TCCR1A = 0; + TCCR1B = 0; + // Page 172-173. ATmega 328/328P or Page 145-146 of ATmega 640/1280/2560 + // Mode 4 => Clear Timer on Compare match (CTC) using OCR1A for counter value + bitWrite(TCCR1B, WGM12, 1); + // No scaling now + bitWrite(TCCR1B, CS10, 1); + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T1"); + #endif + + break; + #endif + + #if defined(TCCR2A) && defined(TCCR2B) + case 2: + // 8 bit timer + TCCR2A = 0; + TCCR2B = 0; + // Page 205-206. ATmegal328, Page 184-185 ATmega 640/1280/2560 + // Mode 2 => Clear Timer on Compare match (CTC) using OCR2A for counter value + bitWrite(TCCR2A, WGM21, 1); + // No scaling now + bitWrite(TCCR2B, CS20, 1); + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T2"); + #endif + + break; + #endif + + #if defined(TCCR3A) && defined(TCCR3B) && defined(TIMSK3) + case 3: + // 16 bit timer + TCCR3A = 0; + TCCR3B = 0; + bitWrite(TCCR3B, WGM32, 1); + bitWrite(TCCR3B, CS30, 1); + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T3"); + #endif + + break; + #endif + + #if defined(TCCR4A) && defined(TCCR4B) && defined(TIMSK4) + case 4: + // 16 bit timer + TCCR4A = 0; + TCCR4B = 0; + #if defined(WGM42) + bitWrite(TCCR4B, WGM42, 1); + #elif defined(CS43) + // TODO this may not be correct + // atmega32u4 + bitWrite(TCCR4B, CS43, 1); + #endif + bitWrite(TCCR4B, CS40, 1); + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T4"); + #endif + + break; + #endif + + #if defined(TCCR5A) && defined(TCCR5B) && defined(TIMSK5) + case 5: + // 16 bit timer + TCCR5A = 0; + TCCR5B = 0; + bitWrite(TCCR5B, WGM52, 1); + bitWrite(TCCR5B, CS50, 1); + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T5"); + #endif + + break; + #endif + } + + _timer = timer; + + //sei();//enable interrupts + interrupts(); } void TimerInterrupt::set_OCR() { - // Run with noInterrupt() - // Set the OCR for the given timer, - // set the toggle count, - // then turn on the interrupts - uint32_t _OCRValueToUse; - -#if TIMER_INTERRUPT_DEBUG > 2 - Serial.println("set_OCR: start with " + String(_timer)); -#endif - - - switch (_timer) - { - case 1: - _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); - OCR1A = _OCRValueToUse; - _OCRValueRemaining -= _OCRValueToUse; - -#if defined(OCR1A) && defined(TIMSK1) && defined(OCIE1A) - // Bit 1 – OCIEA: Output Compare A Match Interrupt Enable - // When this bit is written to '1', and the I-flag in the Status Register is set (interrupts globally enabled), the - // Timer/Counter Output Compare A Match interrupt is enabled. The corresponding Interrupt Vector is - // executed when the OCFA Flag, located in TIFR1, is set. - bitWrite(TIMSK1, OCIE1A, 1); -#elif defined(OCR1A) && defined(TIMSK) && defined(OCIE1A) - // this combination is for at least the ATmega32 - bitWrite(TIMSK, OCIE1A, 1); -#endif - break; - -#if defined(OCR2A) && defined(TIMSK2) && defined(OCIE2A) - case 2: - _OCRValueToUse = min(MAX_COUNT_8BIT, _OCRValueRemaining); - OCR2A = _OCRValueToUse; - _OCRValueRemaining -= _OCRValueToUse; - - bitWrite(TIMSK2, OCIE2A, 1); -#if TIMER_INTERRUPT_DEBUG > 2 - Serial.println("set_OCR: case 2 " + String(_OCRValueToUse)+" "+String(_OCRValueRemaining)); -#endif - break; -#endif - -#if defined(OCR3A) && defined(TIMSK3) && defined(OCIE3A) - case 3: - _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); - OCR3A = _OCRValueToUse; - _OCRValueRemaining -= _OCRValueToUse; - - bitWrite(TIMSK3, OCIE3A, 1); - break; -#endif - -#if defined(OCR4A) && defined(TIMSK4) && defined(OCIE4A) - case 4: - _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); - OCR4A = _OCRValueToUse; - _OCRValueRemaining -= _OCRValueToUse; - - bitWrite(TIMSK4, OCIE4A, 1); - break; -#endif - -#if defined(OCR5A) && defined(TIMSK5) && defined(OCIE5A) - case 5: - _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); - OCR5A = _OCRValueToUse; - _OCRValueRemaining -= _OCRValueToUse; - - bitWrite(TIMSK5, OCIE5A, 1); - break; -#endif - } - - // Flag _OCRValue == 0 => end of long timer - if (_OCRValueRemaining == 0) - _timerDone = true; + // Run with noInterrupt() + // Set the OCR for the given timer, + // set the toggle count, + // then turn on the interrupts + uint32_t _OCRValueToUse; + + #if TIMER_INTERRUPT_DEBUG > 2 + Serial.println("set_OCR: start with " + String(_timer)); + #endif + switch (_timer) + { + case 1: + _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); + OCR1A = _OCRValueToUse; + _OCRValueRemaining -= _OCRValueToUse; + + #if defined(OCR1A) && defined(TIMSK1) && defined(OCIE1A) + // Bit 1 – OCIEA: Output Compare A Match Interrupt Enable + // When this bit is written to '1', and the I-flag in the Status Register is set (interrupts globally enabled), the + // Timer/Counter Output Compare A Match interrupt is enabled. The corresponding Interrupt Vector is + // executed when the OCFA Flag, located in TIFR1, is set. + bitWrite(TIMSK1, OCIE1A, 1); + #elif defined(OCR1A) && defined(TIMSK) && defined(OCIE1A) + // this combination is for at least the ATmega32 + bitWrite(TIMSK, OCIE1A, 1); + #endif + break; + + #if defined(OCR2A) && defined(TIMSK2) && defined(OCIE2A) + case 2: + _OCRValueToUse = min(MAX_COUNT_8BIT, _OCRValueRemaining); + OCR2A = _OCRValueToUse; + _OCRValueRemaining -= _OCRValueToUse; + + bitWrite(TIMSK2, OCIE2A, 1); + #if TIMER_INTERRUPT_DEBUG > 2 + Serial.println("set_OCR: case 2 " + String(_OCRValueToUse) + " " + String(_OCRValueRemaining)); + #endif + break; + #endif + + #if defined(OCR3A) && defined(TIMSK3) && defined(OCIE3A) + case 3: + _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); + OCR3A = _OCRValueToUse; + _OCRValueRemaining -= _OCRValueToUse; + + bitWrite(TIMSK3, OCIE3A, 1); + break; + #endif + + #if defined(OCR4A) && defined(TIMSK4) && defined(OCIE4A) + case 4: + _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); + OCR4A = _OCRValueToUse; + _OCRValueRemaining -= _OCRValueToUse; + + bitWrite(TIMSK4, OCIE4A, 1); + break; + #endif + + #if defined(OCR5A) && defined(TIMSK5) && defined(OCIE5A) + case 5: + _OCRValueToUse = min(MAX_COUNT_16BIT, _OCRValueRemaining); + OCR5A = _OCRValueToUse; + _OCRValueRemaining -= _OCRValueToUse; + + bitWrite(TIMSK5, OCIE5A, 1); + break; + #endif + } + + // Flag _OCRValue == 0 => end of long timer + if (_OCRValueRemaining == 0) + _timerDone = true; } // frequency (in hertz) and duration (in milliseconds). // Return true if frequency is OK with selected timer (OCRValue is in range) bool TimerInterrupt::setFrequency(float frequency, timer_callback_p callback, uint32_t params, unsigned long duration) { -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("setFrequency: called (" + String(frequency, 3) + ")"); -#endif - - uint8_t andMask = 0b11111000; - unsigned long OCRValue; - bool isSuccess = false; - - //frequencyLimit must > 1 - float frequencyLimit = frequency * 17179.840; - - #if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("setFreq: limit is " + String(frequencyLimit, 1)); - #endif - - // Limit frequency to larger than (0.00372529 / 64) Hz or interval 17179.840s / 17179840 ms to avoid uint32_t overflow - if ((_timer <= 0) || (callback == NULL) || ((frequencyLimit) < 1) ) - { -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("setFreq: limit out of range!"); -#endif - return false; - } - else - { - // Calculate the toggle count. Duration must be at least longer then one cycle - if (duration > 0) - { - _toggle_count = frequency * duration / 1000; + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("setFrequency: called (" + String(frequency, 3) + ")"); + #endif + + uint8_t andMask = 0b11111000; + unsigned long OCRValue; + bool isSuccess = false; -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("setFrequency => _toggle_count = " + String(_toggle_count) + ", frequency = " + String(frequency) + ", duration = " + String(duration)); -#endif + //frequencyLimit must > 1 + float frequencyLimit = frequency * 17179.840; - if (_toggle_count < 1) - { + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("setFreq: limit is " + String(frequencyLimit, 1)); + #endif + + // Limit frequency to larger than (0.00372529 / 64) Hz or interval 17179.840s / 17179840 ms to avoid uint32_t overflow + if ((_timer <= 0) || (callback == NULL) || ((frequencyLimit) < 1)) + { + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("setFreq: limit out of range!"); + #endif return false; - } } else { - _toggle_count = -1; - } + // Calculate the toggle count. Duration must be at least longer then one cycle + if (duration > 0) + { + _toggle_count = frequency * duration / 1000; - //Timer0 and timer2 are 8 bit timers, meaning they can store a maximum counter value of 255. - //Timer2 does not have the option of 1024 prescaler, only 1, 8, 32, 64 - //Timer1 is a 16 bit timer, meaning it can store a maximum counter value of 65535. - int prescalerIndexStart; + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("setFrequency => _toggle_count = " + String(_toggle_count) + ", frequency = " + String(frequency) + + ", duration = " + String(duration)); + #endif - //Use smallest prescaler first, then increase until fits (<255) - if (_timer != 2) - { - if (frequencyLimit > 64) - prescalerIndexStart = NO_PRESCALER; - else if (frequencyLimit > 8) - prescalerIndexStart = PRESCALER_8; - else - prescalerIndexStart = PRESCALER_64; - - - for (int prescalerIndex = prescalerIndexStart; prescalerIndex <= PRESCALER_1024; prescalerIndex++) - { - OCRValue = F_CPU / (frequency * prescalerDiv[prescalerIndex]) - 1; - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Freq * 1000 = " + String(frequency * 1000)); - Serial.println("F_CPU = " + String(F_CPU) + ", preScalerDiv = " + String(prescalerDiv[prescalerIndex])); - Serial.println("OCR = " + String(OCRValue) + ", preScalerIndex = " + String(prescalerIndex)); -#endif - - // We use very large _OCRValue now, and every time timer ISR activates, we deduct min(MAX_COUNT_16BIT, _OCRValueRemaining) from _OCRValueRemaining - // So that we can create very long timer, even if the counter is only 16-bit. - // Use very high frequency (OCRValue / MAX_COUNT_16BIT) around 64 * 1024 to achieve higher accuracy - if ( (OCRValue / MAX_COUNT_16BIT) < 16384 ) + if (_toggle_count < 1) + { + return false; + } + } + else { - _OCRValue = OCRValue; - _OCRValueRemaining = OCRValue; - _prescalerIndex = prescalerIndex; - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("OK in loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + ", preScalerDiv = " + String(prescalerDiv[_prescalerIndex])); -#endif + _toggle_count = -1; + } - isSuccess = true; + //Timer0 and timer2 are 8 bit timers, meaning they can store a maximum counter value of 255. + //Timer2 does not have the option of 1024 prescaler, only 1, 8, 32, 64 + //Timer1 is a 16 bit timer, meaning it can store a maximum counter value of 65535. + int prescalerIndexStart; - break; + //Use smallest prescaler first, then increase until fits (<255) + if (_timer != 2) + { + if (frequencyLimit > 64) + prescalerIndexStart = NO_PRESCALER; + else if (frequencyLimit > 8) + prescalerIndexStart = PRESCALER_8; + else + prescalerIndexStart = PRESCALER_64; + + for (int prescalerIndex = prescalerIndexStart; prescalerIndex <= PRESCALER_1024; prescalerIndex++) + { + OCRValue = F_CPU / (frequency * prescalerDiv[prescalerIndex]) - 1; + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Freq * 1000 = " + String(frequency * 1000)); + Serial.println("F_CPU = " + String(F_CPU) + ", preScalerDiv = " + String(prescalerDiv[prescalerIndex])); + Serial.println("OCR = " + String(OCRValue) + ", preScalerIndex = " + String(prescalerIndex)); + #endif + + // We use very large _OCRValue now, and every time timer ISR activates, we deduct min(MAX_COUNT_16BIT, _OCRValueRemaining) from _OCRValueRemaining + // So that we can create very long timer, even if the counter is only 16-bit. + // Use very high frequency (OCRValue / MAX_COUNT_16BIT) around 64 * 1024 to achieve higher accuracy + if ((OCRValue / MAX_COUNT_16BIT) < 16384) + { + _OCRValue = OCRValue; + _OCRValueRemaining = OCRValue; + _prescalerIndex = prescalerIndex; + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("OK in loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + + ", preScalerDiv = " + String(prescalerDiv[_prescalerIndex])); + #endif + + isSuccess = true; + + break; + } + } + + if (!isSuccess) + { + // Always do this + _OCRValue = OCRValue; + _OCRValueRemaining = OCRValue; + _prescalerIndex = PRESCALER_1024; + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("OK out loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + + ", preScalerDiv = " + String(prescalerDiv[_prescalerIndex])); + #endif + } } - } - - if (!isSuccess) - { - // Always do this - _OCRValue = OCRValue; - _OCRValueRemaining = OCRValue; - _prescalerIndex = PRESCALER_1024; - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("OK out loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + ", preScalerDiv = " + String(prescalerDiv[_prescalerIndex])); -#endif - } - } - else - { - if (frequencyLimit > 64) - prescalerIndexStart = T2_NO_PRESCALER; - else if (frequencyLimit > 8) - prescalerIndexStart = T2_PRESCALER_8; - else if (frequencyLimit > 2) - prescalerIndexStart = T2_PRESCALER_32; - else - prescalerIndexStart = T2_PRESCALER_64; - - // Page 206-207. ATmegal328 - //8-bit Timer2 has more options up to 1024 prescaler, from 1, 8, 32, 64, 128, 256 and 1024 - for (int prescalerIndex = prescalerIndexStart; prescalerIndex <= T2_PRESCALER_1024; prescalerIndex++) - { - OCRValue = F_CPU / (frequency * prescalerDivT2[prescalerIndex]) - 1; - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("F_CPU = " + String(F_CPU) + ", preScalerDiv = " + String(prescalerDivT2[prescalerIndex])); - Serial.println("OCR2 = " + String(OCRValue) + ", preScalerIndex = " + String(prescalerIndex)); -#endif - - // We use very large _OCRValue now, and every time timer ISR activates, we deduct min(MAX_COUNT_8BIT, _OCRValue) from _OCRValue - // to create very long timer, even if the counter is only 16-bit. - // Use very high frequency (OCRValue / MAX_COUNT_16BIT) around 64 * 1024 to achieve higher accuracy - if ( (OCRValue / MAX_COUNT_8BIT) < 16384 ) + else { - _OCRValue = OCRValue; - _OCRValueRemaining = OCRValue; - // same as prescalarbits - _prescalerIndex = prescalerIndex; + if (frequencyLimit > 64) + prescalerIndexStart = T2_NO_PRESCALER; + else if (frequencyLimit > 8) + prescalerIndexStart = T2_PRESCALER_8; + else if (frequencyLimit > 2) + prescalerIndexStart = T2_PRESCALER_32; + else + prescalerIndexStart = T2_PRESCALER_64; + + // Page 206-207. ATmegal328 + //8-bit Timer2 has more options up to 1024 prescaler, from 1, 8, 32, 64, 128, 256 and 1024 + for (int prescalerIndex = prescalerIndexStart; prescalerIndex <= T2_PRESCALER_1024; prescalerIndex++) + { + OCRValue = F_CPU / (frequency * prescalerDivT2[prescalerIndex]) - 1; + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("F_CPU = " + String(F_CPU) + ", preScalerDiv = " + String(prescalerDivT2[prescalerIndex])); + Serial.println("OCR2 = " + String(OCRValue) + ", preScalerIndex = " + String(prescalerIndex)); + #endif + + // We use very large _OCRValue now, and every time timer ISR activates, we deduct min(MAX_COUNT_8BIT, _OCRValue) from _OCRValue + // to create very long timer, even if the counter is only 16-bit. + // Use very high frequency (OCRValue / MAX_COUNT_16BIT) around 64 * 1024 to achieve higher accuracy + if ((OCRValue / MAX_COUNT_8BIT) < 16384) + { + _OCRValue = OCRValue; + _OCRValueRemaining = OCRValue; + // same as prescalarbits + _prescalerIndex = prescalerIndex; + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("OK in loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + + ", preScalerDiv = " + String(prescalerDivT2[_prescalerIndex])); + #endif + + isSuccess = true; + + break; + } + } + + if (!isSuccess) + { + // Always do this + _OCRValue = OCRValue; + _OCRValueRemaining = OCRValue; + // same as prescalarbits + _prescalerIndex = T2_PRESCALER_1024; + + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("OK out loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + + ", preScalerDiv = " + String(prescalerDivT2[_prescalerIndex])); + #endif + } + } -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("OK in loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + ", preScalerDiv = " + String(prescalerDivT2[_prescalerIndex])); -#endif + //cli();//stop interrupts + noInterrupts(); - isSuccess = true; + _frequency = frequency; + _callback = (void *) callback; + _params = reinterpret_cast(params); - break; + _timerDone = false; + + // 8 bit timers from here + #if defined(TCCR2B) + if (_timer == 2) + { + TCCR2B = (TCCR2B & andMask) | _prescalerIndex; //prescalarbits; + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("TCCR2B = " + String(TCCR2B)); + #endif + } + #endif + + // 16 bit timers from here + #if defined(TCCR1B) + else if (_timer == 1) + { + TCCR1B = (TCCR1B & andMask) | _prescalerIndex; //prescalarbits; + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("TCCR1B = " + String(TCCR1B)); + #endif } - } - - if (!isSuccess) - { - // Always do this - _OCRValue = OCRValue; - _OCRValueRemaining = OCRValue; - // same as prescalarbits - _prescalerIndex = T2_PRESCALER_1024; - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("OK out loop => _OCR = " + String(_OCRValue) + ", _preScalerIndex = " + String(_prescalerIndex) + ", preScalerDiv = " + String(prescalerDivT2[_prescalerIndex])); -#endif - } + #endif + + #if defined(TCCR3B) + else if (_timer == 3) + TCCR3B = (TCCR3B & andMask) | _prescalerIndex; //prescalarbits; + #endif + + #if defined(TCCR4B) + else if (_timer == 4) + TCCR4B = (TCCR4B & andMask) | _prescalerIndex; //prescalarbits; + #endif + + #if defined(TCCR5B) + else if (_timer == 5) + TCCR5B = (TCCR5B & andMask) | _prescalerIndex; //prescalarbits; + #endif + + // Set the OCR for the given timer, + // set the toggle count, + // then turn on the interrupts + set_OCR(); + + #if TIMER_INTERRUPT_DEBUG > 2 + Serial.println("setfreq: set Ocr returned . Enabling interrupts."); + #endif + + //sei();//allow interrupts + interrupts(); + + return true; } +} +void TimerInterrupt::detachInterrupt(void) +{ //cli();//stop interrupts noInterrupts(); - _frequency = frequency; - _callback = (void*) callback; - _params = reinterpret_cast(params); + switch (_timer) + { + #if defined(TIMSK1) && defined(OCIE1A) + case 1: + bitWrite(TIMSK1, OCIE1A, 0); - _timerDone = false; + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Disable T1"); + #endif - // 8 bit timers from here -#if defined(TCCR2B) - if (_timer == 2) - { - TCCR2B = (TCCR2B & andMask) | _prescalerIndex; //prescalarbits; -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("TCCR2B = " + String(TCCR2B)); -#endif - } -#endif + break; + #endif - // 16 bit timers from here -#if defined(TCCR1B) - else if (_timer == 1) - { - TCCR1B = (TCCR1B & andMask) | _prescalerIndex; //prescalarbits; -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("TCCR1B = " + String(TCCR1B)); -#endif + case 2: + #if defined(TIMSK2) && defined(OCIE2A) + bitWrite(TIMSK2, OCIE2A, 0); // disable interrupt + #endif - } -#endif + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Disable T2"); + #endif -#if defined(TCCR3B) - else if (_timer == 3) - TCCR3B = (TCCR3B & andMask) | _prescalerIndex; //prescalarbits; -#endif + break; -#if defined(TCCR4B) - else if (_timer == 4) - TCCR4B = (TCCR4B & andMask) | _prescalerIndex; //prescalarbits; -#endif + #if defined(TIMSK3) && defined(OCIE3A) + case 3: + bitWrite(TIMSK3, OCIE3A, 0); -#if defined(TCCR5B) - else if (_timer == 5) - TCCR5B = (TCCR5B & andMask) | _prescalerIndex; //prescalarbits; -#endif + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Disable T3"); + #endif - // Set the OCR for the given timer, - // set the toggle count, - // then turn on the interrupts - set_OCR(); + break; + #endif -#if TIMER_INTERRUPT_DEBUG > 2 - Serial.println("setfreq: set Ocr returned . Enabling interrupts."); -#endif + #if defined(TIMSK4) && defined(OCIE4A) + case 4: + bitWrite(TIMSK4, OCIE4A, 0); - //sei();//allow interrupts - interrupts(); + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Disable T4"); + #endif + + break; + #endif + + #if defined(TIMSK5) && defined(OCIE5A) + case 5: + bitWrite(TIMSK5, OCIE5A, 0); + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Disable T5"); + #endif - return true; - } + break; + #endif + } + + //sei();//allow interrupts + interrupts(); } -void TimerInterrupt::detachInterrupt(void) +// Duration (in milliseconds). Duration = 0 or not specified => run indefinitely +void TimerInterrupt::reattachInterrupt(unsigned long duration) { - //cli();//stop interrupts - noInterrupts(); + //cli();//stop interrupts + noInterrupts(); - switch (_timer) - { -#if defined(TIMSK1) && defined(OCIE1A) - case 1: - bitWrite(TIMSK1, OCIE1A, 0); + // Calculate the toggle count + if (duration > 0) + { + _toggle_count = _frequency * duration / 1000; + } + else + { + _toggle_count = -1; + } -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Disable T1"); -#endif + switch (_timer) + { + #if defined(TIMSK1) && defined(OCIE1A) + case 1: + bitWrite(TIMSK1, OCIE1A, 1); - break; -#endif + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Enable T1"); + #endif - case 2: -#if defined(TIMSK2) && defined(OCIE2A) - bitWrite(TIMSK2, OCIE2A, 0); // disable interrupt -#endif + break; + #endif -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Disable T2"); -#endif + case 2: + #if defined(TIMSK2) && defined(OCIE2A) + bitWrite(TIMSK2, OCIE2A, 1); // enable interrupt + #endif - break; + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Enable T2"); + #endif -#if defined(TIMSK3) && defined(OCIE3A) - case 3: - bitWrite(TIMSK3, OCIE3A, 0); + break; -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Disable T3"); -#endif + #if defined(TIMSK3) && defined(OCIE3A) + case 3: + bitWrite(TIMSK3, OCIE3A, 1); - break; -#endif + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Enable T3"); + #endif -#if defined(TIMSK4) && defined(OCIE4A) - case 4: - bitWrite(TIMSK4, OCIE4A, 0); + break; + #endif -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Disable T4"); -#endif + #if defined(TIMSK4) && defined(OCIE4A) + case 4: + bitWrite(TIMSK4, OCIE4A, 1); - break; -#endif + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Enable T4"); + #endif -#if defined(TIMSK5) && defined(OCIE5A) - case 5: - bitWrite(TIMSK5, OCIE5A, 0); + break; + #endif -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Disable T5"); -#endif + #if defined(TIMSK5) && defined(OCIE5A) + case 5: + bitWrite(TIMSK5, OCIE5A, 1); - break; -#endif - } + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("Enable T5"); + #endif - //sei();//allow interrupts - interrupts(); -} + break; + #endif + } -// Duration (in milliseconds). Duration = 0 or not specified => run indefinitely -void TimerInterrupt::reattachInterrupt(unsigned long duration) -{ - //cli();//stop interrupts - noInterrupts(); - - // Calculate the toggle count - if (duration > 0) - { - _toggle_count = _frequency * duration / 1000; - } - else - { - _toggle_count = -1; - } - - switch (_timer) - { -#if defined(TIMSK1) && defined(OCIE1A) - case 1: - bitWrite(TIMSK1, OCIE1A, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Enable T1"); -#endif - - break; -#endif - - case 2: -#if defined(TIMSK2) && defined(OCIE2A) - bitWrite(TIMSK2, OCIE2A, 1); // enable interrupt -#endif - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Enable T2"); -#endif - - break; - -#if defined(TIMSK3) && defined(OCIE3A) - case 3: - bitWrite(TIMSK3, OCIE3A, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Enable T3"); -#endif - - break; -#endif - -#if defined(TIMSK4) && defined(OCIE4A) - case 4: - bitWrite(TIMSK4, OCIE4A, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Enable T4"); -#endif - - break; -#endif - -#if defined(TIMSK5) && defined(OCIE5A) - case 5: - bitWrite(TIMSK5, OCIE5A, 1); - -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("Enable T5"); -#endif - - break; -#endif - } - - //sei();//allow interrupts - interrupts(); + //sei();//allow interrupts + interrupts(); } // Just stop clock source, still keep the count void TimerInterrupt::pauseTimer(void) { - uint8_t andMask = 0b11111000; - - //Just clear the CSx2-CSx0. Still keep the count in TCNT and Timer Interrupt mask TIMKSx. - - // 8 bit timers from here -#if defined(TCCR2B) - if (_timer == 2) - { - TCCR2B = (TCCR2B & andMask); -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("TCCR2B = " + String(TCCR2B)); -#endif - } -#endif - - // 16 bit timers from here -#if defined(TCCR1B) - else if (_timer == 1) - { - TCCR1B = (TCCR1B & andMask); -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("TCCR1B = " + String(TCCR1B)); -#endif - } -#endif - -#if defined(TCCR3B) - else if (_timer == 3) - TCCR3B = (TCCR3B & andMask); -#endif - -#if defined(TCCR4B) - else if (_timer == 4) - TCCR4B = (TCCR4B & andMask); -#endif - -#if defined(TCCR5B) - else if (_timer == 5) - TCCR5B = (TCCR5B & andMask); -#endif + uint8_t andMask = 0b11111000; + + //Just clear the CSx2-CSx0. Still keep the count in TCNT and Timer Interrupt mask TIMKSx. + + // 8 bit timers from here + #if defined(TCCR2B) + if (_timer == 2) + { + TCCR2B = (TCCR2B & andMask); + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("TCCR2B = " + String(TCCR2B)); + #endif + } + #endif + + // 16 bit timers from here + #if defined(TCCR1B) + else if (_timer == 1) + { + TCCR1B = (TCCR1B & andMask); + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("TCCR1B = " + String(TCCR1B)); + #endif + } + #endif + + #if defined(TCCR3B) + else if (_timer == 3) + TCCR3B = (TCCR3B & andMask); + #endif + + #if defined(TCCR4B) + else if (_timer == 4) + TCCR4B = (TCCR4B & andMask); + #endif + + #if defined(TCCR5B) + else if (_timer == 5) + TCCR5B = (TCCR5B & andMask); + #endif } // Just reconnect clock source, continue from the current count void TimerInterrupt::resumeTimer(void) { - uint8_t andMask = 0b11111000; - - //Just restore the CSx2-CSx0 stored in _prescalerIndex. Still keep the count in TCNT and Timer Interrupt mask TIMKSx. - // 8 bit timers from here -#if defined(TCCR2B) - if (_timer == 2) - { - TCCR2B = (TCCR2B & andMask) | _prescalerIndex; //prescalarbits; -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("TCCR2B = " + String(TCCR2B)); -#endif - } -#endif - - // 16 bit timers from here -#if defined(TCCR1B) - else if (_timer == 1) - { - TCCR1B = (TCCR1B & andMask) | _prescalerIndex; //prescalarbits; -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("TCCR1B = " + String(TCCR1B)); -#endif - } -#endif - -#if defined(TCCR3B) - else if (_timer == 3) - TCCR3B = (TCCR3B & andMask) | _prescalerIndex; //prescalarbits; -#endif - -#if defined(TCCR4B) - else if (_timer == 4) - TCCR4B = (TCCR4B & andMask) | _prescalerIndex; //prescalarbits; -#endif - -#if defined(TCCR5B) - else if (_timer == 5) - TCCR5B = (TCCR5B & andMask) | _prescalerIndex; //prescalarbits; -#endif + uint8_t andMask = 0b11111000; + + //Just restore the CSx2-CSx0 stored in _prescalerIndex. Still keep the count in TCNT and Timer Interrupt mask TIMKSx. + // 8 bit timers from here + #if defined(TCCR2B) + if (_timer == 2) + { + TCCR2B = (TCCR2B & andMask) | _prescalerIndex; //prescalarbits; + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("TCCR2B = " + String(TCCR2B)); + #endif + } + #endif + + // 16 bit timers from here + #if defined(TCCR1B) + else if (_timer == 1) + { + TCCR1B = (TCCR1B & andMask) | _prescalerIndex; //prescalarbits; + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("TCCR1B = " + String(TCCR1B)); + #endif + } + #endif + + #if defined(TCCR3B) + else if (_timer == 3) + TCCR3B = (TCCR3B & andMask) | _prescalerIndex; //prescalarbits; + #endif + + #if defined(TCCR4B) + else if (_timer == 4) + TCCR4B = (TCCR4B & andMask) | _prescalerIndex; //prescalarbits; + #endif + + #if defined(TCCR5B) + else if (_timer == 5) + TCCR5B = (TCCR5B & andMask) | _prescalerIndex; //prescalarbits; + #endif } #endif \ No newline at end of file diff --git a/src/libs/TimerInterrupt/TimerInterrupt.h b/src/libs/TimerInterrupt/TimerInterrupt.h index 99e98a73..c9e37597 100644 --- a/src/libs/TimerInterrupt/TimerInterrupt.h +++ b/src/libs/TimerInterrupt/TimerInterrupt.h @@ -30,123 +30,121 @@ #define TimerInterrupt_h #ifndef TIMER_INTERRUPT_DEBUG -#define TIMER_INTERRUPT_DEBUG 0 + #define TIMER_INTERRUPT_DEBUG 0 #endif #if !defined(ESP32) -#include -#include -#include "Arduino.h" -#include "pins_arduino.h" - -#define MAX_COUNT_8BIT 255 -#define MAX_COUNT_16BIT 65535 - -#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) -#define TCCR2A TCCR2 -#define TCCR2B TCCR2 -#define COM2A1 COM21 -#define COM2A0 COM20 -#define OCR2A OCR2 -#define TIMSK2 TIMSK -#define OCIE2A OCIE2 -#define TIMER2_COMPA_vect TIMER2_COMP_vect -#define TIMSK1 TIMSK -#endif + #include + #include + #include "Arduino.h" + #include "pins_arduino.h" + + #define MAX_COUNT_8BIT 255 + #define MAX_COUNT_16BIT 65535 + + #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) + #define TCCR2A TCCR2 + #define TCCR2B TCCR2 + #define COM2A1 COM21 + #define COM2A0 COM20 + #define OCR2A OCR2 + #define TIMSK2 TIMSK + #define OCIE2A OCIE2 + #define TIMER2_COMPA_vect TIMER2_COMP_vect + #define TIMSK1 TIMSK + #endif typedef void (*timer_callback)(void); typedef void (*timer_callback_p)(void *); enum { - HW_TIMER_0 = 0, - HW_TIMER_1, - HW_TIMER_2, - HW_TIMER_3, - HW_TIMER_4, - HW_TIMER_5, - NUM_HW_TIMERS + HW_TIMER_0 = 0, + HW_TIMER_1, + HW_TIMER_2, + HW_TIMER_3, + HW_TIMER_4, + HW_TIMER_5, + NUM_HW_TIMERS }; enum { - NO_CLOCK_SOURCE = 0, - NO_PRESCALER, - PRESCALER_8, - PRESCALER_64, - PRESCALER_256, - PRESCALER_1024, - NUM_ITEMS + NO_CLOCK_SOURCE = 0, + NO_PRESCALER, + PRESCALER_8, + PRESCALER_64, + PRESCALER_256, + PRESCALER_1024, + NUM_ITEMS }; enum { - T2_NO_CLOCK_SOURCE = 0, - T2_NO_PRESCALER, - T2_PRESCALER_8, - T2_PRESCALER_32, - T2_PRESCALER_64, - T2_PRESCALER_128, - T2_PRESCALER_256, - T2_PRESCALER_1024, - T2_NUM_ITEMS + T2_NO_CLOCK_SOURCE = 0, + T2_NO_PRESCALER, + T2_PRESCALER_8, + T2_PRESCALER_32, + T2_PRESCALER_64, + T2_PRESCALER_128, + T2_PRESCALER_256, + T2_PRESCALER_1024, + T2_NUM_ITEMS }; -const unsigned int prescalerDiv [NUM_ITEMS] = { 1, 1, 8, 64, 256, 1024 }; -const unsigned int prescalerDivT2 [T2_NUM_ITEMS] = { 1, 1, 8, 32, 64, 128, 256, 1024 }; +const unsigned int prescalerDiv[NUM_ITEMS] = {1, 1, 8, 64, 256, 1024}; +const unsigned int prescalerDivT2[T2_NUM_ITEMS] = {1, 1, 8, 32, 64, 128, 256, 1024}; class TimerInterrupt { private: + bool _timerDone; + int8_t _timer; + unsigned int _prescalerIndex; + uint32_t _OCRValue; + uint32_t _OCRValueRemaining; + volatile long _toggle_count; + double _frequency; - bool _timerDone; - int8_t _timer; - unsigned int _prescalerIndex; - uint32_t _OCRValue; - uint32_t _OCRValueRemaining; - volatile long _toggle_count; - double _frequency; - - void* _callback; // pointer to the callback function - void* _params; // function parameter + void *_callback; // pointer to the callback function + void *_params; // function parameter void set_OCR(void); public: - TimerInterrupt() { - _timer = -1; - _frequency = 0; - _callback = NULL; - _params = NULL; + _timer = -1; + _frequency = 0; + _callback = NULL; + _params = NULL; }; TimerInterrupt(uint8_t timerNo) { - _timer = timerNo; - _frequency = 0; - _callback = NULL; - _params = NULL; + _timer = timerNo; + _frequency = 0; + _callback = NULL; + _params = NULL; }; void callback() __attribute__((always_inline)) { - if (_callback != NULL) - { - if (_params != NULL) - (*(timer_callback_p)_callback)(_params); - else - (*(timer_callback)_callback)(); - } + if (_callback != NULL) + { + if (_params != NULL) + (*(timer_callback_p) _callback)(_params); + else + (*(timer_callback) _callback)(); + } } void init(int8_t timer); void init() { - init(_timer); + init(_timer); }; // frequency (in hertz) and duration (in milliseconds). Duration = 0 or not specified => run indefinitely @@ -155,61 +153,58 @@ class TimerInterrupt // frequency (in hertz) and duration (in milliseconds). Duration = 0 or not specified => run indefinitely bool setFrequency(float frequency, timer_callback callback, unsigned long duration) { - return setFrequency(frequency, reinterpret_cast(callback), /*NULL*/ 0, duration); + return setFrequency(frequency, reinterpret_cast(callback), /*NULL*/ 0, duration); } // interval (in ms) and duration (in milliseconds). Duration = 0 or not specified => run indefinitely - template - bool setInterval(unsigned long interval, void (*callback)(TArg), TArg params, unsigned long duration = 0) + template bool setInterval(unsigned long interval, void (*callback)(TArg), TArg params, unsigned long duration = 0) { - static_assert(sizeof(TArg) <= sizeof(uint32_t), "setInterval() callback argument size must be <= 4 bytes"); - return setFrequency((float) (1000.0f / interval), reinterpret_cast(callback), (uint32_t) params, duration); + static_assert(sizeof(TArg) <= sizeof(uint32_t), "setInterval() callback argument size must be <= 4 bytes"); + return setFrequency((float) (1000.0f / interval), reinterpret_cast(callback), (uint32_t) params, duration); } // interval (in ms) and duration (in milliseconds). Duration = 0 or not specified => run indefinitely bool setInterval(unsigned long interval, timer_callback callback, unsigned long duration) { - return setFrequency((float) (1000.0f / interval), reinterpret_cast(callback), /*NULL*/ 0, duration); + return setFrequency((float) (1000.0f / interval), reinterpret_cast(callback), /*NULL*/ 0, duration); } - template - bool attachInterrupt(float frequency, void (*callback)(TArg), TArg params, unsigned long duration = 0) + template bool attachInterrupt(float frequency, void (*callback)(TArg), TArg params, unsigned long duration = 0) { - static_assert(sizeof(TArg) <= sizeof(uint32_t), "attachInterrupt() callback argument size must be <= 4 bytes"); - return setFrequency(frequency, reinterpret_cast(callback), (uint32_t) params, duration); + static_assert(sizeof(TArg) <= sizeof(uint32_t), "attachInterrupt() callback argument size must be <= 4 bytes"); + return setFrequency(frequency, reinterpret_cast(callback), (uint32_t) params, duration); } bool attachInterrupt(float frequency, timer_callback callback, unsigned long duration) { - return setFrequency(frequency, reinterpret_cast(callback), /*NULL*/ 0, duration); + return setFrequency(frequency, reinterpret_cast(callback), /*NULL*/ 0, duration); } // Interval (in ms) and duration (in milliseconds). Duration = 0 or not specified => run indefinitely - template - bool attachInterruptInterval(float interval, void (*callback)(TArg), TArg params, unsigned long duration = 0) + template bool attachInterruptInterval(float interval, void (*callback)(TArg), TArg params, unsigned long duration = 0) { -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("attachIntr: size is " +String(sizeof(TArg))); -#endif + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("attachIntr: size is " + String(sizeof(TArg))); + #endif - static_assert(sizeof(TArg) <= sizeof(uint32_t), "attachInterruptInterval() callback argument size must be <= 4 bytes"); -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("attachIntr: calling setFreq(" + String(1000.0f/interval,1)+")"); -#endif - return setFrequency( (float) ( 1000.0f / interval), reinterpret_cast(callback), (uint32_t) params, duration); + static_assert(sizeof(TArg) <= sizeof(uint32_t), "attachInterruptInterval() callback argument size must be <= 4 bytes"); + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("attachIntr: calling setFreq(" + String(1000.0f / interval, 1) + ")"); + #endif + return setFrequency((float) (1000.0f / interval), reinterpret_cast(callback), (uint32_t) params, duration); } // Interval (in ms) and duration (in milliseconds). Duration = 0 or not specified => run indefinitely bool attachInterruptInterval(unsigned long interval, timer_callback callback, unsigned long duration = 0) { - return setFrequency( (float) ( 1000.0f / interval), reinterpret_cast (callback), /*NULL*/ 0, duration); + return setFrequency((float) (1000.0f / interval), reinterpret_cast(callback), /*NULL*/ 0, duration); } void detachInterrupt(); void disableTimer(void) { - detachInterrupt(); + detachInterrupt(); } // Duration (in milliseconds). Duration = 0 or not specified => run indefinitely @@ -218,7 +213,7 @@ class TimerInterrupt // Duration (in milliseconds). Duration = 0 or not specified => run indefinitely void enableTimer(unsigned long duration = 0) __attribute__((always_inline)) { - reattachInterrupt(duration); + reattachInterrupt(duration); } // Just stop clock source, still keep the count @@ -230,98 +225,98 @@ class TimerInterrupt // Just stop clock source, clear the count void stopTimer(void) { - detachInterrupt(); + detachInterrupt(); } // Just reconnect clock source, start current count from 0 void restartTimer(unsigned long duration = 0) { - reattachInterrupt(duration); + reattachInterrupt(duration); } int8_t getTimer() __attribute__((always_inline)) { - return _timer; + return _timer; }; volatile long getCount() __attribute__((always_inline)) { - return _toggle_count; + return _toggle_count; }; void setCount(long countInput) __attribute__((always_inline)) { - //cli();//stop interrupts - //noInterrupts(); + //cli();//stop interrupts + //noInterrupts(); - _toggle_count = countInput; + _toggle_count = countInput; - //sei();//enable interrupts - //interrupts(); + //sei();//enable interrupts + //interrupts(); }; volatile long get_OCRValue() __attribute__((always_inline)) { - return _OCRValue; + return _OCRValue; }; volatile long get_OCRValueRemaining() __attribute__((always_inline)) { - return _OCRValueRemaining; + return _OCRValueRemaining; }; - void adjust_OCRValue() //__attribute__((always_inline)) + void adjust_OCRValue() //__attribute__((always_inline)) { - //cli();//stop interrupts - noInterrupts(); - - if (_timer != 2) - _OCRValueRemaining -= min(MAX_COUNT_16BIT, _OCRValueRemaining); - else - _OCRValueRemaining -= min(MAX_COUNT_8BIT, _OCRValueRemaining); - - if (_OCRValueRemaining <= 0) - { - // Reset value for next cycle - _OCRValueRemaining = _OCRValue; - _timerDone = true; - } - else - _timerDone = false; + //cli();//stop interrupts + noInterrupts(); - //sei();//enable interrupts - interrupts(); + if (_timer != 2) + _OCRValueRemaining -= min(MAX_COUNT_16BIT, _OCRValueRemaining); + else + _OCRValueRemaining -= min(MAX_COUNT_8BIT, _OCRValueRemaining); + + if (_OCRValueRemaining <= 0) + { + // Reset value for next cycle + _OCRValueRemaining = _OCRValue; + _timerDone = true; + } + else + _timerDone = false; + + //sei();//enable interrupts + interrupts(); }; - void reload_OCRValue() //__attribute__((always_inline)) + void reload_OCRValue() //__attribute__((always_inline)) { - //cli();//stop interrupts - noInterrupts(); + //cli();//stop interrupts + noInterrupts(); - // Reset value for next cycle, have to deduct the value already loaded to OCR register + // Reset value for next cycle, have to deduct the value already loaded to OCR register - if (_timer != 2) - _OCRValueRemaining = _OCRValue - min(MAX_COUNT_16BIT, _OCRValueRemaining); - else - _OCRValueRemaining = _OCRValue - min(MAX_COUNT_8BIT, _OCRValueRemaining); + if (_timer != 2) + _OCRValueRemaining = _OCRValue - min(MAX_COUNT_16BIT, _OCRValueRemaining); + else + _OCRValueRemaining = _OCRValue - min(MAX_COUNT_8BIT, _OCRValueRemaining); - _timerDone = false; + _timerDone = false; - //sei();//enable interrupts - interrupts(); + //sei();//enable interrupts + interrupts(); }; - bool checkTimerDone(void) //__attribute__((always_inline)) + bool checkTimerDone(void) //__attribute__((always_inline)) { - return _timerDone; + return _timerDone; }; -}; // class TimerInterrupt +}; // class TimerInterrupt -#if USE_TIMER_1 -#ifndef TIMER1_INSTANTIATED -// To force pre-instatiate only once -#define TIMER1_INSTANTIATED + #if USE_TIMER_1 + #ifndef TIMER1_INSTANTIATED + // To force pre-instatiate only once + #define TIMER1_INSTANTIATED TimerInterrupt ITimer1(HW_TIMER_1); // Timer0 is used for micros(), millis(), delay(), etc and can't be used @@ -329,300 +324,305 @@ TimerInterrupt ITimer1(HW_TIMER_1); ISR(TIMER1_COMPA_vect) { - long countLocal = ITimer1.getCount(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T1 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining()) ); -#endif - - if (ITimer1.getTimer() == 1) - { - if (countLocal != 0) - { - if (ITimer1.checkTimerDone()) - { -#if (TIMER_INTERRUPT_DEBUG > 1) - Serial.println("T1 callback, _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining()) + ", millis = " + String(millis()) ); -#endif - ITimer1.callback(); - - // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT if _OCRValueRemaining > MAX_COUNT_16BIT - ITimer1.reload_OCRValue(); - - if (countLocal > 0) - ITimer1.setCount(countLocal - 1); - } - else - { - //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) - // If _OCRValue == 0, flag _timerDone for next cycle -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T1 before _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining()) ); -#endif - - // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining - ITimer1.adjust_OCRValue(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T1 after _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining()) ); -#endif - } - } - else - { -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T1 done"); -#endif - ITimer1.detachInterrupt(); + long countLocal = ITimer1.getCount(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T1 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining())); + #endif + + if (ITimer1.getTimer() == 1) + { + if (countLocal != 0) + { + if (ITimer1.checkTimerDone()) + { + #if (TIMER_INTERRUPT_DEBUG > 1) + Serial.println("T1 callback, _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining()) + + ", millis = " + String(millis())); + #endif + ITimer1.callback(); + + // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT if _OCRValueRemaining > MAX_COUNT_16BIT + ITimer1.reload_OCRValue(); + + if (countLocal > 0) + ITimer1.setCount(countLocal - 1); + } + else + { + //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) + // If _OCRValue == 0, flag _timerDone for next cycle + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T1 before _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining())); + #endif + + // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining + ITimer1.adjust_OCRValue(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T1 after _OCRValueRemaining = " + String(ITimer1.get_OCRValueRemaining())); + #endif + } + } + else + { + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T1 done"); + #endif + ITimer1.detachInterrupt(); + } } - } } -#endif //#ifndef TIMER1_INSTANTIATED -#endif //#if USE_TIMER_1 + #endif //#ifndef TIMER1_INSTANTIATED + #endif //#if USE_TIMER_1 -#if USE_TIMER_2 -#ifndef TIMER2_INSTANTIATED -#define TIMER2_INSTANTIATED + #if USE_TIMER_2 + #ifndef TIMER2_INSTANTIATED + #define TIMER2_INSTANTIATED TimerInterrupt ITimer2(HW_TIMER_2); ISR(TIMER2_COMPA_vect) { - long countLocal = ITimer2.getCount(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T2 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining()) ); -#endif - - if (ITimer2.getTimer() == 2) - { - if (countLocal != 0) - { - if (ITimer2.checkTimerDone()) - { -#if (TIMER_INTERRUPT_DEBUG > 1) - Serial.println("T2 callback, _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining()) + ", millis = " + String(millis()) ); -#endif - ITimer2.callback(); - // To reload _OCRValue - ITimer2.reload_OCRValue(); - - if (countLocal > 0) - ITimer2.setCount(countLocal - 1); - - } - else - { -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T2 before _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining()) ); -#endif - - //Deduct _OCRValue by min(MAX_COUNT_8BIT, _OCRValue) - // If _OCRValue == 0, flag _timerDone for next cycle - ITimer2.adjust_OCRValue(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T2 after _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining()) ); -#endif - } - } - else - { -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T2 done"); -#endif - ITimer2.detachInterrupt(); + long countLocal = ITimer2.getCount(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T2 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining())); + #endif + + if (ITimer2.getTimer() == 2) + { + if (countLocal != 0) + { + if (ITimer2.checkTimerDone()) + { + #if (TIMER_INTERRUPT_DEBUG > 1) + Serial.println("T2 callback, _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining()) + + ", millis = " + String(millis())); + #endif + ITimer2.callback(); + // To reload _OCRValue + ITimer2.reload_OCRValue(); + + if (countLocal > 0) + ITimer2.setCount(countLocal - 1); + } + else + { + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T2 before _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining())); + #endif + + //Deduct _OCRValue by min(MAX_COUNT_8BIT, _OCRValue) + // If _OCRValue == 0, flag _timerDone for next cycle + ITimer2.adjust_OCRValue(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T2 after _OCRValueRemaining = " + String(ITimer2.get_OCRValueRemaining())); + #endif + } + } + else + { + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T2 done"); + #endif + ITimer2.detachInterrupt(); + } } - } } -#endif //#ifndef TIMER2_INSTANTIATED -#endif //#if USE_TIMER_2 - -#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644__) || defined(__AVR_ATmega644A__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) - -// Pre-instatiate -#if USE_TIMER_3 -#ifndef TIMER3_INSTANTIATED -// To force pre-instatiate only once -#define TIMER3_INSTANTIATED + #endif //#ifndef TIMER2_INSTANTIATED + #endif //#if USE_TIMER_2 + + #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) \ + || defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644__) || defined(__AVR_ATmega644A__) \ + || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) + + // Pre-instatiate + #if USE_TIMER_3 + #ifndef TIMER3_INSTANTIATED + // To force pre-instatiate only once + #define TIMER3_INSTANTIATED TimerInterrupt ITimer3(HW_TIMER_3); ISR(TIMER3_COMPA_vect) { - long countLocal = ITimer3.getCount(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T3 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining()) ); -#endif - - if (ITimer3.getTimer() == 3) - { - if (countLocal != 0) - { - if (ITimer3.checkTimerDone()) - { -#if (TIMER_INTERRUPT_DEBUG > 1) - Serial.println("T3 callback, _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining()) + ", millis = " + String(millis()) ); -#endif - ITimer3.callback(); - - // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT - ITimer3.reload_OCRValue(); - - if (countLocal > 0) - ITimer3.setCount(countLocal - 1); - } - else - { - //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) - // If _OCRValue == 0, flag _timerDone for next cycle -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T3 before _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining()) ); -#endif - - // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining - ITimer3.adjust_OCRValue(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T3 after _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining()) ); -#endif - } - } - else - { -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T3 done"); -#endif - ITimer3.detachInterrupt(); + long countLocal = ITimer3.getCount(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T3 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining())); + #endif + + if (ITimer3.getTimer() == 3) + { + if (countLocal != 0) + { + if (ITimer3.checkTimerDone()) + { + #if (TIMER_INTERRUPT_DEBUG > 1) + Serial.println("T3 callback, _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining()) + + ", millis = " + String(millis())); + #endif + ITimer3.callback(); + + // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT + ITimer3.reload_OCRValue(); + + if (countLocal > 0) + ITimer3.setCount(countLocal - 1); + } + else + { + //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) + // If _OCRValue == 0, flag _timerDone for next cycle + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T3 before _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining())); + #endif + + // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining + ITimer3.adjust_OCRValue(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T3 after _OCRValueRemaining = " + String(ITimer3.get_OCRValueRemaining())); + #endif + } + } + else + { + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T3 done"); + #endif + ITimer3.detachInterrupt(); + } } - } } -#endif //#ifndef TIMER3_INSTANTIATED -#endif //#if USE_TIMER_3 + #endif //#ifndef TIMER3_INSTANTIATED + #endif //#if USE_TIMER_3 -#if USE_TIMER_4 -#ifndef TIMER4_INSTANTIATED -// To force pre-instatiate only once -#define TIMER4_INSTANTIATED + #if USE_TIMER_4 + #ifndef TIMER4_INSTANTIATED + // To force pre-instatiate only once + #define TIMER4_INSTANTIATED TimerInterrupt ITimer4(HW_TIMER_4); ISR(TIMER4_COMPA_vect) { - long countLocal = ITimer4.getCount(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T4 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining()) ); -#endif - - if (ITimer4.getTimer() == 4) - { - if (countLocal != 0) - { - if (ITimer4.checkTimerDone()) - { -#if (TIMER_INTERRUPT_DEBUG > 1) - Serial.println("T4 callback, _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining()) + ", millis = " + String(millis()) ); -#endif - ITimer4.callback(); - - // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT - ITimer4.reload_OCRValue(); - - if (countLocal > 0) - ITimer4.setCount(countLocal - 1); - } - else - { - //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) - // If _OCRValue == 0, flag _timerDone for next cycle -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T4 before _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining()) ); -#endif - - // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining - ITimer4.adjust_OCRValue(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T4 after _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining()) ); -#endif - } - } - else - { -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T4 done"); -#endif - ITimer4.detachInterrupt(); + long countLocal = ITimer4.getCount(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T4 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining())); + #endif + + if (ITimer4.getTimer() == 4) + { + if (countLocal != 0) + { + if (ITimer4.checkTimerDone()) + { + #if (TIMER_INTERRUPT_DEBUG > 1) + Serial.println("T4 callback, _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining()) + + ", millis = " + String(millis())); + #endif + ITimer4.callback(); + + // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT + ITimer4.reload_OCRValue(); + + if (countLocal > 0) + ITimer4.setCount(countLocal - 1); + } + else + { + //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) + // If _OCRValue == 0, flag _timerDone for next cycle + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T4 before _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining())); + #endif + + // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining + ITimer4.adjust_OCRValue(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T4 after _OCRValueRemaining = " + String(ITimer4.get_OCRValueRemaining())); + #endif + } + } + else + { + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T4 done"); + #endif + ITimer4.detachInterrupt(); + } } - } } + #endif //#ifndef TIMER4_INSTANTIATED + #endif //#if USE_TIMER_4 -#endif //#ifndef TIMER4_INSTANTIATED -#endif //#if USE_TIMER_4 - -#if USE_TIMER_5 -#ifndef TIMER5_INSTANTIATED -// To force pre-instatiate only once -#define TIMER5_INSTANTIATED + #if USE_TIMER_5 + #ifndef TIMER5_INSTANTIATED + // To force pre-instatiate only once + #define TIMER5_INSTANTIATED TimerInterrupt ITimer5(HW_TIMER_5); ISR(TIMER5_COMPA_vect) { - long countLocal = ITimer5.getCount(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T5 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining()) ); -#endif - - if (ITimer5.getTimer() == 5) - { - if (countLocal != 0) - { - if (ITimer5.checkTimerDone()) - { -#if (TIMER_INTERRUPT_DEBUG > 1) - Serial.println("T5 callback, _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining()) + ", millis = " + String(millis()) ); -#endif - ITimer5.callback(); - - // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT - ITimer5.reload_OCRValue(); - - if (countLocal > 0) - ITimer5.setCount(countLocal - 1); - } - else - { - //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) - // If _OCRValue == 0, flag _timerDone for next cycle -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T5 before _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining()) ); -#endif - - // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining - ITimer5.adjust_OCRValue(); - -#if (TIMER_INTERRUPT_DEBUG > 2) - Serial.println("T5 after _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining()) ); -#endif - } - } - else - { -#if (TIMER_INTERRUPT_DEBUG > 0) - Serial.println("T5 done"); -#endif - ITimer5.detachInterrupt(); + long countLocal = ITimer5.getCount(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T5 count = " + String(countLocal) + ", _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining())); + #endif + + if (ITimer5.getTimer() == 5) + { + if (countLocal != 0) + { + if (ITimer5.checkTimerDone()) + { + #if (TIMER_INTERRUPT_DEBUG > 1) + Serial.println("T5 callback, _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining()) + + ", millis = " + String(millis())); + #endif + ITimer5.callback(); + + // To reload _OCRValueRemaining as well as _OCR register to MAX_COUNT_16BIT + ITimer5.reload_OCRValue(); + + if (countLocal > 0) + ITimer5.setCount(countLocal - 1); + } + else + { + //Deduct _OCRValue by min(MAX_COUNT_16BIT, _OCRValue) + // If _OCRValue == 0, flag _timerDone for next cycle + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T5 before _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining())); + #endif + + // If last one (_OCRValueRemaining < MAX_COUNT_16BIT) => load _OCR register _OCRValueRemaining + ITimer5.adjust_OCRValue(); + + #if (TIMER_INTERRUPT_DEBUG > 2) + Serial.println("T5 after _OCRValueRemaining = " + String(ITimer5.get_OCRValueRemaining())); + #endif + } + } + else + { + #if (TIMER_INTERRUPT_DEBUG > 0) + Serial.println("T5 done"); + #endif + ITimer5.detachInterrupt(); + } } - } } -#endif //#ifndef TIMER5_INSTANTIATED -#endif //#if USE_TIMER_5 -#endif //#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) + #endif //#ifndef TIMER5_INSTANTIATED + #endif //#if USE_TIMER_5 + #endif //#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) -#endif //#ifndef TimerInterrupt_h +#endif //#ifndef TimerInterrupt_h #endif \ No newline at end of file diff --git a/src/testmenu.cpp b/src/testmenu.cpp index 1b0d1763..2649ea7d 100644 --- a/src/testmenu.cpp +++ b/src/testmenu.cpp @@ -336,9 +336,9 @@ void TestMenu::onCommandReceived(String s) s = s.substring(0, s.length() - 1); } - String reply = MeadeCommandProcessor::instance()->processCommand(s); + const char *reply = MeadeCommandProcessor::instance()->processCommand(s); - if (reply.length() > 0) + if (reply[0] != '\0') { Serial.println(F("-- Command Response --------")); Serial.println(reply);