diff --git a/.gitattributes b/.gitattributes new file mode 100755 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/.gitignore b/.gitignore new file mode 100755 index 0000000..e3ee71a --- /dev/null +++ b/.gitignore @@ -0,0 +1,13 @@ +.vscode/settings.json +*.json +__vm/* +*.vcxproj* +.vs/* +*.sln +Debug/* +Release/* +debug.cfg +esp32.svd +src/plugins/* +debug.svd +z.* diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..eaaae21 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,67 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project type + +OnStepX is an Arduino sketch ([OnStepX.ino](OnStepX.ino)) — not a CMake / PlatformIO / Makefile project. There is no `platformio.ini`. Builds happen in the Arduino IDE or via `arduino-cli`. The MCU target is selected by which board the user picks in their IDE; the code adapts via the HAL. + +There is no test suite, no linter config, and no CI. "Build" means "compile this sketch for the target board." When asked to verify a change, the realistic checks are: +- Read the relevant `Config.defaults.h` and `Validate.h` sections to confirm the change passes preprocessor validation. +- For deeper checks, run `arduino-cli compile --fqbn .` from the repo root if `arduino-cli` is available. + +## Configuration is preprocessor-driven + +User-editable controller configuration lives in three header files at the project root or in `src/plugins/`: + +- [Config.h](Config.h) — the main per-installation config: pinmap, mount type, axis drivers, steps/degree, serial baud, WiFi/BT, etc. This file *will* differ between installations and is edited by the end user. Do not "tidy" it. +- [Extended.config.h](Extended.config.h) — debug/NV-wipe and other infrequently-changed knobs. +- [src/plugins/Plugins.config.h](src/plugins/Plugins.config.h) — enables PLUGIN1..PLUGIN8. + +Defaults live in [src/Config.defaults.h](src/Config.defaults.h); every option `Config.h` does not set is filled in there. Validation rules live in [src/Validate.h](src/Validate.h) — `#error` and `#warning` directives there are the closest thing to a type system this codebase has. When adding a new config option, add the default AND a validation rule. + +`#define FirmwareVersionConfig` in `OnStepX.ino` is bumped whenever `Config.h`'s structure changes; mismatches trigger a `#error` in `Validate.h`. + +`src/plugins/*` is gitignored (except the `sample/` dir). Plugin code is expected to be dropped into that directory by the user. + +## Big picture: HAL → pinmap → Telescope → subsystems + +`OnStepX.ino` is intentionally tiny. It calls `HAL_INIT()`, brings up NV, starts the cooperative task scheduler, instantiates one global `Telescope telescope;`, and registers command channels. Everything else is driven from there. + +Layers, from bottom up: + +1. **HAL** ([src/HAL/](src/HAL/)): one subdirectory per MCU family (`atmel`, `esp`, `stm32`, `teensy`, `mbed`, `arduinoM0`, `default`). `HAL.h` does compile-time `#if defined(__AVR_…)` / `defined(ESP32)` / etc. selection and pulls in the right per-MCU header, which provides `HAL_INIT()`, `HAL_TEMP()`, fast-tick macros, and analog wrappers. Adding a new MCU means adding a HAL subfolder and a branch in `src/HAL/HAL.h`. +2. **Pinmap** ([src/pinmaps/](src/pinmaps/)): `Pins.*.h` per board. The user picks one via `#define PINMAP ` in `Config.h`; [src/pinmaps/Models.h](src/pinmaps/Models.h) maps that to the include. `Pins.defaults.h` fills in anything the chosen pinmap omits. If `PINMAP == OFF`, the user is expected to define `AXIS*_STEP_PIN` etc. directly in `Config.h` (see the user's current config for an example). +3. **Generic libraries** ([src/lib/](src/lib/)): hardware-independent building blocks — `tasks/` (cooperative scheduler, see below), `axis/` (motion control, motor abstraction), `nv/` (non-volatile storage over EEPROM/FRAM/Flash), `commands/` (parser + buffer), plus transports: `serial/`, `bluetooth/`, `wifi/`, `ethernet/`, `canPlus/`, `canTransport/`, and peripherals: `analog/`, `sense/`, `gpioEx/`, `tls/` (time/location source), `encoder/`, `softSpi/`, `convert/`, `math/`, `calendars/`. +4. **App-level shared services** ([src/libApp/](src/libApp/)): `commands/` (the `ProcessCmds` command channel processor and `CommandBroker` for cross-channel request/reply), `weather/`, `temperature/`. +5. **Telescope domain** ([src/telescope/](src/telescope/)): `Telescope.cpp` owns the global object; subdirs implement the four optional subsystems: `mount/`, `rotator/`, `focuser/`, `auxiliary/`. Whether each is compiled in is controlled by `*_PRESENT` macros derived in [src/Common.h](src/Common.h) from the driver-model and feature-purpose settings in `Config.h`. + +A request typically flows: serial/IP byte stream → `CommandProcessor::poll()` ([src/libApp/commands/ProcessCmds.cpp](src/libApp/commands/ProcessCmds.cpp)) → `telescope.command(...)` → subsystem `*.command.cpp` files. Each subsystem has its own `Foo.command.cpp` that handles its slice of the LX200-style protocol; that's where to look when implementing or modifying a `:XX...#` command. + +## Cooperative scheduler (OnTask) + +There is no RTOS abstraction layer above this — everything that isn't an ISR runs through `tasks` from [src/lib/tasks/OnTask.h](src/lib/tasks/OnTask.h). New periodic work is added with `tasks.add(periodMs, durationMs, repeat, priority, function, name)`. Long-running code inside a task must call `tasks.yield()` (or the `Y;` macro) so the scheduler can run higher-priority work — including the step-pulse generator. This is critical: blocking inside a task starves stepper output. + +The main `loop()` is just `tasks.yield();`. + +## Command protocol + +The canonical reference for the wire protocol is [docs/COMMAND_REFERENCE.md](docs/COMMAND_REFERENCE.md), and it's explicitly derived from the current `*.command.cpp` files — if behavior and the older OnStep wiki disagree, the code wins. Frame format: `:CC...#` (plain) or `;CC...CCS#` (with checksum). Error codes returned by `:GE#` are listed in that doc. + +Many commands are compile-time-gated by feature defines. Code under `#ifdef MOUNT_PRESENT`, `#ifdef FOCUSER_PRESENT`, etc. only exists when the corresponding axes/drivers are configured. + +## Subsystem docs (read these before changing related code) + +The `docs/` directory holds the design notes that aren't otherwise discoverable from the code: + +- [COMMAND_REFERENCE.md](docs/COMMAND_REFERENCE.md) — wire protocol. +- [STARTUP_AUTHORITY_NOTES.md](docs/STARTUP_AUTHORITY_NOTES.md) — when the mount trusts its coordinate basis enough to allow goto/sync/park. Important: gating logic here affects nearly every motion command. +- [GOTO_NOTES.md](docs/GOTO_NOTES.md), [HOMING_NOTES.md](docs/HOMING_NOTES.md), [PEC_NOTES.md](docs/PEC_NOTES.md), [SENSING_NOTES.md](docs/SENSING_NOTES.md) — subsystem-specific design notes. +- [SERIAL_NOTES.md](docs/SERIAL_NOTES.md) — three-layer model (platform / pinmap / feature) for understanding why a serial transport may "exist but not work" on a given board. +- [SERVO_SETUP.md](docs/SERVO_SETUP.md), [CAN_NOTES.md](docs/CAN_NOTES.md) — motor and transport specifics. + +## Conventions + +- Each subsystem follows the pattern `Foo.h` / `Foo.cpp` for state, `Foo.command.cpp` for the command handler, optionally `Foo.can.command.cpp` / `Foo.can.encode.cpp` / `Foo.can.decode.cpp` for the CAN-bus remote-control variant of the same surface. +- Logging uses the `V…` / `D…` macros from `src/lib/debug/Debug.h` (e.g. `VLF`, `VF`, `DLF`). Output is gated by the `DEBUG` setting in `Extended.config.h` and goes to `SERIAL_DEBUG`. Don't add raw `Serial.print` — use the macros so debug output can be compiled out. +- Commit messages follow the brief, lowercase-prefixed style shown by `git log --date=short --pretty=format:"%h %ad %<(20)%an %<(150,trunc)%s"` (per [README.md](README.md)). diff --git a/Config.h b/Config.h new file mode 100755 index 0000000..9a04671 --- /dev/null +++ b/Config.h @@ -0,0 +1,400 @@ +/* --------------------------------------------------------------------------------------------------------------------------------- + * Configuration for OnStepX + * + * For more information on setting OnStep up see http://www.stellarjourney.com/index.php?r=site/equipment_onstep + * and join the OnStep Groups.io at https://groups.io/g/onstep + * + * *** Read the compiler warnings and errors, they are there to help guard against invalid configurations *** + * + * --------------------------------------------------------------------------------------------------------------------------------- + * ADJUST THE FOLLOWING TO CONFIGURE YOUR CONTROLLER FEATURES ---------------------------------------------------------------------- + * <-Req'd = always must set, <-Often = usually must set, Option = optional, Adjust = adjust as req'd, Infreq = infrequently changed +*/ +// Parameter Name Value Default Notes Hint + +// ================================================================================================================================= +// CONTROLLER ====================================================================================================================== +#define HOST_NAME "OnStep" // nStep", Hostname for this device up to 16 chars. Adjust + + +#define HOST_NAME "OnStep" + +#define AXIS1_STEP_PIN 14 +#define AXIS1_DIR_PIN 12 + +#define AXIS2_STEP_PIN 27 +#define AXIS2_DIR_PIN 26 + +#define SHARED_ENABLE_PIN 25 // Közös láb, ahogy gyárilag tervezték! +#define STATUS_LED_PIN 2 + +// PINMAP ------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Controller#PINMAP +#define PINMAP OFF // OFF, Choose from: MiniPCB, MiniPCB2, MaxPCB4, MaxESP4, MaxSTM3, FYSETC_E4, <-Req'd + // BTT_SKR_PRO, etc. Other boards and more info. in ~/src/Constants.h + +// SERIAL PORT COMMAND CHANNELS --------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Controller#SERIAL_PORTS +#define SERIAL_A_BAUD_DEFAULT 57600 // 9600, n. Where n=9600,19200,57600,115200,230400,460800 (common baud rates.) Infreq +#define SERIAL_B_BAUD_DEFAULT OFF // 9600, n. Baud rate as above. See (src/pinmaps/) for Serial port assignments. Infreq +#define SERIAL_B_ESP_FLASHING OFF // OFF, ON Upload ESP8266 WiFi firmware through SERIAL_B with :ESPFLASH# cmd. Option +#define SERIAL_C_BAUD_DEFAULT OFF // OFF, n. Baud rate as above. See (src/pinmaps/) for Serial port assignments. Infreq +#define SERIAL_D_BAUD_DEFAULT OFF // OFF, n. Baud rate as above. See (src/pinmaps/) for Serial port assignments. Infreq +#define SERIAL_E_BAUD_DEFAULT OFF // OFF, n. Baud rate as above. See (src/pinmaps/) for Serial port assignments. Infreq +#define SERIAL_RADIO WIFI_ACCESS_POINT // OFF, Use BLUETOOTH or WIFI_ACCESS_POINT or WIFI_STATION (ESP32 only.) Option + +// STATUS --------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Controller#STATUS_LED +#define STATUS_LED ON // OFF, Steady illumination if no error, blinks w/error code otherwise. Option + +// RETICLE CONTROL ------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Controller#RETICLE_CONTROL +#define RETICLE_LED_DEFAULT OFF // OFF, n. Where n=0..255 (0..100%) activates feature sets default brightness. Option +#define RETICLE_LED_MEMORY OFF // OFF, ON Remember reticle brightness across power cycles. Option +#define RETICLE_LED_INVERT OFF // OFF, ON Inverts control for cases where 0V is max brightness. Option + +// WEATHER SENSOR --------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Controller#WEATHER_SENSOR +#define WEATHER OFF // OFF, BME280 (I2C 0x77,) BME280_0x76, BME280_SPI (see pinmap for CS.) Option + // BMP280 (I2C 0x77,) BMP280_0x76, BMP280_SPI (see pinmap for CS.) + // BME280 or BMP280 for temperature, pressure. BME280 for humidity also. + +// SIGNALING ------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Controller#SIGNALING +#define STEP_WAVE_FORM SQUARE // SQUARE, PULSE Step signal wave form faster rates. SQUARE best signal integrity. Adjust + // Applies to all axes. + +// NON-VOLATILE MEMORY ---------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Controller#NV +#define NV_DRIVER NV_DEFAULT // NV_DEF, Use platforms default non-volatile device to remember runtime settings. Option + +// ================================================================================================================================= +// MOUNT =========================================================================================================================== + +// Driver models (Step/Dir and Servo) many have specific requirements so be sure to follow the link below to help learn about these. +// Typically: A4988, DRV8825, LV8729, S109, TMC2130, TMC5160, TMC2209, etc. + +// AXIS1 RA/AZM -------------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Axes +#define AXIS1_DRIVER_MODEL TMC2209S // OFF, Enter motor driver model (above) in both axes to activate the mount. <-Often + +// If runtime axis settings are enabled changes in the section below will be ignored (disable in SWS or by wiping NV/EEPROM): +// \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ +#define AXIS1_STEPS_PER_DEGREE 3582.2 // 12800, n. Number of steps per degree: <-Req'd + // n = (stepper_steps * micro_steps * overall_gear_reduction)/360.0 +#define AXIS1_REVERSE OFF // OFF, ON Reverses movement direction, or reverse wiring instead to correct. <-Often +#define AXIS1_LIMIT_MIN -180 // -180, n. Where n= -90..-360 (degrees.) Minimum "Hour Angle" or Azimuth. Adjust +#define AXIS1_LIMIT_MAX 180 // 180, n. Where n= 90.. 360 (degrees.) Maximum "Hour Angle" or Azimuth. Adjust +#define AXIS1_LIMIT_SYNC OFF // OFF, n. Where n= 0..90 (degrees.) Allow sync/reset only within this +/-range. Option + +#define AXIS1_DRIVER_MICROSTEPS 16 // OFF, n. Microstep mode when tracking. <-Req'd +#define AXIS1_DRIVER_MICROSTEPS_GOTO 4 // OFF, n. Microstep mode used during slews. OFF uses _DRIVER_MICROSTEPS. Option + +// for TMC2130, TMC5160, TMC2209, TMC2226 STEP/DIR driver models: +#define AXIS1_DRIVER_IHOLD OFF // OFF, n, (mA.) Current during standstill. OFF uses IRUN/2.0 Option +#define AXIS1_DRIVER_IRUN OFF // OFF, n, (mA.) Current during tracking, appropriate for stepper/driver/etc. Option +#define AXIS1_DRIVER_IGOTO OFF // OFF, n, (mA.) Current during slews. OFF uses IRUN. Option +// /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ / + +#define AXIS1_DRIVER_STATUS OFF // OFF, ON, HIGH, or LOW. For driver status info/fault detection. Option + +#define AXIS1_DRIVER_DECAY OFF // OFF, Tracking decay mode default override. TMC default is STEALTHCHOP. Infreq +#define AXIS1_DRIVER_DECAY_GOTO OFF // OFF, Decay mode goto default override. TMC default is SPREADCYCLE. Infreq + +#define AXIS1_POWER_DOWN OFF // OFF, ON Powers off 30sec after movement stops or 10min after last<=1x guide. Infreq + +#define AXIS1_SENSE_HOME OFF // OFF, HIGH or LOW enables & state clockwise home position, as seen from front. Option +#define AXIS1_SENSE_LIMIT_MIN LIMIT_SENSE // ...NSE, HIGH or LOW state on limit sense switch stops movement. Option +#define AXIS1_SENSE_LIMIT_MAX LIMIT_SENSE // ...NSE, HIGH or LOW state on limit sense switch stops movement. Option + // Digital, optionally add: |HYST(n) Where n=0..1023 stability time in ms. + // Analog capable sense inputs also allow adding: + // |THLD(n) Where n=1..1023 (ADU) for Analog threshold. + // |HYST(n) Where n=0..1023 (ADU) for +/- Hystersis range. + +// AXIS2 DEC/ALT ------------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Axes +#define AXIS2_DRIVER_MODEL TMC2209S // OFF, Enter motor driver model (above) in both axes to activate the mount. <-Often + +// If runtime axis settings are enabled changes in the section below will be ignored (disable in SWS or by wiping NV/EEPROM): +// \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ +#define AXIS2_STEPS_PER_DEGREE 3591.1 // 12800, n. Number of steps per degree: <-Req'd + // n = (stepper_steps * micro_steps * overall_gear_reduction)/360.0 +#define AXIS2_REVERSE OFF // OFF, ON Reverses movement direction, or reverse wiring instead to correct. <-Often +#define AXIS2_LIMIT_MIN -90 // -90, n. Where n=-90..0 (degrees.) Minimum allowed Declination or Altitude. Infreq +#define AXIS2_LIMIT_MAX 90 // 90, n. Where n= 0..90 (degrees.) Maximum allowed Declination or Altitude. Infreq +#define AXIS2_LIMIT_SYNC OFF // OFF, n. Where n= 0..90 (degrees.) Allow sync/reset only within this +/-range. Option + +#define AXIS2_DRIVER_MICROSTEPS 16 // OFF, n. Microstep mode when tracking. <-Req'd +#define AXIS2_DRIVER_MICROSTEPS_GOTO OFF // OFF, n. Microstep mode used during slews. OFF uses _DRIVER_MICROSTEPS. Option + +// for TMC2130, TMC5160, TMC2209, TMC2226 STEP/DIR driver models: +#define AXIS2_DRIVER_IHOLD OFF // OFF, n, (mA.) Current during standstill. OFF uses IRUN/2.0 Option +#define AXIS2_DRIVER_IRUN OFF // OFF, n, (mA.) Current during tracking, appropriate for stepper/driver/etc. Option +#define AXIS2_DRIVER_IGOTO OFF // OFF, n, (mA.) Current during slews. OFF uses IRUN. Option +// /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ / + +#define AXIS2_DRIVER_STATUS OFF // OFF, ON, HIGH, or LOW. Polling for driver status info/fault detection. Option + +#define AXIS2_DRIVER_DECAY OFF // OFF, Tracking decay mode default override. TMC default is STEALTHCHOP. Infreq +#define AXIS2_DRIVER_DECAY_GOTO OFF // OFF, Decay mode goto default override. TMC default is SPREADCYCLE. Infreq + +#define AXIS2_POWER_DOWN OFF // OFF, ON Powers off 30sec after movement stops or 10min after last<=1x guide. Option + +#define AXIS2_SENSE_HOME OFF // OFF, HIGH or LOW enables & state clockwise home position, as seen from above. Option +#define AXIS2_SENSE_LIMIT_MIN LIMIT_SENSE // ...NSE, HIGH or LOW state on limit sense switch stops movement. Option +#define AXIS2_SENSE_LIMIT_MAX LIMIT_SENSE // ...NSE, HIGH or LOW state on limit sense switch stops movement. Option + +// MOUNT -------------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Mount#MOUNT +#define MOUNT_TYPE GEM // GEM, GEM German Equatorial Mount, etc. that need meridian flips. <-Req'd + // GEM_TA GEM w/tangent arm Declination + // GEM_TAC GEM w/tangent arm Declination and geometry correction + // FORK Fork Mount + // FORK_TA FORK w/tangent arm Declination + // FORK_TAC FORK w/tangent arm Declination and geometry correction + // ALTAZM Altitude/Azimuth Mount, Dobsonians, etc. + // ALTAZM_UNL ALTAZM w/unlimited Azimuth motion + +#define MOUNT_ALTERNATE_ORIENTATION OFF // OFF, ON Enables Meridian Flips for FORK mounts and passing through the Option + // Zenith for ALTAZM mounts. GEM mode ignores this setting. + +#define MOUNT_STARTUP_MODE SA_AUTO // ..AUTO, SA_STRICT, or SA_PERMISSIVE. Controls when startup trust is granted. Option + // SA_AUTO keeps legacy immediate goto only when no absolute position source + // is present and coordinate memory is OFF. + +#define MOUNT_COORDS TOPOCENTRIC // ...RIC, Applies refraction to coordinates to/from OnStep, except exactly Infreq + // at the poles. Use TOPO_STRICT to apply refraction even in that case. + // Use OBSERVED_PLACE for no refraction. + +#define MOUNT_COORDS_MEMORY OFF // OFF, ON Remembers approximate mount coordinates across power cycles. Option + // Requires FRAM NV memory and the mount must not move while powered down. + +#define MOUNT_ENABLE_IN_STANDBY OFF // OFF, ON Enables mount motor drivers while in standby. Infreq + +// TIME AND LOCATION ---------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Mount#TLS +#define TIME_LOCATION_SOURCE OFF // OFF, DS3231 (I2C,) SD3031 (I2C,) TEENSY (T3.2 etc,) GPS, or NTP source. Option + // Provides Date/Time, and if available, PPS & Lat/Long also. +#define TIME_LOCATION_PPS_SENSE OFF // OFF, HIGH senses PPS (pulse per second,) signal rising edge, or use LOW for Option + // falling edge, or use BOTH for rising and falling edges. + +// STATUS ------------------------------------------------------ see https://onstep.groups.io/g/main/wiki/Configuration_Mount#STATUS +#define STATUS_MOUNT_LED OFF // OFF, ON Flashes proportional to rate of movement or solid on for slews. Option +#define STATUS_BUZZER OFF // OFF, ON, n. Where n=100..6000 (Hz freq.) for speaker. ON for piezo buzzer. Option +#define STATUS_BUZZER_DEFAULT OFF // OFF, ON starts w/buzzer sound enabled. Option +#define STATUS_BUZZER_MEMORY OFF // OFF, ON to remember buzzer sound setting across power cycles. Option + +// ST4 INTERFACE -------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Mount#ST4 +// *** It is up to you to verify the interface meets the electrical specifications of any connected device, use at your own risk *** +#define ST4_INTERFACE OFF // OFF, ON enables interface. <= 1X guides unless hand control mode. Option + // During goto btn press: aborts slew or continue meridian flip pause home +#define ST4_HAND_CONTROL ON // ON, ON for hand controller special features and SHC support. Option + // Hold [E]+[W] btns >2s: Guide rate [E]- [W]+ [N] trk on/off [S] sync + // Hold [N]+[S] btns >2s: Usr cat item [E]- [W]+ [N] goto [S] snd on/off +#define ST4_HAND_CONTROL_FOCUSER ON // ON, ON alternate to above: Focuser move [E]f1 [W]f2 [N]- [S]+ Option + +// GUIDING BEHAVIOUR ------------------------------------------ see https://onstep.groups.io/g/main/wiki/Configuration_Mount#GUIDING +#define GUIDE_TIME_LIMIT 10 // 10, n. Time limit n=0..120 seconds. Use 0 to disable. Adjust +#define GUIDE_DISABLE_BACKLASH OFF // OFF, Disable backlash takeup during guiding at <= 1X. Option + +// LIMITS ------------------------------------------------------ see https://onstep.groups.io/g/main/wiki/Configuration_Mount#LIMITS +#define LIMIT_SENSE OFF // OFF, HIGH or LOW state on limit sense switch stops movement. Option +#define LIMIT_STRICT OFF // OFF, disables limits until unpark goto or sync. ON enables limits at startup. Option + // note that ON also disables all motion until date/time are set. + +// PARKING ---------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Mount#PARKING +#define PARK_SENSE OFF // OFF, HIGH or LOW state indicates mount is in the park orientation. Option +#define PARK_SIGNAL OFF // OFF, HIGH or LOW state park input signal triggers parking. Option +#define PARK_STATUS OFF // OFF, signals with a HIGH or LOW state when successfully parked. Option + +// PEC ------------------------------------------------------------ see https://onstep.groups.io/g/main/wiki/Configuration_Mount#PEC +#define PEC_STEPS_PER_WORM_ROTATION 12800 // 0, n. Steps per worm rotation (0 disables else 720 sec buffer allocated.) <-Req'd + // n = (AXIS1_STEPS_PER_DEGREE*360)/reduction_final_stage + +#define PEC_SENSE OFF // OFF, HIGH. Senses the PEC signal rising edge or use LOW for falling edge. Option + // Ignored in ALTAZM mode. +#define PEC_BUFFER_SIZE_LIMIT 720 // 720, Seconds of PEC buffer allowed. Infreq + +// TRACKING BEHAVIOUR ---------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Mount#TRACKING +#define TRACK_BACKLASH_RATE 20 // 20, n. Where n=2..50 (x sidereal rate) during backlash takeup. Option + // Too fast motors stall/gears slam or too slow and sluggish in backlash. +#define TRACK_AUTOSTART OFF // OFF, ON Start with tracking enabled. Option +#define TRACK_COMPENSATION_DEFAULT OFF // OFF, No compensation or REFRACTION, REFRACTION_DUAL, MODEL, MODEL_DUAL. Option +#define TRACK_COMPENSATION_MEMORY OFF // OFF, ON Remembers refraction/pointing model compensated tracking settings. Option + +// SLEWING BEHAVIOUR ------------------------------------------ see https://onstep.groups.io/g/main/wiki/Configuration_Mount#SLEWING +#define SLEW_RATE_BASE_DESIRED 2.5 // 1.0, n. Desired slew rate in deg/sec. Adjustable at run-time from <-Req'd + // 1/2 to 2x this rate, and as performace considerations require. +#define SLEW_RATE_MEMORY OFF // OFF, ON Remembers rates set across power cycles. Option +#define SLEW_ACCELERATION_DIST 5.0 // 5.0, n, (degrees.) Approx. distance for acceleration (and deceleration.) Adjust +#define SLEW_RAPID_STOP_DIST 2.0 // 2.0, n, (degrees.) Approx. distance required to stop when a slew Adjust + // is aborted or a limit is exceeded. +#define GOTO_FEATURE ON // ON, Use OFF to disable mount Goto features. Infreq +#define GOTO_OFFSET 0.25 // 0.25, Offset in deg's for goto target unidirectional approach, 0.0 disables Adjust +#define GOTO_OFFSET_ALIGN OFF // OFF, ON skips final phase of goto for align stars so user tends to approach Option + // from the correct direction when centering. + +// PIER SIDE BEHAVIOUR --------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Mount#PIERSIDE +#define MFLIP_SKIP_HOME OFF // OFF, ON Goto directly to the destination without visiting home position. Option +#define MFLIP_AUTOMATIC_DEFAULT OFF // OFF, ON Start with automatic meridian flips enabled. Option +#define MFLIP_AUTOMATIC_MEMORY OFF // OFF, ON Remember automatic meridian flip setting across power cycles. Option +#define MFLIP_PAUSE_HOME_DEFAULT OFF // OFF, ON Start with meridian flip pause at home enabed. Infreq +#define MFLIP_PAUSE_HOME_MEMORY OFF // OFF, ON Remember meridian flip pause at home setting across power cycles. Infreq + +#define PIER_SIDE_SYNC_CHANGE_SIDES OFF // OFF, ON Allows sync to change pier side, for GEM mounts. Option +#define PIER_SIDE_PREFERRED_DEFAULT BEST // BEST, BEST Stays on current side if possible. EAST or WEST switch if possible. Option + // AUTOMATIC prefers west nearer the under pole meridian or east otherwise. Option +#define PIER_SIDE_PREFERRED_MEMORY OFF // OFF, ON Remember preferred pier side setting across power cycles. Option + +// ALIGN -------------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Mount#ALIGN +#define ALIGN_AUTO_HOME OFF // OFF, ON uses home switches to find home first when starting an align. Option +#define ALIGN_MODEL_MEMORY OFF // OFF, ON Restores any pointing model saved in NV at startup. Option +#define ALIGN_MAX_STARS AUTO // AUTO, Uses HAL specified default (either 6 or 9 stars.) Infreq + // Or use n. Where n=1 (for Sync only) or 3 to 9 (for Goto Assist.) + +// ================================================================================================================================= +// ROTATOR ========================================================================================================================= + +// Driver models (Step/Dir and Servo) many have specific requirements so be sure to follow the link below to help learn about these. +// Typically: A4988, DRV8825, LV8729, S109, TMC2130S, etc. + +// AXIS3 ROTATOR ---------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Rotator +#define AXIS3_DRIVER_MODEL OFF // OFF, Enter motor driver model (above) to activate the rotator. Option +#define AXIS3_SLEW_RATE_BASE_DESIRED 1.0 // 1.0, n. Desired slew rate in deg/sec. Adjustable at run-time from <-Req'd + // 1/2 to 2x this rate, and as performance considerations require. + +// If runtime axis settings are enabled changes in the section below will be ignored (disable in SWS or by wiping NV/EEPROM): +// \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ +#define AXIS3_STEPS_PER_DEGREE 64.0 // 64.0, n. Number of steps per degree for rotator/de-rotator. Adjust + // Alt/Az de-rotation: n = (num_circumference_pixels * 2)/360, minimum +#define AXIS3_REVERSE OFF // OFF, ON Reverses movement direction, or reverse wiring instead to correct. Option +#define AXIS3_LIMIT_MIN 0 // 0, n. Where n=-360..0 (degrees.) Minimum allowed rotator angle. Infreq +#define AXIS3_LIMIT_MAX 360 // 360, n. Where n=0..360 (degrees.) Maximum allowed rotator angle. Infreq + +#define AXIS3_DRIVER_MICROSTEPS OFF // OFF, n. Microstep mode when tracking. Option +#define AXIS3_DRIVER_MICROSTEPS_GOTO OFF // OFF, n. Microstep mode used during slews. OFF uses _DRIVER_MICROSTEPS. Option + +// for TMC2130, TMC5160, TMC2209, TMC2226 STEP/DIR driver models: +#define AXIS3_DRIVER_IHOLD OFF // OFF, n, (mA.) Current during standstill. OFF uses IRUN/2.0 Option +#define AXIS3_DRIVER_IRUN OFF // OFF, n, (mA.) Current during tracking, appropriate for stepper/driver/etc. Option +#define AXIS3_DRIVER_IGOTO OFF // OFF, n, (mA.) Current during slews. OFF uses IRUN. Option +// /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ / + +#define AXIS3_DRIVER_STATUS OFF // OFF, ON, HIGH, or LOW. For driver status info/fault detection. Option + +#define AXIS3_DRIVER_DECAY OFF // OFF, Tracking decay mode default override. TMC default is STEALTHCHOP. Infreq +#define AXIS3_DRIVER_DECAY_GOTO OFF // OFF, Decay mode goto default override. TMC default is SPREADCYCLE. Infreq + +#define AXIS3_POWER_DOWN OFF // OFF, ON Powers off 30 seconds after movement stops. Option + +#define AXIS3_SENSE_HOME OFF // OFF, HIGH or LOW enables & state clockwise home position, as seen from above. Option +#define AXIS3_SENSE_LIMIT_MIN OFF // OFF, HIGH or LOW state on limit sense switch stops movement. Option +#define AXIS3_SENSE_LIMIT_MAX OFF // OFF, HIGH or LOW state on limit sense switch stops movement. Option + +// ================================================================================================================================= +// FOCUSERS ======================================================================================================================== + +// Driver models (Step/Dir and Servo) many have specific requirements so be sure to follow the link below to help learn about these. +// Typically: A4988, DRV8825, LV8729, S109, TMC2130S, etc. + +// AXIS4 FOCUSER 1 -------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Focuser +#define AXIS4_DRIVER_MODEL OFF // OFF, Enter motor driver model (above) to activate the focuser. Option +#define AXIS4_SLEW_RATE_BASE_DESIRED 500 // 500, n, Where n=200..5000 (um/s.) Adjustable at run-time from <-Req'd + // 1/2 to 2x this rate, and as performance considerations require. +#define AXIS4_SLEW_RATE_MINIMUM 20 // 20, n. Where n=5..200 (um/s.) Minimum microns/second. Adjust + +// If runtime axis settings are enabled changes in the section below will be ignored (disable in SWS or by wiping NV/EEPROM): +// \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ +#define AXIS4_STEPS_PER_MICRON 0.5 // 0.5, n. Steps per micrometer. Figure this out by testing or other means. Adjust +#define AXIS4_REVERSE OFF // OFF, ON Reverses movement direction, or reverse wiring instead to correct. Option +#define AXIS4_LIMIT_MIN 0 // 0, n. Where n=0..500 (millimeters.) Minimum allowed position. Adjust +#define AXIS4_LIMIT_MAX 50 // 50, n. Where n=0..500 (millimeters.) Maximum allowed position. Adjust + +#define AXIS4_DRIVER_MICROSTEPS OFF // OFF, n. Microstep mode when tracking. Option +#define AXIS4_DRIVER_MICROSTEPS_GOTO OFF // OFF, n. Microstep mode used during slews. OFF uses _DRIVER_MICROSTEPS. Option + +// for TMC2130, TMC5160, TMC2209, TMC2226 STEP/DIR driver models: +#define AXIS4_DRIVER_IHOLD OFF // OFF, n, (mA.) Current during standstill. OFF uses IRUN/2.0 Option +#define AXIS4_DRIVER_IRUN OFF // OFF, n, (mA.) Current during tracking, appropriate for stepper/driver/etc. Option +#define AXIS4_DRIVER_IGOTO OFF // OFF, n, (mA.) Current during slews. OFF uses IRUN. Option +// /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ /\ / + +#define AXIS4_DRIVER_STATUS OFF // OFF, ON, HIGH, or LOW. For driver status info/fault detection. Option + +#define AXIS4_DRIVER_DECAY OFF // OFF, Tracking decay mode default override. TMC default is STEALTHCHOP. Infreq +#define AXIS4_DRIVER_DECAY_GOTO OFF // OFF, Decay mode goto default override. TMC default is SPREADCYCLE. Infreq + +#define AXIS4_POWER_DOWN OFF // OFF, ON Powers off 30 seconds after movement stops. Option + +#define AXIS4_SENSE_HOME OFF // OFF, HIGH or LOW enables & state clockwise home position, as seen from above. Option +#define AXIS4_SENSE_LIMIT_MIN OFF // OFF, HIGH or LOW state on limit sense switch stops movement. Option +#define AXIS4_SENSE_LIMIT_MAX OFF // OFF, HIGH or LOW state on limit sense switch stops movement. Option + +// AXIS5 FOCUSER 2 ----------------------------------------------------------------------------------------------------------------- +// Up to 6 focusers can be present (AXIS4 to AXIS9) simply copy the above text for focuser 1 and rename to AXIS5_ for focuser 2, etc + +// FOCUSER TEMPERATURE ---------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Focuser +#define FOCUSER_TEMPERATURE OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n for focuser temp. Adjust + +// ================================================================================================================================= +// AUXILIARY FEATURES ============================================================================================================== + +// FEATURES ------------------------------------------------------------- see https://onstep.groups.io/g/main/wiki/Configuration_Aux +// Note: Temporarily set DEBUG mode to VERBOSE and use "FEATURE1_TEMP DS1820" to list the DS18B20 device serial numbers. + +#define FEATURE1_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE1_NAME "FEATURE1" // "FE..", Name of feature being controlled. Adjust +#define FEATURE1_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE1_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE1_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE1_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE1_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +#define FEATURE2_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE2_NAME "FEATURE2" // "FE..", Name of feature being controlled. Adjust +#define FEATURE2_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE2_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE2_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE2_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE2_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +#define FEATURE3_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE3_NAME "FEATURE3" // "FE..", Name of feature being controlled. Adjust +#define FEATURE3_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE3_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE3_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE3_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE3_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +#define FEATURE4_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE4_NAME "FEATURE4" // "FE..", Name of feature being controlled. Adjust +#define FEATURE4_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE4_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE4_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE4_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE4_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +#define FEATURE5_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE5_NAME "FEATURE5" // "FE..", Name of feature being controlled. Adjust +#define FEATURE5_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE5_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE5_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE5_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE5_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +#define FEATURE6_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE6_NAME "FEATURE6" // "FE..", Name of feature being controlled. Adjust +#define FEATURE6_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE6_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE6_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE6_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE6_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +#define FEATURE7_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE7_NAME "FEATURE7" // "FE..", Name of feature being controlled. Adjust +#define FEATURE7_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE7_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE7_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE7_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE7_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +#define FEATURE8_PURPOSE OFF // OFF, SWITCH, MOMENTARY_SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER. Option +#define FEATURE8_NAME "FEATURE8" // "FE..", Name of feature being controlled. Adjust +#define FEATURE8_TEMP OFF // OFF, THERMISTOR or n. Where n is the ds18b20 s/n. For DEW_HEATER temperature. Adjust +#define FEATURE8_PIN OFF // OFF, AUX for auxiliary pin, n. Where n is the pin#. Adjust +#define FEATURE8_VALUE_DEFAULT OFF // OFF, ON, n. Where n=0..255 for ANALOG_OUT purpose. Adjust +#define FEATURE8_VALUE_MEMORY OFF // OFF, ON remembers SWITCH, ANALOG_OUT, DEW_HEATER state across power cycles. Adjust +#define FEATURE8_ON_STATE HIGH // HIGH, LOW to invert so "ON" is 0V and "OFF" is Vcc (3.3V usually.) Adjust + +// --------------------------------------------------------------------------------------------------------------------------------- +#define FileVersionConfig 6 +#include "Extended.config.h" diff --git a/Extended.config.h b/Extended.config.h new file mode 100755 index 0000000..9c7914e --- /dev/null +++ b/Extended.config.h @@ -0,0 +1,38 @@ +/* --------------------------------------------------------------------------------------------------------------------------------- + * Extended configuration for OnStepX INFREQUENTLY USED options + * + * For more information on setting OnStep up see http://www.stellarjourney.com/index.php?r=site/equipment_onstep + * and join the OnStep Groups.io at https://groups.io/g/onstep + * + * *** Read the compiler warnings and errors, they are there to help guard against invalid configurations *** + * + * --------------------------------------------------------------------------------------------------------------------------------- + * ADJUST THE FOLLOWING TO CONFIGURE YOUR CONTROLLER FEATURES ---------------------------------------------------------------------- + * <-Req'd = always must set, <-Often = usually must set, Option = optional, Adjust = adjust as req'd, Infreq = infrequently changed +*/ +// Parameter Name Value Default Notes Hint + +// ================================================================================================================================= +// CONTROLLER ====================================================================================================================== + +// DEBUG --------------------------------------------------------------------------------------------------------------------------- +// Enables additional debugging and/or status messages on the specified SERIAL_DEBUG port (be sure to disable when done!) +// Note: The SERIAL_DEBUG port is often (almost always) attached to the same SERIAL_A port so it cannot be used for normal +// LX200 protocol communications. If this is the case set both to the same (faster) baud rate specified below. +#define DEBUG OFF // OFF, Use ON for background error messages only, use VERBOSE for all Infreq + // error and status messages, use CONSOLE for VT100 debug console, + // or use PROFILER for VT100 task profiler. +#define DEBUG_AXIS OFF // OFF, n. Where n=1 to 9 as the designated axis for logging activity. Option +#define DEBUG_CAN OFF // OFF, Use ON to log CAN interface statistics. Option +#define DEBUG_ECHO_COMMANDS OFF // OFF, Use ON or ERRORS_ONLY to log commands to the debug serial port. Option +#define SERIAL_DEBUG Serial // Serial, Use any available h/w serial port. Serial1 or Serial2, etc. Option +#define SERIAL_DEBUG_BAUD 230400 // 230400, n. Where n=9600,19200,57600,115200,230400,460800 (common baud rates.) Option + +// NON-VOLATILE STORAGE ------------------------------------------------------------------------------------------------------------ +#define NV_WIPE OFF // OFF, Causes the defaults to be written back into NV (FLASH,EEPROM,etc.) Infreq + // *** IMPORTANT: ENABLE THIS OPTION THEN UPLOAD, WAIT A MINUTE *** +// Warning ---> // *** OR TWO THEN SET THIS OPTION TO OFF AND UPLOAD AGAIN. *** + // *** LEAVING THIS OPTION ENABLED CAN CAUSE EXCESSIVE NV *** + // *** WEAR AND DAMAGE THE MICROCONTROLLER NV SUBSYSTEM !!! *** + +// --------------------------------------------------------------------------------------------------------------------------------- diff --git a/LICENSE b/LICENSE new file mode 100755 index 0000000..e62ec04 --- /dev/null +++ b/LICENSE @@ -0,0 +1,674 @@ +GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/OnStepX.ino b/OnStepX.ino new file mode 100755 index 0000000..ed66400 --- /dev/null +++ b/OnStepX.ino @@ -0,0 +1,163 @@ +/* + * Title OnStepX + * by Howard Dutton + * + * Copyright (C) 2021-2026 Howard Dutton + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Description: + * Full featured telescope control for + * Equatorial and Alt-Az mounts + * Rotator + * Focusers + * Accessories (automatic covers, dew heaters, etc.) + * + * Author: Howard Dutton + * http://www.stellarjourney.com + * hjd1964@gmail.com + * + * Revision history, and newer versions: + * See GitHub: https://github.com/hjd1964/OnStep + * + * Documentation: + * https://groups.io/g/onstep/wiki/home + * + * Discussion, Questions, ...etc + * https://groups.io/g/onstep + */ + +// Use tabs "Config..." to configure OnStep to your requirements + +// Firmware version ---------------------------------------------------------------------------------------------------------------- +#define FirmwareName "On-Step" +#define FirmwareVersionMajor 10 +#define FirmwareVersionMinor 28 // minor version 00 to 99 +#define FirmwareVersionPatch "i" // for example major.minor patch: 10.03c +#define FirmwareVersionConfig 6 // internal, for tracking configuration file changes + +#include "src/Common.h" +#include "src/Validate.h" +#include "src/lib/nv/Nv.h" +#include "src/lib/analog/Analog.h" +#include "src/lib/sense/Sense.h" +#include "src/lib/tasks/OnTask.h" + +#include "src/telescope/Telescope.h" +extern Telescope telescope; + +#include "src/plugins/Plugins.config.h" + +#if DEBUG == PROFILER + extern void profiler(); +#endif + +void sensesPoll() { + sense.poll(); +} + +void setup() { + //pinMode(SHARED_ENABLE_PIN, OUTPUT); + //digitalWrite(SHARED_ENABLE_PIN, HIGH); + #if ADDON_SELECT_PIN != OFF + pinMode(ADDON_SELECT_PIN, OUTPUT); + digitalWrite(ADDON_SELECT_PIN, HIGH); + #endif + + #if DEBUG != OFF + SERIAL_DEBUG.begin(SERIAL_DEBUG_BAUD); + delay(2000); + #endif + + // let any special processing the pinmap needs happen + #ifdef PIN_INIT + PIN_INIT(); + #endif + + // say hello + VLF(""); + VF("MSG: OnStepX, version "); V(FirmwareVersionMajor); V("."); V(FirmwareVersionMinor); VL(FirmwareVersionPatch); + VF("MSG: OnStepX, MCU "); VLF(MCU_STR); + VF("MSG: OnStepX, pinmap "); VLF(PINMAP_STR); + + // start low level hardware + VLF("MSG: System, HAL initialize"); + HAL_INIT(); + WIRE_INIT(); + + analog.begin(); + + nv().setGate(&xBusy); + if (!nv().init()) { + DLF("ERR: Setup, NV (EEPROM/FRAM/Flash/etc.) device not found!"); + } + delay(2000); + + #if defined(NV_WIPE) && NV_WIPE == ON + nv().wipe(); + #endif + + // start input sense polling task + int pollingRate = round((1000.0F/HAL_FRACTIONAL_SEC)/2.0F); + if (pollingRate < 1) pollingRate = 1; + VF("MSG: System, start input sense service task (rate "); V(pollingRate); VF("ms priority 7)... "); + if (tasks.add(pollingRate, 0, true, 7, sensesPoll, "SysSens")) { VLF("success"); } else { VLF("FAILED!"); } + + // start telescope object + telescope.init(FirmwareName, FirmwareVersionMajor, FirmwareVersionMinor, FirmwareVersionPatch, FirmwareVersionConfig); + + // start command channel tasks + commandChannelInit(); + + tasks.yield(2000); + + // start any plugins + #if PLUGIN1 != OFF + PLUGIN1.init(); + #endif + #if PLUGIN2 != OFF + PLUGIN2.init(); + #endif + #if PLUGIN3 != OFF + PLUGIN3.init(); + #endif + #if PLUGIN4 != OFF + PLUGIN4.init(); + #endif + #if PLUGIN5 != OFF + PLUGIN5.init(); + #endif + #if PLUGIN6 != OFF + PLUGIN6.init(); + #endif + #if PLUGIN7 != OFF + PLUGIN7.init(); + #endif + #if PLUGIN8 != OFF + PLUGIN8.init(); + #endif + + // start task manager debug events + #if DEBUG == PROFILER + tasks.add(142, 0, true, 7, profiler, "Profilr"); + #endif + + sense.poll(); + + telescope.ready = true; +} + +void loop() { + tasks.yield(); +} diff --git a/app.bin b/app.bin new file mode 100644 index 0000000..2ea7039 Binary files /dev/null and b/app.bin differ diff --git a/docs/CAN_NOTES.md b/docs/CAN_NOTES.md new file mode 100755 index 0000000..7cc0953 --- /dev/null +++ b/docs/CAN_NOTES.md @@ -0,0 +1,437 @@ +# CAN Notes + +## Purpose + +These notes describe the current CAN interface in its most useful practical +role: interconnecting remote OnStepX peripheral features. + +This note is mainly about using CAN so the main controller can talk to: + +- a remote rotator +- one or more remote focusers +- a remote auxiliary-features node + +It is not mainly about low-level motor/axis CAN protocols. Those are better +left for a later axis-oriented note. + +## Quick Summary + +The cleanest mental model is: + +1. the main OnStepX controller runs the mount as usual +2. some peripherals can live on their own CAN-connected nodes +3. the main controller forwards the normal rotator, focuser, or auxiliary + feature operations to those remote nodes + +So the current CAN feature set is best thought of as a way to distribute +peripherals around the system, not as "the whole mount over CAN." + +## What CAN Is Good For Here + +This design is most useful when you want the mount controller and some +peripheral functions to be physically separate. + +Typical reasons include: + +- keeping focuser or rotator electronics close to the OTA +- reducing long motor/sensor wiring runs +- letting one small remote node host accessory features near where they are used +- building a more modular system where peripherals can be added or moved more + easily + +In this arrangement: + +- the rotator, focusers, and auxiliary-feature controllers can stay with an OTA +- plugging in one CAN/power cable brings that whole peripheral set online +- the correct functionality and settings can then appear at the mount + controller immediately and work as one integrated system from Android App, + Smart Hand Controller, Smart Web Server, ASCOM, and similar clients + +So the CAN link is not just carrying commands. It also helps turn an OTA +assembly into a more self-contained module that can be moved from one mount +controller to another with much less rework. + +In other words, CAN is being used here as a practical interconnect between +cooperating OnStepX feature nodes. + +## Connectors And Cable + +For this CAN interconnect role, the preferred connector choices are: + +- RJ12 as the standard light-duty connector +- GX12-6 as the standard rugged connector + +The standard 6-pin wiring order is: + +1. `GND` +2. `+12V` +3. `CAN_L` +4. `CAN_H` +5. `+12V` +6. `GND` + +This duplicates power and ground at the outer pairs and keeps the CAN pair +together in the middle. + +With quality connectors and cables, this arrangement is intended to carry: + +- `12V @ 2A` + +Recommended cable guidance: + +- twisted-pair `24 AWG` is optimal +- straight quality RJ12 cable of `26 AWG` is acceptable for short runs of about + `2 to 3 meters` + +So the practical idea is: + +- use RJ12 for lighter-duty short interconnects +- use GX12-6 where a more rugged connector is needed +- keep cable quality high if you expect the same cable to carry both CAN and + accessory power + +## Termination + +The mount controller is treated as one terminating end of the CAN bus. + +In the standard arrangement: + +- the mount controller has a built-in `120 Ohm` termination resistor +- the remote nodes are daisy-chained in the normal CAN fashion +- the far end of the chain gets the second `120 Ohm` termination resistor + +For this system, that far-end termination can be provided by plugging a custom +terminating modular jack or `GX12-6` connector into the last node in the chain. + +So the intended layout is the normal two-ended CAN termination pattern: + +- one `120 Ohm` terminator at the mount controller +- one `120 Ohm` terminator at the last node + +not a terminator at every intermediate node. + +## The Current Remote Peripheral Types + +The current CAN remote-node support is centered on three feature families: + +### Rotator + +The main controller can treat a rotator as remote. + +That means: + +- the rotator logic still looks like rotator functionality from the user side +- but the actual rotator feature can live on a CAN-connected node +- the remote node hosts the local Axis3 rotator implementation and answers the + requests + +This is a natural fit when the rotator hardware is physically mounted on the +instrument side and you do not want all of that wiring returning to the main +controller. + +### Focusers + +The focuser support is especially useful for distributed systems. + +The current model allows: + +- up to six remote focusers +- heartbeat-based discovery/presence tracking +- the main controller to proxy focuser operations over CAN + +This is a good fit for: + +- separate main and guide focusers +- multiple optical trains +- distributed focus hardware mounted near different instruments + +From the system-design point of view, this is one of the more attractive uses +of CAN in the current tree because focusers are naturally peripheral and often +benefit from being located close to the mechanics they drive. + +### Auxiliary Features + +Auxiliary features can also live on a remote CAN node. + +This is useful for things such as: + +- switches +- dew-heater style outputs +- analog outputs +- intervalometer or related accessory functions +- other feature-board style I/O that is convenient to place away from the main + controller + +So a remote auxiliary node can act like a small distributed utility board for +the telescope system. + +## The Main Idea: Proxy And Remote Roles + +For these feature families, CAN is used in a client/server style arrangement. + +On the main controller: + +- `ON` means "this feature is reached through CAN" +- the main controller becomes a client/proxy for that feature + +On the remote feature node: + +- `REMOTE` means "this node hosts the real implementation" +- that node answers the CAN requests for its local feature + +So the usual pattern is: + +- the mount/main controller proxies +- the physically-near peripheral node serves + +## Example Assignments + +Here is a typical main-controller configuration when the mount controller is +meant to talk to remote CAN-connected peripheral nodes: + +```c +#define CAN_ROTATOR ON +#define CAN_FOCUSER ON +#define CAN_FEATURES ON +#define CAN_SEND_RATE_MS 2 +#define CAN_RECV_RATE_MS 10 +#define CAN_PLUS CAN2_TEENSY4 +#define CAN_BAUD 250000 +``` + +In that role: + +- `CAN_ROTATOR ON` means the main controller expects the rotator to be remote +- `CAN_FOCUSER ON` means the main controller expects focuser control to be remote +- `CAN_FEATURES ON` means the main controller expects the auxiliary-features + provider to be remote + +Typical remote-node assignments then look like this. + +Remote rotator node: + +```c +#define CAN_ROTATOR REMOTE +#define CAN_SEND_RATE_MS 2 +#define CAN_RECV_RATE_MS 10 +#define CAN_PLUS CAN2_TEENSY4 +#define CAN_BAUD 250000 +``` + +Remote focuser node: + +```c +#define CAN_FOCUSER REMOTE +#define CAN_FOCUSER_NUMBER 1 +#define CAN_SEND_RATE_MS 2 +#define CAN_RECV_RATE_MS 10 +#define CAN_PLUS CAN2_TEENSY4 +#define CAN_BAUD 250000 +``` + +Remote auxiliary-features node: + +```c +#define CAN_FEATURES REMOTE +#define CAN_SEND_RATE_MS 2 +#define CAN_RECV_RATE_MS 10 +#define CAN_PLUS CAN2_TEENSY4 +#define CAN_BAUD 250000 +``` + +These examples mirror the common case where the CAN interface is on the ST4 +port through `CAN2_TEENSY4`, with `250000` baud and the faster `2ms` send +pacing. + +The repeated send/receive-rate lines in those examples are mainly there for +documentation clarity. They make the intended CAN setup obvious in one place, +not because every line is a special non-default requirement. + +## Platform Guidance + +In current practical use, a Teensy 4.1 makes a very good mount controller when +CAN is involved. + +That is the preferred direction because: + +- it has worked well as the main controller side +- `CAN2_TEENSY4` on the ST4 port is a natural fit for this role + +ESP32 is a different story. + +Using the built-in TWAI interface on ESP32: + +- ESP32 can feel slow as a CAN client/proxy on the main controller side +- but ESP32 is perfectly reasonable as a remote CAN server for things like a + rotator, focuser, or auxiliary-features node + +The push toward `250000` baud is intentional. + +The reason is mainly practical: + +- `250000` gives more wiring freedom +- it is a better fit for the kind of real-world cable runs and connectors these + distributed peripheral nodes are likely to use + +This whole CAN peripheral-interconnect feature set is still fairly new and +experimental. + +So the current practical guidance is: + +- prefer Teensy 4.1 as the CAN mount controller +- ESP32 is fine for remote peripheral-server roles +- treat `250000` as the preferred default unless there is a clear reason to do + otherwise +- do not assume other CAN platforms are equally proven + +Important reality check: + +- other platforms have not really been tested in this role yet + +## Important Rule: Remote Or Local, Never Both + +For each feature family, a build must choose whether that feature is local or +reached through CAN. + +That means: + +- if `CAN_ROTATOR == ON`, do not also configure a normal local rotator in that + same build +- if `CAN_FOCUSER == ON`, do not also configure normal local focusers in that + same build +- if `CAN_FEATURES == ON`, do not also configure local auxiliary features in + that same build + +`REMOTE` is the opposite role: + +- a `REMOTE` rotator node must actually host the local rotator it is serving +- a `REMOTE` focuser node must actually host the local focuser it is serving +- a `REMOTE` auxiliary node must actually host the local features it is serving + +So the rule is not "both local and remote at once." The rule is: + +- main controller build: client/proxy +- remote peripheral build: local provider/server + +## What This Feels Like In Practice + +From the user-facing point of view, this is meant to feel like the feature is +still part of the overall system. + +Conceptually: + +- you talk to the main controller in the ordinary way +- the main controller forwards the relevant focuser/rotator/auxiliary operation + over CAN +- the remote node performs the work and reports back + +So the goal is not to create a second separate user workflow. The goal is to +let the feature live somewhere else electrically and physically while still +participating in the same overall system. + +## Presence And Heartbeats + +The remote-node model uses simple heartbeats. + +That matters because the main controller should not treat a remote feature as +available just because it was configured at compile time. + +Instead: + +- remote nodes emit a heartbeat about once per second +- the client side considers the feature present only while that heartbeat + remains fresh + +This is especially helpful for distributed peripherals because it gives the +main controller a simple way to decide whether a remote focuser, rotator, or +feature board is really there and alive right now. + +For focusers, the model is a little richer: + +- each remote focuser has its own heartbeat slot +- the main controller can therefore keep track of multiple remote focusers + independently + +## Typical System Layouts + +Some natural ways to use the current CAN feature set are: + +### Main Mount Controller Plus Remote Rotator + +Use the main controller for the mount itself, and put the rotator logic on a +CAN-connected node near the camera train. + +That helps when: + +- the rotator is physically far from the main controller +- you want cleaner wiring through the moving instrument area + +### Main Mount Controller Plus Remote Focusers + +Keep the mount local, but move one or more focusers onto remote CAN nodes. + +That is useful when: + +- focusers are spread around the instrument package +- you have more than one focuser role +- you want each focuser controller physically near its motor and sensors + +### Main Mount Controller Plus Remote Auxiliary Board + +Use a CAN-connected auxiliary node as a distributed accessory board. + +That is a good fit for: + +- dew-heater outputs +- power or switch functions +- intervalometer or small observatory accessory control + +### Mixed Peripheral Layout + +These roles can be mixed. + +For example: + +- main controller handles the mount +- a remote node handles the rotator +- one or more remote nodes handle focusers +- another remote node hosts accessory features + +That is the real strength of this CAN feature set: it lets the system be +physically distributed while still acting like one coordinated instrument. + +## What This Note Is Not Trying To Cover + +This note is intentionally not trying to go deep into: + +- low-level frame packing details +- backend-specific controller quirks +- direct CAN motor/encoder protocols +- a future "axis over CAN" design + +Those are all real topics, but they are different topics. + +For this note, the important point is simply: + +- CAN is currently a practical interconnect for remote rotator, focuser, and + auxiliary-feature nodes + +## Related Notes + +For the current command-family coverage, see +[COMMAND_REFERENCE.md](COMMAND_REFERENCE.md). + +For serial transport/background comparison, see +[SERIAL_NOTES.md](SERIAL_NOTES.md). + +## Good Mental Summary + +The cleanest way to think about the current CAN feature set in OnStepX is: + +- the mount controller stays central +- selected peripherals can live on their own CAN-connected nodes +- rotator, focusers, and auxiliary features are the current distributed-feature + sweet spot + +So CAN is best understood here as a practical peripheral-interconnect feature +for building a cleaner, more modular telescope system. diff --git a/docs/COMMAND_REFERENCE.md b/docs/COMMAND_REFERENCE.md new file mode 100755 index 0000000..c590cd7 --- /dev/null +++ b/docs/COMMAND_REFERENCE.md @@ -0,0 +1,861 @@ +# OnStepX Command Reference + +This reference is derived from the current command handlers in the source tree, primarily the `*.command.cpp` files plus the shared command processor in `src/libApp/commands/ProcessCmds.cpp`. + +Scope notes: + +- This covers the command families implemented in the current tree, not older wiki pages. +- Many commands are compile-time feature gated. If a subsystem is not enabled in the build, its commands will not exist. +- Plugin-defined commands are not documented here because they come from the active plugin set, not the core command handlers. +- A few command comments in source are stale. Where the code and comment disagree, this document follows the code path. + +## Protocol Basics + +### Frames + +- Normal ASCII command frame: `:CC...#` +- Checksummed frame: `;CC...CCS#` + - `;` starts a checksummed command. + - The command processor validates the checksum and sequence character before dispatch. +- The parser ignores ASCII space, LF, and CR. +- Internal command buffer size is 80 bytes including framing, so current implementations are not limited to the older 40-character LX200 convention. + +### Command Parsing + +- `CC` is effectively a 1- or 2-character command family. The rest of the payload is the parameter string. +- Replies are either: + - payload replies, usually terminated with `#` + - numeric/boolean replies, returned as `1` for success and `0` for failure +- Some commands suppress the trailing `#` on purpose. + +### Global Reply/Error Behavior + +- Commands that leave `numericReply=true` return: + - `1` for `CE_NONE` or `CE_1` + - `0` for any other error +- `:GE#` returns the last command error code as a 2-digit payload. + +### Command Error Codes + +These are the codes returned by `:GE#`: + +| Code | Symbol | Meaning | +| --- | --- | --- | +| 00 | `CE_NONE` | No error | +| 01 | `CE_0` | False/fail without a protocol error | +| 02 | `CE_CMD_UNKNOWN` | Unknown command | +| 03 | `CE_REPLY_UNKNOWN` | Invalid reply | +| 04 | `CE_PARAM_RANGE` | Parameter out of range | +| 05 | `CE_PARAM_FORM` | Bad parameter format | +| 06 | `CE_ALIGN_FAIL` | Align failed | +| 07 | `CE_ALIGN_NOT_ACTIVE` | Align not active | +| 08 | `CE_NOT_PARKED_OR_AT_HOME` | Not parked or at home | +| 09 | `CE_PARKED` | Already parked | +| 10 | `CE_PARK_FAILED` | Park failed | +| 11 | `CE_NOT_PARKED` | Not parked | +| 12 | `CE_NO_PARK_POSITION_SET` | No park position set | +| 13 | `CE_GOTO_FAIL` | Goto failed | +| 14 | `CE_LIBRARY_FULL` | Library full | +| 15 | `CE_SLEW_ERR_BELOW_HORIZON` | Target below horizon limit | +| 16 | `CE_SLEW_ERR_ABOVE_OVERHEAD` | Target above overhead limit | +| 17 | `CE_SLEW_ERR_IN_STANDBY` | Controller in standby | +| 18 | `CE_SLEW_ERR_IN_PARK` | Mount parked | +| 19 | `CE_SLEW_IN_SLEW` | Goto already active | +| 20 | `CE_SLEW_ERR_OUTSIDE_LIMITS` | Outside configured limits | +| 21 | `CE_SLEW_ERR_HARDWARE_FAULT` | Hardware fault | +| 22 | `CE_SLEW_IN_MOTION` | Mount already in motion | +| 23 | `CE_SLEW_ERR_UNSPECIFIED` | Other slew error | +| 25 | `CE_1` | Explicit true/success | + +## Global Processor Commands + +These come from `ProcessCmds.cpp`, not from a subsystem `*.command.cpp`. + +| Command | Reply | Description | +| --- | --- | --- | +| `:SBB#` | `1` then switches to 460800 baud | Set serial baud rate | +| `:SBA#` | `1` then switches to 230400 baud | Set serial baud rate | +| `:SB0#` | `1` then switches to 115200 baud | Set serial baud rate | +| `:SB1#` | `1` then switches to 56700 baud | Set serial baud rate | +| `:SB2#` | `1` then switches to 38400 baud | Set serial baud rate | +| `:SB3#` | `1` then switches to 28800 baud | Set serial baud rate | +| `:SB4#` | `1` then switches to 19200 baud | Set serial baud rate | +| `:SB5#` | `1` then switches to 14400 baud | Set serial baud rate | +| `:SB6#` | `1` then switches to 9600 baud | Set serial baud rate | +| `:SB7#` | `1` then switches to 4800 baud | Set serial baud rate | +| `:SB8#` | `1` then switches to 2400 baud | Set serial baud rate | +| `:SB9#` | `1` then switches to 1200 baud | Set serial baud rate | +| `:GE#` | `CC#` | Get last command error code | +| `<0x06>` | `A`, `P`, or `CK_FAIL` style response | Special LX200 status query/checksum-fail path handled by the buffer/processor | + +## Dispatcher Notes + +In the full telescope build, command dispatch order is: + +1. mount +2. guide +3. GPIO/plugin status helpers +4. goto +5. park +6. library +7. site +8. limits +9. home +10. PEC +11. axis 1 / axis 2 +12. rotator +13. focuser +14. auxiliary features + +That matters because some command names are reused by different subsystems, especially `:hP#` and `:hR#`. In an integrated mount build, mount park/unpark wins before the rotator/focuser handlers ever see those frames. The rotator/focuser `h*` commands are still valid for standalone or remote-node builds. + +## Core Telescope / Firmware / Environment + +| Command | Reply | Description | +| --- | --- | --- | +| `:B+#` | none | Increase reticle brightness | +| `:B-#` | none | Decrease reticle brightness | +| `:ECtext#` | none | Echo text to debug output. `_` becomes space, trailing `&` becomes newline. | +| `:ERESET#` | none | Reset the MCU | +| `:ENVRESET#` | text | Mark NV storage to be cleared on next boot | +| `:ESPFLASH#` | `1`/none depending on build path | Put addon/ESP device into firmware flashing mode when supported | +| `:GVD#` | `MTH DD YYYY#` | Firmware build date | +| `:GVM#` | `name version#` | Firmware name plus version | +| `:GVN#` | `M.mm...#` | Firmware version number | +| `:GVP#` | `name#` | Product name | +| `:GVT#` | `HH:MM:SS#` | Firmware build time | +| `:GVC#` | `string#` | Firmware config/product description | +| `:GVH#` | `string#` | Firmware hardware/pinmap string | +| `:GX9A#` | `sn.n#` | Ambient temperature in C | +| `:GX9B#` | `n.n#` | Pressure in mbar | +| `:GX9C#` | `n.n#` | Relative humidity in percent | +| `:GX9E#` | `sn.n#` | Dew point in C | +| `:GX9F#` | `n#` or `0` | MCU temperature in C | +| `:SX9A,sn.n#` | `0/1` | Set ambient temperature | +| `:SX9B,n.n#` | `0/1` | Set pressure | +| `:SX9C,n.n#` | `0/1` | Set humidity | + +## Time / Date / Site + +| Command | Reply | Description | +| --- | --- | --- | +| `:Ga#` | `HH:MM:SS#` | Local standard time, 12-hour format | +| `:GC#` | `MM/DD/YY#` | Local standard calendar date | +| `:Gc#` | `24#` | Local time format indicator | +| `:GG#` | `sHH:MM#` | UTC offset to add to local time to get UT1 | +| `:Gg#` | `sDDD*MM#` | Current longitude | +| `:GgH#` | `sDDD*MM:SS.SSS#` | Longitude, highest precision | +| `:GL#` | `HH:MM:SS#` | Local standard time, 24-hour format | +| `:GLH#` | `HH:MM:SS.SSSS#` | Local standard time, highest precision | +| `:GM#` | `string#` | Site 1 name | +| `:GN#` | `string#` | Site 2 name | +| `:GO#` | `string#` | Site 3 name | +| `:GP#` | `string#` | Site 4 name | +| `:GS#` | `HH:MM:SS#` | Sidereal time | +| `:GSH#` | `HH:MM:SS.ss#` | Sidereal time, highest precision | +| `:Gt#` | `sDD*MM#` | Current latitude | +| `:GtH#` | `sDD*MM:SS.SSS#` | Latitude, highest precision | +| `:Gv#` | `sn.n#` | Elevation in meters | +| `:GX80#` | `HH:MM:SS.ss#` | UT1 time | +| `:GX81#` | `MM/DD/YY#` | UT1 date | +| `:GX89#` | `0` or `1` | Date/time ready status. `0` means ready, `1` means not ready. | +| `:SCMM/DD/YY#` | `0/1` | Set local date | +| `:SCMM/DD/YYYY#` | `0/1` | Set local date | +| `:SGsHH#` | `0/1` | Set UTC offset | +| `:SGsHH:MM#` | `0/1` | Set UTC offset. Comments indicate `MM` should be `00`, `30`, or `45`. | +| `:Sg(s)DDD*MM#` | `0/1` | Set longitude | +| `:Sg(s)DDD*MM:SS#` | `0/1` | Set longitude | +| `:Sg(s)DDD*MM:SS.SSS#` | `0/1` | Set longitude | +| `:SLHH:MM:SS#` | `0/1` | Set local time | +| `:SLHH:MM:SS.SSS#` | `0/1` | Set local time | +| `:SMname#` | `0/1` | Set site 1 name, max 15 chars | +| `:SNname#` | `0/1` | Set site 2 name, max 15 chars | +| `:SOname#` | `0/1` | Set site 3 name, max 15 chars | +| `:SPname#` | `0/1` | Set site 4 name, max 15 chars | +| `:StsDD*MM#` | `0/1` | Set latitude | +| `:StsDD*MM:SS#` | `0/1` | Set latitude | +| `:StsDD*MM:SS.SSS#` | `0/1` | Set latitude | +| `:SUs.s#` | `0/1` | Set DUT1 correction in seconds, nominally `-0.9` to `+0.9` | +| `:Svsn.n#` | `0/1` | Set elevation in meters | +| `:W0#` .. `:W3#` | none | Select active site slot | +| `:W?#` | `n#` | Query active site slot | + +## Mount Position / Tracking / Rates + +Available when `MOUNT_PRESENT` is enabled. + +| Command | Reply | Description | +| --- | --- | --- | +| `:GA#` | `sDD*MM#` or `sDD*MM'SS#` | Current mount altitude | +| `:GAH#` | `sDD*MM'SS.SSS#` | Current mount altitude, highest precision | +| `:GD#` | `sDD*MM#` or `sDD*MM:SS#` | Current declination | +| `:GDH#` | `sDD*MM:SS.SSS#` | Current declination, highest precision | +| `:GR#` | `HH:MM.T#` or `HH:MM:SS#` | Current right ascension | +| `:GRH#` | `HH:MM:SS.SSSS#` | Current right ascension, highest precision | +| `:GT#` | `n.nnnnn#` or `0#` | Tracking rate in Hz, `0` if not tracking | +| `:GZ#` | `DDD*MM#` or `DDD*MM'SS#` | Current azimuth | +| `:GZH#` | `DDD*MM'SS.SSS#` | Current azimuth, highest precision | +| `:GXTD#` | `n.nnnnnnnn#` | Tracking rate offset, Dec, arcsec per sidereal second | +| `:GXTR#` | `n.nnnnnnnn#` | Tracking rate offset, RA, arcsec per sidereal second | +| `:GX40#` | `DDD*MM:SS#` | Axis1 instrument angle | +| `:GX41#` | `DDD*MM:SS#` | Axis2 instrument angle | +| `:GX42#` | `n.nnnnnn#` | Axis1 instrument angle in decimal degrees | +| `:GX43#` | `n.nnnnnn#` | Axis2 instrument angle in decimal degrees | +| `:GX44#` | `n#` | Axis1 encoder count | +| `:GX45#` | `n#` | Axis2 encoder count | +| `:GXE4#` | `n#` | Axis1 steps per degree | +| `:GXE5#` | `n#` | Axis2 steps per degree | +| `:GXEE#` | `0#`, `1#`, ... | Mount coordinate mode (`MOUNT_COORDS - 1`) | +| `:GXEF#` | `1` or `0` | Axis2 tangent-arm presence flag | +| `:GXEG#` | `1` or `0` | Axis1 sector-gear presence flag | +| `:GXEM#` | `n#` | Current mount type | +| `:GXF3#` | `sn.nnnnnn#` | Axis1 step frequency | +| `:GXF4#` | `sn.nnnnnn#` | Axis2 step frequency | +| `:GXFA#` | `50%#` | Reported workload placeholder | +| `:GXFF#` | `n.nnnnnn#` | Axis1 index position | +| `:GXFG#` | `n.nnnnnn#` | Axis2 index position | +| `:STn.n#` | `0/1` | Set tracking rate in Hz. `0` stops tracking. | +| `:SEO#` | `0/1` | Save absolute encoder origin, or initialize mount coordinate memory at home, when supported | +| `:SX40,n#` | `0/1` | Stage encoder axis1 angle in degrees | +| `:SX41,n#` | `0/1` | Stage encoder axis2 angle in degrees | +| `:SX42,1#` | `0/1` | Sync mount from staged encoder axis values | +| `:SX43,0#` | `0/1` | Allow SWS to control sync mode | +| `:SX44,deg1,deg2[a]#` | `0/1` | Stage and sync both encoder axes, append `a` when both SWS encoder values are absolute and trusted | +| `:SXEM,n#` | `0/1` | Set mount type for next restart | +| `:SXTD,n.n#` | `0/1` | Set Dec tracking rate offset, arcsec per sidereal second | +| `:SXTR,n.n#` | `0/1` | Set RA tracking rate offset, arcsec per sidereal second | +| `:TS#` | none | Solar tracking rate | +| `:TK#` | none | King tracking rate | +| `:TL#` | none | Lunar tracking rate | +| `:TQ#` | none | Sidereal tracking rate | +| `:T+#` | none | Increase master sidereal clock by 0.02 Hz | +| `:T-#` | none | Decrease master sidereal clock by 0.02 Hz | +| `:TR#` | none | Reset master sidereal clock | +| `:Te#` | `0/1` | Enable tracking | +| `:Td#` | `0/1` | Disable tracking | +| `:To#` | `0/1` | Enable full compensation model | +| `:Tr#` | `0/1` | Enable refraction compensation | +| `:Tn#` | `0/1` | Disable compensation | +| `:T1#` | `0/1` | Single-axis tracking mode | +| `:T2#` | `0/1` | Dual-axis tracking mode | +| `:$BDn#` | `0/1` | Set Dec/Alt backlash in arcsec | +| `:$BRn#` | `0/1` | Set RA/Azm backlash in arcsec | +| `:%BD#` | `n#` | Get Dec/Alt backlash in arcsec | +| `:%BR#` | `n#` | Get RA/Azm backlash in arcsec | + +### Mount Type Values + +Current core constants are: + +| Value | Mount type | +| --- | --- | +| `1` | GEM | +| `2` | FORK | +| `3` | ALTAZM | +| `4` | ALTALT | +| `5` | GEM_TA | +| `6` | GEM_TAC | +| `7` | FORK_TA | +| `8` | FORK_TAC | +| `9` | ALTAZM_UNL | + +## Goto / Sync / Alignment + +| Command | Reply | Description | +| --- | --- | --- | +| `:AW#` | `1` | Write alignment model to NV | +| `:A?#` | `mno#` | Alignment status: max stars, current star, last required star | +| `:A1#` .. `:A9#` | `0/1` | Start manual alignment with that many stars | +| `:A+#` | `0/1` | Accept current align point | +| `:CS#` | none | Sync to current target coordinates | +| `:CM#` | `N/A#` or `E1#`..`E9#` | Sync to current catalog/database object | +| `:D#` | `0x7f#` while moving, otherwise raw `#` | LX200 distance-bar style motion indicator | +| `:Gr#` | `HH:MM.T#` or `HH:MM:SS#` | Get target RA | +| `:GrH#` | `HH:MM:SS.SSSS#` | Get target RA, highest precision | +| `:Gd#` | `sDD*MM#` or `sDD*MM:SS#` | Get target Dec | +| `:GdH#` | `sDD*MM:SS.SSS#` | Get target Dec, highest precision | +| `:Gal#` | `sDD*MM#` or `sDD*MM'SS#` | Get target altitude | +| `:GaH#` | `sDD*MM'SS.SSS#` | Get target altitude, highest precision | +| `:Gz#` | `DDD*MM#` or `DDD*MM'SS#` | Get target azimuth | +| `:GzH#` | `DDD*MM'SS.SSS#` | Get target azimuth, highest precision | +| `:MA#` | `0`..`9` | Goto target Alt/Az | +| `:MD#` | `0`, `1`, or `2` | Destination pier side for current target | +| `:MN#` | `0`..`9` | Goto current position on opposite pier side | +| `:MNe#` | `0`..`9` | Force same-position goto to east pier side | +| `:MNw#` | `0`..`9` | Force same-position goto to west pier side | +| `:MP#` | `0`..`9` | Polar-align goto | +| `:MS#` | `0`..`9` | Goto current target | +| `:SrHH:MM.T#` | `0/1` | Set target RA | +| `:SrHH:MM:SS#` | `0/1` | Set target RA | +| `:SrHH:MM:SS.SSSS#` | `0/1` | Set target RA | +| `:SdsDD*MM#` | `0/1` | Set target Dec | +| `:SdsDD*MM:SS#` | `0/1` | Set target Dec | +| `:SdsDD*MM:SS.SSS#` | `0/1` | Set target Dec | +| `:SasDD*MM#` | `0/1` | Set target altitude | +| `:SasDD*MM'SS#` | `0/1` | Set target altitude | +| `:SasDD*MM'SS.SSS#` | `0/1` | Set target altitude | +| `:SzDDD*MM#` | `0/1` | Set target azimuth | +| `:SzDDD*MM'SS#` | `0/1` | Set target azimuth | +| `:SzDDD*MM'SS.SSS#` | `0/1` | Set target azimuth | + +### `:MS#` / `:MA#` / `:MN#` / `:MP#` Return Codes + +| Value | Meaning | +| --- | --- | +| `0` | Goto accepted | +| `1` | Below horizon limit | +| `2` | Above overhead limit | +| `3` | Controller in standby | +| `4` | Mount parked | +| `5` | Goto already in progress | +| `6` | Outside limits | +| `7` | Hardware fault | +| `8` | Already in motion | +| `9` | Unspecified error | + +### Alignment Model Extended Commands + +`ALIGN_MAX_NUM_STARS > 1` builds expose: + +| Command | Reply | Description | +| --- | --- | --- | +| `:GX00#` | `n#` | `ax1Cor` in arcsec | +| `:GX01#` | `n#` | `ax2Cor` in arcsec | +| `:GX02#` | `n#` | `altCor` in arcsec | +| `:GX03#` | `n#` | `azmCor` in arcsec | +| `:GX04#` | `n#` | `doCor` in arcsec | +| `:GX05#` | `n#` | `pdCor` in arcsec | +| `:GX06#` | `n#` | `ffCor` for FORK/ALTAZM, else `0` | +| `:GX07#` | `n#` | `dfCor` for GEM-style mounts, else `0` | +| `:GX08#` | `n#` | `tfCor` in arcsec | +| `:GX0a#` | `n#` | `hcp` in degrees | +| `:GX0b#` | `n#` | `hca` in arcsec | +| `:GX0c#` | `n#` | `dcp` in degrees | +| `:GX0d#` | `n#` | `dca` in arcsec | +| `:GX09#` | `n#` | Number of uploaded stars, then resets internal star index | +| `:GX0A#` | `HH:MM:SS#` | Uploaded star actual HA | +| `:GX0B#` | `sDD*MM:SS#` | Uploaded star actual Dec | +| `:GX0C#` | `HH:MM:SS#` | Uploaded star mount HA | +| `:GX0D#` | `sDD*MM:SS#` | Uploaded star mount Dec | +| `:GX0E#` | `n#` | Uploaded star pier side, then advances star index | +| `:SX00,n#` .. `:SX0d,n#` | `0/1` | Set alignment model coefficients listed above | +| `:SX09,0#` | `0/1` | Reset alignment upload state | +| `:SX09,1#` | `0/1` | Build model from uploaded stars | +| `:SX09,2#` | `0/1` | Force model active | +| `:SX0A,HH:MM:SS#` | `0/1` | Upload actual HA for current star | +| `:SX0B,sDD*MM:SS#` | `0/1` | Upload actual Dec for current star | +| `:SX0C,HH:MM:SS#` | `0/1` | Upload mount HA for current star | +| `:SX0D,sDD*MM:SS#` | `0/1` | Upload mount Dec for current star | +| `:SX0E,n#` | `0/1` | Upload pier side and advance to next star | + +### Goto Extended Settings + +| Command | Reply | Description | +| --- | --- | --- | +| `:GX92#` | `n.nnn#` | Current slew period in us/step | +| `:GX93#` | `n.nnn#` | Base/default slew period in us/step | +| `:GX94#` | `n` plus optional ` N` | Current pier side: `0` none, `1` east, `2` west | +| `:GX95#` | `0` or `1` | Auto meridian flip enabled | +| `:GX96#` | `E`, `W`, `B`, or `A` | Preferred pier side | +| `:GX97#` | `n.n#` | Current step rate in deg/s | +| `:GX99#` | `n.nnn#` | Fastest allowed slew period in us/step | +| `:SX92,n.nnn#` | `0/1` | Set current slew period in us/step | +| `:SX93,[1-5]#` | none | Slew preset: `5`=50%, `4`=66.7%, `3`=100%, `2`=150%, `1`=200% | +| `:SX95,0#` or `:SX95,1#` | `0/1` | Disable/enable automatic meridian flip | +| `:SX96,E#` | `0/1` | Preferred pier side east | +| `:SX96,W#` | `0/1` | Preferred pier side west | +| `:SX96,B#` | `0/1` | Preferred pier side best | +| `:SX96,A#` | `0/1` | Preferred pier side automatic | +| `:SX98,0#` or `:SX98,1#` | `0/1` | Disable/enable pause at home during meridian flip | +| `:SX99,1#` | `0/1` | Continue after pause at home | + +## Guide / Manual Motion + +| Command | Reply | Description | +| --- | --- | --- | +| `:GX90#` | `n.nn#` | Pulse guide rate | +| `:Mgdn#` | none | Pulse guide for `n` ms in direction `d` where `d` is `w`, `e`, `n`, or `s` | +| `:MGdn#` | `0/1` | Same as `:Mgdn#`, numeric form | +| `:Mw#` | none | Move west at current guide rate | +| `:Me#` | none | Move east at current guide rate | +| `:Mn#` | none | Move north at current guide rate | +| `:Ms#` | none | Move south at current guide rate | +| `:Mp#` | none | Spiral-search motion | +| `:Q#` | none | Stop all slews, abort goto | +| `:Qe#` / `:Qw#` | none | Stop east/west motion | +| `:Qn#` / `:Qs#` | none | Stop north/south motion | +| `:RAn.n#` | none | Set axis1 custom guide rate in deg/s | +| `:REn.n#` | none | Set axis2 custom guide rate in deg/s | +| `:RG#` | none | Guide rate preset 1x | +| `:RC#` | none | Centering rate preset 8x | +| `:RM#` | none | Find rate preset 20x | +| `:RF#` | none | Fast rate preset 48x | +| `:RS#` | none | Slew rate preset, half current goto rate | +| `:R0#` .. `:R9#` | none | Numeric guide-rate preset | + +## Park / Home / Limits / Status + +### Park + +| Command | Reply | Description | +| --- | --- | --- | +| `:hP#` | `0/1` | Park mount | +| `:hQ#` | `0/1` | Set current position as park | +| `:hR#` | `0/1` | Unpark mount | + +### Home + +| Command | Reply | Description | +| --- | --- | --- | +| `:h?#` | `hasSense,axis1Offset,axis2Offset#` | Home status. Source comment mentions auto-home, but current code returns 3 fields only. | +| `:hA0#` / `:hA1#` | none | Disable/enable automatic home at boot | +| `:hC#` | none | Move to home | +| `:hC1,R#` | none | Toggle axis1 home-sense reversal | +| `:hC1,n#` | none | Set axis1 home offset in arcsec | +| `:hC2,R#` | none | Toggle axis2 home-sense reversal | +| `:hC2,n#` | none | Set axis2 home offset in arcsec | +| `:hF#` | none | Reset mount at home/cold-start position | + +### Limits + +For a visual explanation of confusing GEM east/west pier-side limit geometry, +see [GOTO_NOTES.md](GOTO_NOTES.md#workflow-4-reachability-and-target-unwinding). + +| Command | Reply | Description | +| --- | --- | --- | +| `:Gh#` | `sDD*#` | Horizon limit | +| `:Go#` | `DD*#` | Overhead limit | +| `:GXE9#` | `n#` | East meridian limit in minutes | +| `:GXEA#` | `n#` | West meridian limit in minutes | +| `:GXEe#` | `n#` | Axis1 minimum limit in degrees | +| `:GXEw#` | `n#` | Axis1 maximum limit in degrees | +| `:GXEB#` | `n#` | Axis1 maximum limit in hours | +| `:GXEC#` | `n#` | Axis2 minimum limit in degrees | +| `:GXED#` | `n#` | Axis2 maximum limit in degrees | +| `:ShsDD#` | `0/1` | Set lower altitude limit | +| `:SoDD#` | `0/1` | Set overhead altitude limit | +| `:SXE9,n#` | `0/1` | Set east meridian limit in minutes | +| `:SXEA,n#` | `0/1` | Set west meridian limit in minutes | + +### Status + +| Command | Reply | Description | +| --- | --- | --- | +| `:Gm#` | `E#`, `W#`, or `N#` | Meridian pier side | +| `:GU#` | status string | Human-readable packed status | +| `:Gu#` | packed bytes | Bit-packed status | +| `:GW#` | 4-char status | Mount type, tracking, parked/home, align-done | +| `:SX97,0#` | `0/1` | Disable buzzer | +| `:SX97,1#` | `0/1` | Enable buzzer | +| `:SX97,2#` | `0/1` | Beep | +| `:SX97,3#` | `0/1` | Alert tone | +| `:SX97,4#` | `0/1` | Click | + +#### `:GU#` Status Characters + +The reply is an ordered string assembled from active conditions. Characters currently used by the code include: + +| Char | Meaning | +| --- | --- | +| `n` | Not tracking | +| `N` | No goto active | +| `p` | Not parked | +| `I` | Parking in progress | +| `P` | Parked | +| `F` | Park failed | +| `e` | Sync-to-encoders-only mode | +| `H` | At home | +| `h` | Homing | +| `B` | Auto-home at boot | +| `S` | PPS synced | +| `G` | Pulse guide active | +| `g` | Guide active | +| `r` | Refraction compensation enabled | +| `s` | Single-axis compensation mode | +| `t` | Full compensation model enabled | +| `(` | Lunar tracking rate selected | +| `O` | Solar tracking rate selected | +| `k` | King tracking rate selected | +| `w` | Meridian flip paused at home | +| `u` | Pause-at-home enabled | +| `z` | Buzzer enabled | +| `a` | Automatic meridian flip enabled | +| `R` | PEC data recorded | +| `/`, `,`, `~`, `;`, `^` | PEC state | +| `E`, `K`, `A`, `L` | GEM, FORK, ALTAZM, ALTALT | +| `o`, `T`, `W` | Pier side none, east, west | +| final digits | pulse-guide rate, guide rate, general error code | + +#### `:Gu#` Packed Status Layout + +`Gu` is the binary/pseudo-binary status form. The current implementation fills bytes as: + +| Byte | Contents | +| --- | --- | +| `0` | tracking/goto/PPS/pulse-guide plus compensation mode | +| `1` | tracking-rate selection plus sync-to-encoders and guide-active | +| `2` | home/homing/auto-home/home-pause/buzzer/auto-flip | +| `3` | mount type plus pier side | +| `4` | PEC state and PEC-recorded flag | +| `5` | park state | +| `6` | pulse-guide selection | +| `7` | guide-rate selection | +| `8` | general limits/error code | + +## Object Library + +| Command | Reply | Description | +| --- | --- | --- | +| `:LB#` | none | Previous object matching current constraints | +| `:LCn#` | none | Select record number `n` | +| `:LI#` | `name,type#` | Current object name and type | +| `:LIG#` | none | Load current object into goto target | +| `:LR#` | `name,type,ra,dec#` | Current object info, then advance to next record | +| `:LWname,type#` | `0/1` | Write current target RA/Dec to next free record | +| `:LN#` | none | Next object matching current constraints | +| `:L$#` | `1` | Move to catalog name record | +| `:LD#` | none | Clear current record | +| `:LL#` | none | Clear current catalog | +| `:L!#` | none | Clear all catalogs | +| `:L?#` | `n#` | Free records across all catalogs | +| `:Lon#` | `0/1` | Select catalog `0..14` | + +Object type codes written/read by library commands are: + +`UNK`, `OC`, `GC`, `PN`, `DN`, `SG`, `EG`, `IG`, `KNT`, `SNR`, `GAL`, `CN`, `STR`, `PLA`, `CMT`, `AST` + +## PEC + +Available when PEC support is enabled for axis 1. + +| Command | Reply | Description | +| --- | --- | --- | +| `:GX91#` | `n#` | PEC analog value | +| `:GXE6#` | `n.nnnnnn#` | Steps per sidereal second | +| `:GXE7#` | `n#` | Worm rotation steps from NV | +| `:GXE8#` | `n#` | PEC buffer size in seconds | +| `:SXE7,n#` | `0/1` | Set worm rotation steps | +| `:VH#` | `nnnnn#` | PEC index sense position in sidereal seconds | +| `:VR#` | `snnn,nnn#` | Current PEC segment correction plus segment index | +| `:VRn#` | `snnn#` | PEC correction for segment `n` | +| `:Vrn#` | `x0x1...x9#` | Ten-byte hex frame of PEC data starting at segment `n` | +| `:VS#` | `n.nnnnnn#` | Steps per sidereal second of worm rotation | +| `:VW#` | `nnnnnn#` | Worm rotation steps | +| `:WR+#` | `0/1` | Rotate PEC table forward one second | +| `:WR-#` | `0/1` | Rotate PEC table backward one second | +| `:WRn,sn#` | none | Write PEC correction for segment `n` | +| `:$QZ+#` | none | Enable PEC playback | +| `:$QZ-#` | none | Disable PEC | +| `:$QZ/#` | none | Arm PEC recording | +| `:$QZZ#` | none | Clear PEC buffer | +| `:$QZ!#` | none | Save PEC data to NV | +| `:$QZ?#` | `I#`, `p#`, `P#`, `r#`, `R#`, optionally with `.#` | PEC status | + +PEC status characters from `:$QZ?#`: + +| Char | Meaning | +| --- | --- | +| `I` | Ignore/off | +| `p` | Ready to play | +| `P` | Playing | +| `r` | Ready to record | +| `R` | Recording | +| `.` | Index detected this second | + +### `:GXUa#` Driver Status Flags + +Local ASCII replies are comma-separated flag mnemonics: + +| Flag | Meaning | +| --- | --- | +| `ST` | Standstill | +| `OA` | Output A open load | +| `OB` | Output B open load | +| `GA` | Output A short to ground | +| `GB` | Output B short to ground | +| `OT` | Over-temperature | +| `PW` | Over-temperature warning | +| `GF` | Driver fault | + +## Focuser + +Available in focuser-enabled or remote-focuser builds. + +Addressing rules: + +- `:FA#` returns the currently selected focuser number. +- `:FA1#` .. `:FA6#` selects the active focuser. +- `:F...#` uses the active focuser. +- `:F1...#` .. `:F6...#` directs a command to a specific focuser immediately. + +Unit rules for local ASCII focuser commands: + +- Uppercase `B`, `D`, `G`, `I`, `M`, `R`, `S` use microns. +- Lowercase `b`, `d`, `g`, `i`, `m`, `r`, `s` use raw steps. +- Other focuser commands are unitless or fixed-unit as noted below. + +| Command | Reply | Description | +| --- | --- | --- | +| `:FA#` | `n` | Get active focuser number | +| `:FA1#` .. `:FA6#` | `0/1` | Select active focuser | +| `:hP#` | `0/1` | Park all focusers in standalone/remote focuser builds | +| `:hR#` | `0/1` | Unpark all focusers in standalone/remote focuser builds | +| `:Fa#` | `1` | Primary focuser present/selected | +| `:FT#` | `M1#`, `S3#`, etc. | Focuser status plus goto-rate digit | +| `:Fp#` | `0` or `1` | Mode. Current implementation returns `1` for DC/pseudo-absolute, `0` otherwise. | +| `:FI#` / `:Fi#` | `n#` | Full-in/min position | +| `:FM#` / `:Fm#` | `n#` | Maximum position | +| `:Fe#` | `n.n#` | Temperature delta from TCF baseline | +| `:Ft#` | `n.n#` | Focuser temperature in C | +| `:Fu#` | `n.nnnnn#` | Microns per step | +| `:FB#` / `:Fb#` | `n#` | Backlash | +| `:FBn#` / `:Fbn#` | `0/1` | Set backlash | +| `:FC#` | `n.nnnnn#` | TCF coefficient in microns per C | +| `:FCsn.n#` | `0/1` | Set TCF coefficient | +| `:Fc#` | `0` or `1` | TCF enabled status | +| `:Fc0#` / `:Fc1#` | `0/1` | Disable/enable TCF | +| `:FD#` / `:Fd#` | `n#` | TCF deadband | +| `:FDn#` / `:Fdn#` | `0/1` | Set TCF deadband | +| `:FP#` | `n#` | DC motor power percent | +| `:FPn#` | `0/1` | Set DC motor power percent | +| `:FQ#` | none | Stop focuser | +| `:F1#` .. `:F9#` | none | Set move/goto rate preset | +| `:FW#` | `n#` | Working goto rate in um/s | +| `:F+#` | none | Move inward | +| `:F-#` | none | Move outward | +| `:FG#` / `:Fg#` | `sn#` | Current position | +| `:FRsn#` / `:Frsn#` | none | Relative goto | +| `:FSn#` / `:Fsn#` | `0/1` | Absolute goto | +| `:FZ#` | none | Zero current position | +| `:FH#` | none | Set current position as home | +| `:Fh#` | none | Move to home | +| `:GXU4#` .. `:GXU9#` | flags | Driver status for focuser axes that expose `Axis.command.cpp` | + +Rate preset meaning: + +| Preset | Meaning | +| --- | --- | +| `1` | 1 um/s move rate | +| `2` | 10 um/s move rate | +| `3` | 100 um/s move rate | +| `4` | 0.5x goto rate, move mode | +| `5` | 0.5x goto rate | +| `6` | 0.66x goto rate | +| `7` | 1x goto rate | +| `8` | 1.5x goto rate | +| `9` | 2x goto rate | + +## Rotator + +Available in rotator-enabled or remote-rotator builds. + +| Command | Reply | Description | +| --- | --- | --- | +| `:rA#` | `0/1` | Rotator active | +| `:hP#` | `0/1` | Park rotator in standalone/remote rotator builds | +| `:hR#` | `0/1` | Unpark rotator in standalone/remote rotator builds | +| `:rT#` | `M1#`, `SD3#`, etc. | Status string plus rate digit | +| `:rI#` | `n#` | Minimum angle in degrees | +| `:rM#` | `n#` | Maximum angle in degrees | +| `:rD#` | `n.n#` | Degrees per step | +| `:rb#` | `n#` | Backlash in steps | +| `:rbn#` | `0/1` | Set backlash | +| `:rQ#` | none | Stop motion | +| `:r1#` .. `:r9#` | none | Set move/goto rate preset | +| `:rW#` | `n.n#` | Working slew rate in deg/s | +| `:rc#` | none | Continuous-move no-op, accepted for compatibility | +| `:r>#` | none | Move clockwise | +| `:r<#` | none | Move counter-clockwise | +| `:rG#` | `sDDD*MM#` | Current angle | +| `:rrsDDD*MM#` | none | Relative goto | +| `:rSsDDD*MM#` | `0/1` | Absolute goto | +| `:rZ#` | none | Zero position | +| `:rF#` | none | Set current position to half travel | +| `:rC#` | none | Move to half-travel / home target | +| `:r+#` | none | Enable derotation | +| `:r-#` | none | Disable derotation | +| `:rP#` | none | Move to parallactic angle | +| `:rR#` | none | Toggle derotator reverse direction | +| `:GX98#` | `D#`, `R#`, or `N#` | Rotator capability: derotate, rotate-only, or none | +| `:GXU3#` | flags | Driver status for rotator axis in standalone/remote builds | + +Rotator rate presets: + +| Preset | Meaning | +| --- | --- | +| `1` | 0.01 deg/s move rate | +| `2` | 0.1 deg/s move rate | +| `3` | 1.0 deg/s move rate | +| `4` | 0.5x goto rate used as move rate | +| `5` | 0.5x goto rate | +| `6` | 0.66x goto rate | +| `7` | 1x goto rate | +| `8` | 1.5x goto rate | +| `9` | 2x goto rate | + +## Auxiliary Features + +Available when auxiliary features are enabled. + +Feature slots are numbered `1..8`. + +### Discovery + +| Command | Reply | Description | +| --- | --- | --- | +| `:GXY0#` | `xxxxxxxx#` | Eight-character bitmap of active feature slots, `1` = present | +| `:GXYn#` | `name,purpose#` | Slot name plus purpose code | + +### Slot State + +| Command | Reply | Description | +| --- | --- | --- | +| `:GXXn#` | purpose-specific payload | Get current state for slot `n` | +| `:SXXn,Vv#` | `0/1` | Generic value set | +| `:SXXn,Zf#` | `0/1` | Dew-heater zero temperature | +| `:SXXn,Sf#` | `0/1` | Dew-heater span temperature | +| `:SXXn,Ef#` | `0/1` | Intervalometer exposure seconds | +| `:SXXn,Df#` | `0/1` | Intervalometer delay seconds | +| `:SXXn,Cf#` | `0/1` | Intervalometer count | + +### `:GXXn#` Reply Shapes + +The payload depends on the slot purpose: + +| Purpose | Reply shape | +| --- | --- | +| Switch / momentary switch / cover switch | `value` plus optional power telemetry | +| Analog output | `value` plus optional power telemetry | +| Dew heater | `enabled,zero,span,deltaT` plus optional power telemetry | +| Intervalometer | `currentCount,exposure,delay,count` | + +When power monitoring is compiled in, the local ASCII implementation appends: + +`;,,` + +Where flags are a five-character string using `P`, `C`, `U`, `V`, `T` or `!`. + +## Axis / Motor / Driver Service Commands + +Axis service commands are implemented by `Axis.command.cpp`. For the main telescope/mount build these are reachable for axis 1 and axis 2, if present. The rotator exposes axis3, if present. The focuser(s) expose any of axis 4 to axis 9, if present. + +| Command | Reply | Description | +| --- | --- | --- | +| `:GXAa,p#` | `value,min,max,type,name#` | Get axis parameter `p` for axis `a` (`1..9`) | +| `:GXAa,M#` | `name#` | Motor/driver name for axis `a` | +| `:GXAa,0#` | `n#` | Parameter count for axis `a` | +| `:GXSa#` | `delta,velocity#` | Servo-only delta and velocity for axis `a` | +| `:GXUa#` | `flags#` | Stepper driver status for axis `a` | +| `:SXAC,0#` | `0/1` | Use runtime NV axis settings | +| `:SXAC,1#` | `0/1` | Use compile-time `Config.h` axis settings | +| `:SXAa,R#` | `0/1` | Revert axis `a` settings to defaults on next boot | +| `:SXAa,p,value#` | `0/1` | Set axis parameter `p` for axis `a` | + +### `:GXAa,p#` Reply Format + +The reply is: + +`value,min,max,type,name#` + +Where: + +- `value` is the current NV value +- `min` and `max` are the documented range +- `type` is the axis parameter type code returned by firmware +- `name` is the parameter name from the axis parameter table +- some `name` values are locale tokens such as `$1` or `$12` rather than literal English labels + +Axis parameter type codes: + +| Code | Symbol | Meaning | +| --- | --- | --- | +| `0` | `AXP_INVALID` | Invalid / placeholder entry | +| `1` | `AXP_BOOLEAN` | Boolean value stored in NV and applied on restart/reinit | +| `2` | `AXP_BOOLEAN_IMMEDIATE` | Boolean value applied immediately when set | +| `3` | `AXP_INTEGER` | Integer value stored in NV and applied on restart/reinit | +| `4` | `AXP_INTEGER_IMMEDIATE` | Integer value applied immediately when set | +| `5` | `AXP_FLOAT` | Floating-point value stored in NV and reported as a plain numeric field | +| `6` | `AXP_FLOAT_IMMEDIATE` | Floating-point value applied immediately when set | +| `7` | `AXP_FLOAT_RAD` | Angular float stored internally in radians | +| `8` | `AXP_FLOAT_RAD_INV` | Inverse angular float stored internally with radian/degree conversion | +| `9` | `AXP_POW2` | Power-of-two constrained integer-like value | +| `10` | `AXP_DECAY` | Driver decay-mode selector | + +Protocol note: + +- `:GXAa,p#` normalizes `AXP_FLOAT_RAD` and `AXP_FLOAT_RAD_INV` to reply type code `5` and converts the value/range to degree-based units for transport. +- Axis parameter values are float-backed in firmware; values are always float even when the logical type is boolean or integer so the UI can show the appropriate controls. +- Boolean parameters: some use logical `0`/`1`, while others use the firmware constants `OFF = -1` and `ON = -2`; so any UI can show 'True'/'False' or 'On'/'Off'. +- For `AXP_DECAY`, the UI shows the matching decay-mode text for the numeric value `1=MIXED`, `2=FAST`, `3=SLOW`, `4=SPREADCYCLE`, `5=STEALTHCHOP`. + +Locale-backed axis parameter name tokens currently used by firmware: + +| Token | Meaning | +| --- | --- | +| `$1` | Steps/degree | +| `$2` | Min limit, degs | +| `$3` | Max limit, degs | +| `$4` | Steps/um | +| `$5` | Min limit, um | +| `$6` | Max limit, um | +| `$7` | Reverse | +| `$8` | Microsteps | +| `$9` | Microsteps Goto | +| `$10` | Decay mode | +| `$11` | Decay mode Goto | +| `$12` | mA Hold | +| `$13` | mA Run | +| `$14` | mA Goto | +| `$15` | 256x Interpolate | +| `$16` | P tracking | +| `$17` | I tracking | +| `$18` | D tracking | +| `$19` | P slewing | +| `$20` | I slewing | +| `$21` | D slewing | +| `$22` | Rads/count | +| `$23` | Steps/count ratio | +| `$24` | Max accel, %/s/s | +| `$25` | Min power, % | +| `$26` | Max power, % | + +## CAN Remote-Node Variants + +For architecture, configuration, and current scope, see +[CAN_NOTES.md](CAN_NOTES.md). + +The following files implement the same logical command families over packed CAN messages rather than the ASCII LX200-style transport: + +- `src/telescope/focuser/local/Focuser.can.command.cpp` +- `src/telescope/rotator/local/Rotator.can.command.cpp` +- `src/telescope/auxiliary/local/Features.can.command.cpp` + +These are not separate end-user command names. They are binary transport implementations of the same focuser, rotator, and auxiliary-feature command sets above. + +Notable transport differences: + +- CAN replies are binary-packed, not ASCII text. +- `:GXY0#` for CAN features returns a single bitmask byte rather than an eight-character ASCII bitmap. +- CAN focuser/rotator driver-status replies are packed bitfields rather than comma-separated text flags. + +## Source Coverage + +This document was built from the current handlers in: + +- `src/telescope/Telescope.command.cpp` +- `src/telescope/mount/Mount.command.cpp` +- `src/telescope/mount/goto/Goto.command.cpp` +- `src/telescope/mount/guide/Guide.command.cpp` +- `src/telescope/mount/home/Home.command.cpp` +- `src/telescope/mount/library/Library.command.cpp` +- `src/telescope/mount/limits/Limits.command.cpp` +- `src/telescope/mount/park/Park.command.cpp` +- `src/telescope/mount/pec/Pec.command.cpp` +- `src/telescope/mount/site/Site.command.cpp` +- `src/telescope/mount/status/Status.command.cpp` +- `src/lib/axis/Axis.command.cpp` +- `src/telescope/focuser/local/Focuser.command.cpp` +- `src/telescope/rotator/local/Rotator.command.cpp` +- `src/telescope/auxiliary/local/Features.command.cpp` +- plus the corresponding CAN transport handlers +- plus `src/libApp/commands/ProcessCmds.cpp` diff --git a/docs/GEM LIMITS EXAMPLE EAST OF PIER.jpg b/docs/GEM LIMITS EXAMPLE EAST OF PIER.jpg new file mode 100755 index 0000000..7960717 Binary files /dev/null and b/docs/GEM LIMITS EXAMPLE EAST OF PIER.jpg differ diff --git a/docs/GEM LIMITS EXAMPLE KEEP OUT ZONE.jpg b/docs/GEM LIMITS EXAMPLE KEEP OUT ZONE.jpg new file mode 100755 index 0000000..a954d9c Binary files /dev/null and b/docs/GEM LIMITS EXAMPLE KEEP OUT ZONE.jpg differ diff --git a/docs/GEM LIMITS EXAMPLE WEST OF PIER.jpg b/docs/GEM LIMITS EXAMPLE WEST OF PIER.jpg new file mode 100755 index 0000000..7deab30 Binary files /dev/null and b/docs/GEM LIMITS EXAMPLE WEST OF PIER.jpg differ diff --git a/docs/GOTO_NOTES.md b/docs/GOTO_NOTES.md new file mode 100755 index 0000000..5a057ba --- /dev/null +++ b/docs/GOTO_NOTES.md @@ -0,0 +1,514 @@ +# Goto Notes + +## Purpose + +These notes describe how goto, sync, orientation choice, and target-limit +planning currently work in OnStepX. + +This is a workflow note. It is meant to help explain how the system decides on +a sensible motion plan, not to document every goto command. + +## For related topics also see + +- [STARTUP_AUTHORITY_NOTES.md](STARTUP_AUTHORITY_NOTES.md) +- [HOMING_NOTES.md](HOMING_NOTES.md) +- [COMMAND_REFERENCE.md](COMMAND_REFERENCE.md) + +## Quick Summary + +## The cleanest mental model is + +1. a user target is turned into mount coordinates +2. the system checks whether that target is reachable in one or more valid + orientations +3. the system chooses a sensible branch and motion plan +4. goto executes that plan, possibly with waypoints or alternate orientation + handling +5. runtime limits continue watching during the motion + +## So goto in OnStepX is not just + +- convert coordinates +- slew there + +It is more like a small decision engine. + +## The Main Distinction: Goto vs Sync + +`goTo.setTarget(...)` is the common decision point for both goto and sync. + +## That means both operations share + +- target conversion +- orientation selection +- reachability checks +- axis1 wrap/unwind correction + +## Then they diverge + +- goto moves the mount to the chosen target +- sync keeps the current physical mount orientation and reassigns coordinates to + match the chosen target + +So sync is not a separate simpler target model. It reuses most of the same +target-planning logic, then applies it as a coordinate reassignment instead of +motion. + +## What A Goto Request Really Does + +## At a high level a goto request goes through these steps + +1. make sure goto is allowed right now +2. convert the request into mount coordinates +3. decide which orientation options are valid +4. choose the actual target branch and pier side +5. add a waypoint/home step if needed +6. start the slew +7. continue monitoring limits and refine near the destination + +The important thing is that the target may have more than one valid +representation. + +## Examples + +- east-side vs west-side orientation on a GEM +- axis1 target plus or minus 360 degrees when wrapping/unlimited motion exists +- alternate orientation choices on mounts such as `ALTALT` + +## Workflow 1: Entry Conditions + +Before goto or sync can proceed, the mount checks a number of ordinary +conditions: + +- startup authority must already be trusted +- both axes must be enabled +- the mount must not be parked +- no other goto/guide/motion operation can already be active +- no goto-blocking limit/sense error can already exist +- no motor fault can already exist + +For goto specifically, date/time readiness matters through the ordinary mount +and limit logic as well. + +In other words, goto is not treated as a way to escape unknown state. + +## Workflow 2: Converting The Target + +## The incoming target may start life in + +- native coordinates +- mount coordinates +- equatorial, horizontal, or `ALTALT` forms depending on mount type + +## The goto code converts as needed so the mount can reason about + +- target hour angle / declination where applicable +- target altitude for horizon checks +- target instrument coordinates for each orientation choice + +This is why the same sky target can still lead to more than one valid motion +plan. + +## Workflow 3: Choosing Orientation + +This is one of the core pieces. + +## OnStepX supports a family of orientation-selection policies + +- `PSS_EAST_ONLY` +- `PSS_WEST_ONLY` +- `PSS_EAST` +- `PSS_WEST` +- `PSS_BEST` +- `PSS_AUTO` +- `PSS_SAME_ONLY` + +## What these really mean + +- some insist on one side only +- some prefer one side but can fall back to the other if needed +- some try to stay on the current side +- some ask the planner to choose what seems best + +## `PSS_AUTO` + +This was added mainly to support automatic meridian-flip style behavior when the +mount is in the meridian-overlap region and can legitimately continue tracking +on the other side without interruption. + +In that sense, `AUTO` is not just "pick something convenient." It is meant to +prefer the side/orientation that keeps the mount on a sensible continuing +tracking path when overlap allows it. + +This is still not the same thing on every mount type. + +## For ordinary equatorial cases + +- it usually picks a preferred side from current hour angle +- in overlap cases, that helps place the target on the side where tracking can + continue unimpeded after the orientation change + +## For `ALTAZM` and `ALTALT` + +- it falls back to `PSS_BEST` + +So `AUTO` is really an automatic orientation policy, not one fixed algorithm. + +In practical terms, it is most at home on GEMs, especially imaging setups where +automatic meridian-flip style behavior is desirable. Compared with a fixed +preference such as `PSS_EAST`, `PSS_AUTO` is better at using meridian-overlap +freedom to end up on the side where tracking can keep going cleanly after the +change. + +On fork mounts, the usual practical approach is often different. Users commonly +widen the meridian limits to allow much more freedom, then keep a fixed side +preference such as `PSS_EAST` or `PSS_WEST` for normal operation. If they want +the mount to use the other orientation, they typically change that preferred +side. Forks often do not need, and often should not do, automatic flip-style +behavior in the first place. So wide limits help create freedom, but the +selected side policy still drives whether the planner stays put or flips. + +## `PSS_BEST` + +`BEST` is distance-aware. + +It compares the reachable east- and west-orientation targets in instrument +space and may choose the alternate orientation if that target is closer. + +## This is why OnStepX can sometimes choose what looks like a shortcut + +- through the pole +- through an alternate valid orientation +- through a less-obvious wrapped branch + +That behavior can look surprising, but it is intentional. + +In practical terms, `PSS_BEST` is often attractive for visual use because it +can reduce unnecessary flips and choose the shorter or less awkward path. That +can make the mount feel nicer at the eyepiece even when it is not the same +choice an imaging workflow would prefer. + +## Workflow 4: Reachability And Target Unwinding + +After a tentative target exists, `Limits::validateTarget(...)` checks whether +the target is reachable. + +## This includes + +- altitude min/max +- east-side axis1 window +- west-side axis1 window +- meridian overlap limits for GEMs +- axis2 limits + +These sketches help visualize why the same configured GEM limits can look +different depending on which side of the pier the mount is currently using: + +![GEM limits example when east of the pier]() + +When east of the pier, the active limit window is interpreted against the +east-side geometry shown above. + +![GEM limits example when west of the pier]() + +When west of the pier, the same ideas map onto the mirrored west-side geometry. + +## But there is another subtle step + +- axis1 may be normalized by plus or minus 360 degrees to find a sensible + branch + +That means the planner does not necessarily use the raw first axis1 value that +falls out of the transform. + +## Instead it may + +- keep the current branch if it is already best +- unwind to a nearby branch if that is closer +- shift by `+360` or `-360` to make the target fit the valid motion window + +This is why a coordinate that "looks outside range" on first glance may still +be reachable after normalization. + +It is also why wrapped or effectively unlimited axes feel smarter than a simple +"move to exactly this number" implementation. + +## What `AXIS1_WRAP` Changes + +## If `AXIS1_WRAP == ON` + +- axis1 is configured with a 360-degree wrap +- its configured range becomes `-360 .. +360` +- sync-threshold style coordinate-jump checks are bypassed + +That does not mean the mount ignores all planning. + +## It means + +- axis1 has a broader wrapped coordinate space to work in +- goto can choose a sensible branch in that larger space + +This is most relevant on azimuth-like axes and other mounts where bounded +`-180 .. +180` thinking is too restrictive. + +## Workflow 5: Waypoints And Home During Orientation Change + +If an equatorial goto changes pier side and `MFLIP_SKIP_HOME == OFF`, OnStepX +does not always go straight to the final destination. + +## Instead it may + +- insert a waypoint at home +- or insert an avoid waypoint first, then go to home, then continue + +## That is why the goto stage machine includes + +- `GG_WAYPOINT_AVOID` +- `GG_WAYPOINT_HOME` +- near-destination refine stages +- final destination + +So a meridian flip or alternate-orientation goto is often a multi-stage motion, +not one direct straight-line idea. + +## Horizon Avoidance + +There are two different horizon-avoidance ideas in the goto system. + +The first is path-shaping during side/orientation changes. + +## When a pier-side change is involved, goto may first route through + +- about 4 hours HA +- or about 3 hours HA + +before going home and then heading to the final target. + +## The reason is practical horizon avoidance + +- some otherwise valid geometry would pass too low near the horizon during a + flip or alternate-orientation transition +- low-latitude cases can make that especially awkward + +So the planner uses a small detour heuristic rather than pretending all valid +math paths are equally safe in practice. + +The second is runtime rate shaping based on distance from the altitude limit. + +## There is a runtime slowdown heuristic + +- in equatorial mode with `MOUNT_HORIZON_AVOIDANCE == ON` +- the axis frequencies are scaled down near the minimum altitude limit +- the amount of slowdown varies with distance from that limit +- the code decides whether to slow RA-like or Dec-like motion depending on + whether declination is increasing or decreasing + +This is best thought of as practical protective behavior, not pure geometry. + +## Workflow 6: Near Destination Refinement + +Goto does not always drive directly to the literal final target at full intent. + +## Depending on encoder/tolerance context, it may + +- slew to a near-destination first +- wait to settle +- refine in one or more additional stages +- then finish at the final destination + +That final approach is not only about positional refinement. In practice, the +near-destination exit can also be used to normalize backlash on arrival by +finishing with a consistent last-direction approach instead of simply accepting +whatever direction the earlier slew happened to end with. + +## This is why the code carries + +- `GG_NEAR_DESTINATION_START` +- `GG_NEAR_DESTINATION_WAIT` +- `GG_NEAR_DESTINATION` +- `GG_DESTINATION` + +## It is also why encoder presence changes behavior + +- with suitable encoder/tolerance support, more refined near-target behavior is + allowed +- during homing/parking or on simpler cases, that refinement can be reduced or + skipped + +## Runtime Limits Are Not The Same As Target Validation + +This is another important distinction. + +## Target validation answers + +- "is this planned target/orientation allowed?" + +## Runtime limit monitoring answers + +- "while the mount is moving or tracking, have we crossed something dangerous?" + +## Runtime checks include + +- horizon / overhead limits +- meridian east/west limits +- axis min/max limits +- sensed min/max limits + +## Those runtime checks can + +- stop one axis +- stop the whole mount +- abort goto +- trigger automatic meridian flip in some GEM tracking cases + +## So there are really two layers + +- planned target acceptance +- live motion policing + +## Automatic Meridian Flip + +If automatic meridian flip is enabled and the mount is tracking, some runtime +limit crossings do not simply stop motion. + +Instead the limit code may request a new goto to the current sky position on +the opposite side: + +- east-side request if currently west and beyond the west meridian limit +- west-side request if currently east and beyond the axis1 max side + +## So automatic meridian flip is implemented as + +- a limit-triggered alternate-orientation goto + +not as a separate completely different motion engine. + +## How Non-GEM Mounts Differ + +For fork and alt/az style mounts, some axis-limit reasoning is based on shaft +angles rather than GEM-style pier-side interpretation. + +The limit code therefore sometimes normalizes the current axis1 position back +into a "normal east-like" coordinate form before checking limits. + +That is one reason the same target can feel simpler on some mounts and much +more orientation-sensitive on others. + +For fork mode specifically, this is why wide meridian limits are often used as +a practical way to increase motion freedom without constantly tripping a +side-change style decision. But wide limits are not the whole story. Orientation +selection still matters. In typical fork use, that usually means keeping +`PSS_EAST` or `PSS_WEST` fixed until the user intentionally changes it to get +the opposite orientation, even though the generous limit window would allow +more than one valid branch. + +## `ALTALT` And Other Less Common Modes + +`ALTALT` is one of the modes that can seem obscure from the outside. + +One useful way to picture it is by re-aiming the primary axis of a familiar +equatorial mount. + +- on a GEM, that axis points parallel to Earth's rotation axis +- on an `ALTAZM`, that same idea would point the primary axis at the zenith +- on an `ALTALT`, the primary axis points north on the horizon at `ALT = 0` + +That is why `ALTALT` is not just "equatorial with funny numbers." It is a +different mount geometry with its own natural strengths and weak spots. + +## In practice this means + +- target conversion differs +- `PSS_AUTO` falls back to `PSS_BEST` +- tracking and motion behavior are not identical to GEM or ordinary alt/az +- the mount can move naturally through the zenith and celestial-pole regions +- it is therefore one of the most natural mount geometries for satellite + tracking +- the field will still rotate even if the mount is otherwise tracking the + general sky motion correctly +- the geometry is weakest for targets near due north or due south on the + horizon + +So it is better to think of `ALTALT` as another consumer of the same decision +framework, but with a different singularity pattern and a different relationship +between its primary axis and Earth's rotation. + +## Special Geometry: Sector Gear And Tangent Arm + +Sector gear axis1 and tangent-arm axis2 are special enough that ordinary full +range goto thinking does not always apply. + +## For these mounts + +- target reachability is influenced by limited physical travel +- some checks are relaxed or specialized +- origin handling can matter more than wide-range coordinate elegance + +## So goto on these mounts is partly about + +- choosing a useful target in a constrained motion envelope + +not simply navigating a wide continuous axis range. + +## No-Goto Builds + +## If `GOTO_FEATURE == OFF` + +- real automatic goto motion does not exist +- sync still reuses the target-selection logic +- the old fake-completion paths for home/park are intentionally gone + +## So this mode still benefits from + +- coordinate planning +- orientation choice for sync-like operations + +But it is not pretending to offer the full automatic motion planner. + +## Examples + +## Example 1: Ordinary GEM goto + +- user asks for a sky target +- planner computes east and west solutions +- both may be reachable +- preferred side or best-distance logic chooses one +- goto may go directly, or through home if the side changes + +## Example 2: `PIER_SIDE_BEST` + +- both orientations are valid +- alternate orientation is closer in instrument space +- planner chooses the alternate branch +- this can look like a shortcut through the pole or a less-obvious orientation + +## Example 3: Wrapped axis1 + +- raw target axis1 value is not ideal +- planner shifts by `+360` or `-360` +- resulting branch is closer or simply the only valid one inside limits + +## Example 4: Horizon-avoidance flip + +- side change is needed +- direct route would be too low near the horizon +- goto first detours to an avoid waypoint +- then goes home +- then continues to the final destination + +## Good Mental Summary + +## The cleanest way to think about goto in OnStepX is + +- transforms produce candidate target representations +- limits decide which ones are reachable +- pier-side/orientation policy chooses among them +- wrap normalization may unwind coordinates into better branches +- waypoints and heuristics make the chosen path practical +- runtime limits keep watching after motion begins + +## So the goto system is best understood as + +- a target-planning and motion-decision engine + +not just a coordinate conversion followed by a slew. diff --git a/docs/HOMING_NOTES.md b/docs/HOMING_NOTES.md new file mode 100755 index 0000000..d100045 --- /dev/null +++ b/docs/HOMING_NOTES.md @@ -0,0 +1,461 @@ +# Homing Notes + +## Purpose + +These notes describe how mount homing currently works in OnStepX. + +This is a workflow note, not a deep explanation of low-level input filtering. +For the shared sensing subsystem itself, see: + +- [SENSING_NOTES.md](SENSING_NOTES.md) + +## For how homing relates to startup trust/authority, also see + +- [STARTUP_AUTHORITY_NOTES.md](STARTUP_AUTHORITY_NOTES.md) + +## Quick Summary + +## There are three related but different ideas + +1. Return to home + Move the mount to the configured home coordinates +2. Sensed home + Use home switches/sensors to find home +3. Reset at home + Rebuild the coordinate basis from the known home position + +Those are often used together, but they are not the same operation. + +## The Main Entry Points + +## The core mount home commands are + +- `:hC#` + move to home +- `:hF#` + reset the mount at home / cold-start position +- `:hA0#` / `:hA1#` + disable/enable automatic home at boot +- `:hC1,R#` / `:hC2,R#` + toggle home-sense reversal for axis1/axis2 +- `:hC1,n#` / `:hC2,n#` + set home sense offset in arc-seconds + +## Status command + +- `:h?#` + returns `hasSense,axis1Offset,axis2Offset` + +## What "Home" Means + +## The configured home position is derived from + +- mount type +- configured home defaults +- site latitude where relevant +- optional sense offsets +- special geometry such as meridian-flip handling + +That home position is refreshed by `Home::init()` and then returned through +`home.getPosition(...)`. + +So "home" is not just a fixed pair of axis numbers. It is computed in context. + +## It is also important to separate + +- mount orientation +- coordinates assigned to that orientation + +Without home switches, "home" is primarily a coordinate definition that the +system returns to or resets against. In that case, home is not independently +observed from hardware. It is whatever mount orientation the current coordinate +basis says corresponds to the home coordinates. + +That means the practical truth about what mount orientation counts as "home" +can shift when the coordinate basis shifts. + +## Examples + +- an authoritative reset at home reassigns the coordinate basis +- sync/model/origin changes can improve accuracy by shifting how the system + maps mount orientation into coordinates +- after those changes, the home coordinates may correspond somewhat differently + to mount orientation than before + +With home switches, the home procedure has a stronger mount-orientation anchor +because the system is observing a real sense event during homing. + +## So it is best to think of home as + +- a reference in coordinate space + +that may or may not be strongly anchored to mount orientation depending on +whether home sensing is available. + +## The Three Main Workflows + +## Workflow 1: Return To Home Without Sensors + +This is the simple "move to home coordinates" path. + +## It happens when + +- `home.request()` is called +- no home switches/sense inputs are configured + +## Behavior + +- goto must be available +- startup authority must already be trusted +- date and time must be ready +- the mount slews to the computed home coordinates + +This is just motion to a known coordinate target. It does not establish truth +from hardware sensing. + +## Workflow 2: Sensed Home + +This is the switch/sensor-based path. + +## It happens when + +- `home.request()` is called +- home sense is available + +## Behavior + +- goto must be available +- if startup authority is not trusted, sensed home is still allowed because it + can establish trust +- the guide subsystem starts a special home-guiding mode +- each axis uses `autoSlewHome()` to find the home switch state +- the axis may first move off the switch or toward it depending on the current + sensed state +- the axis refines through fast/slow/fine homing stages + +## If sensing succeeds + +- `home.guideDone(true)` runs +- an authoritative reset is performed +- startup authority becomes trusted + +## If sensing fails + +- `home.guideDone(false)` runs +- a non-authoritative reset/recovery path is attempted +- the home operation is marked failed + +## What The Axis Class Actually Does During Sensed Home + +The mount-level home request is only part of the story. The detailed switch +search sequence lives down in the axis class. + +Each relevant axis uses `Axis::autoSlewHome()`. + +## The important sequence is + +1. Check the current home-sense state +2. Choose an initial direction +3. Slew until the sense state changes in the expected way +4. Decelerate to a stop +5. Refine through additional slower passes + +## More concretely + +- if home sense is already ON, the axis first moves forward off the switch +- if home sense is OFF, the axis first moves reverse toward the switch +- when the sense state changes appropriately, the axis stops +- the homing stage advances from `HOME_FAST` to `HOME_SLOW` +- then usually to `HOME_FINE` + +## During the refinement sequence + +- the slew rate is reduced +- the axis repeats the search with tighter motion +- if the switch state is still not in the expected condition after the slow + pass, an approach correction path is used before the final fine pass + +This is why sensed home is more than "drive until switch closes once." It is a +small staged search procedure implemented at the axis level. + +## Timeout also matters + +- if no explicit timeout is supplied, the axis derives one from the configured + home distance limit and slew rate +- if the search times out, the homing slew aborts + +## Motion safety still applies + +- motion errors abort homing +- motor faults abort homing +- common min/max sense conflicts can also abort the slew + +## So the full sensed-home sequence is + +- mount/home layer decides that sensed home should happen +- guide layer starts coordinated home-guiding +- axis layer performs the actual staged switch search +- home layer interprets success/failure and performs reset/recovery + +## Workflow 3: Reset At Home + +This is the coordinate-basis rebuild step. + +## It happens through + +- `home.reset(...)` +- `:hF#` +- startup reset logic +- successful sensed-home completion + +## Behavior + +- tracking is stopped +- tracking-rate offsets are cleared +- goto first-use state is reset +- motor positions are reset to zero where appropriate +- home instrument coordinates are written back +- nominal index positions are captured +- backlash and slow slew defaults are restored + +## If the reset is authoritative + +- startup trust is granted + +## If it is not authoritative + +- it does not grant trust by itself + +## Authoritative vs Non-Authoritative Reset + +This is one of the key distinctions. + +## Authoritative reset means + +- the software has a good reason to believe the home position being applied is + truly correct + +## Examples + +- successful sensed home +- startup reset using valid paired absolute authority +- explicit authoritative origin/home workflows + +## Non-authoritative reset means + +- coordinates are being reassigned to home, but the software is not treating + that as proof of truth + +## This distinction matters because + +- authoritative reset can establish startup trust +- non-authoritative reset cannot + +## How Trust Interacts With Homing + +Homing is one of the main bridges between untrusted and trusted state. + +## Important rules + +- plain return-to-home without sensors requires trust first +- sensed home can be used to regain trust +- authoritative reset grants trust +- non-authoritative reset does not + +## There is also one permissive carveout + +- in `SA_AUTO` on simple mounts with no absolute encoders and no coordinate + memory, home/reset is allowed to bypass the sync-limit style coordinate jump + restriction + +That keeps older/simple mounts practical without weakening the stricter cases +globally. + +## Auto Home At Boot + +## If `automaticAtBoot` is enabled + +- autostart requests home after boot + +This shares the same sensed-home or return-home machinery as a normal home +request. It is not a separate home implementation. + +## That means + +- success behaves like normal homing success +- failure behaves like normal homing failure +- auto-home does not silently pretend success if the sensed-home flow fails + +## No-Goto Behavior + +## If `GOTO_FEATURE == OFF` + +- home request is rejected +- reset/home-with-motion request is rejected +- sensed home through guide-home is also rejected + +This is intentional. + +## In no-goto builds + +- there is no fast automatic slew/home capability +- home/return is not supposed to pretend to exist as a supported motion mode + +## How Home Sense Offsets Work + +Each mount axis can have a stored sense offset. + +## These offsets mean + +- the physical home sense event is not necessarily the final logical home + coordinate + +## Typical use + +- a switch is placed somewhere repeatable but not exactly at the desired final + home location +- the offset defines the difference between switch-detected home and desired + coordinate home + +## Commands + +- `:hC1,n#` +- `:hC2,n#` + +## Examples + +- `:hC1,120#` + set axis1 home offset to +120 arcsec +- `:hC2,-300#` + set axis2 home offset to -300 arcsec + +## Two Offset Modes + +The config option `MOUNT_HOME_AT_OFFSETS` changes how offsets are used. + +## When `MOUNT_HOME_AT_OFFSETS == OFF` + +- sensed home resets at the switch-derived home first +- then, if offsets are in use, the mount may perform a finishing move toward + the true home coordinates after the authoritative reset + +## When `MOUNT_HOME_AT_OFFSETS == ON` + +- offsets are folded directly into the computed home position + +## So the difference is basically + +- separate finishing move +vs +- home already means the offset-adjusted position + +## Home Sense Reversal + +Each axis can also reverse the meaning of its home sense input. + +## Commands + +- `:hC1,R#` +- `:hC2,R#` + +This is layered on top of the usual home-sense logic and is applied through +`setReversal()`. + +Axis1 also has a latitude-related reversal for equatorial mounts in the +southern hemisphere, so axis1 home sense behavior is slightly more subtle than +just a plain stored boolean. + +## Examples + +## Example 1: Simple mount, no home switches + +- user powers on +- mount already has trusted startup authority or permissive `SA_AUTO` +- `:hC#` moves to home coordinates +- `:hF#` can reset at the home position + +## Example 2: Mount with home switches + +- user or autostart requests home +- guide-home uses switch sensing to find home +- successful sensed home performs authoritative reset +- trust is granted + +## Example 3: Switch is repeatable but not exactly at desired home + +- home sense is configured +- nonzero offsets are stored +- sensed home finds the switch +- reset establishes truth +- final coordinate home may be offset from the literal switch crossing + +## Special Geometry Cases + +Sector gear axis1 and tangent-arm axis2 are special. + +In those cases, homing and reset do not always follow the plain "reset both +axes to zero and write home coordinates" pattern. + +For these geometries, home is not only about returning to a meaningful +coordinate reference. It is also about resetting the mechanism so later gotos +and tracking have useful travel remaining inside the limited range. + +## So on a sector gear or tangent-arm mount, homing is often better understood as + +- mechanical re-baselining of the constrained axis +- restoring useful travel for later motion +- re-establishing the coordinate-to-mechanism relationship for that limited + range + +Because of that, some of the ordinary coordinate-based prechecks are also less +meaningful here. On these mounts the code relaxes or skips parts of the normal +"how far are we from home in coordinates?" logic before sensed homing, because +operation is fundamentally constrained and often more manual. + +## Instead + +- origin/position handling is adjusted to fit the special axis geometry +- successful sensed home may set origin more directly for the special axis + +So if behavior looks a little different on sector gear or tangent-arm mounts, +that is expected. + +## Failure Modes + +## Homing can fail because of + +- goto disabled +- motion already active +- missing date/time for plain return-home +- distance-to-home outside the configured sensed-home search limit +- switch search timeout +- motion error or motor fault during homing +- reset failure after the home-search phase + +The code tries to fail conservatively rather than pretending home succeeded. + +## Relationship To Sensing + +Homing depends heavily on the shared sensing subsystem, but that low-level +behavior is documented separately. + +## If you need to understand + +- how `HIGH | HYST(20)` is interpreted +- how analog threshold/hysteresis sensing works +- how runtime reversal works + +## see + +- [SENSING_NOTES.md](SENSING_NOTES.md) + +## Good Mental Summary + +## The cleanest summary is + +- return-to-home is motion to a known coordinate target +- sensed home is a hardware-based search procedure +- reset-at-home rebuilds the coordinate basis +- authoritative homing/reset can establish startup trust + +Those are related operations, but they are not interchangeable. diff --git a/docs/PEC_NOTES.md b/docs/PEC_NOTES.md new file mode 100755 index 0000000..55cfb05 --- /dev/null +++ b/docs/PEC_NOTES.md @@ -0,0 +1,420 @@ +# PEC Notes + +## Purpose + +These notes describe how PEC currently works in OnStepX. + +## PEC is one of the harder features to reason about because it combines + +- persistent state +- a sampled correction table +- worm indexing +- tracking state +- guide interaction +- park/unpark interaction + +## The implementation is concentrated mainly in + +- `src/telescope/mount/pec/Pec.h` +- `src/telescope/mount/pec/Pec.cpp` +- `src/telescope/mount/pec/Pec.command.cpp` + +## Quick Summary + +## The clean mental model is + +1. PEC stores one correction value per sidereal second of worm rotation +2. a worm index event establishes where the buffer starts +3. playback adds/subtracts a small tracking-rate correction each second +4. recording learns those per-second corrections from accumulated guide action + +## So PEC in OnStepX is basically + +- a one-second-slot correction table wrapped around the worm rotation + +## What PEC Applies To + +## Current mount PEC support is for + +- axis1 only + +Although the implementation and terminology are written around a worm cycle, +the correction model does not technically require a worm. It can also model +other repeatable geared error sources as long as: + +- the cycle can be indexed +- the cycle length is stable +- the cycle period fits within the supported PEC limits/buffer size + +## If `PEC_BUFFER_SIZE_LIMIT == 0` or `PEC_STEPS_PER_WORM_ROTATION == 0` + +- PEC is effectively disabled + +## The Main Stored Data + +## PEC stores + +- whether valid PEC data have been recorded +- current PEC state +- worm rotation length in steps +- the PEC correction buffer itself + +The small settings live in normal NV settings storage. + +## The correction table itself lives in a dedicated NV partition named + +- `PEC` + +## Important persistence detail + +- changing or recording PEC data in RAM does not automatically write the PEC + table to NV +- writing the PEC table to NV only happens on explicit user command + +## This is separate from PEC control state + +- arming playback/record or disabling PEC updates the small PEC settings in NV +- but the actual correction table is only written by explicit save command + +## That means + +- after a recording session, the in-memory PEC table may differ from the + persisted PEC table +- if the user does not explicitly save the new data, a reboot comes back with + whatever PEC table was previously stored + +## This is useful because + +- a bad recording session can simply be abandoned by not saving it + +## The PEC Buffer + +The PEC buffer is not stored as one entry per motor step. + +## Instead + +- one entry represents one sidereal second of worm rotation + +## That means + +- buffer size in bytes is approximately worm rotation time in seconds + +## Example + +- if one worm rotation takes 720 sidereal seconds +- the PEC buffer has 720 entries + +Each entry is an `int8_t` correction value. + +So each second of worm rotation gets a small signed rate correction. + +## How Worm Rotation Length Is Used + +## The configured worm rotation length is stored as + +- `PEC_STEPS_PER_WORM_ROTATION` + +## PEC uses that to derive + +- seconds per worm rotation +- buffer size +- current worm-rotation position from current axis1 motor steps + +## So PEC fundamentally depends on + +- knowing how many axis1 motor steps correspond to one worm rotation + +## Two Ways PEC Gets Its Index + +## PEC can work with + +1. real sense input (`PEC_SENSE != OFF`) +2. virtual wraparound indexing (`PEC_SENSE == OFF`) + +## With real sense input + +- a sense event on `PEC_SENSE_PIN` defines the worm index +- PEC waits for enough worm travel before accepting another index +- this avoids repeated false retriggers too close together + +## Without real sense input + +- PEC treats worm rotation wraparound as a virtual index + +## So + +- hardware sense gives a stronger physical anchor +- no-sense PEC is still possible, but indexing is inferred + +## How The Index Event Works + +## When PEC sense is enabled + +- the sense input is read through the shared `Sense` subsystem +- a valid ON transition after enough worm travel is treated as an index event +- the current axis1 motor step position becomes `wormSenseSteps` +- PEC marks that it has a valid worm reference +- playback buffer position can be realigned to that reference + +## This is why the sensing note matters here too + +- low-level sense filtering decides whether the PEC index input is ON +- PEC then adds its own motion/spacing logic before trusting that event as a + real index + +## When PEC Is Active + +PEC is only allowed to do real work when the mount is in an appropriate state. + +## Main requirements + +- the mount must be tracking +- guide state must not exceed pulse-guide style use +- the mount must not be parking or parked + +## If those conditions are not true + +- PEC disables recording +- PEC pauses playback + +## Important distinction + +- playback pause is not the same as losing PEC data + +## If PEC was playing and the mount becomes ineligible + +- state drops back to `PEC_READY_PLAY` +- playback can resume after re-indexing/alignment timing is right again + +## The PEC State Machine + +## Main states + +- `PEC_NONE` +- `PEC_READY_PLAY` +- `PEC_PLAY` +- `PEC_READY_RECORD` +- `PEC_RECORD` + +## Meaning + +- `PEC_NONE` + PEC inactive +- `PEC_READY_PLAY` + user requested playback; PEC is waiting for the right timing/index position +- `PEC_PLAY` + PEC actively applying corrections +- `PEC_READY_RECORD` + user armed recording; PEC is waiting for the right timing/index position +- `PEC_RECORD` + PEC actively recording guide-derived corrections + +## Why "Ready" States Exist + +PEC does not jump into play or record at an arbitrary instant. + +## Instead, it waits until + +- the worm position is aligned to a one-second slot boundary + +This makes the table alignment cleaner and keeps the one-second buffer indexing +consistent. + +## So "ready" means + +- the user has requested the action +- PEC is waiting for the correct phase/alignment to begin + +## Playback + +## During playback + +- PEC advances the buffer index once per sidereal second +- it looks up the correction for the current worm segment +- it applies that as a fractional tracking-rate offset + +## One subtle detail + +- playback uses the correction from one second earlier than the current segment + +The code comments describe this as a rough latency estimate between image +acquisition and response. + +So PEC playback is intentionally shifted slightly ahead in effect. + +## Recording + +## During recording + +- guide contribution on axis1 is accumulated +- once per sidereal second, that accumulated correction is quantized into a + small signed step value +- that value becomes the PEC table entry for the current buffer slot + +## If this is not the first recording + +- new data are blended with existing data using a weighted average + +## So PEC recording is not just "write raw guide values directly." It is + +- accumulate +- quantize per second +- optionally blend with existing data + +## What "First Recording" Changes + +## If this is the first time PEC is being recorded + +- the new table is taken directly from current recording data + +## If PEC data already exist + +- new data are blended with the old table + +That lets users refine an existing PEC table rather than always replacing it +from scratch. + +## Cleanup After Recording + +## When recording finishes + +- PEC automatically switches to play +- the table is run through a cleanup pass + +## Cleanup includes + +- a low-pass smoothing pass +- a linear-regression style correction so the total sequence sums close to zero + +That last part matters because PEC corrections should not create a long-term +net tracking bias over a whole worm cycle. + +## If the cleanup result looks unreasonable + +- PEC invalidates the recorded data +- PEC drops back to `PEC_NONE` + +## Why The Sum Matters + +A PEC table is supposed to redistribute tracking slightly over the worm cycle, +not permanently add or subtract motion over a full cycle. + +## So after cleanup + +- the total correction sequence should roughly cancel out + +That is why PEC does a "reality check" on the residual sum. + +## Park / Unpark Interaction + +PEC interacts with park in an important way. + +## When parking + +- PEC is turned off +- the current worm sense position is saved into park settings + +## When unparking/restoring + +- that saved worm sense position is restored + +## If a real `PEC_SENSE` input exists + +- worm index truth is also recovered automatically once the worm rotates far + enough to detect the next valid PEC index event + +So the restored saved position helps continuity immediately after unpark, while +the physical PEC sense can later re-anchor the index automatically on the next +cycle. + +So park/unpark is part of how PEC continuity is maintained across a parked +interval. + +This is another reason PEC is harder to reason about than a simple runtime +feature. It is not independent of park state. + +## Examples + +## Example 1: Playback with real worm index + +- PEC data were previously recorded +- user enables PEC playback +- state becomes `PEC_READY_PLAY` +- mount tracks +- PEC waits for the right segment timing +- playback starts +- a later real PEC index event can realign buffer start + +## Example 2: Recording new PEC data + +- mount is tracking +- user arms PEC recording +- state becomes `PEC_READY_RECORD` +- at the right timing boundary PEC enters `PEC_RECORD` +- guide activity is accumulated into one-second corrections +- after one worm cycle, recording finishes +- cleanup runs +- PEC switches into playback + +## Example 3: Mount stops tracking during playback + +- PEC was in `PEC_PLAY` +- tracking stops or mount becomes ineligible +- PEC falls back to `PEC_READY_PLAY` +- rate goes to zero +- playback can later resume when conditions are valid again + +## Commands + +## There are several PEC commands, but the most important control ones are + +- `:$QZ+#` + arm/enable playback +- `:$QZ-#` + disable PEC +- `:$QZ/#` + arm recording +- `:$QZZ#` + clear the PEC buffer +- `:$QZ!#` + save PEC data to NV +- `:$QZ?#` + get PEC status + +## There are also read/write commands for + +- table contents +- worm rotation steps +- buffer size +- current index location + +## For the exact command list, see + +- [COMMAND_REFERENCE.md](COMMAND_REFERENCE.md) + +## Things That Make PEC Hard To Reason About + +## PEC combines several different concerns at once + +- mount motion state +- tracking state +- guide accumulation +- worm index sensing +- one-second slot timing +- persistent storage +- park/unpark continuity + +That is why local code reading can still leave the feature feeling opaque. + +## Good Mental Summary + +## The most useful summary is + +- PEC is a per-second correction table over one worm rotation +- indexing tells PEC where the table starts +- playback applies one correction slot each sidereal second +- recording learns those slots from guide activity +- cleanup makes the finished table smooth and net-neutral over the full cycle + +If you keep that model in mind, the rest of the state machine becomes much +easier to follow. diff --git a/docs/SENSING_NOTES.md b/docs/SENSING_NOTES.md new file mode 100755 index 0000000..2ffcdbd --- /dev/null +++ b/docs/SENSING_NOTES.md @@ -0,0 +1,401 @@ +# Sensing Notes + +## Purpose + +## These notes describe the shared sensing subsystem used across OnStepX for + +- home sense +- limit sense +- park sense +- PEC sense +- some local focuser/button-style inputs + +The main reason this deserves its own note is that a "sense input" in OnStepX +is not just a simple digital switch. The same general machinery supports: + +- ordinary digital active-high or active-low inputs +- digital inputs with software time filtering +- analog threshold sensing +- analog hysteresis behavior +- runtime inversion of the logical sense state + +There is also a related but separate `Button` helper for more button-specific +input behavior, including analog range comparison. That is covered near the end +of this note. + +## Quick Summary + +## The simplest mental model is + +1. raw GPIO or analog input comes in +2. `Sense` turns it into a normalized ON/OFF state +3. higher-level code decides what ON/OFF means operationally + +## Examples + +- home code asks whether home sense is active/ON according to its configured + `HIGH`/`LOW` meaning +- limit code asks whether the min/max sense is ON +- PEC asks whether the worm index sense is ON + +The same low-level sensing code is reused in all of those cases. + +## Where The Shared Sense Code Lives + +## The main subsystem is + +- `src/lib/sense/Sense.h` +- `src/lib/sense/Sense.cpp` + +## Main API + +- `sense.add(...)` +- `sense.isOn(...)` +- `sense.changed(...)` +- `sense.stableMillis(...)` +- `sense.reverse(...)` +- `sense.poll()` + +In most feature code, the higher-level subsystem does not read the GPIO pin +directly. It registers a sense input once and then works with the normalized +result through a sense handle. + +## How Sense Configuration Works + +## Each sense input is defined by + +- a pin +- an init mode such as `INPUT`, `INPUT_PULLUP`, or `INPUT_PULLDOWN` +- a packed trigger value + +The packed trigger value normally combines pieces with `|`. + +That is the usual configuration pattern. + +## Examples + +- `HIGH` +- `LOW` +- `HIGH | HYST(20)` +- `LOW | HYST(50)` +- `HIGH | THLD(512)` +- `LOW | THLD(300) | HYST(25)` + +Combining flags numerically with `|` is typically the intended style. + +## What The Trigger Encodes + +## The trigger value carries + +- the active state bit: `HIGH` or `LOW` +- an optional analog threshold via `THLD(v)` +- an optional hysteresis via `HYST(v)` + +## Important rule + +- if the threshold part is zero, the input is treated as digital +- if the threshold part is nonzero, the input is treated as analog + +## So + +- `HIGH | HYST(20)` is still digital +- `LOW | THLD(400) | HYST(15)` is analog + +## Digital Sense + +Digital mode is used when there is no threshold component. + +## Behavior + +- raw input is read with `digitalReadEx(pin)` +- `HYST(n)` is the required stable time in milliseconds before a digital state + change is accepted +- the new digital sample must remain unchanged for that long before the + logical state changes + +## So in digital mode, `HYST()` does not mean an analog band. It means + +- how long the signal must stay stable before the change is accepted + +## Examples + +- `HIGH` + active-high digital input, no extra filtering +- `LOW` + active-low digital input, no extra filtering +- `HIGH | HYST(10)` + active-high digital input, require about 10ms of stable state before the + logical result changes +- `LOW | HYST(50)` + active-low digital input with heavier software debounce/noise filtering + +## This is useful for + +- switch debounce +- modest EMI/RFI filtering +- noisy digital inputs that should not chatter + +## Analog Sense + +Analog mode is used when a threshold component is present. + +## Behavior + +- raw input is read with `analogRead(...)` +- on ESP32, `analogReadMilliVolts(...)` is converted back into the common + `ANALOG_READ_RANGE` scale for higher accuracy on this platform +- the threshold and hysteresis act like a software Schmitt trigger + +## This means + +- if the sample rises above `threshold + hysteresis`, the logical value becomes + `HIGH` +- if the sample falls below `threshold - hysteresis`, the logical value becomes + `LOW` +- inside the band between those two thresholds, the last logical state is + retained + +## That band is effectively a deadband zone + +- the signal can move around inside it without causing repeated state changes +- a state change only happens after the signal leaves the deadband on one side + or the other + +## Examples + +- `HIGH | THLD(512)` + active-high analog threshold at mid-scale, no hysteresis band +- `LOW | THLD(300) | HYST(25)` + active-low analog threshold input with a hysteresis band around the threshold +- `HIGH | THLD(800) | HYST(50)` + active-high analog input that must move well above and below the threshold + before changing state + +## This is useful when + +- a clean digital threshold is not available from hardware +- analog noise would otherwise cause state chatter + +## Why Hysteresis Means Two Different Things + +This is one of the easiest things to forget. + +## In digital mode + +- `HYST(n)` means time in milliseconds + +## In analog mode + +- `HYST(n)` means an analog hysteresis band around the threshold, creating a + deadband zone where the current state is held + +That dual meaning is intentional, but it is easy to misread in config. + +## What "ON" Means + +## The subsystem separates + +- the filtered internal logical value (`HIGH` or `LOW`) +- whether that logical value counts as ON for the caller + +`sense.isOn(...)` returns true when the filtered value matches the configured +active state. + +## So + +- active-high means HIGH is considered ON +- active-low means LOW is considered ON + +## This lets higher-level code work with + +- "is the home sense active?" +- "is the limit sense active?" + +without caring whether the hardware is electrically active-high or active-low. + +## Runtime Reversal + +`sense.reverse(handle, state)` flips the interpretation at runtime. + +This is different from the original config. + +## Config answers + +- what electrical state counts as active + +## Runtime reversal answers + +- should this sense be interpreted the opposite way right now + +## In practice this is used by code such as + +- `Axis::setHomeReverse()` + +This is useful when the electrical truth stays the same but the motion logic +needs the opposite interpretation. + +## Where `Sense` Is Used + +## Shared `Sense` users currently include + +- axis home sense +- axis min/max limit sense +- park sense +- park signal monitoring +- PEC worm index sense +- local focuser sense inputs + +That is why it is better to think of `Sense` as a general-purpose input +normalization subsystem, not just "home switch code." + +## Example: Home Sense + +Axis home sense is registered in `Axis::init()`. + +## Important behavior + +- homing first checks whether home sense is already ON +- if it is ON, motion first goes forward off the switch +- if it is OFF, motion first goes reverse toward the switch + +So home sense is not just an endpoint detector. It also affects the search +direction and homing flow. + +## Typical home examples + +- `AXIS1_SENSE_HOME HIGH` + simple active-high home switch +- `AXIS1_SENSE_HOME LOW | HYST(20)` + active-low home switch with digital debounce +- `AXIS1_SENSE_HOME HIGH | THLD(450) | HYST(20)` + analog thresholded home sense with hysteresis + +## Example: Limit Sense + +Limit sense uses the same subsystem for min/max axis limits. + +## Runtime behavior + +- `Axis::poll()` updates the sensed limit flags +- `Limits::poll()` reacts and stops the relevant motion direction + +## One important special case + +- min and max can share the same physical pin + +## By default + +- the axis notes this as `commonMinMaxSense` +- the higher-level limit logic avoids treating one shared input as two separate + independent events + +## Config option + +- `LIMIT_SENSE_STRICT == ON` disables that relaxation and treats shared min/max + more strictly + +## Typical limit examples + +- `LIMIT_SENSE LOW` + common active-low limit input +- `LIMIT_SENSE LOW | HYST(10)` + common active-low limit input with digital filtering + +## Example: Park Sense + +## Parking uses the same sensing framework for + +- `PARK_SENSE` +- `PARK_SIGNAL` + +Those are not special sensing implementations. They are just more clients of +the shared normalized ON/OFF sensing subsystem. + +## Example: PEC Sense + +PEC can also use the same shared `Sense` subsystem. + +## That means the PEC index input can also be + +- digital +- analog thresholded +- analog with hysteresis + +Then PEC adds its own higher-level logic on top, such as only accepting a new +index event after enough motion has occurred. + +## So + +- `Sense` decides whether the input is ON +- PEC decides whether that ON transition counts as a valid worm index event + +## What `Sense` Does Not Do + +The shared `Sense` subsystem does not implement every kind of analog input +pattern. + +## In particular, it does not implement + +- "pressed when analog value lies within a range" + +## It is focused on + +- digital filtered ON/OFF sensing +- analog threshold-plus-hysteresis ON/OFF sensing + +## Related But Separate: `Button` + +## There is a related helper in + +- `src/lib/pushButton/PushButton.h` +- `src/lib/pushButton/PushButton.cpp` + +## That helper is more button-focused and adds behavior such as + +- press/release semantics +- click and double-click detection +- tone/frequency detection +- analog threshold mode +- analog range/value mode + +That analog value/range mode is the "pseudo-digital using analog sampling and +range comparison" behavior. + +## So if someone is looking for + +- analog value-in-range detection +- button semantics +- tone-style input behavior + +they should usually look at `Button`, not `Sense`. + +## Practical Rules Of Thumb + +## If the input is a normal switch + +- start with plain `HIGH` or `LOW` + +## If the switch chatters or is noisy + +- add `HYST(n)` and remember that in digital mode it means milliseconds + +## If the input is fundamentally analog + +- use `THLD(...)` +- add `HYST(...)` if you want Schmitt-trigger-like stability + +## If the meaning of the signal must flip at runtime + +- use runtime reversal rather than redefining the electrical config + +## Good Mental Summary + +## The cleanest way to think about sensing in OnStepX is + +- config defines how raw input should be interpreted +- `Sense` turns that raw input into a stable ON/OFF result +- higher-level features decide what ON/OFF means operationally + +That separation is why the same low-level sensing subsystem can be reused for +home, limits, park, PEC, and other features without each subsystem reinventing +analog/digital input conditioning. diff --git a/docs/SERIAL_NOTES.md b/docs/SERIAL_NOTES.md new file mode 100755 index 0000000..5efebc9 --- /dev/null +++ b/docs/SERIAL_NOTES.md @@ -0,0 +1,314 @@ +# Serial Notes + +## Purpose + +These notes explain how serial interfaces are used in OnStepX and why serial +configuration can be confusing across platforms. + +## The short version is + +- a platform may support several kinds of serial interfaces +- a pinmap chooses which ones are exposed for a given board +- a feature may or may not be well suited to the chosen serial transport + +## So "serial is supported" does not always mean + +- easy to configure +- portable to another board +- good for real-time runtime use + +## Quick Summary + +## Think about serial in three layers + +1. Platform: + What the MCU/core can do +2. Pinmap: + Which serial roles are exposed on this board +3. Feature: + Whether the transport is actually a good fit for the workload + +## Examples + +- GPS over `SoftSerial` can be fine +- TMC2209 over `SoftSerial` may compile but still be a poor runtime fit +- Teensy can expose extra USB virtual serial ports +- ESP32 can often move hardware UARTs to different pins + +## The Main Serial Roles + +OnStepX uses generic serial role names instead of hard-coding one board's port +names everywhere. + +## Common roles include + +- `SERIAL_A` +- `SERIAL_B` +- `SERIAL_C` +- `SERIAL_D` +- `SERIAL_E` +- `SERIAL_GPS` +- `SERIAL_TMC` +- `SERIAL_DEBUG` + +These are best thought of as roles, not fixed physical ports. + +## One board might make + +- `SERIAL_B = Serial2` + +## Another board might make + +- `SERIAL_B = HardSerial` with configured RX/TX pins + +Another board might not expose `SERIAL_B` at all. + +## The Main Serial Types + +## In practice, OnStepX may use + +- built-in hardware serial ports such as `Serial1`, `Serial2`, etc. +- created hardware serial objects using chosen RX/TX pins +- `SoftwareSerial` +- USB virtual serial ports +- internal pseudo-serial helpers such as `SERIAL_LOCAL` + +All of these may look stream-like in code, but they are not equally suitable +for all jobs. + +## Platform Differences + +This is where a lot of confusion starts. + +## Teensy + +- often has multiple hardware UARTs +- can also expose extra USB virtual serial ports depending on USB mode +- some pinmaps use that to expose `SERIAL_D = SerialUSB1` and + `SERIAL_E = SerialUSB2` + +## ESP32 + +- usually has flexible hardware UART routing +- can often place UART RX/TX on non-default pins +- that makes created hardware serial or remapped UART use more practical than + on some other Arduino-class targets +- also has optional Bluetooth serial support in the HAL + +## STM32 + +- often supports created/remapped hardware serial well +- also pulls in `SoftwareSerial` support early in the HAL because library/core + behavior can otherwise get awkward + +## Why ESP32 Is Worth Calling Out + +ESP32 is a good example of why "hardware serial" does not always mean +"hard-wired fixed pins." + +## In OnStepX, some ESP32 pinmaps define roles like + +- `SERIAL_B = Serial2` +- `SERIAL_TMC = Serial1` + +## and then separately define + +- `SERIAL_B_RX` +- `SERIAL_B_TX` +- `SERIAL_TMC_RX` +- `SERIAL_TMC_TX` + +That means the feature gets a real hardware UART, but the board still has some +freedom to place it where it makes sense. + +## This is one reason ESP32 can be a nice middle ground + +- better than `SoftSerial` for runtime work +- more flexible than boards with only fixed UART pin assignments + +## Pinmaps Decide What Exists + +The pinmap layer is where generic serial roles become concrete. + +## This means + +- a feature may request `SERIAL_GPS` +- but whether that becomes `SoftSerial`, `HardSerial`, `Serial1`, or something + else depends on the pinmap and platform + +## Compile-time validation catches some problems, for example + +- a baud rate is enabled for a serial role that the pinmap did not define +- a TMC UART driver is enabled but `SERIAL_TMC` is missing + +## What validation does not prove + +- that the chosen serial transport is a good runtime fit +- that it is non-blocking enough +- that the underlying library behaves well on that platform + +## Best Uses For Each Type + +## Built-in or created hardware serial + +## Usually best for + +- continuous runtime traffic +- higher baud rates +- frequent bidirectional communication +- motion-control related serial work + +## Examples + +- shared TMC2209 UART on boards designed for it +- a secondary MCU link +- frequent device polling + +## `SoftwareSerial` + +## Usually best for + +- one-time configuration at boot +- low-rate telemetry +- devices that become less important once startup finishes + +## Examples + +- GPS +- boot-time driver setup in some constrained builds + +## USB virtual serial + +## Usually best for + +- extra host-side channels +- debug, tooling, or auxiliary control on supported platforms + +## Examples + +- Teensy `SerialUSB1` +- Teensy `SerialUSB2` + +## Why `SoftwareSerial` Is Tricky + +`SoftwareSerial` is often the most confusing case because it is sometimes very +useful and sometimes a terrible fit. + +## General concerns + +- it is often blocking or close enough to blocking to matter +- support quality varies by platform +- timing behavior varies by core and library +- library code sitting on top of it may own the actual read/write behavior + +So if a feature "supports `SoftSerial`", that should not automatically be read +as: + +- well tested +- non-blocking +- appropriate for real-time runtime use + +## Example: GPS + +GPS is one of the better cases for less-than-perfect serial. + +## Why + +- relatively low-rate +- mostly an input stream +- used to establish time/location +- OnStepX may close the GPS serial port once current time has been acquired + +So GPS over `SoftSerial` can be a reasonable trade on many builds. + +That does not make `SoftSerial` universally good. It just means GPS is a +feature that can often tolerate it. + +## Example: TMC2209 UART + +TMC2209 is the opposite kind of example. + +## Why it is harder + +- runtime UART interaction can matter +- mode changes may happen during operation +- status/config traffic may be needed while the rest of the mount is busy +- blocking behavior is much more painful in a real-time motion system + +## So for TMC2209 + +- `SoftSerial` may be acceptable for one-time setup at boot +- `SoftSerial` is much less attractive for active runtime interaction + +That does not mean `SoftSerial` is useless here. It means the use case matters. + +## Example: More Than Four TMC2209s + +## There is one place where `SoftSerial` can still be attractive for TMC2209 + +- per-device links can make scaling easier when a shared hardware UART bus gets + awkward + +## So there is a real trade + +- shared hardware UART is usually better for runtime quality +- per-device `SoftSerial` may make some larger or stranger topologies possible + +## This is why the answer is not simply + +- "never use `SoftSerial`" + +## It is more like + +- use it carefully +- prefer it for boot/setup or unusual topologies +- do not assume it is ideal for active runtime control + +## Library Ownership Matters + +## Another subtle point + +OnStepX may define the transport, but the feature library often performs the +real reads and writes. + +## That means serial behavior depends on + +- the platform core +- the chosen transport +- the feature library + +## This is especially relevant for + +- TMC driver libraries +- any feature with frequent serial traffic + +So a configuration can be theoretically valid while still being lightly tested +or operationally uncertain. + +## Practical Rules Of Thumb + +## If the serial link is mostly needed at boot + +- `SoftSerial` may be acceptable + +## If the serial link is needed continuously during runtime + +- prefer hardware UART where possible + +## If the board/platform offers extra USB serial channels + +- treat that as a platform-specific bonus, not a general Arduino assumption + +## If a user is choosing between "works in theory" and "works robustly" + +- favor the more boring hardware-UART option + +## Good Mental Summary + +## The simplest reliable way to think about serial in OnStepX is + +- HAL defines what the platform can plausibly do +- the pinmap assigns generic serial roles to actual transports +- the feature decides whether that transport is really suitable + +That last step is where most serial surprises come from. diff --git a/docs/SERVO_SETUP.md b/docs/SERVO_SETUP.md new file mode 100755 index 0000000..479aabc --- /dev/null +++ b/docs/SERVO_SETUP.md @@ -0,0 +1,496 @@ +# Servo Setup Guide + +## Purpose + +This guide is for setting up a mount axis that uses servo feedback in OnStepX. + +It is written for users who want a practical bring-up sequence: +- what to configure first +- what to test before moving faster +- what to adjust when things go the wrong way + +This guide is intentionally light on control theory. The goal is to help you +get to a safe, working setup without having to understand every low-level +detail first. + +## Before you begin + +Have these basics sorted out first: +- the controller pinmap matches your hardware +- power wiring is correct +- motor wiring is correct enough that the driver powers up safely +- encoder wiring is correct enough that counts can be read reliably +- hard limits or other motion safeguards are planned before unattended use + +Important safety note: +- do not leave a new servo setup tracking unattended until you have verified + limits, stop behavior, and the direction of both the motor and encoder +- a servo that is "mostly working" can still drive hard into a stop if the + setup is wrong + +## What "servo" means here + +In OnStepX, a servo axis is a motor axis that closes the loop using encoder +feedback. + +In practical terms that means: +- the motor moves the axis +- the encoder tells OnStepX where the axis actually is +- OnStepX keeps correcting the motor so the axis reaches and holds the target + +For the mount axes, the main items are usually: +- a servo-capable driver model such as `SERVO_PE`, `SERVO_EE`, `SERVO_TMC2209`, + `SERVO_TMC5160`, or `SERVO_KTECH` +- an encoder type such as `AB`, `KTECH_IME`, or another supported encoder +- sensible axis scale and direction settings +- usable PID values + +This guide is mainly about the built-in servo axis types. ODrive-based systems +have their own workflow and deserve separate documentation. + +## How the current `#define`s tie together + +If you are comparing old example configs, this is the compile-time chain that +matters in the current code. + +### 1. `AXIS*_DRIVER_MODEL` decides whether the axis is treated as a servo + +If `AXIS1_DRIVER_MODEL` or `AXIS2_DRIVER_MODEL` is in the servo-driver range, +`Config.defaults.h` defines `AXIS*_SERVO_PRESENT`. + +That is the switch that makes the mount axis build as a servo axis instead of a +step/dir axis. + +### 2. `AXIS*_ENCODER` selects the encoder class + +When `AXIS*_SERVO_PRESENT` is true, `Mount.axis.cpp` selects the encoder from +`AXIS*_ENCODER`. + +Examples: +- `AB` selects a quadrature encoder +- `KTECH_IME` selects the KTech encoder class +- `SERIAL_BRIDGE` selects the serial bridge encoder path + +### 3. `AXIS*_SERVO_FEEDBACK` selects the feedback controller + +`Mount.axis.cpp` currently uses: +- `PID` for a single PID set +- `DUAL_PID` for separate tracking and goto PID sets + +That means these defines feed directly into the live controller: +- `AXIS*_PID_P` +- `AXIS*_PID_I` +- `AXIS*_PID_D` +- `AXIS*_PID_P_GOTO` +- `AXIS*_PID_I_GOTO` +- `AXIS*_PID_D_GOTO` +- `AXIS*_PID_SENSITIVITY` + +### 4. `AXIS*_SERVO_FLTR` selects the encoder filter + +Current mount-axis choices are wired in through `Mount.axis.cpp`: +- `OFF` +- `KALMAN` +- `LEARNING` for Axis1 +- `ROLLING` + +If you are starting fresh, `OFF` is the simplest baseline. + +### 5. Some servo driver models also use motor-side scaling + +For stepper-based servo drivers such as `SERVO_TMC2209`, `SERVO_TMC5160`, and +`SERVO_KTECH`, the code uses this ratio: + +```cpp +AXIS*_MOTOR_STEPS_PER_DEGREE / AXIS*_STEPS_PER_DEGREE +``` + +That ratio is passed into the servo driver in `Mount.axis.cpp`. + +In practical terms: +- `AXIS*_STEPS_PER_DEGREE` is still the mount axis scale +- `AXIS*_MOTOR_STEPS_PER_DEGREE` matters when the motor-side step scale is not + the same as the encoder-side axis scale + +These two are rarely the same. Sometimes the encoders are on the mount axes and there is not reduction and other times there is. Then the encoder has a resolution as well. On the other hand the motor almost always has an associated reduction and full-steps per rotation and 256 micro-steps per full-step. + +### 6. Direction and origin are separate inputs to `ServoMotor` + +The final `ServoMotor` object is built with: +- `AXIS*_REVERSE` +- `AXIS*_ENCODER_REVERSE` +- `AXIS*_ENCODER_ORIGIN` + +These do different jobs: +- `AXIS*_REVERSE` changes commanded motor direction +- `AXIS*_ENCODER_REVERSE` changes encoder sign +- `AXIS*_ENCODER_ORIGIN` is the stored zero/origin for absolute encoder cases + +There is also a runtime reverse path. + +In practical terms: +- `AXIS*_ENCODER_REVERSE` is the baseline wiring/sign correction for the encoder +- `AXIS*_REVERSE` is the baseline motion-direction choice for the axis +- if the axis reverse state is changed at runtime, OnStepX also flips the + control direction so the feedback loop still makes sense + +That is why direction can appear to be handled at more than one level. It is. +Those levels are there for different reasons. + +### 7. `AXIS*_TARGET_TOLERANCE` is not servo-only, but it matters a lot for servo behavior + +`AXIS*_TARGET_TOLERANCE` is passed into the `Axis` object and affects when the +system considers the axis near its target. + +That is why many servo example configs override it even though it is not +strictly a servo-only define. + +### 8. Absolute encoder authority is inferred from encoder type + +In `Validate.h`, some encoder types automatically imply absolute encoder +authority on servo axes. That affects startup trust behavior. + +So for servo users, `AXIS*_ENCODER` is doing more than just picking a driver. +It can also affect how startup authority is determined. + +## Practical takeaway about old example configs + +Some example servo configs are older than the current code and may not reflect +today's best defaults. + +Treat them as: +- examples of working combinations +- hints for starting values +- not as an exact description of the current wiring between settings + +If there is a conflict, trust the current code path in: +- [Config.defaults.h](/c:/Users/hjd19/Documents/Programs/GitHub/OnStepX/src/Config.defaults.h) +- [Mount.axis.cpp](/c:/Users/hjd19/Documents/Programs/GitHub/OnStepX/src/telescope/mount/Mount.axis.cpp) +- [Validate.h](/c:/Users/hjd19/Documents/Programs/GitHub/OnStepX/src/Validate.h) + +## Minimum settings to get right first + +For each servo mount axis, focus on these first: + +1. Driver model +- set the axis driver model to the correct servo type + +Example: + +```cpp +#define AXIS1_DRIVER_MODEL SERVO_PE +``` + +2. Axis scale +- set `AXIS*_STEPS_PER_DEGREE` correctly +- for stepper-based servo drivers, also check `AXIS*_MOTOR_STEPS_PER_DEGREE` + if your hardware needs it + +If the scale is wrong, almost everything else becomes confusing. + +3. Encoder type +- set `AXIS*_ENCODER` to the actual encoder you are using + +4. Directions +- `AXIS*_REVERSE` controls motor direction +- `AXIS*_ENCODER_REVERSE` controls encoder direction + +These are separate on purpose. Do not assume that fixing one also fixes the +other. + +The practical rule is: +- use `AXIS*_ENCODER_REVERSE` to make encoder counts agree with real motion +- use `AXIS*_REVERSE` to make commanded axis motion go the right way + +Do not use motor reverse as a substitute for fixing an encoder that counts the +wrong direction. + +If the motor and encoder direction are NOT in agreement the axis will run-away. +There is code that attempts to detect run-away, or oscillation, which will hopefully +shut the axis down in short order if this happens. + +5. Conservative limits +- set reasonable axis limits before doing long moves +- keep first tests short and slow + +## Recommended bring-up order + +This is the safest order for a fresh servo setup. + +### 1. Bring up one axis at a time + +If possible, get Axis1 behaving first, then copy the pattern to Axis2. + +Trying to solve both axes at once can hide simple mistakes. + +### 2. Verify encoder direction before worrying about motor direction + +The encoder direction must make physical sense. + +When the axis moves in a positive direction, the encoder should report that +same direction. If it does not: +- fix `AXIS*_ENCODER_REVERSE` first + +Do this before serious PID tuning. + +### 3. Verify motor direction + +Once the encoder reads correctly: +- use a small manual move +- confirm the axis moves the expected way +- if not, fix `AXIS*_REVERSE` + +This order matters. + +If the encoder sign is wrong, the control loop can "fight" the motion and a +later motor-direction change may only hide the real problem. + +### 4. Start with small motions only + +For early tests: +- use guide moves or very short slews +- keep your hand near power-off +- do not start with full-length gotos + +### 5. Confirm the axis can stop cleanly + +Before tuning for performance, verify that: +- the axis starts moving promptly +- the axis stops when motion stops +- there is no runaway, buzzing, or repeated overshoot + +If anything looks wrong, stop and correct direction, scale, or wiring before +going further. + +## A simple way to think about PID + +You do not need to be a controls engineer to get started. + +Use this mental model: +- `P` reacts to present error +- `I` corrects lingering offset +- `D` helps calm motion and reduce overshoot + +OnStepX commonly uses: +- one PID set for tracking +- another PID set for slewing/goto + +Those are the `_GOTO` values: +- `AXIS*_PID_P_GOTO` +- `AXIS*_PID_I_GOTO` +- `AXIS*_PID_D_GOTO` + +Often since this uses feed-forward the goto parameters are less important. Also +these can be changed instantly at runtime from the SWS or Website Plugin which makes tuning far far faster. + +## How motion feed-forward works now + +This is one area where older servo examples can be misleading. + +In current OnStepX, the servo path does not rely on PID correction alone. +The motion command already includes a feed-forward component, and the exact +motor-side behavior depends on the servo driver type. + +### TMC stepper servos + +For TMC stepper servos such as `SERVO_TMC2209` and `SERVO_TMC5160`: +- motion feed-forward is automatic +- OnStepX combines the requested motion with the feedback correction and sends + a velocity command to the driver + +In user terms: +- you do not usually "turn feed-forward on" +- it is part of how these servo drivers normally work + +The main coupling setting is the counts-to-steps ratio: +- it starts from the config default based on + `AXIS*_MOTOR_STEPS_PER_DEGREE / AXIS*_STEPS_PER_DEGREE` +- that ratio is also a runtime-adjustable axis parameter + +So for TMC stepper servos, the important question is usually not whether +feed-forward exists, but whether the counts-to-steps coupling is correct. + +### DC servos + +For DC servo drivers such as `SERVO_PE`, `SERVO_EE`, `SERVO_TMC2130_DC`, and +`SERVO_TMC5160_DC`: +- the same upstream motion command still includes feed-forward +- but the driver side now has more explicit runtime coupling controls + +The practical runtime knobs are: +- minimum motor power +- maximum motor power +- reversal protection delay + +That means DC servo behavior is more dependent on the runtime motor-output +settings than the TMC stepper-servo path is. + +### DC servo velocity control loop + +DC servos can also use the newer motor-side velocity loop. + +When enabled: +- the driver uses encoder-derived velocity as a measured feedback signal +- the motor-side control becomes feed-forward plus a velocity PI correction + +When not enabled: +- the motor-side control is basically feed-forward duty mapping + +In practical terms: +- without the velocity loop, DC servos depend more heavily on a good power map +- with the velocity loop, DC servos can regulate motor behavior more directly + from measured encoder speed + +### Encoder velocity support + +The newer DC velocity-loop behavior depends on encoder velocity estimation. + +That comes from the encoder subsystem itself, not from the PID section. + +So if you are working on a DC servo and velocity control is enabled: +- encoder quality matters even more than usual +- noisy or weak encoder velocity estimates can make tuning harder + +## Deadband and reversal protection + +Two settings in the servo path can sound similar, but they do different jobs. + +### Zero deadband + +The servo driver also has a low-speed command deadband. + +In practical terms: +- very small requested motor velocities are treated as zero +- this helps prevent constant tiny nudges near standstill +- it is not the same thing as backlash compensation or target tolerance + +If this is set too high: +- the axis can feel reluctant to make very small corrections + +If it is set too low: +- the motor may keep twitching or chattering around zero + +### Reversal protection delay + +DC servos also have a direction-change delay. + +This is better thought of as reversal protection: +- when the commanded direction flips, the output is briefly held at zero +- that gives the drivetrain and driver a moment before power is applied in the + opposite direction + +This can be helpful when: +- the motor drive does not like abrupt reversals +- the axis has noticeable mechanical shock on direction changes +- you want gentler reversal behavior during tuning + +So the practical distinction is: +- zero deadband suppresses tiny commands near zero +- reversal protection delay softens direction changes + +## Practical PID advice + +Start simple: +- begin with modest `P` +- keep `I` small +- use little or no `D` at first + +What to watch for: +- if the axis feels weak or lags badly, `P` may be too low +- if it hunts or oscillates, `P` may be too high +- if it reaches the target but stays offset, add a little `I` +- if it overshoots and rings, reduce `P` or add a small amount of `D` + +Make small changes, then test again. + +Avoid changing many things at once. + +## About `PID_SENSITIVITY` + +`AXIS*_PID_SENSITIVITY` controls how OnStepX transitions between the tracking +PID set and the slewing PID set. + +A practical rule of thumb: +- for many DC servos, a nonzero value can help +- for many stepper-based servos, `0` is a reasonable starting point + +If you are unsure: +- start with the value used by a similar example config + +## About target tolerance + +`AXIS*_TARGET_TOLERANCE` gives the axis a little room around the target. + +In user terms: +- too small can make the axis fussy and restless +- too large can make the final position sloppier than you want + +If the axis seems to chatter around the target, this setting may be too tight. + +## First successful motion checklist + +Once you have basic motion, verify these before calling it "working": +- guide moves are in the correct direction +- encoder direction remains correct through the full move +- the axis does not run away when motion starts +- the axis does not oscillate badly when motion stops +- tracking starts and holds without obvious hunting +- limits and stop behavior work as expected + +Only after that should you move on to longer slews and unattended use. + +## Absolute encoders and origin + +If your servo setup uses absolute encoders: +- the encoder origin must be established correctly +- startup trust behavior depends on that origin being believable + +If you are not using absolute encoders: +- skip this part for initial bring-up +- get basic motion stable first + +## Useful optional UI aids + +Some test configs enable extra web controls such as: +- `DISPLAY_SERVO_MONITOR` +- `DISPLAY_SERVO_ORIGIN_CONTROLS` +- `DISPLAY_SERVO_CALIBRATION` + +These can make bring-up easier, especially while checking direction, settling, +and origin behavior. + +## Common mistakes + +These are the usual trouble spots: +- wrong encoder direction +- wrong motor direction +- wrong steps-per-degree +- tuning before verifying scale and direction +- trying full-speed slews too early +- leaving tracking enabled before limits and stop behavior are proven + +If the setup behaves strangely, go back to: +1. scale +2. encoder direction +3. motor direction +4. only then PID tuning + +## A good first goal + +Do not aim for perfect tracking on the first pass. + +A good first milestone is: +- the axis moves the right way +- encoder feedback agrees with motion +- it stops cleanly +- it holds position without obvious instability + +After that, refine tracking, slewing response, and final pointing performance. + +## Future improvements for this guide + +Useful follow-up documentation would be: +- a separate ODrive setup guide +- a short PID tuning cookbook with symptom-based advice +- a checklist for absolute encoder origin setup +- a servo troubleshooting page with "if it does this, check that" diff --git a/docs/STARTUP_AUTHORITY_NOTES.md b/docs/STARTUP_AUTHORITY_NOTES.md new file mode 100755 index 0000000..f7c9a77 --- /dev/null +++ b/docs/STARTUP_AUTHORITY_NOTES.md @@ -0,0 +1,366 @@ +# Startup Authority And Coordinate Trust Notes + +## Purpose + +These notes describe how startup authority and coordinate trust currently work +for the mount, and how that affects automatic motion such as goto, sync, +park/unpark, and home/return. + +## Core idea + +The mount tracks a runtime trust state whose only question is: +"Do we trust the current coordinate basis enough for coordinate-based +automatic motion?" + +If trust is true: +- coordinate-based automatic operations are allowed + +If trust is false: +- coordinate-based automatic operations are blocked +- manual guide recovery is still allowed +- plain sidereal tracking is not gated by startup authority itself, but it may + still be blocked by other safety policy + +## Two useful distinctions + +1. Immediate startup authority +- trust is available immediately at boot from an already-available authority + source + +2. Deferred startup authority +- boot starts untrusted +- an authoritative startup-time action later establishes trust + +Date/time readiness is separate from both of the above. + +Limit enforcement is related but separate too: +- startup authority answers whether the coordinate basis is trusted +- date/time readiness answers whether sky-referenced operation is ready +- limit enforcement can become active as soon as both are available + +## What currently establishes trust + +Trust can currently come from either immediate startup authority sources or +later authoritative actions. + +Immediate startup authority sources: +- paired mount-axis absolute position sources at boot, with origin(s) + established +- trusted mount coordinate memory restore + +Boot policy is selected by MOUNT_STARTUP_MODE. +Default: +- SA_AUTO + +Modes: +- SA_STRICT: + boot stays untrusted unless an immediate startup authority source succeeds +- SA_AUTO: + if neither mount axis has an absolute position source and mount coordinate + memory is OFF, boot grants legacy session trust automatically +- SA_PERMISSIVE: + boot grants session trust automatically even when stricter authority sources + exist + +Later authoritative actions: +- authoritative set-origin at a known home position +- authoritative reset at a known home position +- successful switch-based home +- SWS v2.10d or later hosting absolute encoders (for pointing correction) + +Ordinary sync is not used to restore trust. + +## What currently revokes trust + +Trust is revoked conservatively when the system can no longer honestly assume +the coordinate basis is valid. + +Currently implemented: +- motor/encoder fault +- NV fault (default behavior, because NV_INIT_ERROR_REVOKES_AUTHORITY defaults to ON) + +Important non-revocation note: +- a motion stop caused by limits or StallGuard is not, by itself, a trust + revocation event + +## What is blocked while untrusted + +Blocked: +- goto +- set park +- park +- unpark +- normal sync +- home/return unless switch-based home is available + +Allowed: +- manual guide +- plain sidereal tracking, subject to the normal tracking safety policy + +Important tracking note: +- startup authority does not itself gate plain sidereal tracking +- however, tracking is still subject to other conditions such as parked state, + date/time-dependent enable rules, and limit policy +- by default, tracking is disallowed when limits are not enforced + +## Parking workflow + +Parking is part of the same trust model because park/set-park/unpark all depend +on the mount coordinate basis being believable. + +While untrusted: +- set park is blocked +- park is blocked +- unpark/restore is blocked + +Why: +- set park stores a coordinate-based reference that should not be accepted if + the current coordinate basis is already in doubt +- park and unpark are automatic coordinate-based motions, so they should follow + the same trust rules as goto + +Operationally this means: +- a parked state is only useful if it was established from a trusted coordinate + basis +- unpark/restore is allowed when resuming from previously trusted parked state +- unpark/restore should not be thought of as a way to recover trust from unknown + state +- the legacy "unpark from home" exception is only allowed on simpler mounts; it + is disabled for SA_STRICT, for mounts using coordinate memory, and for mounts + with absolute position sources + +## Workflow 1: Boot With Immediate Startup Authority + +This is the cleanest startup path. + +On these mounts, trust can be granted automatically at boot because the mount +already has an authoritative coordinate basis before any manual recovery is +needed. + +Current immediate startup authority sources are: +- paired mount-axis absolute position sources +- trusted mount coordinate memory + +## Workflow 1a: Boot With Paired Mount-Axis Absolute Coordinate Authority + +This applies when: +- both mount axes report absolute position at startup +- the required encoder origin(s) are already established +- startup reset is therefore allowed to treat the mount-axis absolute readings + as authoritative for the initial coordinate basis + +This is intentionally stricter than "one axis happens to be absolute." +For the mount axes, absolute coordinate authority is only valid as a paired +startup basis. + +In practice this covers the supported mount-axis absolute-authority cases, +including: +- paired servo absolute encoders +- paired ODrive absolute-authority configuration + +For runtime/NV origin storage: +- UINT32_MAX is used as the "origin not established" sentinel + +Compile-time configured nonzero AXIS*_ENCODER_ORIGIN values are treated as +already established by configuration. + +## Workflow 1b: Boot With Trusted Mount Coordinate Memory + +Mount coordinate memory acts much like an "effective absolute startup source" +in the trust model: +- it can re-establish a coordinate basis at boot +- but only if the saved record is marked trusted + +Important assumption: +- this depends on the mount being moved only by the drive system +- if clutches are present, they must not slip and must not be disengaged while + coordinate memory is being relied on as a startup authority source + +Mount coordinate memory already uses: +- alternating A/B records +- CRC validation + +It now also uses a trust bit: +- trusted records may restore authority +- untrusted records do not restore authority + +This separates: +- storage validity +- semantic trust + +So coordinate memory is not just "last known position storage." It is also a +startup authority source when, and only when, the saved record is trusted. + +## Workflow 2: Boot Without Immediate Startup Authority + +What happens here depends on MOUNT_STARTUP_MODE. + +In SA_STRICT: +- if the mount has neither: + - paired mount-axis absolute position sources + - trusted mount coordinate memory +- trust is not granted automatically at startup + +In SA_AUTO: +- if the mount has neither: + - paired mount-axis absolute position sources + - mount coordinate memory enabled +- boot grants legacy session trust automatically +- this preserves the long-standing "power up and goto" workflow on simpler + mounts + +If mount coordinate memory is configured but the saved record is missing or +untrusted, SA_AUTO does not silently fall back to permissive trust. In that +case the system stays untrusted until an authoritative action succeeds. + +In SA_PERMISSIVE: +- boot grants session trust automatically regardless of startup authority + sources + +Current recovery model: +1. user manually guides to a known place if needed +2. user performs an authoritative action: + - set origin + - authoritative reset at a known home position + - successful switch-based home + - reboot with a valid startup authority source +3. trust becomes true again + +Important practical consequence: +- the default SA_AUTO mode preserves permissive startup for simple mounts with + neither paired absolute position sources nor coordinate memory enabled +- SA_STRICT remains available for observatory and interlock-sensitive installs + +## Workflow 3: Boot Untrusted, Then Establish Trust With Auto-Home + +Auto-home with switches is best thought of as deferred startup authority. + +It is not an immediate startup authority source. +Instead: +- boot begins untrusted +- switch-based home runs +- if successful, trust is established afterward + +Switch-based home and auto-home-at-boot share the same sensed-home path. + +Success: +- successful sensed home leads to authoritative reset +- trust is restored + +Failure: +- coordinates are still reset for consistency +- failure is latched +- trust is not restored +- auto-home at boot does not continue as if homing succeeded + +This behavior is intentionally consistent with normal switch-based return/home, +not just boot homing. + +## Coordinate memory during homing + +Important edge case: +when switch-based homing is in progress, mount coordinates are effectively +arbitrary relative to physical truth. + +To prevent a power loss during homing from producing a misleading trusted +restore candidate: +- coordinate memory is marked untrusted as soon as switch-based homing starts +- periodic saves during HS_HOMING remain untrusted +- after successful authoritative homing, trust is restored normally + +So if power is lost mid-home: +- the mount will come up with coordinate memory present but untrusted +- automatic coordinate-based motion will remain blocked +- the user can home/reset again, regain trust, and continue + +## Workflow 4: Date/Time Not Ready + +Date/time readiness and startup trust are related but not the same thing. + +Trust answers: +- "Do we believe the coordinate basis is valid?" + +Date/time answers: +- "Can we safely/usefully perform sky-referenced operations right now?" + +So a mount may have trust while date/time is still not ready. + +In that state: +- the coordinate basis may still be considered authoritative +- but some operations may remain postponed or blocked until date/time is ready + +For mounts using trusted coordinate memory restore or trusted paired absolute +startup position: +- once date/time becomes ready, limit enforcement can be enabled immediately +- this does not require a later goto, reset/home, or unpark just to make + limits active +- that in turn allows tracking requests to succeed normally under the usual + limit-safety policy + +Example: +- unpark recovery may wait for the normal site/time flow even when trust is + already valid + +## Workflow 5: Losing Trust During Runtime + +If trust is lost after startup: +- coordinate-based automatic motion stops being allowed +- manual guide recovery remains available +- sidereal tracking is still not gated by startup authority itself, but it + remains subject to the normal tracking safety policy + +The guiding rule is: +- if the system knows where it is, coordinate-based automatic motion is allowed +- if it does not know where it is, coordinate-based automatic motion is blocked +- guide-based manual recovery remains available + +## Important startup workflows to remember + +1. Basic mount with clutches, no absolute encoders, no coordinate memory: +- common user workflow is manually place at home, power on, then goto/sync +- SA_AUTO preserves this by granting session trust at boot +- SA_STRICT intentionally blocks this until an authoritative action succeeds + +2. Mount without coordinate memory and without clutches: +- mount may power up far from home +- user must guide back to home +- then perform an authoritative home/reset action +- then begin normal operation +- current model fits this workflow well + +3. User wants only sidereal tracking at startup: +- trust does not gate sidereal tracking by itself +- but default safety behavior still disallows tracking when limits are not + enforced +- this is intentional and should remain so + +## NV policy + +The compile-time switch: +- NV_INIT_ERROR_REVOKES_AUTHORITY + +Default: +- ON + +Meaning: +- if NV reports an init/read fault that may undermine coordinate or motion + confidence, trust is not granted or is revoked + +This is intentionally strict, but it has an escape hatch for expert use. + +## Status exposure + +The trust state is exposed in :Gu# so clients can detect it without extra +status calls. + +Longer term, a small circular event log is probably a better place to record +why trust changed than trying to pack too much into live status. + +## Open future work + +- circular timestamped event log for trust loss/recovery reasons +- possibly record a simple trust-loss reason code +- maybe future fault-tolerant KV/partition design +- refine the basic clutch/manual-home startup case so normal users are not + surprised while still preserving the stricter safety model for systems with + stronger authority sources diff --git a/juwei_gyari_backup.bin b/juwei_gyari_backup.bin new file mode 100755 index 0000000..4147f7a Binary files /dev/null and b/juwei_gyari_backup.bin differ diff --git a/src/Common.h b/src/Common.h new file mode 100755 index 0000000..8f3f979 --- /dev/null +++ b/src/Common.h @@ -0,0 +1,76 @@ +// ----------------------------------------------------------------------------------- +// Common includes +#pragma once + +#define SERIAL_LOCAL_MODE ON +#define STANDARD_IPSERIAL_CHANNEL ON +#define PERSISTENT_IPSERIAL_CHANNEL ON + +#include +#include "Constants.h" +#include "lib/Constants.h" +#include "../Config.h" +#include "Config.defaults.h" + +#ifdef USES_HW_SPI +#include +#endif + +#include "HAL/HAL.h" +#include "lib/Macros.h" +#include "pinmaps/Models.h" +#include "lib/debug/Debug.h" + +#if ST4_HAND_CONTROL == ON + #define SERIAL_ST4_MASTER ON +#endif + +#if AXIS1_DRIVER_MODEL != OFF && AXIS2_DRIVER_MODEL != OFF + #define MOUNT_PRESENT +#endif + +#if AXIS3_DRIVER_MODEL != OFF + #define ROTATOR_PRESENT +#endif + +#if AXIS4_DRIVER_MODEL != OFF || AXIS5_DRIVER_MODEL != OFF || AXIS6_DRIVER_MODEL != OFF || AXIS7_DRIVER_MODEL != OFF || AXIS8_DRIVER_MODEL != OFF || AXIS9_DRIVER_MODEL != OFF + #define FOCUSER_PRESENT +#endif + +#if FEATURE1_PURPOSE != OFF || FEATURE2_PURPOSE != OFF || FEATURE3_PURPOSE != OFF || FEATURE4_PURPOSE != OFF || FEATURE5_PURPOSE != OFF || FEATURE6_PURPOSE != OFF || FEATURE7_PURPOSE != OFF || FEATURE8_PURPOSE != OFF + #define FEATURES_PRESENT +#endif + +#if FEATURE1_PURPOSE == DEW_HEATER || FEATURE2_PURPOSE == DEW_HEATER || FEATURE3_PURPOSE == DEW_HEATER || FEATURE4_PURPOSE == DEW_HEATER || FEATURE5_PURPOSE == DEW_HEATER || FEATURE6_PURPOSE == DEW_HEATER || FEATURE7_PURPOSE == DEW_HEATER || FEATURE8_PURPOSE == DEW_HEATER + #define DEW_HEATER_PRESENT + + #if (FEATURE1_TEMP & DS_MASK) == DS1820 || (FEATURE2_TEMP & DS_MASK) == DS1820 || (FEATURE3_TEMP & DS_MASK) == DS1820 || (FEATURE4_TEMP & DS_MASK) == DS1820 || \ + (FEATURE5_TEMP & DS_MASK) == DS1820 || (FEATURE6_TEMP & DS_MASK) == DS1820 || (FEATURE7_TEMP & DS_MASK) == DS1820 || (FEATURE8_TEMP & DS_MASK) == DS1820 || \ + (FEATURE1_TEMP & DS_MASK) == DS18S20 || (FEATURE2_TEMP & DS_MASK) == DS18S20 || (FEATURE3_TEMP & DS_MASK) == DS18S20 || (FEATURE4_TEMP & DS_MASK) == DS18S20 || \ + (FEATURE5_TEMP & DS_MASK) == DS18S20 || (FEATURE6_TEMP & DS_MASK) == DS18S20 || (FEATURE7_TEMP & DS_MASK) == DS18S20 || (FEATURE8_TEMP & DS_MASK) == DS18S20 + #define DS1820_DEVICES_PRESENT + #endif + + #if FEATURE1_TEMP == THERMISTOR1 || FEATURE2_TEMP == THERMISTOR1 || FEATURE3_TEMP == THERMISTOR1 || FEATURE4_TEMP == THERMISTOR1 || \ + FEATURE5_TEMP == THERMISTOR1 || FEATURE6_TEMP == THERMISTOR1 || FEATURE7_TEMP == THERMISTOR1 || FEATURE8_TEMP == THERMISTOR1 || \ + FEATURE1_TEMP == THERMISTOR2 || FEATURE2_TEMP == THERMISTOR2 || FEATURE3_TEMP == THERMISTOR2 || FEATURE4_TEMP == THERMISTOR2 || \ + FEATURE5_TEMP == THERMISTOR2 || FEATURE6_TEMP == THERMISTOR2 || FEATURE7_TEMP == THERMISTOR2 || FEATURE8_TEMP == THERMISTOR2 + #define THERMISTOR_DEVICES_PRESENT + #endif +#endif + +#if FEATURE1_PURPOSE == COVER_SWITCH || FEATURE2_PURPOSE == COVER_SWITCH || FEATURE3_PURPOSE == COVER_SWITCH || FEATURE4_PURPOSE == COVER_SWITCH || FEATURE5_PURPOSE == COVER_SWITCH || FEATURE6_PURPOSE == COVER_SWITCH || FEATURE7_PURPOSE == COVER_SWITCH || FEATURE8_PURPOSE == COVER_SWITCH + #define COVER_SWITCH_SERVO_PRESENT +#endif + +#if (FOCUSER_TEMPERATURE & DS_MASK) == DS1820 || (FOCUSER_TEMPERATURE & DS_MASK) == DS18S20 + #ifndef DS1820_DEVICES_PRESENT + #define DS1820_DEVICES_PRESENT + #endif +#endif + +#if FOCUSER_TEMPERATURE == THERMISTOR1 || FOCUSER_TEMPERATURE == THERMISTOR2 + #ifndef THERMISTOR_DEVICES_PRESENT + #define THERMISTOR_DEVICES_PRESENT + #endif +#endif diff --git a/src/Config.defaults.h b/src/Config.defaults.h new file mode 100755 index 0000000..fe687ff --- /dev/null +++ b/src/Config.defaults.h @@ -0,0 +1,2460 @@ +// ----------------------------------------------------------------------------------- +// controller settings +#pragma once + +// host name for this microcontroller, by default used for the following if enabled/supported: +// PRODUCT_DESCRIPTION the user friendly name for this device, appears on websites etc. +// HOST_NAME the name ESP WiFi provides to any DHCP server (Ethernet doesn't support this) +// MDNS_NAME the name mDNS (Multicast DNS) clients see for IP address resolution +// AP_SSID the SSID WiFi clients see when the ESP WiFi Soft Access Point is enabled +// SERIAL_BT_NAME the name Bluetooth Servers see when the ESP32 Bluetooth client is enabled +#ifndef HOST_NAME +#define HOST_NAME "OnStep" +#endif + +// settings identification +#ifndef PRODUCT_DESCRIPTION +#define PRODUCT_DESCRIPTION HOST_NAME +#endif + +// use the HAL specified default NV driver +#ifndef NV_DRIVER +#define NV_DRIVER NV_DEFAULT +#endif + +// pinmap +#ifndef PINMAP +#define PINMAP OFF +#endif + +// debug +#ifndef DEBUG +#define DEBUG OFF +#endif +#ifndef DEBUG_ECHO_COMMANDS +#define DEBUG_ECHO_COMMANDS OFF +#endif +#ifndef SERIAL_DEBUG +#define SERIAL_DEBUG Serial +#endif +#ifndef SERIAL_DEBUG_BAUD +#define SERIAL_DEBUG_BAUD 9600 +#endif + +// flag hardware SPI as active +#ifdef DRIVER_TMC_STEPPER_HW_SPI +#ifndef DRIVER_TMC_STEPPER +#define DRIVER_TMC_STEPPER +#endif +#define USES_HW_SPI +#endif + +// serial ports +#ifndef SERIAL_A_BAUD_DEFAULT +#define SERIAL_A_BAUD_DEFAULT 9600 +#endif +#ifndef SERIAL_B_BAUD_DEFAULT +#define SERIAL_B_BAUD_DEFAULT OFF +#endif +#ifndef SERIAL_B_ESP_FLASHING +#define SERIAL_B_ESP_FLASHING OFF +#endif +#ifndef SERIAL_C_BAUD_DEFAULT +#define SERIAL_C_BAUD_DEFAULT OFF +#endif +#ifndef SERIAL_D_BAUD_DEFAULT +#define SERIAL_D_BAUD_DEFAULT OFF +#endif +#ifndef SERIAL_E_BAUD_DEFAULT +#define SERIAL_E_BAUD_DEFAULT OFF +#endif +#ifndef SERIAL_GPS_BAUD +#define SERIAL_GPS_BAUD 9600 +#endif + +// ESP32 automatically set wifi radio for bluetooth or IP modes +#ifdef SERIAL_RADIO +#if SERIAL_RADIO == BLUETOOTH +#define SERIAL_BT_MODE SLAVE +#elif SERIAL_RADIO == WIFI_ACCESS_POINT +#define SERIAL_IP_MODE WIFI_ACCESS_POINT +#define WEB_SERVER ON +#elif SERIAL_RADIO == WIFI_STATION +#define SERIAL_IP_MODE WIFI_STATION +#define WEB_SERVER ON +#endif +#endif + +// ESP32 virtual serial bluetooth command channel +#ifndef SERIAL_BT_MODE +#define SERIAL_BT_MODE OFF // use SLAVE to enable the interface (ESP32 only) +#endif +#ifndef SERIAL_BT_NAME +#define SERIAL_BT_NAME HOST_NAME // Bluetooth name of command channel +#endif +#ifndef SERIAL_BT_PASSKEY +#define SERIAL_BT_PASSKEY "" // Bluetooth four digit passkey +#endif + +// enable and customize WiFi functionality +// for other default IP settings see the file: +// src/lib/wifi/WifiManager.defaults.h + +#ifndef MDNS_SERVER +#define MDNS_SERVER ON +#endif + +#ifndef MDNS_NAME +#define MDNS_NAME HOST_NAME +#endif + +// translate Config.h IP settings into low level library settings +#ifndef SERIAL_IP_MODE +#define SERIAL_IP_MODE OFF // use settings shown below to enable the interface +#endif +#ifndef SERIAL_SERVER +#define SERIAL_SERVER BOTH // STANDARD (port 9999) or PERSISTENT (ports 9996 to 9998) +#endif + +#if SERIAL_IP_MODE == ETHERNET_W5500 +#define OPERATIONAL_MODE ETHERNET_W5500 +#elif SERIAL_IP_MODE == ETHERNET_W5100 +#define OPERATIONAL_MODE ETHERNET_W5100 +#elif SERIAL_IP_MODE == WIFI_ACCESS_POINT +#define OPERATIONAL_MODE WIFI +#define AP_ENABLED true +#elif SERIAL_IP_MODE == WIFI_STATION +#define OPERATIONAL_MODE WIFI +#define STA_ENABLED true +#elif SERIAL_IP_MODE == BOTH +#define OPERATIONAL_MODE WIFI +#define AP_ENABLED true +#define STA_ENABLED true +#endif + +#if OPERATIONAL_MODE == WIFI +#ifndef NV_WIFI_SETTINGS +#define NV_WIFI_SETTINGS // allow NV storage of WiFi settings +#endif +#endif + +#ifndef STA_AP_FALLBACK +#define STA_AP_FALLBACK true // activate SoftAP if station fails to connect +#endif + +#ifndef STA_AUTO_RECONNECT +#define STA_AUTO_RECONNECT true // automatically reconnect if connection is dropped +#endif + +#ifndef TIME_IP_ADDR +#define TIME_IP_ADDR {129,6,15,28} // for NTP if enabled we often use an address like +#endif // time-a-g.nist.gov at 129,6,15,28 or 129,6,15,29, 129,6,15,30, etc. + +// some CAN defaults +#ifndef CAN_PLUS +#define CAN_PLUS OFF // Select from CAN_SAN, CAN_ESP32, CAN_MCP2515, CANn_TEENSY4 +#endif +#ifndef CAN_BAUD +#define CAN_BAUD 1000000 // 1000000 baud default +#endif +#ifndef CAN_SEND_RATE_MS +#define CAN_SEND_RATE_MS 10 // 100 Hz CAN controller send message processing rate +#endif +#ifndef CAN_RECV_RATE_MS +#define CAN_RECV_RATE_MS 10 // 100 Hz CAN controller recv message processing rate +#endif +#ifndef CAN_RX_PIN +#define CAN_RX_PIN OFF // for ESP32 CAN interface +#endif +#ifndef CAN_TX_PIN +#define CAN_TX_PIN OFF // for ESP32 CAN interface +#endif +#ifndef CAN_CS_PIN +#define CAN_CS_PIN OFF // for MCP2515 SPI CAN controller +#endif +#ifndef CAN_INT_PIN +#define CAN_INT_PIN OFF // for MCP2515 SPI CAN controller +#endif + +// sensors +#ifndef WEATHER +#define WEATHER OFF +#endif + +// step signal +#ifndef STEP_WAVE_FORM +#define STEP_WAVE_FORM PULSE +#endif + +#if AXIS1_STEP_STATE == AXIS2_STEP_STATE == AXIS3_STEP_STATE == \ + AXIS4_STEP_STATE == AXIS5_STEP_STATE == AXIS6_STEP_STATE == \ + AXIS7_STEP_STATE == AXIS8_STEP_STATE == AXIS9_STEP_STATE == HIGH + #define DRIVER_STEP_DEFAULTS +#endif + +// gpio device +// DS2413: for 2-ch or 4-ch using 1-wire gpio's (one or two devices.) +// SWS: for 8-ch Serial gpio (normally 4 unused encoder pins.) +// MCP23008: for 8-ch I2C gpio. +// MCP23017, X9555, or X8575: for 16-ch I2C gpio. +// SSR74HC595: for up to 32-ch gpio (serial shift register, output only.) +// Works w/most OnStep features, channels assigned in order pin# 512 and up. +#ifndef GPIO_DEVICE +#define GPIO_DEVICE OFF +#endif + +#ifndef FileVersionConfig +#warning "Configuration (Config.h): FileVersionConfig is undefined, assuming version 5." +#define FileVersionConfig 5 +#endif + +// shared driver enable pin configuration +#ifndef SHARED_ENABLE_STATE +#define SHARED_ENABLE_STATE LOW // default state of shared ENable pin for motor power on +#endif +#ifndef SHARED2_ENABLE_STATE +#define SHARED2_ENABLE_STATE LOW +#endif +#ifndef SHARED3_ENABLE_STATE +#define SHARED3_ENABLE_STATE LOW +#endif + +// ----------------------------------------------------------------------------------- +// mount settings + +// axes +#ifndef AXIS1_DRIVER_MODEL +#define AXIS1_DRIVER_MODEL OFF // specify a driver model to enable +#endif +#ifndef AXIS1_STEPS_PER_DEGREE +#define AXIS1_STEPS_PER_DEGREE 12800.0 // (micro)steps per degree +#endif +#ifndef AXIS1_REVERSE +#define AXIS1_REVERSE OFF // reverse rotation direction +#endif +#ifndef AXIS1_POWER_DOWN +#define AXIS1_POWER_DOWN OFF // power down if at standstill +#endif +#ifndef AXIS1_POWER_DOWN_TIME +#define AXIS1_POWER_DOWN_TIME 30000 // power down time in milliseconds +#endif +#ifndef AXIS1_ENABLE_STATE +#define AXIS1_ENABLE_STATE LOW // default state of ENable pin for motor power on +#endif +#ifndef AXIS1_LIMIT_MIN +#define AXIS1_LIMIT_MIN -180 // in degrees +#endif +#ifndef AXIS1_LIMIT_MAX +#define AXIS1_LIMIT_MAX 180 // in degrees +#endif +#ifndef AXIS1_LIMIT_SYNC +#define AXIS1_LIMIT_SYNC OFF // sync limit in degrees, or OFF +#endif +#ifndef AXIS1_SENSE_HOME +#define AXIS1_SENSE_HOME OFF // HIGH or LOW state when clockwise of home position, seen from front +#endif +#ifndef AXIS1_SENSE_HOME_INIT +#define AXIS1_SENSE_HOME_INIT INPUT_PULLUP // pin mode for home sensing +#endif +#ifndef AXIS1_SENSE_HOME_OFFSET +#define AXIS1_SENSE_HOME_OFFSET 0 // default offset in arc-seconds to home from the sense position +#endif +#ifndef AXIS1_SENSE_HOME_DIST_LIMIT +#define AXIS1_SENSE_HOME_DIST_LIMIT 180.0 // max distance in degrees +#endif +#ifndef AXIS1_SENSE_LIMIT_MIN +#define AXIS1_SENSE_LIMIT_MIN LIMIT_SENSE // HIGH/LOW to activate min limit +#endif +#ifndef AXIS1_SENSE_LIMIT_MAX +#define AXIS1_SENSE_LIMIT_MAX LIMIT_SENSE // HIGH/LOW to activate max limit +#endif +#ifndef AXIS1_SENSE_LIMIT_INIT +#define AXIS1_SENSE_LIMIT_INIT LIMIT_SENSE_INIT // pin mode for limit sensing +#endif + +#if AXIS1_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS1_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS1_STEP_DIR_PRESENT + + #ifndef AXIS1_STEP_STATE + #define AXIS1_STEP_STATE HIGH // default signal transition state for a step + #endif + #ifndef AXIS1_M2_ON_STATE + #define AXIS1_M2_ON_STATE HIGH // default ON state for M2 + #endif + #ifndef AXIS1_DRIVER_MICROSTEPS + #define AXIS1_DRIVER_MICROSTEPS OFF // normal microstep mode + #endif + #ifndef AXIS1_DRIVER_MICROSTEPS_GOTO + #define AXIS1_DRIVER_MICROSTEPS_GOTO OFF // microstep mode to use during slews + #endif + #ifndef AXIS1_DRIVER_INTPOL + #define AXIS1_DRIVER_INTPOL ON // use interpolation if available + #endif + #ifndef AXIS1_DRIVER_DECAY + #define AXIS1_DRIVER_DECAY OFF // OFF for default, TMC STEALTHCHOP + #endif + #ifndef AXIS1_DRIVER_DECAY_GOTO + #define AXIS1_DRIVER_DECAY_GOTO OFF // OFF for default, TMC SPREADCYCLE + #endif + #ifndef AXIS1_DRIVER_IHOLD + #define AXIS1_DRIVER_IHOLD OFF // in mA + #endif + #ifndef AXIS1_DRIVER_IRUN + #define AXIS1_DRIVER_IRUN OFF // in mA + #endif + #ifndef AXIS1_DRIVER_IGOTO + #define AXIS1_DRIVER_IGOTO OFF // in mA + #endif + #ifndef AXIS1_DRIVER_STATUS + #define AXIS1_DRIVER_STATUS OFF // driver status reporting (ON for TMC SPI or HIGH/LOW for fault pin) + #endif +#endif + +#if AXIS1_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS1_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS1_SERVO_PRESENT + + #ifndef AXIS1_SERVO_PH1_STATE + #define AXIS1_SERVO_PH1_STATE LOW // default (inactive) motor driver state, IN1 (SERVO_EE) or PHASE (SERVO_PE) pin + #endif + #ifndef AXIS1_SERVO_PH2_STATE + #define AXIS1_SERVO_PH2_STATE LOW // default (inactive) motor driver state, IN2 or ENABLE (pwm) pin + #endif + + #ifndef AXIS1_SERVO_ACCELERATION + #define AXIS1_SERVO_ACCELERATION 100 // acceleration limit, in %/s + #endif + #ifndef AXIS1_SERVO_VELOCITY_PWMTHRS + #define AXIS1_SERVO_VELOCITY_PWMTHRS OFF // velocity (in steps/s) to switch from stealthChop to spreadCycle + #endif // should happen at the 2x sidereal rate, for TMC2209/TMC5160 only + #ifndef AXIS1_MOTOR_STEPS_PER_DEGREE + #define AXIS1_MOTOR_STEPS_PER_DEGREE AXIS1_STEPS_PER_DEGREE // for calculating ratio of encoder counts/deg to motor steps/deg + #endif // for TMC stepper motors only + #ifndef AXIS1_SERVO_DC_PWR_MIN + #define AXIS1_SERVO_DC_PWR_MIN 0.0F // minimum power just below where axis motion starts, in % + #endif // approximate, for DC motors only + #ifndef AXIS1_SERVO_DC_PWR_MAX + #define AXIS1_SERVO_DC_PWR_MAX 100.0F // power required for fastest slew rate, in % + #endif // approximate, for DC motors only + + #ifndef AXIS1_SERVO_FEEDBACK + #define AXIS1_SERVO_FEEDBACK DUAL_PID // type of feedback: DUAL_PID + #endif + #ifndef AXIS1_PID_SENSITIVITY + #define AXIS1_PID_SENSITIVITY 0 // 0 to use slewing state, or % power for 100% pid set two (_GOTO) + #endif + #ifndef AXIS1_PID_P + #define AXIS1_PID_P 0.0 // P = proportional + #endif + #ifndef AXIS1_PID_I + #define AXIS1_PID_I 0.0 // I = integral + #endif + #ifndef AXIS1_PID_D + #define AXIS1_PID_D 0.0 // D = derivative + #endif + #ifndef AXIS1_PID_P_GOTO + #define AXIS1_PID_P_GOTO AXIS1_PID_P // P = proportional + #endif + #ifndef AXIS1_PID_I_GOTO + #define AXIS1_PID_I_GOTO AXIS1_PID_I // I = integral + #endif + #ifndef AXIS1_PID_D_GOTO + #define AXIS1_PID_D_GOTO AXIS1_PID_D // D = derivative + #endif + + #ifndef AXIS1_SERVO_FLTR + #define AXIS1_SERVO_FLTR OFF // servo encoder filter: OFF + #endif + #ifndef AXIS1_ENCODER + #define AXIS1_ENCODER AB // type of encoder: AB, CW_CCW, PULSE_DIR, PULSE_ONLY, SERIAL_BRIDGE + #endif + #ifndef AXIS1_ENCODER_ORIGIN + #define AXIS1_ENCODER_ORIGIN 0 // +/- offset so encoder count is 0 at home (for absolute encoders) + #endif + #ifndef AXIS1_ENCODER_REVERSE + #define AXIS1_ENCODER_REVERSE OFF // reverse count direction of encoder + #endif +#endif + +#if AXIS1_DRIVER_MODEL == ODRIVE + #define AXIS1_ODRIVE_PRESENT + #ifndef AXIS1_ODRIVE_P + #define AXIS1_ODRIVE_P 2.0 // P = proportional + #endif + #ifndef AXIS1_ODRIVE_I + #define AXIS1_ODRIVE_I 5.0 // I = integral + #endif + #ifndef AXIS1_ODRIVE_D + #define AXIS1_ODRIVE_D 1.0 // D = derivative + #endif +#endif + +#if AXIS1_DRIVER_MODEL == KTECH + #define AXIS1_KTECH_PRESENT +#endif + +#if AXIS1_DRIVER_MODEL == MKS42D + #define AXIS1_MKS42D_PRESENT +#endif + +#ifndef AXIS2_DRIVER_MODEL +#define AXIS2_DRIVER_MODEL OFF // specify a driver to enable +#endif +#ifndef AXIS2_STEPS_PER_DEGREE +#define AXIS2_STEPS_PER_DEGREE 12800.0 // (micro)steps per degree of movement +#endif +#ifndef AXIS2_REVERSE +#define AXIS2_REVERSE OFF // reverse movement +#endif +#ifndef AXIS2_POWER_DOWN +#define AXIS2_POWER_DOWN OFF // power down at standstill +#endif +#ifndef AXIS2_POWER_DOWN_TIME +#define AXIS2_POWER_DOWN_TIME 30000 // power off time in milliseconds +#endif +#ifndef AXIS2_ENABLE_STATE +#define AXIS2_ENABLE_STATE LOW // stepper enable state +#endif +#ifndef AXIS2_LIMIT_MIN +#define AXIS2_LIMIT_MIN -90 // in degrees +#endif +#ifndef AXIS2_LIMIT_MAX +#define AXIS2_LIMIT_MAX 90 // in degrees +#endif +#ifndef AXIS2_LIMIT_SYNC +#define AXIS2_LIMIT_SYNC OFF // sync limit in degrees, or OFF +#endif +#ifndef AXIS2_SENSE_HOME +#define AXIS2_SENSE_HOME OFF // HIGH or LOW state when clockwise of home position, seen from above +#endif +#ifndef AXIS2_SENSE_HOME_INIT +#define AXIS2_SENSE_HOME_INIT INPUT_PULLUP // pin mode for home sensing +#endif +#ifndef AXIS2_SENSE_HOME_OFFSET +#define AXIS2_SENSE_HOME_OFFSET 0 // default offset in arc-seconds to home from the sense position +#endif +#ifndef AXIS2_SENSE_HOME_DIST_LIMIT +#define AXIS2_SENSE_HOME_DIST_LIMIT 180.0 // max distance in degrees +#endif +#ifndef AXIS2_SENSE_LIMIT_MIN +#define AXIS2_SENSE_LIMIT_MIN LIMIT_SENSE // HIGH/LOW to activate min limit +#endif +#ifndef AXIS2_SENSE_LIMIT_MAX +#define AXIS2_SENSE_LIMIT_MAX LIMIT_SENSE // HIGH/LOW to activate max limit +#endif +#ifndef AXIS2_SENSE_LIMIT_INIT +#define AXIS2_SENSE_LIMIT_INIT LIMIT_SENSE_INIT // pin mode for limit sensing +#endif + +#if AXIS2_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS2_STEP_DIR_PRESENT + + #ifndef AXIS2_STEP_STATE + #define AXIS2_STEP_STATE HIGH + #endif + #ifndef AXIS2_M2_ON_STATE + #define AXIS2_M2_ON_STATE HIGH + #endif + #ifndef AXIS2_DRIVER_MICROSTEPS + #define AXIS2_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS2_DRIVER_MICROSTEPS_GOTO + #define AXIS2_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS2_DRIVER_INTPOL + #define AXIS2_DRIVER_INTPOL ON + #endif + #ifndef AXIS2_DRIVER_DECAY + #define AXIS2_DRIVER_DECAY OFF + #endif + #ifndef AXIS2_DRIVER_DECAY_GOTO + #define AXIS2_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS2_DRIVER_IHOLD + #define AXIS2_DRIVER_IHOLD OFF + #endif + #ifndef AXIS2_DRIVER_IRUN + #define AXIS2_DRIVER_IRUN OFF + #endif + #ifndef AXIS2_DRIVER_IGOTO + #define AXIS2_DRIVER_IGOTO OFF + #endif + #ifndef AXIS2_DRIVER_STATUS + #define AXIS2_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS2_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS2_SERVO_PRESENT + + #ifndef AXIS2_SERVO_PH1_STATE + #define AXIS2_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS2_SERVO_PH2_STATE + #define AXIS2_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS2_SERVO_ACCELERATION + #define AXIS2_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS2_SERVO_VELOCITY_PWMTHRS + #define AXIS2_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS2_MOTOR_STEPS_PER_DEGREE + #define AXIS2_MOTOR_STEPS_PER_DEGREE AXIS2_STEPS_PER_DEGREE + #endif + #ifndef AXIS2_SERVO_DC_PWR_MIN + #define AXIS2_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS2_SERVO_DC_PWR_MAX + #define AXIS2_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS2_SERVO_FEEDBACK + #define AXIS2_SERVO_FEEDBACK DUAL_PID + #endif + #ifndef AXIS2_PID_SENSITIVITY + #define AXIS2_PID_SENSITIVITY 0 + #endif + #ifndef AXIS2_PID_P + #define AXIS2_PID_P 0.0 + #endif + #ifndef AXIS2_PID_I + #define AXIS2_PID_I 0.0 + #endif + #ifndef AXIS2_PID_D + #define AXIS2_PID_D 0.0 + #endif + #ifndef AXIS2_PID_P_GOTO + #define AXIS2_PID_P_GOTO AXIS2_PID_P + #endif + #ifndef AXIS2_PID_I_GOTO + #define AXIS2_PID_I_GOTO AXIS2_PID_I + #endif + #ifndef AXIS2_PID_D_GOTO + #define AXIS2_PID_D_GOTO AXIS2_PID_D + #endif + + #ifndef AXIS2_SERVO_FLTR + #define AXIS2_SERVO_FLTR OFF + #endif + #ifndef AXIS2_ENCODER + #define AXIS2_ENCODER AB + #endif + #ifndef AXIS2_ENCODER_ORIGIN + #define AXIS2_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS2_ENCODER_REVERSE + #define AXIS2_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS2_DRIVER_MODEL == ODRIVE + #define AXIS2_ODRIVE_PRESENT + #ifndef AXIS2_ODRIVE_P + #define AXIS2_ODRIVE_P 2.0 // P = proportional + #endif + #ifndef AXIS2_ODRIVE_I + #define AXIS2_ODRIVE_I 5.0 // I = integral + #endif + #ifndef AXIS2_ODRIVE_D + #define AXIS2_ODRIVE_D 1.0 // D = derivative + #endif +#endif + +#if AXIS2_DRIVER_MODEL == KTECH + #define AXIS2_KTECH_PRESENT +#endif + +#if AXIS2_DRIVER_MODEL == MKS42D + #define AXIS2_MKS42D_PRESENT +#endif + +// decode internal mount type, tangent arm, azm wrap +#ifndef MOUNT_TYPE +#define MOUNT_TYPE GEM +#endif +#if MOUNT_TYPE == GEM +#define MOUNT_SUBTYPE GEM +#endif +#if MOUNT_TYPE == GEM_TA +#define MOUNT_SUBTYPE GEM +#define AXIS2_TANGENT_ARM ON +#endif +#if MOUNT_TYPE == GEM_TAC +#define MOUNT_SUBTYPE GEM +#define AXIS2_TANGENT_ARM ON +#define AXIS2_TANGENT_ARM_CORRECTION ON +#endif +#if MOUNT_TYPE == FORK +#define MOUNT_SUBTYPE FORK +#endif +#if MOUNT_TYPE == FORK_TA +#define MOUNT_SUBTYPE FORK +#define AXIS2_TANGENT_ARM ON +#endif +#if MOUNT_TYPE == FORK_TAC +#define MOUNT_SUBTYPE FORK +#define AXIS2_TANGENT_ARM ON +#define AXIS2_TANGENT_ARM_CORRECTION ON +#endif +#if MOUNT_TYPE == ALTALT +#define MOUNT_SUBTYPE ALTALT +#endif +#if MOUNT_TYPE == ALTAZM +#define MOUNT_SUBTYPE ALTAZM +#endif +#if MOUNT_TYPE == ALTAZM_UNL +#define MOUNT_SUBTYPE ALTAZM +#define AXIS1_WRAP ON +#endif + +#ifndef MOUNT_SUBTYPE +#define MOUNT_SUBTYPE OFF +#endif +#ifndef MOUNT_ALTERNATE_ORIENTATION +#define MOUNT_ALTERNATE_ORIENTATION OFF // use ON or OFF, for FORK and ALTAZM mounts only +#endif +#ifndef MOUNT_COORDS +#define MOUNT_COORDS TOPOCENTRIC // mount coordinate system +#endif +#ifndef MOUNT_COORDS_MEMORY +#define MOUNT_COORDS_MEMORY OFF // ON Enables mount position memory +#endif +// SA_STRICT requires an authority source +// SA_AUTO grants legacy session trust only when neither paired absolute authority nor coordinate memory are configured +// SA_PERMISSIVE always grants boot trust +#ifndef MOUNT_STARTUP_MODE +#define MOUNT_STARTUP_MODE SA_AUTO +#endif +#ifndef NV_INIT_ERROR_REVOKES_AUTHORITY +#define NV_INIT_ERROR_REVOKES_AUTHORITY ON // ON revokes startup authority trust if NV reports an init/read fault +#endif +#ifndef MOUNT_ENABLE_IN_STANDBY +#define MOUNT_ENABLE_IN_STANDBY OFF // ON Enables mount motor drivers in standby +#endif +#ifndef MOUNT_AUTO_HOME_DEFAULT +#define MOUNT_AUTO_HOME_DEFAULT OFF // ON default find home at boot +#endif +#ifndef MOUNT_HOME_AT_OFFSETS +#define MOUNT_HOME_AT_OFFSETS OFF // ON to incorporate runtime offsets into home position (switches) +#endif +#ifndef MOUNT_HORIZON_AVOIDANCE +#define MOUNT_HORIZON_AVOIDANCE ON // ON allows eq mode horizon avoidance +#endif + +#ifndef AXIS1_TARGET_TOLERANCE +#define AXIS1_TARGET_TOLERANCE 0.0F // distance in arc-seconds when goto is at destination +#endif +#ifndef AXIS1_HOME_TOLERANCE // distance in arc-seconds when at home +#define AXIS1_HOME_TOLERANCE AXIS1_TARGET_TOLERANCE + (1800.0/AXIS1_STEPS_PER_DEGREE) +#endif +#ifndef AXIS1_SECTOR_GEAR +#define AXIS1_SECTOR_GEAR OFF // special case of a Sector Gear RA with limited travel +#endif +#ifndef AXIS1_WRAP +#define AXIS1_WRAP OFF // allow unlimited motion about an Azimuth axis +#endif + +#ifndef AXIS2_TARGET_TOLERANCE +#define AXIS2_TARGET_TOLERANCE 0.0F // in arc-seconds +#endif +#ifndef AXIS2_HOME_TOLERANCE // in arc-seconds +#define AXIS2_HOME_TOLERANCE AXIS2_TARGET_TOLERANCE + (1800.0/AXIS2_STEPS_PER_DEGREE) +#endif +#ifndef AXIS2_TANGENT_ARM +#define AXIS2_TANGENT_ARM OFF // special case of a Tangent Arm Dec with limited travel +#endif +#ifndef AXIS2_TANGENT_ARM_CORRECTION +#define AXIS2_TANGENT_ARM_CORRECTION OFF // apply formula below to correct for Tangent Arm geometry +#endif +#ifndef TANGENT_ARM_INSTRUMENT_TO_MOUNT +#define TANGENT_ARM_INSTRUMENT_TO_MOUNT(a) (atan(a)) // returns angle in radians +#endif +#ifndef TANGENT_ARM_MOUNT_TO_INSTRUMENT +#define TANGENT_ARM_MOUNT_TO_INSTRUMENT(a) (tan(a)) // angle (a) in radians +#endif + +// user feedback +#ifndef STATUS_LED +#define STATUS_LED OFF +#endif +#ifndef STATUS_LED_ON_STATE +#define STATUS_LED_ON_STATE LOW +#endif +#ifndef STATUS_MOUNT_LED +#define STATUS_MOUNT_LED OFF +#endif +#ifndef MOUNT_LED_ON_STATE +#define MOUNT_LED_ON_STATE LOW +#endif +#ifndef STATUS_BUZZER +#define STATUS_BUZZER OFF +#endif +#ifndef STATUS_BUZZER_ON_STATE +#define STATUS_BUZZER_ON_STATE HIGH +#endif +#ifndef STATUS_BUZZER_DEFAULT +#define STATUS_BUZZER_DEFAULT OFF +#endif +#ifndef STATUS_BUZZER_MEMORY +#define STATUS_BUZZER_MEMORY OFF +#endif + +// reticle +#ifndef RETICLE_LED_DEFAULT +#define RETICLE_LED_DEFAULT OFF +#endif +#ifndef RETICLE_LED_MEMORY +#define RETICLE_LED_MEMORY OFF +#endif +#ifndef RETICLE_LED_INVERT +#define RETICLE_LED_INVERT OFF +#endif + +// time and location +#ifndef TIME_LOCATION_SOURCE +#define TIME_LOCATION_SOURCE OFF // specify device to get date/time and optionally location +#endif +#ifndef TIME_LOCATION_SOURCE_FALLBACK // alternate TLS, must be differnet than above and not GPS or NTP +#define TIME_LOCATION_SOURCE_FALLBACK OFF +#endif +#ifndef TIME_LOCATION_PPS_SENSE +#define TIME_LOCATION_PPS_SENSE OFF +#endif +#ifndef TIME_LOCATION_PPS_SYNC +#define TIME_LOCATION_PPS_SYNC OFF // adjust timer rates to keep time in sync with PPS +#endif + +// limits +#ifndef LIMIT_SENSE +#define LIMIT_SENSE OFF // applied to AXIS1 and AXIS2 +#endif +#ifndef LIMIT_SENSE_INIT +#define LIMIT_SENSE_INIT INPUT_PULLUP // applied to AXIS1 and AXIS2 +#endif +#ifndef LIMIT_SENSE_STRICT +#define LIMIT_SENSE_STRICT OFF // for all axes, ON blocks motion if min/max are on the same pin +#endif +#ifndef LIMIT_STRICT +#define LIMIT_STRICT ON // ON enables Mount limits at startup if date/time are set +#endif +#ifndef LIMIT_RECOVERY +#define LIMIT_RECOVERY OFF // ON allows 1s for gotos away from horizon, meridian w, and axis1 max limits +#endif +#ifndef LIMIT_RECOVERY_WITH_TRACKING +#define LIMIT_RECOVERY_WITH_TRACKING OFF // ON to automatically enable tracking on limit recovery +#endif + +// st4 +#ifndef ST4_INTERFACE +#define ST4_INTERFACE OFF +#endif +#ifndef ST4_INTERFACE_INIT +#define ST4_INTERFACE_INIT INPUT_PULLUP +#endif +#ifndef ST4_HAND_CONTROL +#define ST4_HAND_CONTROL OFF +#endif +#ifndef ST4_HAND_CONTROL_FOCUSER +#define ST4_HAND_CONTROL_FOCUSER OFF +#endif + +// park +#ifndef PARK_SENSE +#define PARK_SENSE OFF +#endif +#ifndef PARK_SENSE_INIT +#define PARK_SENSE_INIT INPUT_PULLUP +#endif + +#ifndef PARK_SIGNAL +#define PARK_SIGNAL OFF +#endif +#ifndef PARK_SIGNAL_INIT +#define PARK_SIGNAL_INIT INPUT_PULLUP +#endif + +#ifndef PARK_STATUS +#define PARK_STATUS OFF +#endif + +// pec +#ifndef PEC_STEPS_PER_WORM_ROTATION +#define PEC_STEPS_PER_WORM_ROTATION 0 +#endif +#ifndef PEC_BUFFER_SIZE_LIMIT +#define PEC_BUFFER_SIZE_LIMIT 720 // fixed PEC buffer maximum size +#endif +#ifndef PEC_SENSE +#define PEC_SENSE OFF +#endif +#ifndef PEC_SENSE_INIT +#define PEC_SENSE_INIT INPUT_PULLUP +#endif + +// guiding +#ifndef GUIDE_TIME_LIMIT +#define GUIDE_TIME_LIMIT 0 // in seconds, 0 to disable +#endif +#ifndef GUIDE_DISABLE_BACKLASH +#define GUIDE_DISABLE_BACKLASH OFF // disables backlash while pulse-guiding +#endif +#ifndef GUIDE_SEPARATE_PULSE_RATE +#define GUIDE_SEPARATE_PULSE_RATE ON // normally always enabled +#endif + +// tracking +#ifndef TRACK_AUTOSTART +#define TRACK_AUTOSTART OFF // automatically begins tracking at startup +#endif +#ifndef TRACK_WITHOUT_LIMITS +#define TRACK_WITHOUT_LIMITS OFF // allows tracking even if limits are disabled +#endif +#ifndef TRACK_COMPENSATION_DEFAULT +#define TRACK_COMPENSATION_DEFAULT OFF // use OFF, REFRACTION, REFRACTION_DUAL, MODEL, MODEL_DUAL +#endif +#ifndef TRACK_COMPENSATION_MEMORY +#define TRACK_COMPENSATION_MEMORY OFF // remembers the last runtime tracking compensation setting +#endif +#ifndef TRACK_BACKLASH_RATE +#define TRACK_BACKLASH_RATE 25 // the backlash takeup rate in x the sidereal rate +#endif // this must be within stepper motors torque limits (no acceleration) +#ifndef TRACKING_RATE_DEFAULT_HZ + #define TRACKING_RATE_DEFAULT_HZ SIDEREAL_RATE_HZ // the normal sidereal tracking rate +#endif + +// slewing +#ifndef GOTO_FEATURE +#define GOTO_FEATURE ON // OFF disables goto functionality +#endif +#ifndef SLEW_RATE_BASE_DESIRED +#define SLEW_RATE_BASE_DESIRED 1.0 // *desired* maximum slew rate, actual slew rate depends on many factors +#endif +#ifndef SLEW_RATE_MEMORY +#define SLEW_RATE_MEMORY OFF // ON to retain runtime slew rate settings across power cycles +#endif +#ifndef AXIS2_SLEW_RATE_PERCENT +#define AXIS2_SLEW_RATE_PERCENT 100.0F // allows max slew rate slow down of Axis2 relative to Axis1 +#endif +#ifndef SLEW_ACCELERATION_DIST +#define SLEW_ACCELERATION_DIST 5.0 // distance in degrees to complete acceleration/deceleration +#endif +#ifndef SLEW_RAPID_STOP_DIST +#define SLEW_RAPID_STOP_DIST 2.0 // distance in degrees for emergency stop +#endif +#ifndef GOTO_OFFSET +#define GOTO_OFFSET 0.25 // distance in degrees for goto target unidirectional approach, 0.0 disables +#endif +#ifndef GOTO_OFFSET_ALIGN +#define GOTO_OFFSET_ALIGN OFF // skip final phase of goto for align stars so user tends to +#endif // approach from the correct direction when centering +#ifndef GOTO_SETTLE_TIME +#define GOTO_SETTLE_TIME 1500 // settle time in milliseconds for final phase of goto offset +#endif // allows for settle and encoder sync if available +#ifndef GOTO_REFINE_STAGES +#define GOTO_REFINE_STAGES 1 // number of times to perform the goto refinement stage +#endif + +// meridian flip, pier side +#ifndef MFLIP_SKIP_HOME +#define MFLIP_SKIP_HOME OFF +#endif +#ifndef MFLIP_PAUSE_HOME_DEFAULT +#define MFLIP_PAUSE_HOME_DEFAULT OFF +#endif +#ifndef MFLIP_PAUSE_HOME_MEMORY +#define MFLIP_PAUSE_HOME_MEMORY OFF +#endif +#ifndef MFLIP_AUTOMATIC_DEFAULT +#define MFLIP_AUTOMATIC_DEFAULT OFF +#endif +#ifndef MFLIP_AUTOMATIC_MEMORY +#define MFLIP_AUTOMATIC_MEMORY OFF +#endif +#ifndef PIER_SIDE_SYNC_CHANGE_SIDES +#define PIER_SIDE_SYNC_CHANGE_SIDES OFF +#endif +#ifndef PIER_SIDE_PREFERRED_DEFAULT +#define PIER_SIDE_PREFERRED_DEFAULT BEST +#endif +#ifndef PIER_SIDE_PREFERRED_MEMORY +#define PIER_SIDE_PREFERRED_MEMORY OFF +#endif + +// align +#ifndef ALIGN_MAX_STARS +#define ALIGN_MAX_STARS AUTO // max num align stars, AUTO for HAL specified default +#endif + +#ifndef ALIGN_AUTO_HOME +#define ALIGN_AUTO_HOME OFF // uses home switches to find home before starting the align +#endif + +#ifndef ALIGN_MODEL_MEMORY +#define ALIGN_MODEL_MEMORY OFF // restores any pointing model saved in NV at startup +#endif + +#define HIGH_SPEED_ALIGN + +// ----------------------------------------------------------------------------------- +// rotator settings, ROTATOR +#ifndef AXIS3_DRIVER_MODEL +#define AXIS3_DRIVER_MODEL OFF // specify a driver to enable +#endif +#ifndef AXIS3_STEPS_PER_DEGREE +#define AXIS3_STEPS_PER_DEGREE 64.0 // (micro)steps per degree of movement +#endif +#ifndef AXIS3_REVERSE +#define AXIS3_REVERSE OFF // reverse movement +#endif +#ifndef AXIS3_POWER_DOWN +#define AXIS3_POWER_DOWN OFF // power down at standstill +#endif +#ifndef AXIS3_POWER_DOWN_TIME +#define AXIS3_POWER_DOWN_TIME 30000 // power down time in milliseconds +#endif +#ifndef AXIS3_ENABLE_STATE +#define AXIS3_ENABLE_STATE LOW // stepper enable state +#endif +#ifndef AXIS3_SLEW_RATE_BASE_DESIRED +#define AXIS3_SLEW_RATE_BASE_DESIRED 3.0 // in degrees/sec +#endif +#ifndef AXIS3_ACCELERATION_TIME +#define AXIS3_ACCELERATION_TIME 1.0 // in seconds, to selected rate +#endif +#ifndef AXIS3_RAPID_STOP_TIME +#define AXIS3_RAPID_STOP_TIME 1.0 // in seconds, to stop +#endif +#ifndef AXIS3_BACKLASH_RATE +#define AXIS3_BACKLASH_RATE (AXIS3_SLEW_RATE_BASE_DESIRED/4) // in degrees/sec +#endif +#ifndef AXIS3_LIMIT_MIN +#define AXIS3_LIMIT_MIN -180 // in degrees +#endif +#ifndef AXIS3_LIMIT_MAX +#define AXIS3_LIMIT_MAX 180 // in degrees +#endif +#ifndef AXIS3_SENSE_HOME +#define AXIS3_SENSE_HOME OFF +#endif +#ifndef AXIS3_SENSE_HOME_INIT +#define AXIS3_SENSE_HOME_INIT INPUT_PULLUP +#endif +#ifndef AXIS3_SENSE_HOME_DIST_LIMIT +#define AXIS3_SENSE_HOME_DIST_LIMIT 180.0 // max home sense distance in degrees +#endif +#ifndef AXIS3_SENSE_LIMIT_MIN +#define AXIS3_SENSE_LIMIT_MIN OFF +#endif +#ifndef AXIS3_SENSE_LIMIT_MAX +#define AXIS3_SENSE_LIMIT_MAX OFF +#endif +#ifndef AXIS3_SENSE_LIMIT_INIT +#define AXIS3_SENSE_LIMIT_INIT INPUT_PULLUP +#endif +#if AXIS3_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS3_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS3_STEP_DIR_PRESENT + + #ifndef AXIS3_STEP_STATE + #define AXIS3_STEP_STATE HIGH + #endif + #ifndef AXIS3_M2_ON_STATE + #define AXIS3_M2_ON_STATE HIGH + #endif + #ifndef AXIS3_DRIVER_MICROSTEPS + #define AXIS3_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS3_DRIVER_MICROSTEPS_GOTO + #define AXIS3_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS3_DRIVER_INTPOL + #define AXIS3_DRIVER_INTPOL ON + #endif + #ifndef AXIS3_DRIVER_DECAY + #define AXIS3_DRIVER_DECAY OFF + #endif + #ifndef AXIS3_DRIVER_DECAY_GOTO + #define AXIS3_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS3_DRIVER_IHOLD + #define AXIS3_DRIVER_IHOLD OFF + #endif + #ifndef AXIS3_DRIVER_IRUN + #define AXIS3_DRIVER_IRUN OFF + #endif + #ifndef AXIS3_DRIVER_IGOTO + #define AXIS3_DRIVER_IGOTO OFF + #endif + #ifndef AXIS3_DRIVER_STATUS + #define AXIS3_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS3_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS3_SERVO_PRESENT + + #ifndef AXIS3_SERVO_PH1_STATE + #define AXIS3_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS3_SERVO_PH2_STATE + #define AXIS3_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS3_SERVO_ACCELERATION + #define AXIS3_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS3_SERVO_VELOCITY_PWMTHRS + #define AXIS3_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS3_MOTOR_STEPS_PER_DEGREE + #define AXIS3_MOTOR_STEPS_PER_DEGREE AXIS3_STEPS_PER_DEGREE + #endif + #ifndef AXIS3_SERVO_DC_PWR_MIN + #define AXIS3_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS3_SERVO_DC_PWR_MAX + #define AXIS3_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS3_SERVO_FEEDBACK + #define AXIS3_SERVO_FEEDBACK PID + #endif + #ifndef AXIS3_PID_P + #define AXIS3_PID_P 0.0 + #endif + #ifndef AXIS3_PID_I + #define AXIS3_PID_I 0.0 + #endif + #ifndef AXIS3_PID_D + #define AXIS3_PID_D 0.0 + #endif + + #ifndef AXIS3_SERVO_FLTR + #define AXIS3_SERVO_FLTR OFF + #endif + #ifndef AXIS3_ENCODER + #define AXIS3_ENCODER AB + #endif + #ifndef AXIS3_ENCODER_ORIGIN + #define AXIS3_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS3_ENCODER_REVERSE + #define AXIS3_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS3_DRIVER_MODEL == KTECH + #define AXIS3_KTECH_PRESENT +#endif + +#if AXIS3_DRIVER_MODEL == MKS42D + #define AXIS3_MKS42D_PRESENT +#endif + +// ----------------------------------------------------------------------------------- +// focuser settings, all + +#ifndef FOCUSER_TEMPERATURE +#define FOCUSER_TEMPERATURE OFF // activate and set focuser sensor type DS18B20, THERMISTOR, etc. +#endif +#ifndef FOCUSER_BUTTON_SENSE_OUT +#define FOCUSER_BUTTON_SENSE_OUT OFF // moves focuser out +#endif +#ifndef FOCUSER_BUTTON_SENSE_IN +#define FOCUSER_BUTTON_SENSE_IN OFF // moves focuser in +#endif +#ifndef FOCUSER_BUTTON_SENSE_INIT +#define FOCUSER_BUTTON_SENSE_INIT INPUT_PULLUP // pin mode for focuser buttons +#endif +#ifndef FOCUSER_BUTTON_FOCUSER_INDEX +#define FOCUSER_BUTTON_FOCUSER_INDEX 1 // which focuser to associate pushbutton control with (1 to 6) +#endif +#ifndef FOCUSER_BUTTON_MOVE_RATE +#define FOCUSER_BUTTON_MOVE_RATE 0 // focuser button move rate, 0 uses last set or specify fixed rate in um/sec +#endif + +// ----------------------------------------------------------------------------------- +// focuser settings, FOCUSER1 +#ifndef AXIS4_DRIVER_MODEL +#define AXIS4_DRIVER_MODEL OFF // specify a driver to enable +#endif +#ifndef AXIS4_STEPS_PER_MICRON +#define AXIS4_STEPS_PER_MICRON 0.5 // (micro)steps per micron of movement +#endif +#ifndef AXIS4_REVERSE +#define AXIS4_REVERSE OFF // reverse movement direction +#endif +#ifndef AXIS4_POWER_DOWN +#define AXIS4_POWER_DOWN OFF // power down at standstill +#endif +#ifndef AXIS4_POWER_DOWN_TIME +#define AXIS4_POWER_DOWN_TIME 30000 // power down time in milliseconds +#endif +#ifndef AXIS4_SLAVED_TO_FOCUSER +#define AXIS4_SLAVED_TO_FOCUSER 0 // focuser to slave to, or 0 to disable +#endif +#ifndef AXIS4_ENABLE_STATE +#define AXIS4_ENABLE_STATE LOW // enable pin state when driver is active +#endif +#ifndef AXIS4_SLEW_RATE_MINIMUM +#define AXIS4_SLEW_RATE_MINIMUM 20 // in microns/sec +#endif +#ifndef AXIS4_SLEW_RATE_BASE_DESIRED +#define AXIS4_SLEW_RATE_BASE_DESIRED 500 // in microns/sec +#endif +#ifndef AXIS4_ACCELERATION_TIME +#define AXIS4_ACCELERATION_TIME 1.0 // in seconds, to selected rate +#endif +#ifndef AXIS4_RAPID_STOP_TIME +#define AXIS4_RAPID_STOP_TIME 1.0 // in seconds, to stop +#endif +#ifndef AXIS4_BACKLASH_RATE +#define AXIS4_BACKLASH_RATE (AXIS4_SLEW_RATE_BASE_DESIRED/4) // in microns/sec +#endif +#ifndef AXIS4_LIMIT_MIN +#define AXIS4_LIMIT_MIN 0 // in mm +#endif +#ifndef AXIS4_LIMIT_MAX +#define AXIS4_LIMIT_MAX 50 // in mm +#endif +#ifndef AXIS4_HOME_DEFAULT +#define AXIS4_HOME_DEFAULT MIDDLE // use MINIMUM (zero), MIDDLE (half travel), MAXIMUM (full travel), or a position in microns +#endif +#ifndef AXIS4_SENSE_HOME +#define AXIS4_SENSE_HOME OFF +#endif +#ifndef AXIS4_SENSE_HOME_INIT +#define AXIS4_SENSE_HOME_INIT INPUT_PULLUP +#endif +#ifndef AXIS4_SENSE_HOME_DIST_LIMIT +#define AXIS4_SENSE_HOME_DIST_LIMIT 50 // max home sense distance in mm +#endif +#ifndef AXIS4_SENSE_LIMIT_MIN +#define AXIS4_SENSE_LIMIT_MIN OFF +#endif +#ifndef AXIS4_SENSE_LIMIT_MAX +#define AXIS4_SENSE_LIMIT_MAX OFF +#endif +#ifndef AXIS4_SENSE_LIMIT_INIT +#define AXIS4_SENSE_LIMIT_INIT INPUT_PULLUP +#endif + +#if AXIS4_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS4_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS4_STEP_DIR_PRESENT + + #ifndef AXIS4_STEP_STATE + #define AXIS4_STEP_STATE HIGH + #endif + #ifndef AXIS4_M2_ON_STATE + #define AXIS4_M2_ON_STATE HIGH + #endif + #ifndef AXIS4_DRIVER_MICROSTEPS + #define AXIS4_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS4_DRIVER_MICROSTEPS_GOTO + #define AXIS4_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS4_DRIVER_INTPOL + #define AXIS4_DRIVER_INTPOL ON + #endif + #ifndef AXIS4_DRIVER_DECAY + #define AXIS4_DRIVER_DECAY OFF + #endif + #ifndef AXIS4_DRIVER_DECAY_GOTO + #define AXIS4_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS4_DRIVER_IHOLD + #define AXIS4_DRIVER_IHOLD OFF + #endif + #ifndef AXIS4_DRIVER_IRUN + #define AXIS4_DRIVER_IRUN OFF + #endif + #ifndef AXIS4_DRIVER_IGOTO + #define AXIS4_DRIVER_IGOTO OFF + #endif + #ifndef AXIS4_DRIVER_STATUS + #define AXIS4_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS4_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS4_SERVO_PRESENT + + #ifndef AXIS4_SERVO_PH1_STATE + #define AXIS4_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS4_SERVO_PH2_STATE + #define AXIS4_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS4_SERVO_ACCELERATION + #define AXIS4_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS4_SERVO_VELOCITY_PWMTHRS + #define AXIS4_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS4_MOTOR_STEPS_PER_DEGREE + #define AXIS4_MOTOR_STEPS_PER_DEGREE AXIS4_STEPS_PER_DEGREE + #endif + #ifndef AXIS4_SERVO_DC_PWR_MIN + #define AXIS4_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS4_SERVO_DC_PWR_MAX + #define AXIS4_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS4_SERVO_FEEDBACK + #define AXIS4_SERVO_FEEDBACK PID + #endif + #ifndef AXIS4_PID_P + #define AXIS4_PID_P 0.0 + #endif + #ifndef AXIS4_PID_I + #define AXIS4_PID_I 0.0 + #endif + #ifndef AXIS4_PID_D + #define AXIS4_PID_D 0.0 + #endif + + #ifndef AXIS4_SERVO_FLTR + #define AXIS4_SERVO_FLTR OFF + #endif + #ifndef AXIS4_ENCODER + #define AXIS4_ENCODER AB + #endif + #ifndef AXIS4_ENCODER_ORIGIN + #define AXIS4_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS4_ENCODER_REVERSE + #define AXIS4_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS4_DRIVER_MODEL == KTECH + #define AXIS4_KTECH_PRESENT +#endif + +#if AXIS4_DRIVER_MODEL == MKS42D + #define AXIS4_MKS42D_PRESENT +#endif + +// focuser settings, FOCUSER2 +#ifndef AXIS5_DRIVER_MODEL +#define AXIS5_DRIVER_MODEL OFF +#endif +#ifndef AXIS5_STEPS_PER_MICRON +#define AXIS5_STEPS_PER_MICRON 0.5 +#endif +#ifndef AXIS5_REVERSE +#define AXIS5_REVERSE OFF +#endif +#ifndef AXIS5_POWER_DOWN +#define AXIS5_POWER_DOWN OFF +#endif +#ifndef AXIS5_POWER_DOWN_TIME +#define AXIS5_POWER_DOWN_TIME 30000 +#endif +#ifndef AXIS5_SLAVED_TO_FOCUSER +#define AXIS5_SLAVED_TO_FOCUSER 0 +#endif +#ifndef AXIS5_ENABLE_STATE +#define AXIS5_ENABLE_STATE LOW +#endif +#ifndef AXIS5_SLEW_RATE_MINIMUM +#define AXIS5_SLEW_RATE_MINIMUM 20 +#endif +#ifndef AXIS5_SLEW_RATE_BASE_DESIRED +#define AXIS5_SLEW_RATE_BASE_DESIRED 500 +#endif +#ifndef AXIS5_ACCELERATION_TIME +#define AXIS5_ACCELERATION_TIME 1.0 +#endif +#ifndef AXIS5_RAPID_STOP_TIME +#define AXIS5_RAPID_STOP_TIME 1.0 +#endif +#ifndef AXIS5_BACKLASH_RATE +#define AXIS5_BACKLASH_RATE (AXIS5_SLEW_RATE_BASE_DESIRED/4) +#endif +#ifndef AXIS5_LIMIT_MIN +#define AXIS5_LIMIT_MIN 0 +#endif +#ifndef AXIS5_LIMIT_MAX +#define AXIS5_LIMIT_MAX 50 +#endif +#ifndef AXIS5_HOME_DEFAULT +#define AXIS5_HOME_DEFAULT MIDDLE +#endif +#ifndef AXIS5_SENSE_HOME +#define AXIS5_SENSE_HOME OFF +#endif +#ifndef AXIS5_SENSE_HOME_INIT +#define AXIS5_SENSE_HOME_INIT INPUT_PULLUP +#endif +#ifndef AXIS5_SENSE_HOME_DIST_LIMIT +#define AXIS5_SENSE_HOME_DIST_LIMIT 50 +#endif +#ifndef AXIS5_SENSE_LIMIT_MIN +#define AXIS5_SENSE_LIMIT_MIN OFF +#endif +#ifndef AXIS5_SENSE_LIMIT_MAX +#define AXIS5_SENSE_LIMIT_MAX OFF +#endif +#ifndef AXIS5_SENSE_LIMIT_INIT +#define AXIS5_SENSE_LIMIT_INIT INPUT_PULLUP +#endif + +#if AXIS5_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS5_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS5_STEP_DIR_PRESENT + + #ifndef AXIS5_STEP_STATE + #define AXIS5_STEP_STATE HIGH + #endif + #ifndef AXIS5_M2_ON_STATE + #define AXIS5_M2_ON_STATE HIGH + #endif + #ifndef AXIS5_DRIVER_MICROSTEPS + #define AXIS5_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS5_DRIVER_MICROSTEPS_GOTO + #define AXIS5_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS5_DRIVER_INTPOL + #define AXIS5_DRIVER_INTPOL ON + #endif + #ifndef AXIS5_DRIVER_DECAY + #define AXIS5_DRIVER_DECAY OFF + #endif + #ifndef AXIS5_DRIVER_DECAY_GOTO + #define AXIS5_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS5_DRIVER_IHOLD + #define AXIS5_DRIVER_IHOLD OFF + #endif + #ifndef AXIS5_DRIVER_IRUN + #define AXIS5_DRIVER_IRUN OFF + #endif + #ifndef AXIS5_DRIVER_IGOTO + #define AXIS5_DRIVER_IGOTO OFF + #endif + #ifndef AXIS5_DRIVER_STATUS + #define AXIS5_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS5_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS5_SERVO_PRESENT + + #ifndef AXIS5_SERVO_PH1_STATE + #define AXIS5_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS5_SERVO_PH2_STATE + #define AXIS5_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS5_SERVO_ACCELERATION + #define AXIS5_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS5_SERVO_VELOCITY_PWMTHRS + #define AXIS5_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS5_MOTOR_STEPS_PER_DEGREE + #define AXIS5_MOTOR_STEPS_PER_DEGREE AXIS5_STEPS_PER_DEGREE + #endif + #ifndef AXIS5_SERVO_DC_PWR_MIN + #define AXIS5_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS5_SERVO_DC_PWR_MAX + #define AXIS5_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS5_SERVO_FEEDBACK + #define AXIS5_SERVO_FEEDBACK PID + #endif + #ifndef AXIS5_PID_P + #define AXIS5_PID_P 0.0 + #endif + #ifndef AXIS5_PID_I + #define AXIS5_PID_I 0.0 + #endif + #ifndef AXIS5_PID_D + #define AXIS5_PID_D 0.0 + #endif + #ifndef AXIS5_PID_P_GOTO + #define AXIS5_PID_P_GOTO AXIS5_PID_P + #endif + #ifndef AXIS5_PID_I_GOTO + #define AXIS5_PID_I_GOTO AXIS5_PID_I + #endif + #ifndef AXIS5_PID_D_GOTO + #define AXIS5_PID_D_GOTO AXIS5_PID_D + #endif + + #ifndef AXIS5_SERVO_FLTR + #define AXIS5_SERVO_FLTR OFF + #endif + #ifndef AXIS5_ENCODER + #define AXIS5_ENCODER AB + #endif + #ifndef AXIS5_ENCODER_ORIGIN + #define AXIS5_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS5_ENCODER_REVERSE + #define AXIS5_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS5_DRIVER_MODEL == KTECH + #define AXIS5_KTECH_PRESENT +#endif + +#if AXIS5_DRIVER_MODEL == MKS42D + #define AXIS5_MKS42D_PRESENT +#endif + +// focuser settings, FOCUSER3 +#ifndef AXIS6_DRIVER_MODEL +#define AXIS6_DRIVER_MODEL OFF +#endif +#ifndef AXIS6_STEPS_PER_MICRON +#define AXIS6_STEPS_PER_MICRON 0.5 +#endif +#ifndef AXIS6_REVERSE +#define AXIS6_REVERSE OFF +#endif +#ifndef AXIS6_POWER_DOWN +#define AXIS6_POWER_DOWN OFF +#endif +#ifndef AXIS6_POWER_DOWN_TIME +#define AXIS6_POWER_DOWN_TIME 30000 +#endif +#ifndef AXIS6_SLAVED_TO_FOCUSER +#define AXIS6_SLAVED_TO_FOCUSER 0 +#endif +#ifndef AXIS6_ENABLE_STATE +#define AXIS6_ENABLE_STATE LOW +#endif +#ifndef AXIS6_SLEW_RATE_MINIMUM +#define AXIS6_SLEW_RATE_MINIMUM 20 +#endif +#ifndef AXIS6_SLEW_RATE_BASE_DESIRED +#define AXIS6_SLEW_RATE_BASE_DESIRED 500 +#endif +#ifndef AXIS6_ACCELERATION_TIME +#define AXIS6_ACCELERATION_TIME 1.0 +#endif +#ifndef AXIS6_RAPID_STOP_TIME +#define AXIS6_RAPID_STOP_TIME 1.0 +#endif +#ifndef AXIS6_BACKLASH_RATE +#define AXIS6_BACKLASH_RATE (AXIS6_SLEW_RATE_BASE_DESIRED/4) +#endif +#ifndef AXIS6_LIMIT_MIN +#define AXIS6_LIMIT_MIN 0 +#endif +#ifndef AXIS6_LIMIT_MAX +#define AXIS6_LIMIT_MAX 50 +#endif +#ifndef AXIS6_HOME_DEFAULT +#define AXIS6_HOME_DEFAULT MIDDLE +#endif +#ifndef AXIS6_SENSE_HOME +#define AXIS6_SENSE_HOME OFF +#endif +#ifndef AXIS6_SENSE_HOME_INIT +#define AXIS6_SENSE_HOME_INIT INPUT_PULLUP +#endif +#ifndef AXIS6_SENSE_HOME_DIST_LIMIT +#define AXIS6_SENSE_HOME_DIST_LIMIT 50 +#endif +#ifndef AXIS6_SENSE_LIMIT_MIN +#define AXIS6_SENSE_LIMIT_MIN OFF +#endif +#ifndef AXIS6_SENSE_LIMIT_MAX +#define AXIS6_SENSE_LIMIT_MAX OFF +#endif +#ifndef AXIS6_SENSE_LIMIT_INIT +#define AXIS6_SENSE_LIMIT_INIT INPUT_PULLUP +#endif + +#if AXIS6_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS6_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS6_STEP_DIR_PRESENT + + #ifndef AXIS6_STEP_STATE + #define AXIS6_STEP_STATE HIGH + #endif + #ifndef AXIS6_M2_ON_STATE + #define AXIS6_M2_ON_STATE HIGH + #endif + #ifndef AXIS6_DRIVER_MICROSTEPS + #define AXIS6_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS6_DRIVER_MICROSTEPS_GOTO + #define AXIS6_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS6_DRIVER_INTPOL + #define AXIS6_DRIVER_INTPOL ON + #endif + #ifndef AXIS6_DRIVER_DECAY + #define AXIS6_DRIVER_DECAY OFF + #endif + #ifndef AXIS6_DRIVER_DECAY_GOTO + #define AXIS6_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS6_DRIVER_IHOLD + #define AXIS6_DRIVER_IHOLD OFF + #endif + #ifndef AXIS6_DRIVER_IRUN + #define AXIS6_DRIVER_IRUN OFF + #endif + #ifndef AXIS6_DRIVER_IGOTO + #define AXIS6_DRIVER_IGOTO OFF + #endif + #ifndef AXIS6_DRIVER_STATUS + #define AXIS6_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS6_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS6_SERVO_PRESENT + + #ifndef AXIS6_SERVO_PH1_STATE + #define AXIS6_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS6_SERVO_PH2_STATE + #define AXIS6_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS6_SERVO_ACCELERATION + #define AXIS6_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS6_SERVO_VELOCITY_PWMTHRS + #define AXIS6_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS6_MOTOR_STEPS_PER_DEGREE + #define AXIS6_MOTOR_STEPS_PER_DEGREE AXIS6_STEPS_PER_DEGREE + #endif + #ifndef AXIS6_SERVO_DC_PWR_MIN + #define AXIS6_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS6_SERVO_DC_PWR_MAX + #define AXIS6_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS6_SERVO_FEEDBACK + #define AXIS6_SERVO_FEEDBACK PID + #endif + #ifndef AXIS6_PID_P + #define AXIS6_PID_P 0.0 + #endif + #ifndef AXIS6_PID_I + #define AXIS6_PID_I 0.0 + #endif + #ifndef AXIS6_PID_D + #define AXIS6_PID_D 0.0 + #endif + + #ifndef AXIS6_SERVO_FLTR + #define AXIS6_SERVO_FLTR OFF + #endif + #ifndef AXIS6_ENCODER + #define AXIS6_ENCODER AB + #endif + #ifndef AXIS6_ENCODER_ORIGIN + #define AXIS6_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS6_ENCODER_REVERSE + #define AXIS6_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS6_DRIVER_MODEL == KTECH + #define AXIS6_KTECH_PRESENT +#endif + +#if AXIS6_DRIVER_MODEL == MKS42D + #define AXIS6_MKS42D_PRESENT +#endif + +// focuser settings, FOCUSER4 +#ifndef AXIS7_DRIVER_MODEL +#define AXIS7_DRIVER_MODEL OFF +#endif +#ifndef AXIS7_STEPS_PER_MICRON +#define AXIS7_STEPS_PER_MICRON 0.5 +#endif +#ifndef AXIS7_REVERSE +#define AXIS7_REVERSE OFF +#endif +#ifndef AXIS7_POWER_DOWN +#define AXIS7_POWER_DOWN OFF +#endif +#ifndef AXIS7_POWER_DOWN_TIME +#define AXIS7_POWER_DOWN_TIME 30000 +#endif +#ifndef AXIS7_SLAVED_TO_FOCUSER +#define AXIS7_SLAVED_TO_FOCUSER 0 +#endif +#ifndef AXIS7_ENABLE_STATE +#define AXIS7_ENABLE_STATE LOW +#endif +#ifndef AXIS7_SLEW_RATE_MINIMUM +#define AXIS7_SLEW_RATE_MINIMUM 20 +#endif +#ifndef AXIS7_SLEW_RATE_BASE_DESIRED +#define AXIS7_SLEW_RATE_BASE_DESIRED 500 +#endif +#ifndef AXIS7_ACCELERATION_TIME +#define AXIS7_ACCELERATION_TIME 1.0 +#endif +#ifndef AXIS7_RAPID_STOP_TIME +#define AXIS7_RAPID_STOP_TIME 1.0 +#endif +#ifndef AXIS7_BACKLASH_RATE +#define AXIS7_BACKLASH_RATE (AXIS7_SLEW_RATE_BASE_DESIRED/4) +#endif +#ifndef AXIS7_LIMIT_MIN +#define AXIS7_LIMIT_MIN 0 +#endif +#ifndef AXIS7_LIMIT_MAX +#define AXIS7_LIMIT_MAX 50 +#endif +#ifndef AXIS7_HOME_DEFAULT +#define AXIS7_HOME_DEFAULT MIDDLE +#endif +#ifndef AXIS7_SENSE_HOME +#define AXIS7_SENSE_HOME OFF +#endif +#ifndef AXIS7_SENSE_HOME_INIT +#define AXIS7_SENSE_HOME_INIT INPUT_PULLUP +#endif +#ifndef AXIS7_SENSE_HOME_DIST_LIMIT +#define AXIS7_SENSE_HOME_DIST_LIMIT 50 +#endif +#ifndef AXIS7_SENSE_LIMIT_MIN +#define AXIS7_SENSE_LIMIT_MIN OFF +#endif +#ifndef AXIS7_SENSE_LIMIT_MAX +#define AXIS7_SENSE_LIMIT_MAX OFF +#endif +#ifndef AXIS7_SENSE_LIMIT_INIT +#define AXIS7_SENSE_LIMIT_INIT INPUT_PULLUP +#endif +#if AXIS7_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS7_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS7_STEP_DIR_PRESENT + + #ifndef AXIS7_STEP_STATE + #define AXIS7_STEP_STATE HIGH + #endif + #ifndef AXIS7_M2_ON_STATE + #define AXIS7_M2_ON_STATE HIGH + #endif + #ifndef AXIS7_DRIVER_MICROSTEPS + #define AXIS7_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS7_DRIVER_MICROSTEPS_GOTO + #define AXIS7_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS7_DRIVER_INTPOL + #define AXIS7_DRIVER_INTPOL ON + #endif + #ifndef AXIS7_DRIVER_DECAY + #define AXIS7_DRIVER_DECAY OFF + #endif + #ifndef AXIS7_DRIVER_DECAY_GOTO + #define AXIS7_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS7_DRIVER_IHOLD + #define AXIS7_DRIVER_IHOLD OFF + #endif + #ifndef AXIS7_DRIVER_IRUN + #define AXIS7_DRIVER_IRUN OFF + #endif + #ifndef AXIS7_DRIVER_IGOTO + #define AXIS7_DRIVER_IGOTO OFF + #endif + #ifndef AXIS7_DRIVER_STATUS + #define AXIS7_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS7_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS7_SERVO_PRESENT + + #ifndef AXIS7_SERVO_PH1_STATE + #define AXIS7_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS7_SERVO_PH2_STATE + #define AXIS7_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS7_SERVO_ACCELERATION + #define AXIS7_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS7_SERVO_VELOCITY_PWMTHRS + #define AXIS7_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS7_MOTOR_STEPS_PER_DEGREE + #define AXIS7_MOTOR_STEPS_PER_DEGREE AXIS7_STEPS_PER_DEGREE + #endif + #ifndef AXIS7_SERVO_DC_PWR_MIN + #define AXIS7_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS7_SERVO_DC_PWR_MAX + #define AXIS7_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS7_SERVO_FEEDBACK + #define AXIS7_SERVO_FEEDBACK PID + #endif + #ifndef AXIS7_PID_P + #define AXIS7_PID_P 0.0 + #endif + #ifndef AXIS7_PID_I + #define AXIS7_PID_I 0.0 + #endif + #ifndef AXIS7_PID_D + #define AXIS7_PID_D 0.0 + #endif + + #ifndef AXIS7_SERVO_FLTR + #define AXIS7_SERVO_FLTR OFF + #endif + #ifndef AXIS7_ENCODER + #define AXIS7_ENCODER AB + #endif + #ifndef AXIS7_ENCODER_ORIGIN + #define AXIS7_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS7_ENCODER_REVERSE + #define AXIS7_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS7_DRIVER_MODEL == KTECH + #define AXIS7_KTECH_PRESENT +#endif + +#if AXIS7_DRIVER_MODEL == MKS42D + #define AXIS7_MKS42D_PRESENT +#endif + +// focuser settings, FOCUSER5 +#ifndef AXIS8_DRIVER_MODEL +#define AXIS8_DRIVER_MODEL OFF +#endif +#ifndef AXIS8_STEPS_PER_MICRON +#define AXIS8_STEPS_PER_MICRON 0.5 +#endif +#ifndef AXIS8_REVERSE +#define AXIS8_REVERSE OFF +#endif +#ifndef AXIS8_POWER_DOWN +#define AXIS8_POWER_DOWN OFF +#endif +#ifndef AXIS8_POWER_DOWN_TIME +#define AXIS8_POWER_DOWN_TIME 30000 +#endif +#ifndef AXIS8_SLAVED_TO_FOCUSER +#define AXIS8_SLAVED_TO_FOCUSER 0 +#endif +#ifndef AXIS8_ENABLE_STATE +#define AXIS8_ENABLE_STATE LOW +#endif +#ifndef AXIS8_SLEW_RATE_MINIMUM +#define AXIS8_SLEW_RATE_MINIMUM 20 +#endif +#ifndef AXIS8_SLEW_RATE_BASE_DESIRED +#define AXIS8_SLEW_RATE_BASE_DESIRED 500 +#endif +#ifndef AXIS8_ACCELERATION_TIME +#define AXIS8_ACCELERATION_TIME 1.0 +#endif +#ifndef AXIS8_RAPID_STOP_TIME +#define AXIS8_RAPID_STOP_TIME 1.0 +#endif +#ifndef AXIS8_BACKLASH_RATE +#define AXIS8_BACKLASH_RATE (AXIS8_SLEW_RATE_BASE_DESIRED/4) +#endif +#ifndef AXIS8_LIMIT_MIN +#define AXIS8_LIMIT_MIN 0 +#endif +#ifndef AXIS8_LIMIT_MAX +#define AXIS8_LIMIT_MAX 50 +#endif +#ifndef AXIS8_HOME_DEFAULT +#define AXIS8_HOME_DEFAULT MIDDLE +#endif +#ifndef AXIS8_SENSE_HOME +#define AXIS8_SENSE_HOME OFF +#endif +#ifndef AXIS8_SENSE_HOME_INIT +#define AXIS8_SENSE_HOME_INIT INPUT_PULLUP +#endif +#ifndef AXIS8_SENSE_HOME_DIST_LIMIT +#define AXIS8_SENSE_HOME_DIST_LIMIT 50 +#endif +#ifndef AXIS8_SENSE_LIMIT_MIN +#define AXIS8_SENSE_LIMIT_MIN OFF +#endif +#ifndef AXIS8_SENSE_LIMIT_MAX +#define AXIS8_SENSE_LIMIT_MAX OFF +#endif +#ifndef AXIS8_SENSE_LIMIT_INIT +#define AXIS8_SENSE_LIMIT_INIT INPUT_PULLUP +#endif + +#if AXIS8_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS8_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS8_STEP_DIR_PRESENT + + #ifndef AXIS8_STEP_STATE + #define AXIS8_STEP_STATE HIGH + #endif + #ifndef AXIS8_M2_ON_STATE + #define AXIS8_M2_ON_STATE HIGH + #endif + #ifndef AXIS8_DRIVER_MICROSTEPS + #define AXIS8_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS8_DRIVER_MICROSTEPS_GOTO + #define AXIS8_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS8_DRIVER_INTPOL + #define AXIS8_DRIVER_INTPOL ON + #endif + #ifndef AXIS8_DRIVER_DECAY + #define AXIS8_DRIVER_DECAY OFF + #endif + #ifndef AXIS8_DRIVER_DECAY_GOTO + #define AXIS8_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS8_DRIVER_IHOLD + #define AXIS8_DRIVER_IHOLD OFF + #endif + #ifndef AXIS8_DRIVER_IRUN + #define AXIS8_DRIVER_IRUN OFF + #endif + #ifndef AXIS8_DRIVER_IGOTO + #define AXIS8_DRIVER_IGOTO OFF + #endif + #ifndef AXIS8_DRIVER_STATUS + #define AXIS8_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS8_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS8_SERVO_PRESENT + + #ifndef AXIS8_SERVO_PH1_STATE + #define AXIS8_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS8_SERVO_PH2_STATE + #define AXIS8_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS8_SERVO_ACCELERATION + #define AXIS8_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS8_SERVO_VELOCITY_PWMTHRS + #define AXIS8_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS8_MOTOR_STEPS_PER_DEGREE + #define AXIS8_MOTOR_STEPS_PER_DEGREE AXIS8_STEPS_PER_DEGREE + #endif + #ifndef AXIS8_SERVO_DC_PWR_MIN + #define AXIS8_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS8_SERVO_DC_PWR_MAX + #define AXIS8_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS8_SERVO_FEEDBACK + #define AXIS8_SERVO_FEEDBACK PID + #endif + #ifndef AXIS8_PID_P + #define AXIS8_PID_P 0.0 + #endif + #ifndef AXIS8_PID_I + #define AXIS8_PID_I 0.0 + #endif + #ifndef AXIS8_PID_D + #define AXIS8_PID_D 0.0 + #endif + + #ifndef AXIS8_SERVO_FLTR + #define AXIS8_SERVO_FLTR OFF + #endif + #ifndef AXIS8_ENCODER + #define AXIS8_ENCODER AB + #endif + #ifndef AXIS8_ENCODER_ORIGIN + #define AXIS8_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS8_ENCODER_REVERSE + #define AXIS8_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS8_DRIVER_MODEL == KTECH + #define AXIS8_KTECH_PRESENT +#endif + +#if AXIS8_DRIVER_MODEL == MKS42D + #define AXIS8_MKS42D_PRESENT +#endif + +// focuser settings, FOCUSER6 +#ifndef AXIS9_DRIVER_MODEL +#define AXIS9_DRIVER_MODEL OFF +#endif +#ifndef AXIS9_STEPS_PER_MICRON +#define AXIS9_STEPS_PER_MICRON 0.5 +#endif +#ifndef AXIS9_REVERSE +#define AXIS9_REVERSE OFF +#endif +#ifndef AXIS9_POWER_DOWN +#define AXIS9_POWER_DOWN OFF +#endif +#ifndef AXIS9_POWER_DOWN_TIME +#define AXIS9_POWER_DOWN_TIME 30000 +#endif +#ifndef AXIS9_SLAVED_TO_FOCUSER +#define AXIS9_SLAVED_TO_FOCUSER 0 +#endif +#ifndef AXIS9_ENABLE_STATE +#define AXIS9_ENABLE_STATE LOW +#endif +#ifndef AXIS9_SLEW_RATE_MINIMUM +#define AXIS9_SLEW_RATE_MINIMUM 20 +#endif +#ifndef AXIS9_SLEW_RATE_BASE_DESIRED +#define AXIS9_SLEW_RATE_BASE_DESIRED 500 +#endif +#ifndef AXIS9_ACCELERATION_TIME +#define AXIS9_ACCELERATION_TIME 1.0 +#endif +#ifndef AXIS9_RAPID_STOP_TIME +#define AXIS9_RAPID_STOP_TIME 1.0 +#endif +#ifndef AXIS9_BACKLASH_RATE +#define AXIS9_BACKLASH_RATE (AXIS9_SLEW_RATE_BASE_DESIRED/4) +#endif +#ifndef AXIS9_LIMIT_MIN +#define AXIS9_LIMIT_MIN 0 +#endif +#ifndef AXIS9_LIMIT_MAX +#define AXIS9_LIMIT_MAX 50 +#endif +#ifndef AXIS9_HOME_DEFAULT +#define AXIS9_HOME_DEFAULT MIDDLE +#endif +#ifndef AXIS9_SENSE_HOME +#define AXIS9_SENSE_HOME OFF +#endif +#ifndef AXIS9_SENSE_HOME_INIT +#define AXIS9_SENSE_HOME_INIT INPUT_PULLUP +#endif +#ifndef AXIS9_SENSE_HOME_DIST_LIMIT +#define AXIS9_SENSE_HOME_DIST_LIMIT 50 +#endif +#ifndef AXIS9_SENSE_LIMIT_MIN +#define AXIS9_SENSE_LIMIT_MIN OFF +#endif +#ifndef AXIS9_SENSE_LIMIT_MAX +#define AXIS9_SENSE_LIMIT_MAX OFF +#endif +#ifndef AXIS9_SENSE_LIMIT_INIT +#define AXIS9_SENSE_LIMIT_INIT INPUT_PULLUP +#endif + +#if AXIS9_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS9_DRIVER_MODEL <= STEP_DIR_DRIVER_LAST + #define AXIS9_STEP_DIR_PRESENT + + #ifndef AXIS9_STEP_STATE + #define AXIS9_STEP_STATE HIGH + #endif + #ifndef AXIS9_M2_ON_STATE + #define AXIS9_M2_ON_STATE HIGH + #endif + #ifndef AXIS9_DRIVER_MICROSTEPS + #define AXIS9_DRIVER_MICROSTEPS OFF + #endif + #ifndef AXIS9_DRIVER_MICROSTEPS_GOTO + #define AXIS9_DRIVER_MICROSTEPS_GOTO OFF + #endif + #ifndef AXIS9_DRIVER_INTPOL + #define AXIS9_DRIVER_INTPOL ON + #endif + #ifndef AXIS9_DRIVER_DECAY + #define AXIS9_DRIVER_DECAY OFF + #endif + #ifndef AXIS9_DRIVER_DECAY_GOTO + #define AXIS9_DRIVER_DECAY_GOTO OFF + #endif + #ifndef AXIS9_DRIVER_IHOLD + #define AXIS9_DRIVER_IHOLD OFF + #endif + #ifndef AXIS9_DRIVER_IRUN + #define AXIS9_DRIVER_IRUN OFF + #endif + #ifndef AXIS9_DRIVER_IGOTO + #define AXIS9_DRIVER_IGOTO OFF + #endif + #ifndef AXIS9_DRIVER_STATUS + #define AXIS9_DRIVER_STATUS OFF + #endif +#endif + +#if AXIS9_DRIVER_MODEL >= SERVO_DRIVER_FIRST && AXIS2_DRIVER_MODEL <= SERVO_DRIVER_LAST + #define AXIS9_SERVO_PRESENT + + #ifndef AXIS9_SERVO_PH1_STATE + #define AXIS9_SERVO_PH1_STATE LOW + #endif + #ifndef AXIS9_SERVO_PH2_STATE + #define AXIS9_SERVO_PH2_STATE LOW + #endif + + #ifndef AXIS9_SERVO_ACCELERATION + #define AXIS9_SERVO_ACCELERATION 100 + #endif + #ifndef AXIS9_SERVO_VELOCITY_PWMTHRS + #define AXIS9_SERVO_VELOCITY_PWMTHRS OFF + #endif + #ifndef AXIS9_MOTOR_STEPS_PER_DEGREE + #define AXIS9_MOTOR_STEPS_PER_DEGREE AXIS9_STEPS_PER_DEGREE + #endif + #ifndef AXIS9_SERVO_DC_PWR_MIN + #define AXIS9_SERVO_DC_PWR_MIN 0.0F + #endif + #ifndef AXIS9_SERVO_DC_PWR_MAX + #define AXIS9_SERVO_DC_PWR_MAX 100.0F + #endif + + #ifndef AXIS9_SERVO_FEEDBACK + #define AXIS9_SERVO_FEEDBACK PID + #endif + #ifndef AXIS9_PID_P + #define AXIS9_PID_P 0.0 + #endif + #ifndef AXIS9_PID_I + #define AXIS9_PID_I 0.0 + #endif + #ifndef AXIS9_PID_D + #define AXIS9_PID_D 0.0 + #endif + + #ifndef AXIS9_SERVO_FLTR + #define AXIS9_SERVO_FLTR OFF + #endif + #ifndef AXIS9_ENCODER + #define AXIS9_ENCODER AB + #endif + #ifndef AXIS9_ENCODER_ORIGIN + #define AXIS9_ENCODER_ORIGIN 0 + #endif + #ifndef AXIS9_ENCODER_REVERSE + #define AXIS9_ENCODER_REVERSE OFF + #endif +#endif + +#if AXIS9_DRIVER_MODEL == KTECH + #define AXIS9_KTECH_PRESENT +#endif + +#if AXIS9_DRIVER_MODEL == MKS42D + #define AXIS9_MKS42D_PRESENT +#endif + +// ----------------------------------------------------------------------------------- +// helper for checking driver presence +#define DRIVER_CHECK(model) \ + AXIS1_DRIVER_MODEL == model || \ + AXIS2_DRIVER_MODEL == model || \ + AXIS3_DRIVER_MODEL == model || \ + AXIS4_DRIVER_MODEL == model || \ + AXIS5_DRIVER_MODEL == model || \ + AXIS6_DRIVER_MODEL == model || \ + AXIS7_DRIVER_MODEL == model || \ + AXIS8_DRIVER_MODEL == model || \ + AXIS9_DRIVER_MODEL == model + +// flag presence of step/dir legacy drivers +#if AXIS1_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS1_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS2_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS2_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS3_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS3_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS4_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS4_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS5_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS5_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS6_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS6_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS7_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS7_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS8_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS8_DRIVER_MODEL < TMC_DRIVER_FIRST || \ + AXIS9_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS9_DRIVER_MODEL < TMC_DRIVER_FIRST + #define STEP_DIR_LEGACY_PRESENT +#endif + +// flag presence of Step/Dir TMC UART and SPI drivers +#if DRIVER_CHECK(TMC2130) + #define TMC2130_PRESENT +#endif +#if DRIVER_CHECK(TMC2160) + #define TMC2160_PRESENT +#endif +#if DRIVER_CHECK(TMC2208) + #define TMC2208_PRESENT +#endif +#if DRIVER_CHECK(TMC2209) + #define TMC2209_PRESENT +#endif +#if DRIVER_CHECK(TMC2226) + #define TMC2209_PRESENT +#endif +#if DRIVER_CHECK(TMC2660) + #define TMC2660_PRESENT +#endif +#if DRIVER_CHECK(TMC5160) + #define TMC5160_PRESENT +#endif +#if DRIVER_CHECK(TMC5161) + #define TMC5161_PRESENT +#endif + +#if defined(TMC2130_PRESENT) || defined(TMC2160_PRESENT) || defined(TMC2660_PRESENT) || \ + defined(TMC5160_PRESENT) || defined(TMC5161_PRESENT) + #define STEP_DIR_TMC_SPI_PRESENT +#endif + +#if defined(TMC2208_PRESENT) || defined(TMC2209_PRESENT) + #define STEP_DIR_TMC_UART_PRESENT +#endif + +#if defined(STEP_DIR_LEGACY_PRESENT) || defined(STEP_DIR_TMC_SPI_PRESENT) || defined(STEP_DIR_TMC_UART_PRESENT) + #define STEP_DIR_MOTOR_PRESENT +#endif + +// flag presence of Servo motors +#if DRIVER_CHECK(SERVO_PE) + #define SERVO_PE_PRESENT +#endif +#if DRIVER_CHECK(SERVO_EE) + #define SERVO_EE_PRESENT +#endif +#if DRIVER_CHECK(SERVO_TMC2130_DC) + #define SERVO_TMC2130_DC_PRESENT +#endif +#if DRIVER_CHECK(SERVO_TMC5160_DC) + #define SERVO_TMC5160_DC_PRESENT +#endif +#if DRIVER_CHECK(SERVO_TMC2209) + #define SERVO_TMC2209_PRESENT +#endif +#if DRIVER_CHECK(SERVO_TMC5160) + #define SERVO_TMC5160_PRESENT +#endif +#if DRIVER_CHECK(SERVO_KTECH) + #define SERVO_KTECH_PRESENT +#endif + +#if defined(SERVO_PE_PRESENT) || defined(SERVO_EE_PRESENT) || defined(SERVO_TMC2130_DC_PRESENT) || \ + defined(SERVO_TMC5160_DC_PRESENT) || defined(SERVO_TMC2209_PRESENT) || defined(SERVO_TMC5160_PRESENT) || \ + defined(SERVO_KTECH_PRESENT) + #define SERVO_MOTOR_PRESENT +#endif + +// flag presence of ODRIVE motors +#if defined(AXIS1_ODRIVE_PRESENT) || defined(AXIS2_ODRIVE_PRESENT) + #define ODRIVE_MOTOR_PRESENT +#endif + +// flag presence of KTECH motors +#if DRIVER_CHECK(KTECH) + #define KTECH_MOTOR_PRESENT +#endif + +// flag presence of MKS SERVO42D/57D motors +#if DRIVER_CHECK(MKS42D) + #define MKS42D_MOTOR_PRESENT +#endif + +// flag to indicate if any motor is present +#if defined(SERVO_MOTOR_PRESENT) || defined(STEP_DIR_MOTOR_PRESENT) || defined(ODRIVE_MOTOR_PRESENT) || defined(KTECH_MOTOR_PRESENT) + #define MOTOR_PRESENT +#endif + +// ----------------------------------------------------------------------------------- +// auxiliary feature settings + +#ifndef FEATURE1_PURPOSE +#define FEATURE1_PURPOSE OFF // OFF or SWITCH, ANALOG_OUT, DEW_HEATER, INTERVALOMETER +#endif +#ifndef FEATURE1_NAME +#define FEATURE1_NAME "FEATURE1" // user friendly name of feature, up to 10 chars +#endif +#ifndef FEATURE1_TEMP +#define FEATURE1_TEMP OFF // temperature sensor, thermistor or DS1820 +#endif +#ifndef FEATURE1_PIN +#define FEATURE1_PIN OFF // OUTPUT control pin +#endif +#ifndef FEATURE1_VALUE_DEFAULT +#define FEATURE1_VALUE_DEFAULT OFF // OUTPUT control pin default value/state ON, OFF, 0..255 +#endif +#ifndef FEATURE1_VALUE_MEMORY +#define FEATURE1_VALUE_MEMORY OFF // ON remembers feature value across power cycles +#endif +#ifndef FEATURE1_ON_STATE +#define FEATURE1_ON_STATE HIGH // OUTPUT control pin ON (active) state +#endif + +#ifndef FEATURE2_PURPOSE +#define FEATURE2_PURPOSE OFF +#endif +#ifndef FEATURE2_NAME +#define FEATURE2_NAME "FEATURE2" +#endif +#ifndef FEATURE2_TEMP +#define FEATURE2_TEMP OFF +#endif +#ifndef FEATURE2_PIN +#define FEATURE2_PIN OFF +#endif +#ifndef FEATURE2_VALUE_DEFAULT +#define FEATURE2_VALUE_DEFAULT OFF +#endif +#ifndef FEATURE2_VALUE_MEMORY +#define FEATURE2_VALUE_MEMORY OFF +#endif +#ifndef FEATURE2_ON_STATE +#define FEATURE2_ON_STATE HIGH +#endif + +#ifndef FEATURE3_PURPOSE +#define FEATURE3_PURPOSE OFF +#endif +#ifndef FEATURE3_NAME +#define FEATURE3_NAME "FEATURE3" +#endif +#ifndef FEATURE3_TEMP +#define FEATURE3_TEMP OFF +#endif +#ifndef FEATURE3_PIN +#define FEATURE3_PIN OFF +#endif +#ifndef FEATURE3_VALUE_DEFAULT +#define FEATURE3_VALUE_DEFAULT OFF +#endif +#ifndef FEATURE3_VALUE_MEMORY +#define FEATURE3_VALUE_MEMORY OFF +#endif +#ifndef FEATURE3_ON_STATE +#define FEATURE3_ON_STATE HIGH +#endif + +#ifndef FEATURE4_PURPOSE +#define FEATURE4_PURPOSE OFF +#endif +#ifndef FEATURE4_NAME +#define FEATURE4_NAME "FEATURE4" +#endif +#ifndef FEATURE4_TEMP +#define FEATURE4_TEMP OFF +#endif +#ifndef FEATURE4_PIN +#define FEATURE4_PIN OFF +#endif +#ifndef FEATURE4_VALUE_DEFAULT +#define FEATURE4_VALUE_DEFAULT OFF +#endif +#ifndef FEATURE4_VALUE_MEMORY +#define FEATURE4_VALUE_MEMORY OFF +#endif +#ifndef FEATURE4_ON_STATE +#define FEATURE4_ON_STATE HIGH +#endif + +#ifndef FEATURE5_PURPOSE +#define FEATURE5_PURPOSE OFF +#endif +#ifndef FEATURE5_NAME +#define FEATURE5_NAME "FEATURE5" +#endif +#ifndef FEATURE5_TEMP +#define FEATURE5_TEMP OFF +#endif +#ifndef FEATURE5_PIN +#define FEATURE5_PIN OFF +#endif +#ifndef FEATURE5_VALUE_DEFAULT +#define FEATURE5_VALUE_DEFAULT OFF +#endif +#ifndef FEATURE5_VALUE_MEMORY +#define FEATURE5_VALUE_MEMORY OFF +#endif +#ifndef FEATURE5_ON_STATE +#define FEATURE5_ON_STATE HIGH +#endif + +#ifndef FEATURE6_PURPOSE +#define FEATURE6_PURPOSE OFF +#endif +#ifndef FEATURE6_NAME +#define FEATURE6_NAME "FEATURE6" +#endif +#ifndef FEATURE6_TEMP +#define FEATURE6_TEMP OFF +#endif +#ifndef FEATURE6_PIN +#define FEATURE6_PIN OFF +#endif +#ifndef FEATURE6_VALUE_DEFAULT +#define FEATURE6_VALUE_DEFAULT OFF +#endif +#ifndef FEATURE6_VALUE_MEMORY +#define FEATURE6_VALUE_MEMORY OFF +#endif +#ifndef FEATURE6_ON_STATE +#define FEATURE6_ON_STATE HIGH +#endif + +#ifndef FEATURE7_PURPOSE +#define FEATURE7_PURPOSE OFF +#endif +#ifndef FEATURE7_NAME +#define FEATURE7_NAME "FEATURE7" +#endif +#ifndef FEATURE7_TEMP +#define FEATURE7_TEMP OFF +#endif +#ifndef FEATURE7_PIN +#define FEATURE7_PIN OFF +#endif +#ifndef FEATURE7_VALUE_DEFAULT +#define FEATURE7_VALUE_DEFAULT OFF +#endif +#ifndef FEATURE7_VALUE_MEMORY +#define FEATURE7_VALUE_MEMORY OFF +#endif +#ifndef FEATURE7_ON_STATE +#define FEATURE7_ON_STATE HIGH +#endif + +#ifndef FEATURE8_PURPOSE +#define FEATURE8_PURPOSE OFF +#endif +#ifndef FEATURE8_NAME +#define FEATURE8_NAME "FEATURE8" +#endif +#ifndef FEATURE8_TEMP +#define FEATURE8_TEMP OFF +#endif +#ifndef FEATURE8_PIN +#define FEATURE8_PIN OFF +#endif +#ifndef FEATURE8_VALUE_DEFAULT +#define FEATURE8_VALUE_DEFAULT OFF +#endif +#ifndef FEATURE8_VALUE_MEMORY +#define FEATURE8_VALUE_MEMORY OFF +#endif +#ifndef FEATURE8_ON_STATE +#define FEATURE8_ON_STATE HIGH +#endif + +// auxiliary feature power monitor configuration + +// fan control +#ifndef FAN_PIN +#define FAN_PIN OFF // PWM capable pin to run a small fan to help cool the controller +#endif +#ifndef FAN_THRESHOLD_LOW +#define FAN_THRESHOLD_LOW 35 // Deg. C +#endif +#ifndef FAN_POWER_LOW +#define FAN_POWER_LOW 60 // % +#endif +#ifndef FAN_THRESHOLD_MID +#define FAN_THRESHOLD_MID 45 // Deg. C +#endif +#ifndef FAN_POWER_MID +#define FAN_POWER_MID 80 // % +#endif +#ifndef FAN_THRESHOLD_HIGH +#define FAN_THRESHOLD_HIGH 50 // Deg. C +#endif +#ifndef FAN_POWER_HIGH +#define FAN_POWER_HIGH 100 // % +#endif +#ifndef FAN_THRESHOLD_OT +#define FAN_THRESHOLD_OT 60 // turn off all channels if MCU temperature exceeds this (Deg. C) +#endif +#if defined(FAN_PIN) + #define POWER_MONITOR_FAN_PRESENT +#endif + +// voltage sensing +//#define V_SENSE_PINS {OFF,OFF,OFF,OFF,OFF,OFF,OFF,OFF} // an array of pin#'s corresponding to auxiliary features 1..8 +#ifndef V_SENSE_FORMULA +#define V_SENSE_FORMULA (v*18.405) // scales up typical 0..3.3V to actual V (47k/2.7k resistor voltage divider) +#endif +#ifndef V_SENSE_LIMIT_LOW +#define V_SENSE_LIMIT_LOW 10.5 // 12V nominal, low limit below this all channels are turned OFF +#endif +#ifndef V_SENSE_LIMIT_HIGH +#define V_SENSE_LIMIT_HIGH 13.8 // 12V nominal, high limit above this all channels are turned OFF +#endif +#ifndef V_SENSE_LIMIT_EXCLUDE +#define V_SENSE_LIMIT_EXCLUDE OFF // allow excluding pin# from the checks (variable voltage output) +#endif +#if defined(V_SENSE_PINS) && defined(V_SENSE_FORMULA) + #define POWER_MONITOR_VOLTAGE_PRESENT +#endif + +// current sensing +//#define I_SENSE_PINS {OFF,OFF,OFF,OFF,OFF,OFF,OFF,OFF} // an array of pin#'s corresponding to auxiliary features 1..8 +#ifndef I_SENSE_FORMULA +#define I_SENSE_FORMULA (-((v-1.65)/0.09)) // nominal 3.3V Vcc/2 (at 0A) with scaling (down) at 0.09V/Amp +#endif +#ifndef I_SENSE_CHANNEL_MAX +#define I_SENSE_CHANNEL_MAX {OFF,OFF,OFF,OFF,OFF,OFF,OFF,OFF} // turn off individual channel (1..8) if current exceeds this (Amps) +#endif +#ifndef I_SENSE_COMBINED_MAX +#define I_SENSE_COMBINED_MAX 12 // turn off all channels if combined current exceeds this (Amps) +#endif +#if defined(I_SENSE_PINS) && defined(I_SENSE_FORMULA) + #define POWER_MONITOR_CURRENT_PRESENT +#endif + +#if defined(POWER_MONITOR_VOLTAGE_PRESENT) && defined(POWER_MONITOR_CURRENT_PRESENT) + #define POWER_MONITOR_PRESENT +#endif + +// ----------------------------------------------------------------------------------- +// thermistor configuration settings + +// type 1 +#ifndef THERMISTOR1_TNOM +#define THERMISTOR1_TNOM 25 // nominal temperature (Celsius) +#endif +#ifndef THERMISTOR1_RNOM +#define THERMISTOR1_RNOM 10000 // nominal resistance (Ohms) at nominal temperature +#endif +#ifndef THERMISTOR1_BETA +#define THERMISTOR1_BETA 3950 // beta coefficient +#endif +#ifndef THERMISTOR1_RSERIES +#define THERMISTOR1_RSERIES 4700 // series resistor value (Ohms) +#endif + +// type 2 +#ifndef THERMISTOR2_TNOM +#define THERMISTOR2_TNOM 25 // nominal temperature (Celsius) +#endif +#ifndef THERMISTOR2_RNOM +#define THERMISTOR2_RNOM 10000 // nominal resistance (Ohms) at nominal temperature +#endif +#ifndef THERMISTOR2_BETA +#define THERMISTOR2_BETA 3950 // beta coefficient +#endif +#ifndef THERMISTOR2_RSERIES +#define THERMISTOR2_RSERIES 4700 // series resistor value (Ohms) +#endif + diff --git a/src/Constants.h b/src/Constants.h new file mode 100755 index 0000000..8aff151 --- /dev/null +++ b/src/Constants.h @@ -0,0 +1,163 @@ +// ----------------------------------------------------------------------------------- +// Constants +#pragma once + +// Configuration options ---------------------------------------------------------------------------------------------------- + +// PINMAPS +#define PINMAP_FIRST 1 + +#define FYSETC_E4 1 // FYSETC E4 Version 1.0, *** EXPERIMENTAL, MAY BE REMOVED AT ANY TIME *** + +#define FYSETC_S6 2 // FYSETC S6 Version 1.2, 3D printer board, a 6-axis design +#define FYSETC_S6_1 3 // FYSETC S6 Version 1.2 +#define FYSETC_S6_2 4 // FYSETC S6 Version 2.0, 3D printer board, a 6-axis design + +#define BTT_SKR_PRO 5 // BigTreeTech SKR PRO Version 1.2 +#define BTT_OCTOPUS_PRO 6 // BigTreeTech Octopus Pro Version 1.1 + +#define MiniPCB 7 // small 2-axis design for embedded or mounting behind a panel, Teensy3.2 +#define MiniPCB13 8 // improved version 1.3 adds better support for ESP-01 flashing and optional I2C + +#define MiniPCB2 9 // 2-axis design for small alum. case, Teensy3.2 + +#define MaxPCB 10 // first generation custom 4-axis board, Teensy3.5/Teensy3.6 +#define MaxPCB2 11 // second generation adds alum. case, Teensy3.5/Teensy3.6 +#define MaxPCB3 12 // third generation adds SPI four axis support, Teensy4.1 +#define MaxPCB4 13 // forth generation with SPI/TMC2209 support, Teensy4.1 +#define RESERVED 14 // RESERVED + +#define MaxSTM3 15 // update to the MaxPCB3 using an Blackpill F411CE instead of the Teensy3.5/3.6 +#define MaxSTM3I 16 // as above but using an onboard STM32F411CE with M24C64 EEPROM as default + +#define MaxESP3 17 // adds 4th axis and option to flash the WeMos D1 Mini WiFi through OnStep +#define MaxESP4 18 // for ESP32S w/TMC2209 support +#define CNC3 19 // Arduino CNC Sheild on WeMos D1 R32 (ESP32) +#define MicroScope 20 // MicroScope PCB (ESP32, experimental and may be removed at any point!, USE AY YOUR OWN RISK!!!) + +#define STM32Blue 21 // Khalid and Dave's PCB for STM32 Blue pill (STM32F103CB and STM32F303CC) + +#define JTWSTM 22 // JTW Astronomy JTWSTM telescope mount controller Rev 2.1 +#define MANTICORE 23 // JTW Astronomy MANTICORE telescope mount controller Rev 1.0 + +#define SAL_XB1 24 // SAL-XB1 telescope mount controller + +#define PINMAP_LAST 24 + +// WEATHER sensors (temperature, pressure, and humidity) +#define WEATHER_FIRST 1 +#define BME280 1 // BME280 on I2C (at default address 0x77) +#define BME280_0x77 1 // BME280 on I2C (at address 0x77) +#define BME280_0x76 2 // BME280 on I2C (at address 0x76) +#define BME280_SPI 3 // BME280 on SPI (default CS) +#define BMP280 4 // BMP280 on I2C (at default address 0x77) +#define BMP280_0x77 4 // BMP280 on I2C (at address 0x77) +#define BMP280_0x76 5 // BMP280 on I2C (at address 0x76) +#define BMP280_SPI 6 // BMP280 on SPI (default CS) +#define WEATHER_LAST 6 + +// STEP WAVE FORM +#define STEP_WAVE_FORM_FIRST 1 +#define SQUARE 1 +#define PULSE 2 +#define STEP_WAVE_FORM_LAST 2 + +// MOUNT TYPE +#define MOUNT_TYPE_FIRST 1 +#define MOUNT_SUBTYPE_FIRST 1 +#define GEM 1 // German Equatorial Mount, meridian flips enabled +#define FORK 2 // Fork Mount, meridian flips disabled +#define ALTAZM 3 // Altitude Azimuth Mounts, Dobsonians, etc. +#define ALTALT 4 // Altitude Altitude Mounts +#define MOUNT_SUBTYPE_LAST 4 +#define GEM_TA 5 // GEM, w/tangent arm Declination +#define GEM_TAC 6 // GEM, w/tangent arm Declination and geometry correction +#define FORK_TA 7 // FORK, w/tangent arm Declination +#define FORK_TAC 8 // FORK, w/tangent arm Declination and geometry correction +#define ALTAZM_UNL 9 // ALTAZM, w/unlimited Azmiuth motion +#define MOUNT_TYPE_LAST 9 + +// MOUNT COORDS +#define MOUNT_COORDS_FIRST 1 +#define OBSERVED_PLACE 1 +#define TOPOCENTRIC 2 +#define TOPO_STRICT 3 +#define ASTROMETRIC_J2000 4 +#define MOUNT_COORDS_LAST 4 + +// STARTUP AUTHORITY MODE +#define SA_STRICT 1 +#define SA_AUTO 2 +#define SA_PERMISSIVE 3 + +// TIME LOCATION SOURCE devices supported +#define TLS_FIRST 1 +#define DS3231 1 // DS3231 RTC on I2C +#define DS3234 2 // DS3234 RTC on SPI (DS3234_CS_PIN) Makuna library +#define SD3031 3 // SD3031 RTC on I2C +#define TEENSY 4 // TEENSY3.2 RTC (Built-in) +#define GPS 5 // GPS device +#define NTP 6 // NTP +#define TLS_LAST 6 + +// PIER SIDE +#define PIER_SIDE_FIRST 1 +#define EAST 1 // same as PSS_EAST +#define WEST 2 // same as PSS_WEST +#define BEST 3 // same as PSS_BEST +#define AUTOMATIC 4 // same as PSS_AUTO +#define PIER_SIDE_LAST 4 + +// COMPENSATED TRACKING +#define COMPENSATED_TRACKING_FIRST 1 +#define REFRACTION 1 // refraction compensated tracking RA only +#define REFRACTION_DUAL 2 // refraction compensated tracking both axes +#define MODEL 3 // pointing model compensated tracking RA only +#define MODEL_DUAL 4 // pointing model compensated tracking both axes +#define COMPENSATED_TRACKING_LAST 4 + +// TEMPERATURE sensing devices +#define TEMPERATURE_FIRST 1 +#define DS1820 0x2800000000000000 // DS18B20 1-wire temperature sensors for focusing and dew heaters +#define DS18B20 0x2800000000000000 // as above +#define DS18S20 0x1000000000000000 // as above, except for DS18S20 +#define DS_MASK 0x3F00000000000000 // not for use in Config.h +#define THERMISTOR 1 // General purpose thermistor sensor, type 1 (see Config.defaults.h) +#define THERMISTOR1 1 // as above +#define THERMISTOR2 2 // General purpose thermistor sensor, type 2 (see Config.defaults.h) +#define TEMPERATURE_LAST 2 + +// AUXILIARY FEATURE purpose +#define AUX_FEATURE_PURPOSE_FIRST 1 +#define SWITCH 1 // control an simple on/off switch +#define ANALOG_OUTPUT 2 // control an analog (pwm) output, depends on MCU support +#define ANALOG_OUT 2 // as above +#define DEW_HEATER 3 // control an dew heater +#define INTERVALOMETER 4 // control an camera shutter +#define MOMENTARY_SWITCH 5 // control an simple momentary on/off switch +#define HIDDEN_SWITCH 6 // control an hidden on/off switch (for controlling a pin state at boot) +#define COVER_SWITCH 7 // control an servo driven OTA cover where "on" is closed and "off" is open +#define AUX_FEATURE_PURPOSE_LAST 7 + +// -------------------------------------------------------------------------------------------------------------------------- + +// task manager +#define TASKS_MAX 60 // up to 60 tasks +#define TASKS_SKIP_MISSED // just skip missed tasks if too late +#ifdef ESP32 + #define TASKS_HWTIMERS 4 // up to 4 hardware timers +#else + #define TASKS_HWTIMERS 3 +#endif + +// default start of axis class hardware timers +#define AXIS_HARDWARE_TIMER_BASE 2 // in the OnStepX timer#1 is the sidereal clock + +// enable library features +#define SERIAL_LOCAL_PRESENT +#define SERIAL_ST4_SERVER_PRESENT + +// NV ------------------------------------------------------------------------------------------------------------------- + +// unique volume signature for this volume/partition layout +#define NV_VOLUME_SIGNATURE 0x0001u diff --git a/src/HAL/HAL.cpp b/src/HAL/HAL.cpp new file mode 100755 index 0000000..2d9fa16 --- /dev/null +++ b/src/HAL/HAL.cpp @@ -0,0 +1,6 @@ +// HAL global variables +#include + +// Must match the externs in HAL_FAST_TICKS.h exactly +uint32_t _halTicksPerSecond = 1000000UL; +uint32_t _halTicksPerMicro = 1; diff --git a/src/HAL/HAL.h b/src/HAL/HAL.h new file mode 100755 index 0000000..649449f --- /dev/null +++ b/src/HAL/HAL.h @@ -0,0 +1,190 @@ +/* + * Hardware Abstraction Layer (HAL) for OnStep + * + * Copyright (C) 2018 Khalid Baheyeldin + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once + +// null decoration for non-ESP processors +#ifndef IRAM_ATTR + #define IRAM_ATTR +#endif + +#ifndef ICACHE_RAM_ATTR + #define ICACHE_RAM_ATTR +#endif + +#ifndef FPSTR + #define FPSTR +#endif + +#include "HAL_FAST_TICKS.h" + +#if defined(__AVR_ATmega328P__) + #define MCU_STR "AtMega328" + #include "atmel/Mega328.h" + +#elif defined(__AVR_ATmega1280__) + #define MCU_STR "Mega1280" + #include "atmel/Mega2560.h" + +#elif defined(__AVR_ATmega2560__) + #define MCU_STR "Mega2560" + #include "atmel/Mega2560.h" + +#elif defined(ARDUINO_UNOWIFIR4) + // Arduino UNO R4 WIFI + #define MCU_STR "RENESAS RA4M1 (Arduino UNO R4 WIFI)" + #include "esp32/ESP32UnoR4WiFi.h" + +#elif defined(ARDUINO_ARCH_SAMD) + // ARDUINO M0 + #define MCU_STR "ARDUINO M0" + #include "arduinoM0/ArduinoM0.h" + +#elif defined(ESP32) && ESP_ARDUINO_VERSION >= 0x30000 + // ESP32 w/libraries 3.x + #define MCU_STR "ESP32" + #include "esp/ESP32Libraries3.h" + +#elif defined(ESP32) && ESP_ARDUINO_VERSION >= 0x20000 + // ESP32 w/libraries 2.x + #define MCU_STR "ESP32" + #include "esp/ESP32Libraries2.h" + +#elif defined(ESP8266) + // ESP8266 + #define MCU_STR "ESP8266" + #include "esp/ESP8266.h" + +#elif defined(STM32F103xB) + // STM32F103C8/CB: 72MHz, 128K flash, 64K RAM, ARM Cortex M3 + #define MCU_STR "STM32F103" + #include "stm32/STM32F103.h" + +#elif defined(STM32F303xC) + // RobotDyn BlackPill STM32F303, 256K flash, ARM Cortex M4 (STM32duino board manager) + #define MCU_STR "STM32F303" + #include "stm32/STM32F303.h" + +#elif defined(STM32F401xC) + // Blackpill board with STM32F401CC + #define MCU_STR "STM32F401" + #include "stm32/STM32F4x1.h" + +#elif defined(STM32F411xE) + // Blackpill board with STM32F411CE + #define MCU_STR "STM32F411" + #include "stm32/STM32F4x1.h" + +#elif defined(STM32F407xx) + // BTT SKR PRO board with STM32F407 + #define MCU_STR "STM32F407" + #include "stm32/STM32F407.h" + +#elif defined(STM32F446xx) + // FYSETC S6 board with STM32F446 + #define MCU_STR "STM32F446" + #include "stm32/STM32F446.h" + +#elif defined(STM32H723xx) + #define MCU_STR "STM32H723" + #include "stm32/STM32H7xx.h" + +#elif defined(STM32H743xx) + // WeAct Studio board with STM32H743 + #define MCU_STR "STM32H743" + #include "stm32/STM32H7xx.h" + +#elif defined(STM32H750xx) + // WeAct Studio board with STM32H750 + #define MCU_STR "STM32H750" + #include "stm32/STM32H7xx.h" + +#elif defined(_mk20dx128_h_) || defined(__MK20DX128__) + // Teensy 3.0 + #define MCU_STR "Teensy3.0" + #include "teensy/Teensy3.2.h" + +#elif defined(__MK20DX256__) + // Teensy 3.2 + #define MCU_STR "Teensy3.2" + #include "teensy/Teensy3.2.h" + +#elif defined(__MK64FX512__) + // Teensy 3.5 + #define MCU_STR "Teensy3.5" + #include "teensy/Teensy3.5.h" + +#elif defined(__MK66FX1M0__) + // Teensy 3.6 + #define MCU_STR "Teensy3.6" + #include "teensy/Teensy3.6.h" + +#elif defined(ARDUINO_TEENSY40) + // Teensy 4.0 + #define MCU_STR "Teensy4.0" + #include "teensy/Teensy4.0.h" + +#elif defined(ARDUINO_TEENSY41) + // Teensy 4.1 + #define MCU_STR "Teensy4.1" + #include "teensy/Teensy4.1.h" + +#elif defined(ARDUINO_TEENSY_MICROMOD) + // Teensy MicroMod + #define MCU_STR "TeensyMicroMod" + #include "teensy/Teensy4.1.h" + +#elif defined(ARDUINO_ARCH_RP2040) + // Raspberry pi pico + #define MCU_STR "Raspberry Pi Pico" + #include "mbed/Rpi2040.h" + +#elif defined(ARDUINO_ARCH_RP2350) + // Raspberry pi pico2 + #define MCU_STR "Raspberry Pi Pico2" + #include "mbed/Rpi2350.h" + +#else + // Generic + #warning "Unknown Platform! If this is a new platform, it would probably do best with a new HAL designed for it." + #define MCU_STR "Generic (Unknown)" + #include "default/Default.h" +#endif + +#include "HAL_ANALOG.h" + +#ifndef HAL_INIT + #define HAL_INIT() do { HAL_FAST_TICKS_INIT(); } while (0) +#endif + +// baseline critical task timing +#ifdef HAL_FRACTIONAL_SEC + #define FRACTIONAL_SEC HAL_FRACTIONAL_SEC +#else + #define FRACTIONAL_SEC 100.0F +#endif + +// progmem standin for platforms that don't have it +#ifndef CAT_ATTR + #define CAT_ATTR +#endif + +// default I2C interface +#if defined(HAL_WIRE_CLOCK) + #define HAL_WIRE_SET_CLOCK() HAL_WIRE.setClock(HAL_WIRE_CLOCK) +#else + #define HAL_WIRE_SET_CLOCK() +#endif diff --git a/src/HAL/HAL_ANALOG.h b/src/HAL/HAL_ANALOG.h new file mode 100755 index 0000000..1986fa3 --- /dev/null +++ b/src/HAL/HAL_ANALOG.h @@ -0,0 +1,73 @@ + +// Analog HAL capabilities (defaults) ---------------------------------------------------------------- +// Defaults: no per-pin control, no global reconfig (safe) +#pragma once + +#ifndef HAL_HAS_PER_PIN_PWM_RESOLUTION + #define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#endif +#ifndef HAL_HAS_PER_PIN_PWM_FREQUENCY + #define HAL_HAS_PER_PIN_PWM_FREQUENCY 0 +#endif +#ifndef HAL_HAS_GLOBAL_PWM_RESOLUTION + #define HAL_HAS_GLOBAL_PWM_RESOLUTION 0 +#endif +#ifndef HAL_HAS_GLOBAL_PWM_FREQUENCY + #define HAL_HAS_GLOBAL_PWM_FREQUENCY 0 +#endif + +#ifndef HAL_HAS_PER_PIN_ADC_RESOLUTION + #define HAL_HAS_PER_PIN_ADC_RESOLUTION 0 +#endif +#ifndef HAL_HAS_GLOBAL_ADC_RESOLUTION + #define HAL_HAS_GLOBAL_ADC_RESOLUTION 0 +#endif + +#ifndef HAL_ALLOW_GLOBAL_PWM_RECONFIG + #define HAL_ALLOW_GLOBAL_PWM_RECONFIG 0 +#endif +#ifndef HAL_ALLOW_GLOBAL_ADC_RECONFIG + #define HAL_ALLOW_GLOBAL_ADC_RECONFIG 0 +#endif + +// reasonable bounds (overridden per platform below) +#ifndef HAL_PWM_BITS_MIN + #define HAL_PWM_BITS_MIN 1 +#endif +#ifndef HAL_PWM_BITS_MAX + #define HAL_PWM_BITS_MAX 16 +#endif +#ifndef HAL_ADC_BITS_MIN + #define HAL_ADC_BITS_MIN 1 +#endif +#ifndef HAL_ADC_BITS_MAX + #define HAL_ADC_BITS_MAX 12 +#endif +#ifndef HAL_PWM_HZ_MIN + #define HAL_PWM_HZ_MIN 1U +#endif +#ifndef HAL_PWM_HZ_MAX + #define HAL_PWM_HZ_MAX 40000U +#endif + +// some validation +#if HAL_PWM_BITS_MIN < 1 || HAL_PWM_BITS_MIN > 32 + #error "Error: HAL_PWM_BITS_MIN outside of 1 to 32 range." +#endif +#if HAL_PWM_BITS_MAX < 1 || HAL_PWM_BITS_MAX > 32 + #error "Error: HAL_PWM_BITS_MAX outside of 1 to 32 range." +#endif +#if HAL_ADC_BITS_MIN < 1 || HAL_ADC_BITS_MIN > 32 + #error "Error: HAL_ADC_BITS_MIN outside of 1 to 32 range." +#endif +#if HAL_ADC_BITS_MAX < 1 || HAL_ADC_BITS_MAX > 32 + #error "Error: HAL_ADC_BITS_MAX outside of 1 to 32 range." +#endif + +// global default ranges +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023U // 2^n - 1 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255U // 2^n - 1 +#endif diff --git a/src/HAL/HAL_FAST_TICKS.h b/src/HAL/HAL_FAST_TICKS.h new file mode 100755 index 0000000..70e3117 --- /dev/null +++ b/src/HAL/HAL_FAST_TICKS.h @@ -0,0 +1,248 @@ +// ------------------------------------------------------------------------------------------------- +// HAL_FAST_TICKS.h +// +// Fast ticks: 32-bit wrapping timestamp intended for ISRs / high-rate timing. +// +// Interface: +// HAL_FAST_TICKS_INIT() call once at startup (safe no-op where not needed) +// HAL_FAST_TICKS() returns uint32_t ticks (wrapping OK) +// HAL_TICKS_PER_SECOND() returns uint32_t tick frequency in Hz (cached) +// delayNanoseconds(ns) short busy-wait delay (NOT intended for ISR use) +// nanoseconds32() convenience conversion (NOT intended for ISR use) +// +// Policy (as requested): +// - Teensy 3.x/4.x: use DWT->CYCCNT (enable once). +// - ESP32 Xtensa (ESP32/S2/S3): use CCOUNT. +// - ESP8266: use ESP.getCycleCount(). +// - STM32: most use micros(), STM32H723/H743/H750 use DWT->CYCCNT. +// - ESP32 RISC-V (C3/C6/H2): use micros(). +// - Everything else: use micros(). +// +// Notes: +// - Tick frequency is cached in _halTicksPerSecond/_halTicksPerMicro (defined in HAL.cpp). +// ------------------------------------------------------------------------------------------------- +#pragma once + +#include +#include + +// Make header self-contained (HAL.h may not have been included yet) +#ifndef IRAM_ATTR + #define IRAM_ATTR +#endif + +// Must be defined once in HAL.cpp: +// uint32_t _halTicksPerSecond = 1000000UL; +// uint32_t _halTicksPerMicro = 1; +extern uint32_t _halTicksPerSecond; +extern uint32_t _halTicksPerMicro; + +// ---------------------------------------- +// Platform identification helpers +// ---------------------------------------- + +// Teensy (3.x/4.x) detection +#if defined(TEENSYDUINO) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || \ + defined(ARDUINO_TEENSY_MICROMOD) || \ + defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || \ + defined(__IMXRT1062__) + #define HAL_IS_TEENSY 1 +#else + #define HAL_IS_TEENSY 0 +#endif + +// STM32 H7 detection (STM32duino) +#if defined(ARDUINO_ARCH_STM32) && (defined(STM32H723xx) || defined(STM32H743xx) || defined(STM32H750xx)) + #define HAL_IS_STM32_H7 1 +#else + #define HAL_IS_STM32_H7 0 +#endif + +// ESP32 RISC-V detection (C3/C6/H2) +#if (defined(ARDUINO_ARCH_ESP32) || defined(ESP_PLATFORM)) && (defined(__riscv) || defined(__riscv__)) + #define HAL_IS_ESP32_RISCV 1 +#else + #define HAL_IS_ESP32_RISCV 0 +#endif + +// ESP32 Xtensa detection (ESP32/S2/S3) +#if (defined(ARDUINO_ARCH_ESP32) || defined(ESP_PLATFORM)) && (defined(__XTENSA__) || defined(__xtensa__)) + #define HAL_IS_ESP32_XTENSA 1 +#else + #define HAL_IS_ESP32_XTENSA 0 +#endif + +// ---------------------------------------- +// Platform-specific fast tick source +// ---------------------------------------- + +// ---------- ESP32 Xtensa: CCOUNT ---------- +#if HAL_IS_ESP32_XTENSA + +static inline uint32_t IRAM_ATTR HAL_FAST_TICKS() { + uint32_t ccount; + __asm__ __volatile__("rsr %0, ccount" : "=a"(ccount)); + return ccount; +} + +static inline uint32_t HAL_TICKS_PER_SECOND() { return _halTicksPerSecond; } + +static inline void HAL_FAST_TICKS_INIT() { + #if defined(F_CPU) + _halTicksPerSecond = (uint32_t)F_CPU; + #else + _halTicksPerSecond = (uint32_t)ESP.getCpuFreqMHz() * 1000000UL; + #endif + _halTicksPerMicro = _halTicksPerSecond / 1000000UL; + if (_halTicksPerMicro == 0) _halTicksPerMicro = 1; +} + +// ---------- ESP8266: cycle count ---------- +#elif defined(ESP8266) + +static inline uint32_t IRAM_ATTR HAL_FAST_TICKS() { + return (uint32_t)ESP.getCycleCount(); +} + +static inline uint32_t HAL_TICKS_PER_SECOND() { return _halTicksPerSecond; } + +static inline void HAL_FAST_TICKS_INIT() { + #if defined(F_CPU) + _halTicksPerSecond = (uint32_t)F_CPU; // typically 80 or 160 MHz + #else + _halTicksPerSecond = 80000000UL; + #endif + _halTicksPerMicro = _halTicksPerSecond / 1000000UL; + if (_halTicksPerMicro == 0) _halTicksPerMicro = 1; +} + +// ---------- Teensy 3.x/4.x: DWT->CYCCNT ---------- +#elif HAL_IS_TEENSY + +static inline void HAL_FAST_TICKS_INIT() { + #if defined(DWT) && defined(CoreDebug) && defined(DWT_CTRL_CYCCNTENA_Msk) + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + if ((DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk) == 0) { + DWT->CYCCNT = 0; // optional but recommended + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + __DSB(); __ISB(); + } + #if defined(F_CPU) + _halTicksPerSecond = (uint32_t)F_CPU; + #else + _halTicksPerSecond = 1000000UL; + #endif + #else + // fallback tick source is micros() + _halTicksPerSecond = 1000000UL; + #endif + + _halTicksPerMicro = _halTicksPerSecond / 1000000UL; + if (_halTicksPerMicro == 0) _halTicksPerMicro = 1; +} + +static inline uint32_t HAL_TICKS_PER_SECOND() { return _halTicksPerSecond; } + +static inline uint32_t IRAM_ATTR HAL_FAST_TICKS() { + #if defined(DWT) && defined(DWT_CTRL_CYCCNTENA_Msk) + return (uint32_t)DWT->CYCCNT; + #else + return (uint32_t)micros(); + #endif +} + +// - STM32H7: use DWT->CYCCNT (enable once). +// - Other STM32: use micros(). +#elif HAL_IS_STM32_H7 + +// Some STM32 cores expose these via CMSIS headers already included by Arduino.h. +// If not, you may need: #include "core_cm7.h" + +static inline void HAL_FAST_TICKS_INIT() { + // Ensure SystemCoreClock is up to date (harmless if already correct). + #if defined(SystemCoreClockUpdate) + SystemCoreClockUpdate(); + #endif + + // Enable DWT CYCCNT + #if defined(DWT) && defined(CoreDebug) && defined(DWT_CTRL_CYCCNTENA_Msk) && defined(CoreDebug_DEMCR_TRCENA_Msk) + // Enable tracing (required to access DWT on Cortex-M) + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + #if defined(DWT_LAR) + // DWT->LAR = 0xC5ACCE55; // unlock (if implemented) + #endif + + // Enable the cycle counter + if ((DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk) == 0) { + DWT->CYCCNT = 0; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + __DSB(); __ISB(); + } + + // Tick frequency is core clock + #if defined(F_CPU) + _halTicksPerSecond = (uint32_t)F_CPU; + #elif defined(SystemCoreClock) + _halTicksPerSecond = (uint32_t)SystemCoreClock; + #else + _halTicksPerSecond = 1000000UL; // last-resort fallback + #endif + #else + // If DWT isn't available for some reason, fall back to micros(). + _halTicksPerSecond = 1000000UL; + #endif + + _halTicksPerMicro = _halTicksPerSecond / 1000000UL; + if (_halTicksPerMicro == 0) _halTicksPerMicro = 1; +} + +static inline uint32_t HAL_TICKS_PER_SECOND() { return _halTicksPerSecond; } + +static inline uint32_t IRAM_ATTR HAL_FAST_TICKS() { + #if defined(DWT) && defined(CoreDebug) && defined(DWT_CTRL_CYCCNTENA_Msk) && defined(CoreDebug_DEMCR_TRCENA_Msk) + return (uint32_t)DWT->CYCCNT; // 32-bit wraps naturally + #else + return (uint32_t)micros(); // fallback + #endif +} + +#else + +static inline void HAL_FAST_TICKS_INIT() { + _halTicksPerSecond = 1000000UL; + _halTicksPerMicro = 1; +} + +static inline uint32_t HAL_TICKS_PER_SECOND() { return _halTicksPerSecond; } + +static inline uint32_t IRAM_ATTR HAL_FAST_TICKS() { return (uint32_t)micros(); } + +#endif + +// ---------------------------------------- +// Helpers (non-ISR intent) +// ---------------------------------------- + +static inline uint32_t nanoseconds32() { + // rolls over ~4.29s when returning uint32_t + return (uint32_t)((uint64_t)HAL_FAST_TICKS() * 1000000000ULL / (uint64_t)HAL_TICKS_PER_SECOND()); +} + +static inline uint32_t halNsToTicks(uint32_t ns) { + // ticks = ns * (ticks/us) / 1000 + return (uint32_t)(((uint64_t)ns * (uint64_t)_halTicksPerMicro + 999ULL) / 1000ULL); +} + +#if !HAL_IS_TEENSY + static inline void delayNanoseconds(uint32_t ns) { + if (ns >= 20000UL) { + delayMicroseconds((ns + 999UL) / 1000UL); + return; + } + const uint32_t wait = halNsToTicks(ns); + if (wait == 0) return; + const uint32_t start = HAL_FAST_TICKS(); + while ((uint32_t)(HAL_FAST_TICKS() - start) < wait) { } + } +#endif diff --git a/src/HAL/arduinoM0/ArduinoM0.h b/src/HAL/arduinoM0/ArduinoM0.h new file mode 100755 index 0000000..7801921 --- /dev/null +++ b/src/HAL/arduinoM0/ArduinoM0.h @@ -0,0 +1,57 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +#define __M0__ + +#include + +// Base rate for critical task timing +#define HAL_FRACTIONAL_SEC 100.0F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_WRITE_RANGE can't be changed on this platform" +#endif + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 40 +#define HAL_PULSE_WIDTH 900 // in ns, estimated + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#if NV_DRIVER == NV_DEFAULT + // Library FlashStorage defaults to 1024 bytes https://github.com/cmaglie/FlashStorage + #define E2END 1023 + #undef NV_DRIVER + #define NV_DRIVER NV_M0 +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//---------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +//----------------------------------------------------------------------------------------------------- +// Misc. includes and defines to support this processor's operation + diff --git a/src/HAL/atmel/Mega2560.h b/src/HAL/atmel/Mega2560.h new file mode 100755 index 0000000..2e0fed8 --- /dev/null +++ b/src/HAL/atmel/Mega2560.h @@ -0,0 +1,68 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +// __AVR__ + +// 1/100 second resolution +#define HAL_FRACTIONAL_SEC 100.0F + +// This platform has 8 bit PWM +#ifndef HAL_VCC + #define HAL_VCC 5.0F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#else + #error "Configuration (Config.h): ANALOG_WRITE_RANGE can't be changed on this platform" +#endif + +// This platform doesn't support true double precision math +#define HAL_NO_DOUBLE_PRECISION + +// This is for ~16MHz AVR processors or similar. +#define HAL_SLOW_PROCESSOR + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 76.8 +#define HAL_PULSE_WIDTH 10000 // in ns, estimated + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + analogReference(DEFAULT); \ + HAL_FAST_TICKS_INIT(); \ +} + +#define HAL_RESET() mega2560restart(); +#define HAL_RESET_FUNC void (*mega2560restart) (void) = 0; + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +#define strtof strtod // there is no strtof defined for the AVR platform diff --git a/src/HAL/atmel/Mega328.h b/src/HAL/atmel/Mega328.h new file mode 100755 index 0000000..1d43863 --- /dev/null +++ b/src/HAL/atmel/Mega328.h @@ -0,0 +1,63 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +// __AVR__ + +// 1/100 second resolution +#define HAL_FRACTIONAL_SEC 100.0F + +// This platform has 8 bit PWM +#ifndef HAL_VCC + #define HAL_VCC 5.0F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_WRITE_RANGE can't be changed on this platform" +#endif + +// This platform doesn't support true double precision math +#define HAL_NO_DOUBLE_PRECISION + +// This is for ~16MHz AVR processors or similar. +#define HAL_SLOW_PROCESSOR + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 76.8 +#define HAL_PULSE_WIDTH 10000 // in ns, estimated + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + analogReference(DEFAULT); \ + HAL_FAST_TICKS_INIT(); \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/default/Default.h b/src/HAL/default/Default.h new file mode 100755 index 0000000..a368774 --- /dev/null +++ b/src/HAL/default/Default.h @@ -0,0 +1,55 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more Platform_Name boards based on different lines are supported + +// Base rate for critical task timing +#define HAL_FRACTIONAL_SEC 100.0F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#else + #error "Configuration (Config.h): ANALOG_WRITE_RANGE can't be changed on this platform" +#endif + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#ifndef HAL_MAXRATE_LOWER_LIMIT + #define HAL_MAXRATE_LOWER_LIMIT 60 +#endif +#ifndef HAL_PULSE_WIDTH + #define HAL_PULSE_WIDTH 10000 // in ns, estimated +#endif + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/esp/ESP32Libraries2.h b/src/HAL/esp/ESP32Libraries2.h new file mode 100755 index 0000000..76557a7 --- /dev/null +++ b/src/HAL/esp/ESP32Libraries2.h @@ -0,0 +1,96 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// This is for fast processors with hardware FP +#define HAL_FAST_PROCESSOR + +// Base rate for critical task timing (0.0095s = 0.14", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 105.2631579F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 // always use 2^n - 1, within the platform's limits +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 // always use 2^n - 1, within the platform's limits +#endif + +// analog read/write capabilities +#define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#define HAL_HAS_PER_PIN_PWM_FREQUENCY 0 +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_PWM_FREQUENCY 1 +#define HAL_PWM_HZ_MAX 200000U + +#define HAL_HAS_GLOBAL_ADC_RESOLUTION 1 + +#define HAL_PWM_BITS_MAX 16 +#define HAL_ADC_BITS_MAX 12 + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#if CAN_PLUS != OFF + #define HAL_MAXRATE_LOWER_LIMIT 60 +#else + #define HAL_MAXRATE_LOWER_LIMIT 40 +#endif +#define HAL_PULSE_WIDTH 300 // in ns, measured 12/5/25 (ESP32 v2.0.17) + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_ESP +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +// Correction for ESP32's internal temperture sensor +#ifndef INTERNAL_TEMP_CORRECTION + #define INTERNAL_TEMP_CORRECTION 0 +#endif +#define HAL_TEMP() ( temperatureRead() + INTERNAL_TEMP_CORRECTION ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL, optionally also early init of SERIAL_SIP/PIP or SERIAL_BT +#ifndef SERIAL_BT_MODE + #define SERIAL_BT_MODE OFF +#endif +#if SERIAL_BT_MODE != OFF + #include + extern BluetoothSerial bluetoothSerial; + #define SERIAL_BT bluetoothSerial +#endif +#if SERIAL_BT_MODE == SLAVE + #ifdef SERIAL_BT_PASSKEY + #define SERIAL_BT_BEGIN() if (strlen(SERIAL_BT_PASSKEY) != 0) SERIAL_BT.setPin(SERIAL_BT_PASSKEY); SERIAL_BT.begin(SERIAL_BT_NAME) + #else + #define SERIAL_BT_BEGIN() SERIAL_BT.begin(SERIAL_BT_NAME) + #endif +#else + #define SERIAL_BT_BEGIN() +#endif + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + SERIAL_BT_BEGIN(); \ + HAL_FAST_TICKS_INIT(); \ +} + +// MCU reset +#define HAL_RESET() ESP.restart() + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/esp/ESP32Libraries3.h b/src/HAL/esp/ESP32Libraries3.h new file mode 100755 index 0000000..7858f9a --- /dev/null +++ b/src/HAL/esp/ESP32Libraries3.h @@ -0,0 +1,98 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// This is for fast processors with hardware FP +#define HAL_FAST_PROCESSOR + +// Base rate for critical task timing (0.0095s = 0.14", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 105.2631579F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#else + #error "Configuration (Config.h): ANALOG_WRITE_RANGE can't be changed on this platform" +#endif + +// analog read/write capabilities +#define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#define HAL_HAS_PER_PIN_PWM_FREQUENCY 0 +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_PWM_FREQUENCY 1 +#define HAL_PWM_HZ_MAX 200000U + +#define HAL_HAS_GLOBAL_ADC_RESOLUTION 1 + +#define HAL_PWM_BITS_MAX 16 +#define HAL_ADC_BITS_MAX 12 + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#if CAN_PLUS != OFF + #define HAL_MAXRATE_LOWER_LIMIT 60 +#else + #define HAL_MAXRATE_LOWER_LIMIT 40 +#endif +#define HAL_PULSE_WIDTH 200 // in ns, measured 1/18/22 (ESP32 v2.0.0) + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_ESP +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +// Correction for ESP32's internal temperture sensor +#ifndef INTERNAL_TEMP_CORRECTION + #define INTERNAL_TEMP_CORRECTION 0 +#endif +#define HAL_TEMP() ( temperatureRead() + INTERNAL_TEMP_CORRECTION ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL, optionally also early init of SERIAL_SIP/PIP or SERIAL_BT +#ifndef SERIAL_BT_MODE + #define SERIAL_BT_MODE OFF +#endif +#if SERIAL_BT_MODE != OFF + #include + extern BluetoothSerial bluetoothSerial; + #define SERIAL_BT bluetoothSerial +#endif +#if SERIAL_BT_MODE == SLAVE + #ifdef SERIAL_BT_PASSKEY + #define SERIAL_BT_BEGIN() if (strlen(SERIAL_BT_PASSKEY) != 0) SERIAL_BT.setPin(SERIAL_BT_PASSKEY, strlen(SERIAL_BT_PASSKEY)); SERIAL_BT.begin(SERIAL_BT_NAME) + #else + #define SERIAL_BT_BEGIN() SERIAL_BT.begin(SERIAL_BT_NAME) + #endif +#else + #define SERIAL_BT_BEGIN() +#endif + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + SERIAL_BT_BEGIN(); \ + HAL_FAST_TICKS_INIT(); \ +} + +// MCU reset +#define HAL_RESET() ESP.restart() + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/esp/ESP32UnoR4WiFi.h b/src/HAL/esp/ESP32UnoR4WiFi.h new file mode 100755 index 0000000..d21bd43 --- /dev/null +++ b/src/HAL/esp/ESP32UnoR4WiFi.h @@ -0,0 +1,56 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// This is for fast processors with hardware FP +#define HAL_FAST_PROCESSOR + +// Base rate for critical task timing (0.0095s = 0.14", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 105.2631579F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 60 +#define HAL_PULSE_WIDTH 10000 // in ns, estimated + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #define E2END 2047 + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + HAL_FAST_TICKS_INIT(); \ +} + +// MCU reset +#define HAL_RESET() ESP.restart() + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/esp/ESP8266.h b/src/HAL/esp/ESP8266.h new file mode 100755 index 0000000..205b436 --- /dev/null +++ b/src/HAL/esp/ESP8266.h @@ -0,0 +1,51 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more Platform_Name boards based on different lines are supported + +// Base rate for critical task timing +#define HAL_FRACTIONAL_SEC 100.0F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 60 +#define HAL_PULSE_WIDTH 10000 // in ns, estimated + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#if NV_DRIVER == NV_DEFAULT + #define E2END 1023 + #undef NV_DRIVER + #define NV_DRIVER NV_ESP +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +//----------------------------------------------------------------------------------------------------- +// Misc. includes and defines to support this processor's operation + +// MCU reset +#define HAL_RESET() ESP.restart() diff --git a/src/HAL/mbed/Rpi2040.h b/src/HAL/mbed/Rpi2040.h new file mode 100755 index 0000000..efa4d52 --- /dev/null +++ b/src/HAL/mbed/Rpi2040.h @@ -0,0 +1,52 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// 1/100 second resolution +#define HAL_FRACTIONAL_SEC 100.0F + +// This platform has 8 bit PWM +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_WRITE_RANGE can't be changed on this platform" +#endif + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 76.8 +#define HAL_PULSE_WIDTH 10000 // in ns, estimated + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_AT24C32 +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +// MCU reset +#define HAL_RESET() (*((volatile uint32_t*)(PPB_BASE + 0x0ED0C))) = 0x5FA0004; + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/mbed/Rpi2350.h b/src/HAL/mbed/Rpi2350.h new file mode 100755 index 0000000..efa4d52 --- /dev/null +++ b/src/HAL/mbed/Rpi2350.h @@ -0,0 +1,52 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// 1/100 second resolution +#define HAL_FRACTIONAL_SEC 100.0F + +// This platform has 8 bit PWM +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 // always use 2^n - 1, within the platform's limits +#else + #error "Configuration (Config.h): ANALOG_WRITE_RANGE can't be changed on this platform" +#endif + +// Lower limit (fastest) step rate in us for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 76.8 +#define HAL_PULSE_WIDTH 10000 // in ns, estimated + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_AT24C32 +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +// MCU reset +#define HAL_RESET() (*((volatile uint32_t*)(PPB_BASE + 0x0ED0C))) = 0x5FA0004; + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/stm32/STM32F103.h b/src/HAL/stm32/STM32F103.h new file mode 100755 index 0000000..49cfa29 --- /dev/null +++ b/src/HAL/stm32/STM32F103.h @@ -0,0 +1,65 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more STM32 boards based on different lines are supported +#define __ARM_STM32__ + +// Base rate for critical task timing (0.0095s = 0.14", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 105.2631579F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 34 // assumes optimization set to Fastest (-O3) +#define HAL_PULSE_WIDTH 900 // in ns, estimated + +#include + +// Interrupts +#define cli() noInterrupts() +#define sei() interrupts() + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#undef E2END +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_AT24C32 +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL +#define HAL_INIT() { \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +// always bring in the software serial library early as strange things happen otherwise +#include + +// MCU reset +#define HAL_RESET() NVIC_SystemReset() diff --git a/src/HAL/stm32/STM32F303.h b/src/HAL/stm32/STM32F303.h new file mode 100755 index 0000000..7a67b7e --- /dev/null +++ b/src/HAL/stm32/STM32F303.h @@ -0,0 +1,67 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more STM32 boards based on different lines are supported +#define __ARM_STM32__ + +#define HAL_FAST_PROCESSOR + +// Base rate for critical task timing (0.0057s = 0.09", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 175.4385965F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 20 // assumes optimization set to Fastest (-O3) +#define HAL_PULSE_WIDTH 450 // in ns, estimated + +#include + +// Interrupts +#define cli() noInterrupts() +#define sei() interrupts() + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#undef E2END +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_AT24C32 +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL +#define HAL_INIT() { \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +// always bring in the software serial library early as strange things happen otherwise +#include + +// MCU reset +#define HAL_RESET() NVIC_SystemReset() diff --git a/src/HAL/stm32/STM32F407.h b/src/HAL/stm32/STM32F407.h new file mode 100755 index 0000000..019a1e9 --- /dev/null +++ b/src/HAL/stm32/STM32F407.h @@ -0,0 +1,75 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more STM32 boards based on different lines are supported +#define __ARM_STM32__ + +// Base rate for critical task timing (0.0038s = 0.06", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 263.1578947F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// STM32 Arduino cores (F4 + H7): +// assume global resolution functions exist; per-pin freq/res is not generic. +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_ADC_RESOLUTION 1 + +#define HAL_PWM_BITS_MAX 16 + +// F4 ADC is 12-bit; H7 can be higher, but core support varies. +// Default to 12 and let advanced HAL override to 16 for known-good H7 setup. +#define HAL_ADC_BITS_MAX 12 + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_FAST_PROCESSOR +#define HAL_MAXRATE_LOWER_LIMIT 14 // assumes optimization set to Fastest (-O3) +#define HAL_PULSE_WIDTH 250 // in ns, estimated + +#include + +// Interrupts +#define cli() noInterrupts() +#define sei() interrupts() + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +// no HAL_WIRE_CLOCK, it breaks the STM32 library + +// Non-volatile storage ------------------------------------------------------------------------------ +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_AT24C32 +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL +#define HAL_INIT() { \ + HAL_WIRE.setSDA(PB7); \ + HAL_WIRE.setSCL(PB6); \ +} +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +// always bring in the software serial library early as strange things happen otherwise +#include + +// MCU reset +#define HAL_RESET() NVIC_SystemReset() diff --git a/src/HAL/stm32/STM32F446.h b/src/HAL/stm32/STM32F446.h new file mode 100755 index 0000000..c68c6a3 --- /dev/null +++ b/src/HAL/stm32/STM32F446.h @@ -0,0 +1,86 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more STM32 boards based on different lines are supported +#define __ARM_STM32__ + +// Base rate for critical task timing (0.0038s = 0.06", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 263.1578947F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// STM32 Arduino cores (F4 + H7): +// assume global resolution functions exist; per-pin freq/res is not generic. +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_ADC_RESOLUTION 1 + +#define HAL_PWM_BITS_MAX 16 + +// F4 ADC is 12-bit; H7 can be higher, but core support varies. +// Default to 12 and let advanced HAL override to 16 for known-good H7 setup. +#define HAL_ADC_BITS_MAX 12 + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_FAST_PROCESSOR +#define HAL_MAXRATE_LOWER_LIMIT 14 // assumes optimization set to Fastest (-O3) +#define HAL_PULSE_WIDTH 250 // in ns, estimated + +#include + +// Interrupts +#define cli() noInterrupts() +#define sei() interrupts() + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#undef E2END +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #if PINMAP == FYSETC_S6_2 + // The FYSETC S6 v2 has a 4096 byte EEPROM built-in + #define NV_DRIVER NV_2432 + #elif PINMAP == FYSETC_S6 + // The FYSETC S6 has a 2048 byte EEPROM built-in + #define NV_DRIVER NV_2416 + #else + // Fallback to the EEPROM present on an ZS-042 module + #define NV_DRIVER NV_AT24C32 + #endif +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL +#define HAL_INIT() { \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +// always bring in the software serial library early as strange things happen otherwise +#include + +// MCU reset +#define HAL_RESET() NVIC_SystemReset() diff --git a/src/HAL/stm32/STM32F4x1.h b/src/HAL/stm32/STM32F4x1.h new file mode 100755 index 0000000..405fc0e --- /dev/null +++ b/src/HAL/stm32/STM32F4x1.h @@ -0,0 +1,81 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more STM32 boards based on different lines are supported +#define __ARM_STM32__ + +#define HAL_FAST_PROCESSOR + +// Base rate for critical task timing (0.0057s = 0.09", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 175.4385965F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#ifdef STM32F401xC + #define HAL_MAXRATE_LOWER_LIMIT 20 // assumes optimization set to Fastest (-O3) + #define HAL_PULSE_WIDTH 300 // in ns, estimated +#elif STM32F411xE + #define HAL_MAXRATE_LOWER_LIMIT 20 // Lower limit (fastest) step rate in us (in SQW mode) assumes optimization set to Fastest (-O3) + #define HAL_PULSE_WIDTH 300 // in ns, estimated +#else + #define HAL_MAXRATE_LOWER_LIMIT 60 + #define HAL_PULSE_WIDTH 900 // in ns, estimated +#endif + +#include + +// Interrupts +#define cli() noInterrupts() +#define sei() interrupts() + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#undef E2END +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #if PINMAP == MaxSTM3I + // The MaxPCB3I has an 8192 byte EEPROM built-in (rated for 5M write cycles) + #define NV_DRIVER NV_2464 + #else + // fall back to the DS3231/AT24C32 + #define NV_DRIVER NV_AT24C32 + #endif +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL +#define HAL_INIT() { \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +// always bring in the software serial library early as strange things happen otherwise +#include + +// MCU reset +#define HAL_RESET() NVIC_SystemReset() diff --git a/src/HAL/stm32/STM32H7xx.h b/src/HAL/stm32/STM32H7xx.h new file mode 100755 index 0000000..a80a7e1 --- /dev/null +++ b/src/HAL/stm32/STM32H7xx.h @@ -0,0 +1,80 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more STM32 boards based on different lines are supported +#define __ARM_STM32__ + +// Base rate for critical task timing (0.0019s = 0.03", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 526.3157895F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#else + #error "Configuration (Config.h): ANALOG_READ_RANGE can't be changed on this platform" +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// STM32 Arduino cores (F4 + H7): +// assume global resolution functions exist; per-pin freq/res is not generic. +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_ADC_RESOLUTION 1 + +#define HAL_PWM_BITS_MAX 16 + +// F4 ADC is 12-bit; H7 can be higher, but core support varies. +// Default to 12 and let advanced HAL override to 16 for known-good H7 setup. +#ifndef HAL_ADC_BITS_MAX + #define HAL_ADC_BITS_MAX 12 +#endif + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 1.5 +#define HAL_PULSE_WIDTH 0 // effectively disable pulse mode +#define HAL_FAST_PROCESSOR +#define HAL_VFAST_PROCESSOR + +#include + +// Interrupts +#define cli() noInterrupts() +#define sei() interrupts() + +// New symbol for the default I2C port --------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#undef E2END +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_24256 +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( NAN ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL +#define HAL_INIT() { \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +// always bring in the software serial library early as strange things happen otherwise +#include + +// MCU reset +#define HAL_RESET() NVIC_SystemReset() diff --git a/src/HAL/teensy/Teensy3.2.h b/src/HAL/teensy/Teensy3.2.h new file mode 100755 index 0000000..d38df79 --- /dev/null +++ b/src/HAL/teensy/Teensy3.2.h @@ -0,0 +1,82 @@ +// Platform setup ------------------------------------------------------------------------------------ +// Teensy3.0, 3.1, 3.2 +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +#define __TEENSYDUINO__ + +// This platform has digitalReadFast, digitalWriteFast, etc. +#define HAL_HAS_DIGITAL_FAST + +// Base rate for critical task timing (0.0076s = 0.11", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 131.5789474F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// analog read/write capabilities +#define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#define HAL_HAS_PER_PIN_PWM_FREQUENCY 1 +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_PWM_FREQUENCY 0 +#define HAL_PWM_BITS_MAX 16 +#define HAL_PWM_HZ_MAX 200000U + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#if F_CPU >= 120000000 + #define HAL_MAXRATE_LOWER_LIMIT 10 + #define HAL_PULSE_WIDTH 400 // in ns, estimated +#elif F_CPU >= 96000000 + #define HAL_MAXRATE_LOWER_LIMIT 12 + #define HAL_PULSE_WIDTH 450 // in ns, estimated +#elif F_CPU >= 72000000 + #define HAL_MAXRATE_LOWER_LIMIT 14 + #define HAL_PULSE_WIDTH 500 // in ns, estimated +#else + #define HAL_MAXRATE_LOWER_LIMIT 28 + #define HAL_PULSE_WIDTH 750 // in ns, estimated +#endif + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define _Tpin 38 +#define HAL_TEMP() ( (-((analog.readV(_Tpin) - 0.719F)/0.001715F) + 25.0F ) ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + HAL_FAST_TICKS_INIT(); \ +} + +#define HAL_RESET() { \ + SCB_AIRCR = 0x05FA0004; \ + asm volatile ("dsb"); \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + diff --git a/src/HAL/teensy/Teensy3.5.h b/src/HAL/teensy/Teensy3.5.h new file mode 100755 index 0000000..6c8f1fd --- /dev/null +++ b/src/HAL/teensy/Teensy3.5.h @@ -0,0 +1,74 @@ +// Platform setup ---------------------------------------------------------------------------------- +// Teensy3.5 +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +#define __TEENSYDUINO__ + +// This platform has digitalReadFast, digitalWriteFast, etc. +#define HAL_HAS_DIGITAL_FAST + +// Turn on support for using DACs to output digital signals using Arduino standard commands +#define HAL_DAC_AS_DIGITAL + +// Base rate for critical task timing (0.0057s = 0.09", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 175.4385965F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// analog read/write capabilities +#define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#define HAL_HAS_PER_PIN_PWM_FREQUENCY 1 +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_PWM_FREQUENCY 0 +#define HAL_PWM_BITS_MAX 16 +#define HAL_PWM_HZ_MAX 200000U + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 12 +#define HAL_PULSE_WIDTH 400 // in ns, measured 1/18/22 +#define HAL_FAST_PROCESSOR + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire1 +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ---------------------------------------------------------------------------- +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define _Tpin 70 +#define HAL_TEMP() ( (-(analog.readV(_Tpin)-0.719F)/0.001715F)+25.0F ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + HAL_FAST_TICKS_INIT(); \ +} + +#define HAL_RESET() { \ + SCB_AIRCR = 0x05FA0004; \ + asm volatile ("dsb"); \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/teensy/Teensy3.6.h b/src/HAL/teensy/Teensy3.6.h new file mode 100755 index 0000000..40bfab0 --- /dev/null +++ b/src/HAL/teensy/Teensy3.6.h @@ -0,0 +1,82 @@ +// Platform setup ------------------------------------------------------------------------------------ +// Teensy3.6 +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +#define __TEENSYDUINO__ + +// This platform has digitalReadFast, digitalWriteFast, etc. +#define HAL_HAS_DIGITAL_FAST + +// Turn on support for using DACs to output digital signals using Arduino standard commands +#define HAL_DAC_AS_DIGITAL + +// Base rate for critical task timing (0.0038s = 0.06", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 263.1578947F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// analog read/write capabilities +#define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#define HAL_HAS_PER_PIN_PWM_FREQUENCY 1 +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_PWM_FREQUENCY 0 +#define HAL_PWM_BITS_MAX 16 +#define HAL_PWM_HZ_MAX 200000U + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#if F_CPU >= 240000000 + #define HAL_MAXRATE_LOWER_LIMIT 2 + #define HAL_PULSE_WIDTH 200 // in ns, estimated +#elif F_CPU >= 180000000 + #define HAL_MAXRATE_LOWER_LIMIT 2.6 + #define HAL_PULSE_WIDTH 300 // in ns, estimated +#else + #define HAL_MAXRATE_LOWER_LIMIT 4.8 + #define HAL_PULSE_WIDTH 400 // in ns, estimated +#endif +#define HAL_FAST_PROCESSOR + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire1 +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define _Tpin 70 +#define HAL_TEMP() ( (-(analog.readV(_Tpin)-0.719F)/0.001715F)+25.0F ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + HAL_FAST_TICKS_INIT(); \ +} + +#define HAL_RESET() { \ + SCB_AIRCR = 0x05FA0004; \ + asm volatile ("dsb"); \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation diff --git a/src/HAL/teensy/Teensy4.0.h b/src/HAL/teensy/Teensy4.0.h new file mode 100755 index 0000000..97cbcb3 --- /dev/null +++ b/src/HAL/teensy/Teensy4.0.h @@ -0,0 +1,77 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +#define __TEENSYDUINO__ + +// This platform has digitalReadFast, digitalWriteFast, etc. +#define HAL_HAS_DIGITAL_FAST + +// Base rate for critical task timing (0.0019s = 0.03", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 526.3157895F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// analog read/write capabilities +#define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#define HAL_HAS_PER_PIN_PWM_FREQUENCY 1 +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_PWM_FREQUENCY 0 +#define HAL_PWM_BITS_MAX 16 +#define HAL_PWM_HZ_MAX 200000U + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 1.5 +#define HAL_PULSE_WIDTH 0 // effectively disable pulse mode +#define HAL_FAST_PROCESSOR +#define HAL_VFAST_PROCESSOR + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( tempmonGetTemp() ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + HAL_FAST_TICKS_INIT(); \ +} + +#define HAL_RESET() { \ + SCB_AIRCR = 0x05FA0004; \ + asm volatile ("dsb"); \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +#ifdef EmptyStr + #undef EmptyStr +#endif +#define EmptyStr "\1" + +#define CAT_ATTR PROGMEM diff --git a/src/HAL/teensy/Teensy4.1.h b/src/HAL/teensy/Teensy4.1.h new file mode 100755 index 0000000..97cbcb3 --- /dev/null +++ b/src/HAL/teensy/Teensy4.1.h @@ -0,0 +1,77 @@ +// Platform setup ------------------------------------------------------------------------------------ +#pragma once + +// We define a more generic symbol, in case more Teensy boards based on different lines are supported +#define __TEENSYDUINO__ + +// This platform has digitalReadFast, digitalWriteFast, etc. +#define HAL_HAS_DIGITAL_FAST + +// Base rate for critical task timing (0.0019s = 0.03", 0.2 sec/day) +#define HAL_FRACTIONAL_SEC 526.3157895F + +// Analog read and write +#ifndef HAL_VCC + #define HAL_VCC 3.3F +#endif +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +// analog read/write capabilities +#define HAL_HAS_PER_PIN_PWM_RESOLUTION 0 +#define HAL_HAS_PER_PIN_PWM_FREQUENCY 1 +#define HAL_HAS_GLOBAL_PWM_RESOLUTION 1 +#define HAL_HAS_GLOBAL_PWM_FREQUENCY 0 +#define HAL_PWM_BITS_MAX 16 +#define HAL_PWM_HZ_MAX 200000U + +// Lower limit (fastest) step rate in uS for this platform (in SQW mode) and width of step pulse +#define HAL_MAXRATE_LOWER_LIMIT 1.5 +#define HAL_PULSE_WIDTH 0 // effectively disable pulse mode +#define HAL_FAST_PROCESSOR +#define HAL_VFAST_PROCESSOR + +// New symbol for the default I2C port ------------------------------------------------------------- +#include +#ifndef HAL_WIRE + #define HAL_WIRE Wire +#endif +#ifndef HAL_WIRE_CLOCK + #define HAL_WIRE_CLOCK 100000 +#endif + +// Non-volatile storage ------------------------------------------------------------------------------ +#if NV_DRIVER == NV_DEFAULT + #undef NV_DRIVER + #define NV_DRIVER NV_EEPROM +#endif + +//-------------------------------------------------------------------------------------------------- +// Internal MCU temperature (in degrees C) +#define HAL_TEMP() ( tempmonGetTemp() ) + +//-------------------------------------------------------------------------------------------------- +// General purpose initialize for HAL + +#define HAL_INIT() { \ + HAL_FAST_TICKS_INIT(); \ +} + +#define HAL_RESET() { \ + SCB_AIRCR = 0x05FA0004; \ + asm volatile ("dsb"); \ +} + +//--------------------------------------------------------------------------------------------------- +// Misc. includes to support this processor's operation + +#ifdef EmptyStr + #undef EmptyStr +#endif +#define EmptyStr "\1" + +#define CAT_ATTR PROGMEM diff --git a/src/Validate.h b/src/Validate.h new file mode 100755 index 0000000..9610dc8 --- /dev/null +++ b/src/Validate.h @@ -0,0 +1,1466 @@ +// ------------------------------------------------------------------------------------------------- +// Validate configuration +#pragma once +#include "Common.h" + +#if !defined(FileVersionConfig) || FileVersionConfig != 6 // per FirmwareVersionConfig + #error "Configuration (Config.h): FileVersionConfig (Config.h version) must be 6 for this OnStep." +#endif + +// BACKWARDS COMPATABILITY ------------------------ +#ifdef AXIS3_SLEW_RATE_DESIRED + #warning "Configuration (Config.h): AXIS3_SLEW_RATE_DESIRED has been replaced with AXIS3_SLEW_RATE_BASE_DESIRED please update" + #define AXIS3_SLEW_RATE_BASE_DESIRED AXIS3_SLEW_RATE_DESIRED +#endif +#ifdef AXIS4_SLEW_RATE_DESIRED + #warning "Configuration (Config.h): AXIS4_SLEW_RATE_DESIRED has been replaced with AXIS4_SLEW_RATE_BASE_DESIRED please update" + #define AXIS4_SLEW_RATE_BASE_DESIRED AXIS4_SLEW_RATE_DESIRED +#endif +#ifdef AXIS5_SLEW_RATE_DESIRED + #warning "Configuration (Config.h): AXIS5_SLEW_RATE_DESIRED has been replaced with AXIS5_SLEW_RATE_BASE_DESIRED please update" + #define AXIS5_SLEW_RATE_BASE_DESIRED AXIS5_SLEW_RATE_DESIRED +#endif + +// GENERAL --------------------------------------- +#if defined(STEP_DIR_TMC_UART_PRESENT) && (!defined(SERIAL_TMC) || !defined(SERIAL_TMC_BAUD)) + #error "Configuration (Config.h): This PINMAP doesn't support TMC UART mode drivers" +#endif + +// TELESCOPE ------------------------------------- +#if PINMAP != OFF && (PINMAP < PINMAP_FIRST || PINMAP > PINMAP_LAST) + #error "Configuration (Config.h): PINMAP must be set to a valid board (from Constants.h) or OFF (for user pin defs in Config.h)" +#endif + +#if SERIAL_A_BAUD_DEFAULT != 9600 && SERIAL_A_BAUD_DEFAULT != 19200 && SERIAL_A_BAUD_DEFAULT != 38400 && \ + SERIAL_A_BAUD_DEFAULT != 57600 && SERIAL_A_BAUD_DEFAULT != 115200 && SERIAL_A_BAUD_DEFAULT != 230400 && \ + SERIAL_A_BAUD_DEFAULT != 460800 && SERIAL_A_BAUD_DEFAULT != 921600 && SERIAL_A_BAUD_DEFAULT != OFF + #warning "Configuration (Config.h): Setting SERIAL_A_BAUD_DEFAULT unknown, use 9600, 19200, 38400, 57600, 115200, 230400, 460800, or 921600 (baud.)" +#endif + +#if SERIAL_B_BAUD_DEFAULT != 9600 && SERIAL_B_BAUD_DEFAULT != 19200 && SERIAL_B_BAUD_DEFAULT != 38400 && \ + SERIAL_B_BAUD_DEFAULT != 57600 && SERIAL_B_BAUD_DEFAULT != 115200 && SERIAL_B_BAUD_DEFAULT != 230400 && \ + SERIAL_B_BAUD_DEFAULT != 460800 && SERIAL_B_BAUD_DEFAULT != 921600 && SERIAL_B_BAUD_DEFAULT != OFF + #warning "Configuration (Config.h): Setting SERIAL_B_BAUD_DEFAULT unknown, use OFF, 9600, 19200, 38400, 57600, 115200, 230400, 460800, or 921600 (baud.)" +#endif + +#if SERIAL_B_ESP_FLASHING != ON && SERIAL_B_ESP_FLASHING != OFF + #error "Configuration (Config.h): Setting SERIAL_B_ESP_FLASHING unknown, use OFF or ON." +#endif + +#if SERIAL_B_ESP_FLASHING == ON && SERIAL_B_BAUD_DEFAULT == OFF + #error "Configuration (Config.h): Setting SERIAL_B_ESP_FLASHING only supported if SERIAL_B_BAUD_DEFAULT is defined." +#endif + +#if SERIAL_B_ESP_FLASHING == ON && (ADDON_GPIO0_PIN == OFF || ADDON_RESET_PIN == OFF) + #error "Configuration (Config.h): Setting SERIAL_B_ESP_FLASHING only supported if ADDON_GPIO0_PIN and ADDON_RESET_PIN are defined." +#endif + +#if SERIAL_C_BAUD_DEFAULT != 9600 && SERIAL_C_BAUD_DEFAULT != 19200 && SERIAL_C_BAUD_DEFAULT != 38400 && \ + SERIAL_C_BAUD_DEFAULT != 57600 && SERIAL_C_BAUD_DEFAULT != 115200 && SERIAL_C_BAUD_DEFAULT != 230400 && \ + SERIAL_C_BAUD_DEFAULT != 460800 && SERIAL_C_BAUD_DEFAULT != 921600 && SERIAL_C_BAUD_DEFAULT != OFF + #warning "Configuration (Config.h): Setting SERIAL_C_BAUD_DEFAULT unknown, use OFF, 9600, 19200, 38400, 57600, 115200, 230400, 460800, or 921600 (baud.)" +#endif + +#if SERIAL_D_BAUD_DEFAULT != 9600 && SERIAL_D_BAUD_DEFAULT != 19200 && SERIAL_D_BAUD_DEFAULT != 38400 && \ + SERIAL_D_BAUD_DEFAULT != 57600 && SERIAL_D_BAUD_DEFAULT != 115200 && SERIAL_D_BAUD_DEFAULT != 230400 && \ + SERIAL_D_BAUD_DEFAULT != 460800 && SERIAL_D_BAUD_DEFAULT != 921600 && SERIAL_D_BAUD_DEFAULT != OFF + #warning "Configuration (Config.h): Setting SERIAL_D_BAUD_DEFAULT unknown, use OFF, 9600, 19200, 38400, 57600, 115200, 230400, 460800, or 921600 (baud.)" +#endif + +#if SERIAL_E_BAUD_DEFAULT != 9600 && SERIAL_E_BAUD_DEFAULT != 19200 && SERIAL_E_BAUD_DEFAULT != 38400 && \ + SERIAL_E_BAUD_DEFAULT != 57600 && SERIAL_E_BAUD_DEFAULT != 115200 && SERIAL_E_BAUD_DEFAULT != 230400 && \ + SERIAL_E_BAUD_DEFAULT != 460800 && SERIAL_E_BAUD_DEFAULT != 921600 && SERIAL_E_BAUD_DEFAULT != OFF + #warning "Configuration (Config.h): Setting SERIAL_E_BAUD_DEFAULT unknown, use OFF, 9600, 19200, 38400, 57600, 115200, 230400, 460800, or 921600 (baud.)" +#endif + +#if SERIAL_RADIO != OFF && SERIAL_RADIO != BLUETOOTH && SERIAL_RADIO != WIFI_ACCESS_POINT && SERIAL_RADIO != WIFI_STATION + #warning "Configuration (Config.h): Setting SERIAL_RADIO unknown, use OFF, BLUETOOTH, WIFI_ACCESS_POINT, or WIFI_STATION" +#endif + +#if STATUS_LED != OFF && STATUS_LED != ON + #error "Configuration (Config.h): Setting STATUS_LED unknown, use OFF or ON." +#endif + +#if RETICLE_LED_DEFAULT != OFF && (RETICLE_LED_DEFAULT < 0 || RETICLE_LED_DEFAULT > 255) + #error "Configuration (Config.h): Setting RETICLE_LED_DEFAULT unknown, use OFF or 0 to 255." +#endif + +#if RETICLE_LED_MEMORY != OFF && RETICLE_LED_MEMORY != ON + #error "Configuration (Config.h): Setting RETICLE_LED_MEMORY unknown, use OFF or ON." +#endif + +#if WEATHER != OFF && (WEATHER < WEATHER_FIRST || WEATHER > WEATHER_LAST) + #error "Configuration (Config.h): Setting WEATHER unknown, use OFF or valid WEATHER SENSOR (from Constants.h)" +#endif + +#if STEP_WAVE_FORM < STEP_WAVE_FORM_FIRST || STEP_WAVE_FORM > STEP_WAVE_FORM_LAST + #error "Configuration (Config.h): Setting STEP_WAVE_FORM unknown, use a valid STEP WAVE FORM (from Constants.h)" +#endif + +#if STEP_WAVE_FORM != SQUARE && (defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41)) + #error "Configuration (Config.h): Setting STEP_WAVE_FORM SQUARE is required for the Teensy4.0 and 4.1" +#endif + +// MOUNT ----------------------------------------- + +#if (AXIS1_DRIVER_MODEL != OFF && AXIS2_DRIVER_MODEL == OFF) || \ + (AXIS1_DRIVER_MODEL == OFF && AXIS2_DRIVER_MODEL != OFF) + #error "Configuration (Config.h): Settings AXIS1_DRIVER_MODEL and AXIS2_DRIVER_MODEL must both be OFF or set to a valid DRIVER (from Constants.h)" +#endif + +// AXIS1 RA/AZM +#if AXIS1_DRIVER_MODEL != OFF && \ + (AXIS1_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS1_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS1_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS1_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS1_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS1_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS1_DRIVER_STATUS != OFF && AXIS1_DRIVER_STATUS != ON && AXIS1_DRIVER_STATUS != HIGH && AXIS1_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS1_DRIVER_STATUS unknown, use OFF or a valid driver status." +#endif + +#ifdef AXIS1_STEP_DIR_PRESENT + #if AXIS1_DRIVER_MICROSTEPS != OFF && (AXIS1_DRIVER_MICROSTEPS < 1 || AXIS1_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS1_DRIVER_MICROSTEPS != 1 && AXIS1_DRIVER_MICROSTEPS != 2 && AXIS1_DRIVER_MICROSTEPS != 4 && \ + AXIS1_DRIVER_MICROSTEPS != 8 && AXIS1_DRIVER_MICROSTEPS != 16 && AXIS1_DRIVER_MICROSTEPS != 32 && \ + AXIS1_DRIVER_MICROSTEPS != 64 && AXIS1_DRIVER_MICROSTEPS != 128 && AXIS1_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS1_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS1_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS1_DRIVER_MICROSTEPS_GOTO < 1 || AXIS1_DRIVER_MICROSTEPS_GOTO >= AXIS1_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_MICROSTEPS_GOTO unknown, use a valid microstep setting (range 1 to < AXIS1_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS1_DRIVER_MICROSTEPS_GOTO != 1 && AXIS1_DRIVER_MICROSTEPS_GOTO != 2 && AXIS1_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS1_DRIVER_MICROSTEPS_GOTO != 8 && AXIS1_DRIVER_MICROSTEPS_GOTO != 16 && AXIS1_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS1_DRIVER_MICROSTEPS_GOTO != 64 && AXIS1_DRIVER_MICROSTEPS_GOTO != 128 && AXIS1_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS1_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS1_DRIVER_MICROSTEPS == OFF && AXIS1_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS1_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS1_DRIVER_MICROSTEPS." + #endif + #if AXIS1_DRIVER_DECAY != OFF && (AXIS1_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS1_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS1_DRIVER_DECAY_GOTO != OFF && (AXIS1_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS1_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS1_DRIVER_IRUN != OFF && (AXIS1_DRIVER_IRUN < 0 || AXIS1_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS1_DRIVER_IHOLD != OFF && (AXIS1_DRIVER_IHOLD < 0 || AXIS1_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS1_DRIVER_IGOTO != OFF && (AXIS1_DRIVER_IGOTO < 0 || AXIS1_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS1_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif +#endif + +#ifdef AXIS1_SERVO_PRESENT + #if AXIS1_ENCODER < ENC_FIRST || AXIS1_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS1_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +#if AXIS1_REVERSE != ON && AXIS1_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS1_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS1_POWER_DOWN != ON && AXIS1_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS1_POWER_DOWN unknown, use OFF or ON." +#endif + +#if AXIS1_SECTOR_GEAR != ON && AXIS1_SECTOR_GEAR != OFF + #error "Configuration (Config.h): Setting AXIS1_SECTOR_GEAR unknown, use OFF or ON." +#endif + +#if AXIS1_SECTOR_GEAR == ON + #if MOUNT_SUBTYPE != GEM && MOUNT_SUBTYPE != FORK + #error "Configuration (Config.h): Setting MOUNT_TYPE is not compatible with AXIS1_SECTOR_GEAR ON (from Constants.h)" + #endif + #if (AXIS1_SENSE_HOME) != OFF && (AXIS1_SENSE_HOME_OFFSET) != 0 + #error "Configuration (Config.h): Enabling AXIS1_SECTOR_GEAR and AXIS1_SENSE_HOME requires an AXIS1_SENSE_HOME_OFFSET of 0." + #endif + #if (AXIS2_SENSE_HOME) != OFF && AXIS2_TANGENT_ARM == OFF + #error "Configuration (Config.h): Enabling AXIS1_SECTOR_GEAR requires AXIS2_SENSE_HOME to be OFF (except for tangent arm Dec mounts.)" + #endif +#else + #if AXIS2_TANGENT_ARM == OFF && ((AXIS1_SENSE_HOME) == OFF && (AXIS2_SENSE_HOME) != OFF) + #error "Configuration (Config.h): Enabling AXIS2_SENSE_HOME requires enabling AXIS1_SENSE_HOME or AXIS2_TANGENT_ARM." + #endif + #if AXIS1_LIMIT_MIN < -360 || AXIS1_LIMIT_MIN > -90 + #error "Configuration (Config.h): Setting AXIS1_LIMIT_MIN unknown, use value in the range -90 to -360." + #endif + #if AXIS1_LIMIT_MAX < 90 || AXIS1_LIMIT_MAX > 360 + #error "Configuration (Config.h): Setting AXIS1_LIMIT_MAX unknown, use value in the range 90 to 360." + #endif +#endif + +#if (AXIS1_SENSE_HOME) != OFF && (AXIS1_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS1_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS1_SENSE_LIMIT_MIN) != OFF && (AXIS1_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS1_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS1_SENSE_LIMIT_MAX) != OFF && (AXIS1_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS1_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +// AXIS2 DEC/ALT +#if AXIS2_DRIVER_MODEL != OFF && \ + (AXIS2_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS2_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS2_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS2_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS2_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS2_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_MODEL unknown, use a valid DRIVER (from Constants.h)" +#endif + +#if AXIS2_DRIVER_STATUS != OFF && AXIS2_DRIVER_STATUS != ON && AXIS2_DRIVER_STATUS != HIGH && AXIS2_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS2_DRIVER_STATUS unknown, use OFF or a valid driver status." +#endif + +#ifdef AXIS2_STEP_DIR_PRESENT + #if AXIS2_DRIVER_MICROSTEPS != OFF && (AXIS2_DRIVER_MICROSTEPS < 1 || AXIS2_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS2_DRIVER_MICROSTEPS != 1 && AXIS2_DRIVER_MICROSTEPS != 2 && AXIS2_DRIVER_MICROSTEPS != 4 && \ + AXIS2_DRIVER_MICROSTEPS != 8 && AXIS2_DRIVER_MICROSTEPS != 16 && AXIS2_DRIVER_MICROSTEPS != 32 && \ + AXIS2_DRIVER_MICROSTEPS != 64 && AXIS2_DRIVER_MICROSTEPS != 128 && AXIS2_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS2_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS2_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS2_DRIVER_MICROSTEPS_GOTO < 1 || AXIS2_DRIVER_MICROSTEPS_GOTO >= AXIS2_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS2_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS2_DRIVER_MICROSTEPS_GOTO != 1 && AXIS2_DRIVER_MICROSTEPS_GOTO != 2 && AXIS2_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS2_DRIVER_MICROSTEPS_GOTO != 8 && AXIS2_DRIVER_MICROSTEPS_GOTO != 16 && AXIS2_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS2_DRIVER_MICROSTEPS_GOTO != 64 && AXIS2_DRIVER_MICROSTEPS_GOTO != 128 && AXIS2_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS2_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS2_DRIVER_MICROSTEPS == OFF && AXIS2_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS2_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS2_DRIVER_MICROSTEPS." + #endif + #if AXIS2_DRIVER_DECAY != OFF && (AXIS2_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS2_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS2_DRIVER_DECAY_GOTO != OFF && (AXIS2_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS2_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS2_DRIVER_IRUN != OFF && (AXIS2_DRIVER_IRUN < 0 || AXIS2_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS2_DRIVER_IHOLD != OFF && (AXIS2_DRIVER_IHOLD < 0 || AXIS2_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS2_DRIVER_IGOTO != OFF && (AXIS2_DRIVER_IGOTO < 0 || AXIS2_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS2_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif +#endif + +#ifdef AXIS2_SERVO_PRESENT + #if AXIS2_ENCODER < ENC_FIRST || AXIS2_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS2_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +#if defined(AXIS1_SERVO_PRESENT) && \ + (AXIS1_ENCODER == AS37_H39B_B || AXIS1_ENCODER == JTW_24BIT || AXIS1_ENCODER == JTW_26BIT || AXIS1_ENCODER == LIKA_ASC85 || \ + (AXIS1_ENCODER == SERIAL_BRIDGE && SERIAL_ENCODER_ABSOLUTE == ON)) + #define HAS_ABSOLUTE_ENCODER_SERVO_AXIS1 +#endif + +#if defined(AXIS2_SERVO_PRESENT) && \ + (AXIS2_ENCODER == AS37_H39B_B || AXIS2_ENCODER == JTW_24BIT || AXIS2_ENCODER == JTW_26BIT || AXIS2_ENCODER == LIKA_ASC85 || \ + (AXIS2_ENCODER == SERIAL_BRIDGE && SERIAL_ENCODER_ABSOLUTE == ON)) + #define HAS_ABSOLUTE_ENCODER_SERVO_AXIS2 +#endif + +#if defined(HAS_ABSOLUTE_ENCODER_SERVO_AXIS1) && defined(HAS_ABSOLUTE_ENCODER_SERVO_AXIS2) + #define HAS_ABSOLUTE_ENCODER_SERVO +#endif + +#if AXIS1_DRIVER_MODEL == ODRIVE + #define AXIS1_HAS_ABSOLUTE_COORD_AUTHORITY ODRIVE_ABSOLUTE +#elif defined(HAS_ABSOLUTE_ENCODER_SERVO_AXIS1) + #define AXIS1_HAS_ABSOLUTE_COORD_AUTHORITY ON +#else + #define AXIS1_HAS_ABSOLUTE_COORD_AUTHORITY OFF +#endif + +#if AXIS2_DRIVER_MODEL == ODRIVE + #define AXIS2_HAS_ABSOLUTE_COORD_AUTHORITY ODRIVE_ABSOLUTE +#elif defined(HAS_ABSOLUTE_ENCODER_SERVO_AXIS2) + #define AXIS2_HAS_ABSOLUTE_COORD_AUTHORITY ON +#else + #define AXIS2_HAS_ABSOLUTE_COORD_AUTHORITY OFF +#endif + +#if (AXIS1_HAS_ABSOLUTE_COORD_AUTHORITY == ON && AXIS2_HAS_ABSOLUTE_COORD_AUTHORITY == OFF) || \ + (AXIS1_HAS_ABSOLUTE_COORD_AUTHORITY == OFF && AXIS2_HAS_ABSOLUTE_COORD_AUTHORITY == ON) + #error "Configuration (Config.h): Axes 1 and 2 must both use absolute coordinate authority or neither may use it." +#endif + +#if AXIS2_REVERSE != ON && AXIS2_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS2_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS2_POWER_DOWN != ON && AXIS2_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS2_POWER_DOWN unknown, use OFF or ON." +#endif + +#if AXIS2_LIMIT_MIN < -90 || AXIS2_LIMIT_MIN > 0 + #error "Configuration (Config.h): Setting AXIS2_LIMIT_MIN unknown, use value in the range -90 to 0." +#endif + +#if AXIS2_LIMIT_MAX < 0 || AXIS2_LIMIT_MAX > 90 + #error "Configuration (Config.h): Setting AXIS2_LIMIT_MAX unknown, use value in the range 0 to 90." +#endif + +#if (AXIS2_SENSE_HOME) != OFF && (AXIS2_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS2_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS2_SENSE_LIMIT_MIN) != OFF && (AXIS2_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS2_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS2_SENSE_LIMIT_MAX) != OFF && (AXIS2_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS2_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if MOUNT_SUBTYPE < MOUNT_SUBTYPE_FIRST || MOUNT_SUBTYPE > MOUNT_SUBTYPE_LAST + #error "Configuration (Config.h): Setting MOUNT_TYPE unknown, use a valid MOUNT TYPE (from Constants.h)" +#endif + +#if AXIS2_TANGENT_ARM != ON && AXIS2_TANGENT_ARM != OFF + #error "Configuration (Config.h): Setting AXIS2_TANGENT_ARM unknown, use OFF or ON." +#endif + +#if AXIS2_TANGENT_ARM == ON + #if MOUNT_SUBTYPE != GEM && MOUNT_SUBTYPE != FORK + #error "Configuration (Config.h): Setting MOUNT_TYPE is not compatible with AXIS2_TANGENT_ARM ON (from Constants.h)" + #endif + #if (AXIS2_SENSE_HOME) != OFF && (AXIS2_SENSE_HOME_OFFSET) != 0 + #error "Configuration (Config.h): Enabling AXIS2_TANGENT_ARM and AXIS2_SENSE_HOME requires an AXIS2_SENSE_HOME_OFFSET of 0." + #endif + #if (AXIS1_SENSE_HOME) != OFF && AXIS1_SECTOR_GEAR == OFF + #error "Configuration (Config.h): Enabling AXIS2_TANGENT_ARM requires AXIS1_SENSE_HOME to be OFF (except for sector gear RA mounts.)" + #endif +#else + #if (AXIS2_SENSE_HOME) != OFF && (AXIS1_SENSE_HOME) == OFF + #error "Configuration (Config.h): Enabling AXIS2_SENSE_HOME requires enabling AXIS1_SENSE_HOME or AXIS2_TANGENT_ARM." + #endif +#endif + +#if AXIS2_TANGENT_ARM_CORRECTION != ON && AXIS2_TANGENT_ARM_CORRECTION != OFF + #error "Configuration (Config.h): Setting AXIS2_TANGENT_ARM_CORRECTION unknown, use OFF or ON." +#endif + +#if MOUNT_ALTERNATE_ORIENTATION != OFF && MOUNT_ALTERNATE_ORIENTATION != ON + #error "Configuration (Config.h): Setting MOUNT_ALTERNATE_ORIENTATION unknown, use ON or OFF" +#endif + +#if MOUNT_COORDS < MOUNT_COORDS_FIRST && MOUNT_COORDS > MOUNT_COORDS_LAST + #error "Configuration (Config.h): Setting MOUNT_COORDS unknown, use a valid MOUNT COORDS (from Constants.h)" +#endif + +#if MOUNT_COORDS_MEMORY != ON && MOUNT_COORDS_MEMORY != OFF + #error "Configuration (Config.h): Setting MOUNT_COORDS_MEMORY unknown, use ON or OFF" +#endif + +#if MOUNT_STARTUP_MODE < SA_STRICT || MOUNT_STARTUP_MODE > SA_PERMISSIVE + #error "Configuration (Config.h): Setting MOUNT_STARTUP_MODE unknown, use SA_STRICT, SA_AUTO, or SA_PERMISSIVE" +#endif + +#if NV_INIT_ERROR_REVOKES_AUTHORITY != ON && NV_INIT_ERROR_REVOKES_AUTHORITY != OFF + #error "Configuration (Config.h): Setting NV_INIT_ERROR_REVOKES_AUTHORITY unknown, use OFF or ON." +#endif + +#if MOUNT_ENABLE_IN_STANDBY != ON && MOUNT_ENABLE_IN_STANDBY != OFF + #error "Configuration (Config.h): Setting MOUNT_ENABLE_IN_STANDBY unknown, use ON or OFF" +#endif + +#if ALIGN_MAX_STARS != AUTO && (ALIGN_MAX_STARS < 1 && ALIGN_MAX_STARS > 9) + #error "Configuration (Config.h): Setting ALIGN_MAX_STARS unknown, use AUTO or a value from 1 to 9." +#endif + +// TIME AND LOCATION +#if TIME_LOCATION_SOURCE < TLS_FIRST && TIME_LOCATION_SOURCE > TLS_LAST + #error "Configuration (Config.h): Setting TIME_LOCATION_SOURCE unknown, use OFF or valid TIME LOCATION SOURCE (from Constants.h)" +#endif + +#if TIME_LOCATION_SOURCE_FALLBACK != OFF + #if (TIME_LOCATION_SOURCE_FALLBACK < TLS_FIRST && TIME_LOCATION_SOURCE_FALLBACK > TLS_LAST) || \ + TIME_LOCATION_SOURCE_FALLBACK == GPS || TIME_LOCATION_SOURCE_FALLBACK == NTP || \ + TIME_LOCATION_SOURCE_FALLBACK == TIME_LOCATION_SOURCE + #error "Configuration (Config.h): Setting TIME_LOCATION_SOURCE_FALLBACK unknown, use OFF or valid alternate TIME LOCATION SOURCE (except GPS or NTP, from Constants.h)" + #endif +#endif + +#if (TIME_LOCATION_PPS_SENSE) != OFF && \ + (TIME_LOCATION_PPS_SENSE) != LOW && \ + (TIME_LOCATION_PPS_SENSE) != HIGH && \ + (TIME_LOCATION_PPS_SENSE) != BOTH + #error "Configuration (Config.h): Setting TIME_LOCATION_PPS_SENSE unknown, use OFF or LOW or HIGH or BOTH." +#endif + +// USER FEEDBACK +#if STATUS_MOUNT_LED != ON && STATUS_MOUNT_LED != OFF + #error "Configuration (Config.h): Setting STATUS_MOUNT_LED unknown, use OFF or ON." +#endif + +#if STATUS_BUZZER != ON && STATUS_BUZZER != OFF && (STATUS_BUZZER < 100 || STATUS_BUZZER > 6000) + #error "Configuration (Config.h): Setting STATUS_BUZZER unknown, use OFF or ON or a value from 100 to 6000 (Hz.)" +#endif + +#if STATUS_BUZZER_DEFAULT != ON && STATUS_BUZZER_DEFAULT != OFF + #error "Configuration (Config.h): Setting STATUS_BUZZER_DEFAULT unknown, use OFF or ON." +#endif + +#if STATUS_BUZZER_MEMORY != ON && STATUS_BUZZER_MEMORY != OFF + #error "Configuration (Config.h): Setting STATUS_BUZZER_MEMORY unknown, use OFF or ON." +#endif + +// ST4 INTERFACE +#if ST4_INTERFACE != ON && ST4_INTERFACE != OFF + #error "Configuration (Config.h): Setting ST4_INTERFACE unknown, use OFF or ON." +#endif + +#if ST4_HAND_CONTROL != ON && ST4_HAND_CONTROL != OFF + #error "Configuration (Config.h): Setting ST4_HAND_CONTROL unknown, use OFF or ON." +#endif + +#if ST4_HAND_CONTROL_FOCUSER != ON && ST4_HAND_CONTROL_FOCUSER != OFF + #error "Configuration (Config.h): Setting ST4_HAND_CONTROL_FOCUSER unknown, use OFF or ON." +#endif + +// GUIDING +#if GUIDE_TIME_LIMIT < 0 || GUIDE_TIME_LIMIT > 120 + #error "Configuration (Config.h): Setting GUIDE_TIME_LIMIT unknown, use the value 0 to disable or 1 to 120 (seconds.)" +#endif + +#if GUIDE_DISABLE_BACKLASH != ON && GUIDE_DISABLE_BACKLASH != OFF + #error "Configuration (Config.h): Setting GUIDE_DISABLE_BACKLASH unknown, use OFF or ON." +#endif + +#if GUIDE_SEPARATE_PULSE_RATE != ON && GUIDE_SEPARATE_PULSE_RATE != OFF + #error "Configuration (Config.h): Setting GUIDE_SEPARATE_PULSE_RATE unknown, use OFF or ON." +#endif + +// SENSORS +#if (LIMIT_SENSE) != OFF && (LIMIT_SENSE) < 0 + #error "Configuration (Config.h): Setting LIMIT_SENSE unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +// PEC (max steps per degree 360000 * 360 degrees) +#if PEC_STEPS_PER_WORM_ROTATION < 0 || PEC_STEPS_PER_WORM_ROTATION > 129600000 + #error "Configuration (Config.h): Setting PEC_STEPS_PER_WORM_ROTATION unknown, use the value 0 to disable or 1 to 129600000 (steps.)" +#endif + +#if (PEC_SENSE) != OFF && (PEC_SENSE) < 0 + #error "Configuration (Config.h): Setting PEC_SENSE unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (PEC_BUFFER_SIZE_LIMIT != 0) && (PEC_BUFFER_SIZE_LIMIT < 61 && PEC_BUFFER_SIZE_LIMIT > 30000) + #error "Configuration (Config.h): Setting PEC_BUFFER_SIZE_LIMIT unknown, use the value 0 to disable or 61 to 30000 (seconds.)" +#endif + +// SLEWING BEHAVIOUR +#if GOTO_FEATURE != ON && GOTO_FEATURE != OFF + #error "Configuration (Config.h): Setting GOTO_FEATURE unknown, use OFF or ON." +#endif + +#if SLEW_RATE_MEMORY != ON && SLEW_RATE_MEMORY != OFF + #error "Configuration (Config.h): Setting SLEW_RATE_MEMORY unknown, use OFF or ON." +#endif + +// TRACKING BEHAVIOUR +#if TRACK_AUTOSTART != ON && TRACK_AUTOSTART != OFF + #error "Configuration (Config.h): Setting TRACK_AUTOSTART unknown, use OFF or ON." +#endif + +#if TRACK_COMPENSATION_DEFAULT != OFF && (TRACK_COMPENSATION_DEFAULT < COMPENSATED_TRACKING_FIRST || TRACK_COMPENSATION_DEFAULT > COMPENSATED_TRACKING_LAST) + #error "Configuration (Config.h): Setting TRACK_COMPENSATION_DEFAULT unknown, use OFF or REFRACTION or REFRACTION_DUAL or MODEL or MODEL_DUAL." +#endif + +#if TRACK_COMPENSATION_MEMORY != ON && TRACK_COMPENSATION_MEMORY != OFF + #error "Configuration (Config.h): Setting TRACK_COMPENSATION_MEMORY unknown, use OFF or ON." +#endif + +#if TRACK_BACKLASH_RATE < 2 && TRACK_BACKLASH_RATE > 100 + #error "Configuration (Config.h): Setting TRACK_BACKLASH_RATE unknown, use a value between 2 and 100 (x Sidereal.)" +#endif + +// GOTO_FEATURE CHECKS +#if GOTO_FEATURE == OFF && TRACK_BACKLASH_RATE > 20 + #error "Configuration (Config.h): Setting TRACK_BACKLASH_RATE must be <= 20 when GOTO_FEATURE is OFF." +#endif + +#if GOTO_FEATURE == OFF && (MOUNT_TYPE == GEM_TA || MOUNT_TYPE == GEM_TAC) + #error "Configuration (Config.h): Setting GEM_TA/GEM_TAC not available when GOTO_FEATURE is OFF." +#endif + +#if GOTO_FEATURE == OFF && (MOUNT_TYPE == FORK_TA || MOUNT_TYPE == FORK_TAC) + #error "Configuration (Config.h): Setting FORK_TA/FORK_TAC not available when GOTO_FEATURE is OFF." +#endif + +// PIER SIDE BEHAVIOUR +#if MFLIP_SKIP_HOME != ON && MFLIP_SKIP_HOME != OFF + #error "Configuration (Config.h): Setting MFLIP_SKIP_HOME unknown, use OFF or ON." +#endif + +#if MFLIP_PAUSE_HOME_DEFAULT != ON && MFLIP_PAUSE_HOME_DEFAULT != OFF + #error "Configuration (Config.h): Setting MFLIP_PAUSE_HOME_DEFAULT unknown, use OFF or ON." +#endif + +#if MFLIP_PAUSE_HOME_MEMORY != ON && MFLIP_PAUSE_HOME_MEMORY != OFF + #error "Configuration (Config.h): Setting MFLIP_PAUSE_HOME_MEMORY unknown, use OFF or ON." +#endif + +#if MFLIP_AUTOMATIC_DEFAULT != ON && MFLIP_AUTOMATIC_DEFAULT != OFF + #error "Configuration (Config.h): Setting MFLIP_AUTOMATIC_DEFAULT unknown, use OFF or ON." +#endif + +#if MFLIP_AUTOMATIC_MEMORY != ON && MFLIP_AUTOMATIC_MEMORY != OFF + #error "Configuration (Config.h): Setting MFLIP_AUTOMATIC_MEMORY unknown, use OFF or ON." +#endif + +#if PIER_SIDE_SYNC_CHANGE_SIDES != ON && PIER_SIDE_SYNC_CHANGE_SIDES != OFF + #error "Configuration (Config.h): Setting PIER_SIDE_SYNC_CHANGE_SIDES unknown, use OFF or ON." +#endif + +static_assert(AXIS1_LIMIT_SYNC == OFF || (AXIS1_LIMIT_SYNC >= 0 && AXIS1_LIMIT_SYNC <= 90), + "Configuration (Config.h): Setting AXIS1_LIMIT_SYNC unknown, use OFF or value in the range 0 to 90."); + +static_assert(AXIS2_LIMIT_SYNC == OFF || (AXIS2_LIMIT_SYNC >= 0 && AXIS2_LIMIT_SYNC <= 90), + "Configuration (Config.h): Setting AXIS2_LIMIT_SYNC unknown, use OFF or value in the range 0 to 90."); + +#if PIER_SIDE_SYNC_CHANGE_SIDES == ON + static_assert(AXIS1_LIMIT_SYNC == OFF && AXIS2_LIMIT_SYNC == OFF, + "Configuration (Config.h): Enabling AXIS1_LIMIT_SYNC or AXIS2_LIMIT_SYNC requires PIER_SIDE_SYNC_CHANGE_SIDES to be OFF."); +#endif + +#if PIER_SIDE_PREFERRED_DEFAULT < PIER_SIDE_FIRST && PIER_SIDE_PREFERRED_DEFAULT > PIER_SIDE_LAST + #error "Configuration (Config.h): Setting PIER_SIDE_PREFERRED_DEFAULT unknown, use EAST or WEST or BEST or AUTOMATIC." +#endif + +#if PIER_SIDE_PREFERRED_MEMORY != OFF && PIER_SIDE_PREFERRED_MEMORY != ON + #error "Configuration (Config.h): Setting PIER_SIDE_PREFERRED_MEMORY unknown, use ON or OFF." +#endif + +// PARKING BEHAVIOUR +#ifdef PARK_STRICT + #if PARK_STRICT == ON + #error "Configuration (Config.h): PARK_STRICT is obsolete. Remove it and use the startup authority / parking policy defaults instead." + #endif +#endif + +// ROTATOR --------------------------------------- + +// AXIS3 ROTATOR +#if AXIS3_DRIVER_MODEL != OFF && \ + (AXIS3_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS3_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS3_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS3_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS3_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS3_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS3_DRIVER_STATUS != OFF && AXIS3_DRIVER_STATUS != ON && AXIS3_DRIVER_STATUS != HIGH && AXIS3_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS3_DRIVER_STATUS unknown, use OFF or valid driver status." +#endif + +#ifdef AXIS3_STEP_DIR_PRESENT + #if AXIS3_DRIVER_MICROSTEPS != OFF && (AXIS3_DRIVER_MICROSTEPS < 1 || AXIS3_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS3_DRIVER_MICROSTEPS != 1 && AXIS3_DRIVER_MICROSTEPS != 2 && AXIS3_DRIVER_MICROSTEPS != 4 && \ + AXIS3_DRIVER_MICROSTEPS != 8 && AXIS3_DRIVER_MICROSTEPS != 16 && AXIS3_DRIVER_MICROSTEPS != 32 && \ + AXIS3_DRIVER_MICROSTEPS != 64 && AXIS3_DRIVER_MICROSTEPS != 128 && AXIS3_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS3_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS3_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS3_DRIVER_MICROSTEPS_GOTO < 1 || AXIS3_DRIVER_MICROSTEPS_GOTO >= AXIS3_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS3_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS3_DRIVER_MICROSTEPS_GOTO != 1 && AXIS3_DRIVER_MICROSTEPS_GOTO != 2 && AXIS3_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS3_DRIVER_MICROSTEPS_GOTO != 8 && AXIS3_DRIVER_MICROSTEPS_GOTO != 16 && AXIS3_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS3_DRIVER_MICROSTEPS_GOTO != 64 && AXIS3_DRIVER_MICROSTEPS_GOTO != 128 && AXIS3_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS3_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS3_DRIVER_MICROSTEPS == OFF && AXIS3_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS3_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS3_DRIVER_MICROSTEPS." + #endif + #if AXIS3_DRIVER_DECAY != OFF && (AXIS3_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS3_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS3_DRIVER_DECAY_GOTO != OFF && (AXIS3_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS3_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS3_DRIVER_IRUN != OFF && (AXIS3_DRIVER_IRUN < 0 || AXIS3_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS3_DRIVER_IHOLD != OFF && (AXIS3_DRIVER_IHOLD < 0 || AXIS3_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS3_DRIVER_IGOTO != OFF && (AXIS3_DRIVER_IGOTO < 0 || AXIS3_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS3_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if (AXIS3_DRIVER_IRUN > 1000 || AXIS3_DRIVER_IHOLD > 1000 || AXIS3_DRIVER_IGOTO > 1000) + #warning "Configuration (Config.h): Setting AXIS3_DRIVER_IHOLD or _IRUN or _IGOTO > 1000 (mA) this Axis on many boards is not designed to operate at high current" + #endif +#endif + +#ifdef AXIS3_SERVO_PRESENT + #if AXIS3_ENCODER != OFF && (AXIS3_ENCODER < ENC_FIRST || AXIS3_ENCODER > ENC_LAST) + #error "Configuration (Config.h): Setting AXIS3_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +#if AXIS3_REVERSE != ON && AXIS3_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS3_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS3_POWER_DOWN != ON && AXIS3_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS3_POWER_DOWN unknown, use OFF or ON." +#endif + +#if AXIS3_LIMIT_MIN < -360 || AXIS3_LIMIT_MIN > 0 + #error "Configuration (Config.h): Setting AXIS3_LIMIT_MIN unknown, use value in the range 0 to -360." +#endif + +#if AXIS3_LIMIT_MAX < 0 || AXIS3_LIMIT_MAX > 360 + #error "Configuration (Config.h): Setting AXIS3_LIMIT_MAX unknown, use value in the range 0 to 360." +#endif + +#if (AXIS3_SENSE_HOME) != OFF && (AXIS3_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS3_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS3_SENSE_LIMIT_MIN) != OFF && (AXIS3_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS3_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS3_SENSE_LIMIT_MAX) != OFF && (AXIS3_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS3_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +// FOCUSER --------------------------------------- + +// AXIS4 FOCUSER +#if AXIS4_DRIVER_MODEL != OFF && \ + (AXIS4_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS4_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS4_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS4_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS4_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS4_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS4_SLEW_RATE_MINIMUM < 5 || AXIS4_SLEW_RATE_MINIMUM > 200 + #error "Configuration (Config.h): Setting AXIS4_SLEW_RATE_MINIMUM out of range, use a value between 5 and 200" +#endif + +#if AXIS4_SLEW_RATE_BASE_DESIRED < 200 || AXIS4_SLEW_RATE_BASE_DESIRED > 5000 + #error "Configuration (Config.h): Setting AXIS4_SLEW_RATE_BASE_DESIRED out of range, use a value between 200 and 5000" +#endif + +#if AXIS4_DRIVER_STATUS != OFF && AXIS4_DRIVER_STATUS != ON && AXIS4_DRIVER_STATUS != HIGH && AXIS4_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS4_DRIVER_STATUS unknown, use OFF or valid driver status." +#endif + +#if AXIS4_REVERSE != ON && AXIS4_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS4_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS4_POWER_DOWN != ON && AXIS4_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS4_POWER_DOWN unknown, use OFF or ON." +#endif +#if AXIS4_POWER_DOWN == ON && (AXIS4_ENABLE_PIN == OFF || AXIS4_ENABLE_PIN == SHARED) && \ + (AXIS4_DRIVER_MODEL < TMC_DRIVER_FIRST || AXIS4_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) + #error "Configuration (Config.h): AXIS4_POWER_DOWN requires a dedicated enable pin or a software-controlled step/dir TMC driver." +#endif + +#if AXIS4_LIMIT_MIN < 0 || AXIS4_LIMIT_MIN > 500 + #error "Configuration (Config.h): Setting AXIS4_LIMIT_MIN unknown, use value in the range 0 to 500 (mm.)" +#endif + +#if AXIS4_LIMIT_MAX < AXIS4_LIMIT_MIN || AXIS4_LIMIT_MAX > 500 + #error "Configuration (Config.h): Setting AXIS4_LIMIT_MAX unknown, use value in the range AXIS4_LIMIT_MIN to 500 (mm.)" +#endif + +#if AXIS4_HOME_DEFAULT != MINIMUM && AXIS4_HOME_DEFAULT != MIDDLE && AXIS4_HOME_DEFAULT != MAXIMUM && (AXIS4_HOME_DEFAULT < 0 || AXIS4_HOME_DEFAULT > 500000) + #error "Configuration (Config.h): Setting AXIS4_HOME_DEFAULT unknown, use MINIMUM (zero) or MIDDLE (half travel) or MAXIMUM (full travel) or the position in microns (0 to 500000.)" +#endif + +#if (AXIS4_SENSE_HOME) != OFF && (AXIS4_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS4_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS4_SENSE_LIMIT_MIN) != OFF && (AXIS4_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS4_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS4_SENSE_LIMIT_MAX) != OFF && (AXIS4_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS4_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#ifdef AXIS4_STEP_DIR_PRESENT + #if AXIS4_DRIVER_MICROSTEPS != OFF && (AXIS4_DRIVER_MICROSTEPS < 1 || AXIS4_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS4_DRIVER_MICROSTEPS != 1 && AXIS4_DRIVER_MICROSTEPS != 2 && AXIS4_DRIVER_MICROSTEPS != 4 && \ + AXIS4_DRIVER_MICROSTEPS != 8 && AXIS4_DRIVER_MICROSTEPS != 16 && AXIS4_DRIVER_MICROSTEPS != 32 && \ + AXIS4_DRIVER_MICROSTEPS != 64 && AXIS4_DRIVER_MICROSTEPS != 128 && AXIS4_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS4_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS4_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS4_DRIVER_MICROSTEPS_GOTO < 1 || AXIS4_DRIVER_MICROSTEPS_GOTO >= AXIS4_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS4_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS4_DRIVER_MICROSTEPS_GOTO != 1 && AXIS4_DRIVER_MICROSTEPS_GOTO != 2 && AXIS4_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS4_DRIVER_MICROSTEPS_GOTO != 8 && AXIS4_DRIVER_MICROSTEPS_GOTO != 16 && AXIS4_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS4_DRIVER_MICROSTEPS_GOTO != 64 && AXIS4_DRIVER_MICROSTEPS_GOTO != 128 && AXIS4_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS4_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS4_DRIVER_MICROSTEPS == OFF && AXIS4_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS4_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS4_DRIVER_MICROSTEPS." + #endif + #if AXIS4_DRIVER_DECAY != OFF && (AXIS4_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS4_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS4_DRIVER_DECAY_GOTO != OFF && (AXIS4_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS4_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS4_DRIVER_IRUN != OFF && (AXIS4_DRIVER_IRUN < 0 || AXIS4_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS4_DRIVER_IHOLD != OFF && (AXIS4_DRIVER_IHOLD < 0 || AXIS4_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS4_DRIVER_IGOTO != OFF && (AXIS4_DRIVER_IGOTO < 0 || AXIS4_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS4_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if (AXIS4_DRIVER_IRUN > 1000 || AXIS4_DRIVER_IHOLD > 1000 || AXIS4_DRIVER_IGOTO > 1000) + #warning "Configuration (Config.h): Setting AXIS4_DRIVER_IHOLD or _IRUN or _IGOTO > 1000 (mA) this Axis on many boards is not designed to operate at high current" + #endif +#endif + +#ifdef AXIS4_SERVO_PRESENT + #if AXIS4_ENCODER < ENC_FIRST || AXIS4_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS4_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +// AXIS5 FOCUSER +#if AXIS5_DRIVER_MODEL != OFF && \ + (AXIS5_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS5_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS5_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS5_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS5_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS5_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS5_SLEW_RATE_MINIMUM < 5 || AXIS5_SLEW_RATE_MINIMUM > 200 + #error "Configuration (Config.h): Setting AXIS5_SLEW_RATE_MINIMUM out of range, use a value between 5 and 200" +#endif + +#if AXIS5_SLEW_RATE_BASE_DESIRED < 200 || AXIS5_SLEW_RATE_BASE_DESIRED > 5000 + #error "Configuration (Config.h): Setting AXIS5_SLEW_RATE_BASE_DESIRED out of range, use a value between 200 and 5000" +#endif + +#if AXIS5_DRIVER_STATUS != OFF && AXIS5_DRIVER_STATUS != ON && AXIS5_DRIVER_STATUS != HIGH && AXIS5_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS5_DRIVER_STATUS unknown, use OFF or valid driver status." +#endif + +#if AXIS5_REVERSE != ON && AXIS5_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS5_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS5_POWER_DOWN != ON && AXIS5_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS5_POWER_DOWN unknown, use OFF or ON." +#endif +#if AXIS5_POWER_DOWN == ON && (AXIS5_ENABLE_PIN == OFF || AXIS5_ENABLE_PIN == SHARED) && \ + (AXIS5_DRIVER_MODEL < TMC_DRIVER_FIRST || AXIS5_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) + #error "Configuration (Config.h): AXIS5_POWER_DOWN requires a dedicated enable pin or a software-controlled step/dir TMC driver." +#endif + +#if AXIS5_LIMIT_MIN < 0 || AXIS5_LIMIT_MIN > 500 + #error "Configuration (Config.h): Setting AXIS5_LIMIT_MIN unknown, use value in the range 0 to 500 (mm.)" +#endif + +#if AXIS5_LIMIT_MAX < AXIS5_LIMIT_MIN || AXIS5_LIMIT_MAX > 500 + #error "Configuration (Config.h): Setting AXIS5_LIMIT_MAX unknown, use value in the range AXIS5_LIMIT_MIN to 500 (mm.)" +#endif + +#if AXIS5_HOME_DEFAULT != MINIMUM && AXIS5_HOME_DEFAULT != MIDDLE && AXIS5_HOME_DEFAULT != MAXIMUM && (AXIS5_HOME_DEFAULT < 0 || AXIS5_HOME_DEFAULT > 500000) + #error "Configuration (Config.h): Setting AXIS5_HOME_DEFAULT unknown, use MINIMUM (zero) or MIDDLE (half travel) or MAXIMUM (full travel) or the position in microns (0 to 500000.)" +#endif + +#if (AXIS5_SENSE_HOME) != OFF && (AXIS5_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS5_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS5_SENSE_LIMIT_MIN) != OFF && (AXIS5_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS5_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS5_SENSE_LIMIT_MAX) != OFF && (AXIS5_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS5_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#ifdef AXIS5_STEP_DIR_PRESENT + #if AXIS5_DRIVER_MICROSTEPS != OFF && (AXIS5_DRIVER_MICROSTEPS < 1 || AXIS5_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS5_DRIVER_MICROSTEPS != 1 && AXIS5_DRIVER_MICROSTEPS != 2 && AXIS5_DRIVER_MICROSTEPS != 4 && \ + AXIS5_DRIVER_MICROSTEPS != 8 && AXIS5_DRIVER_MICROSTEPS != 16 && AXIS5_DRIVER_MICROSTEPS != 32 && \ + AXIS5_DRIVER_MICROSTEPS != 64 && AXIS5_DRIVER_MICROSTEPS != 128 && AXIS5_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS5_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS5_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS5_DRIVER_MICROSTEPS_GOTO < 1 || AXIS5_DRIVER_MICROSTEPS_GOTO >= AXIS5_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS5_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS5_DRIVER_MICROSTEPS_GOTO != 1 && AXIS5_DRIVER_MICROSTEPS_GOTO != 2 && AXIS5_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS5_DRIVER_MICROSTEPS_GOTO != 8 && AXIS5_DRIVER_MICROSTEPS_GOTO != 16 && AXIS5_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS5_DRIVER_MICROSTEPS_GOTO != 64 && AXIS5_DRIVER_MICROSTEPS_GOTO != 128 && AXIS5_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS5_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS5_DRIVER_MICROSTEPS == OFF && AXIS5_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS5_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS5_DRIVER_MICROSTEPS." + #endif + #if AXIS5_DRIVER_DECAY != OFF && (AXIS5_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS5_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS5_DRIVER_DECAY_GOTO != OFF && (AXIS5_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS5_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS5_DRIVER_IRUN != OFF && (AXIS5_DRIVER_IRUN < 0 || AXIS5_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS5_DRIVER_IHOLD != OFF && (AXIS5_DRIVER_IHOLD < 0 || AXIS5_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS5_DRIVER_IGOTO != OFF && (AXIS5_DRIVER_IGOTO < 0 || AXIS5_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS5_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if (AXIS5_DRIVER_IRUN > 1000 || AXIS5_DRIVER_IHOLD > 1000 || AXIS5_DRIVER_IGOTO > 1000) + #warning "Configuration (Config.h): Setting AXIS5_DRIVER_IHOLD or _IRUN or _IGOTO > 1000 (mA) this Axis on many boards is not designed to operate at high current" + #endif +#endif + +#ifdef AXIS5_SERVO_PRESENT + #if AXIS5_ENCODER < ENC_FIRST || AXIS5_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS5_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +// AXIS6 FOCUSER +#if AXIS6_DRIVER_MODEL != OFF && \ + (AXIS6_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS6_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS6_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS6_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS6_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS6_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS6_SLEW_RATE_MINIMUM < 5 || AXIS6_SLEW_RATE_MINIMUM > 200 + #error "Configuration (Config.h): Setting AXIS6_SLEW_RATE_MINIMUM out of range, use a value between 5 and 200" +#endif + +#if AXIS6_SLEW_RATE_BASE_DESIRED < 200 || AXIS6_SLEW_RATE_BASE_DESIRED > 5000 + #error "Configuration (Config.h): Setting AXIS6_SLEW_RATE_BASE_DESIRED out of range, use a value between 200 and 5000" +#endif + +#if AXIS6_DRIVER_STATUS != OFF && AXIS6_DRIVER_STATUS != ON && AXIS6_DRIVER_STATUS != HIGH && AXIS6_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS6_DRIVER_STATUS unknown, use OFF or valid driver status." +#endif + +#if AXIS6_REVERSE != ON && AXIS6_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS6_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS6_POWER_DOWN != ON && AXIS6_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS6_POWER_DOWN unknown, use OFF or ON." +#endif +#if AXIS6_POWER_DOWN == ON && (AXIS6_ENABLE_PIN == OFF || AXIS6_ENABLE_PIN == SHARED) && \ + (AXIS6_DRIVER_MODEL < TMC_DRIVER_FIRST || AXIS6_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) + #error "Configuration (Config.h): AXIS6_POWER_DOWN requires a dedicated enable pin or a software-controlled step/dir TMC driver." +#endif + +#if AXIS6_LIMIT_MIN < 0 || AXIS6_LIMIT_MIN > 500 + #error "Configuration (Config.h): Setting AXIS6_LIMIT_MIN unknown, use value in the range 0 to 500 (mm.)" +#endif + +#if AXIS6_LIMIT_MAX < AXIS6_LIMIT_MIN || AXIS6_LIMIT_MAX > 500 + #error "Configuration (Config.h): Setting AXIS6_LIMIT_MAX unknown, use value in the range AXIS6_LIMIT_MIN to 500 (mm.)" +#endif + +#if AXIS6_HOME_DEFAULT != MINIMUM && AXIS6_HOME_DEFAULT != MIDDLE && AXIS6_HOME_DEFAULT != MAXIMUM && (AXIS6_HOME_DEFAULT < 0 || AXIS6_HOME_DEFAULT > 500000) + #error "Configuration (Config.h): Setting AXIS6_HOME_DEFAULT unknown, use MINIMUM (zero) or MIDDLE (half travel) or MAXIMUM (full travel) or the position in microns (0 to 500000.)" +#endif + +#if (AXIS6_SENSE_HOME) != OFF && (AXIS6_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS6_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS6_SENSE_LIMIT_MIN) != OFF && (AXIS6_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS6_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS6_SENSE_LIMIT_MAX) != OFF && (AXIS6_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS6_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#ifdef AXIS6_STEP_DIR_PRESENT + #if AXIS6_DRIVER_MICROSTEPS != OFF && (AXIS6_DRIVER_MICROSTEPS < 1 || AXIS6_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS6_DRIVER_MICROSTEPS != 1 && AXIS6_DRIVER_MICROSTEPS != 2 && AXIS6_DRIVER_MICROSTEPS != 4 && \ + AXIS6_DRIVER_MICROSTEPS != 8 && AXIS6_DRIVER_MICROSTEPS != 16 && AXIS6_DRIVER_MICROSTEPS != 32 && \ + AXIS6_DRIVER_MICROSTEPS != 64 && AXIS6_DRIVER_MICROSTEPS != 128 && AXIS6_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS6_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS6_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS6_DRIVER_MICROSTEPS_GOTO < 1 || AXIS6_DRIVER_MICROSTEPS_GOTO >= AXIS6_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS6_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS6_DRIVER_MICROSTEPS_GOTO != 1 && AXIS6_DRIVER_MICROSTEPS_GOTO != 2 && AXIS6_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS6_DRIVER_MICROSTEPS_GOTO != 8 && AXIS6_DRIVER_MICROSTEPS_GOTO != 16 && AXIS6_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS6_DRIVER_MICROSTEPS_GOTO != 64 && AXIS6_DRIVER_MICROSTEPS_GOTO != 128 && AXIS6_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS6_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS6_DRIVER_MICROSTEPS == OFF && AXIS6_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS6_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS6_DRIVER_MICROSTEPS." + #endif + #if AXIS6_DRIVER_DECAY != OFF && (AXIS6_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS6_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS6_DRIVER_DECAY_GOTO != OFF && (AXIS6_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS6_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS6_DRIVER_IRUN != OFF && (AXIS6_DRIVER_IRUN < 0 || AXIS6_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS6_DRIVER_IHOLD != OFF && (AXIS6_DRIVER_IHOLD < 0 || AXIS6_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS6_DRIVER_IGOTO != OFF && (AXIS6_DRIVER_IGOTO < 0 || AXIS6_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS6_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif +#endif + +#ifdef AXIS6_SERVO_PRESENT + #if AXIS6_ENCODER < ENC_FIRST || AXIS6_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS6_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +// AXIS7 FOCUSER +#if AXIS7_DRIVER_MODEL != OFF && \ + (AXIS7_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS7_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS7_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS7_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS7_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS7_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS7_SLEW_RATE_MINIMUM < 5 || AXIS7_SLEW_RATE_MINIMUM > 200 + #error "Configuration (Config.h): Setting AXIS7_SLEW_RATE_MINIMUM out of range, use a value between 5 and 200" +#endif + +#if AXIS7_SLEW_RATE_BASE_DESIRED < 200 || AXIS7_SLEW_RATE_BASE_DESIRED > 5000 + #error "Configuration (Config.h): Setting AXIS7_SLEW_RATE_BASE_DESIRED out of range, use a value between 200 and 5000" +#endif + +#if AXIS7_DRIVER_STATUS != OFF && AXIS7_DRIVER_STATUS != ON && AXIS7_DRIVER_STATUS != HIGH && AXIS7_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS7_DRIVER_STATUS unknown, use OFF or valid driver status." +#endif + +#if AXIS7_REVERSE != ON && AXIS7_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS7_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS7_POWER_DOWN != ON && AXIS7_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS7_POWER_DOWN unknown, use OFF or ON." +#endif +#if AXIS7_POWER_DOWN == ON && (AXIS7_ENABLE_PIN == OFF || AXIS7_ENABLE_PIN == SHARED) && \ + (AXIS7_DRIVER_MODEL < TMC_DRIVER_FIRST || AXIS7_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) + #error "Configuration (Config.h): AXIS7_POWER_DOWN requires a dedicated enable pin or a software-controlled step/dir TMC driver." +#endif + +#if AXIS7_LIMIT_MIN < 0 || AXIS7_LIMIT_MIN > 500 + #error "Configuration (Config.h): Setting AXIS7_LIMIT_MIN unknown, use value in the range 0 to 500 (mm.)" +#endif + +#if AXIS7_LIMIT_MAX < AXIS7_LIMIT_MIN || AXIS7_LIMIT_MAX > 500 + #error "Configuration (Config.h): Setting AXIS7_LIMIT_MAX unknown, use value in the range AXIS7_LIMIT_MIN to 500 (mm.)" +#endif + +#if AXIS7_HOME_DEFAULT != MINIMUM && AXIS7_HOME_DEFAULT != MIDDLE && AXIS7_HOME_DEFAULT != MAXIMUM && (AXIS7_HOME_DEFAULT < 0 || AXIS7_HOME_DEFAULT > 500000) + #error "Configuration (Config.h): Setting AXIS7_HOME_DEFAULT unknown, use MINIMUM (zero) or MIDDLE (half travel) or MAXIMUM (full travel) or the position in microns (0 to 500000.)" +#endif + +#if (AXIS7_SENSE_HOME) != OFF && (AXIS7_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS7_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS7_SENSE_LIMIT_MIN) != OFF && (AXIS7_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS7_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS7_SENSE_LIMIT_MAX) != OFF && (AXIS7_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS7_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#ifdef AXIS7_STEP_DIR_PRESENT + #if AXIS7_DRIVER_MICROSTEPS != OFF && (AXIS7_DRIVER_MICROSTEPS < 1 || AXIS7_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS7_DRIVER_MICROSTEPS != 1 && AXIS7_DRIVER_MICROSTEPS != 2 && AXIS7_DRIVER_MICROSTEPS != 4 && \ + AXIS7_DRIVER_MICROSTEPS != 8 && AXIS7_DRIVER_MICROSTEPS != 16 && AXIS7_DRIVER_MICROSTEPS != 32 && \ + AXIS7_DRIVER_MICROSTEPS != 64 && AXIS7_DRIVER_MICROSTEPS != 128 && AXIS7_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS7_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS7_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS7_DRIVER_MICROSTEPS_GOTO < 1 || AXIS7_DRIVER_MICROSTEPS_GOTO >= AXIS7_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS7_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS7_DRIVER_MICROSTEPS_GOTO != 1 && AXIS7_DRIVER_MICROSTEPS_GOTO != 2 && AXIS7_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS7_DRIVER_MICROSTEPS_GOTO != 8 && AXIS7_DRIVER_MICROSTEPS_GOTO != 16 && AXIS7_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS7_DRIVER_MICROSTEPS_GOTO != 64 && AXIS7_DRIVER_MICROSTEPS_GOTO != 128 && AXIS7_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS7_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS7_DRIVER_MICROSTEPS == OFF && AXIS7_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS7_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS7_DRIVER_MICROSTEPS." + #endif + #if AXIS7_DRIVER_DECAY != OFF && (AXIS7_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS7_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS7_DRIVER_DECAY_GOTO != OFF && (AXIS7_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS7_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS7_DRIVER_IRUN != OFF && (AXIS7_DRIVER_IRUN < 0 || AXIS7_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS7_DRIVER_IHOLD != OFF && (AXIS7_DRIVER_IHOLD < 0 || AXIS7_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS7_DRIVER_IGOTO != OFF && (AXIS7_DRIVER_IGOTO < 0 || AXIS7_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS7_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif +#endif + +#ifdef AXIS7_SERVO_PRESENT + #if AXIS7_ENCODER < ENC_FIRST || AXIS7_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS7_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +// AXIS8 FOCUSER +#if AXIS8_DRIVER_MODEL != OFF && \ + (AXIS8_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS8_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS8_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS8_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS8_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS8_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS8_SLEW_RATE_MINIMUM < 5 || AXIS8_SLEW_RATE_MINIMUM > 200 + #error "Configuration (Config.h): Setting AXIS8_SLEW_RATE_MINIMUM out of range, use a value between 5 and 200" +#endif + +#if AXIS8_SLEW_RATE_BASE_DESIRED < 200 || AXIS8_SLEW_RATE_BASE_DESIRED > 5000 + #error "Configuration (Config.h): Setting AXIS8_SLEW_RATE_BASE_DESIRED out of range, use a value between 200 and 5000" +#endif + +#if AXIS8_DRIVER_STATUS != OFF && AXIS8_DRIVER_STATUS != ON && AXIS8_DRIVER_STATUS != HIGH && AXIS8_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS8_DRIVER_STATUS unknown, use OFF or valid driver status." +#endif + +#if AXIS8_REVERSE != ON && AXIS8_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS8_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS8_POWER_DOWN != ON && AXIS8_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS8_POWER_DOWN unknown, use OFF or ON." +#endif +#if AXIS8_POWER_DOWN == ON && (AXIS8_ENABLE_PIN == OFF || AXIS8_ENABLE_PIN == SHARED) && \ + (AXIS8_DRIVER_MODEL < TMC_DRIVER_FIRST || AXIS8_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) + #error "Configuration (Config.h): AXIS8_POWER_DOWN requires a dedicated enable pin or a software-controlled step/dir TMC driver." +#endif + +#if AXIS8_LIMIT_MIN < 0 || AXIS8_LIMIT_MIN > 500 + #error "Configuration (Config.h): Setting AXIS8_LIMIT_MIN unknown, use value in the range 0 to 500 (mm.)" +#endif + +#if AXIS8_LIMIT_MAX < AXIS8_LIMIT_MIN || AXIS8_LIMIT_MAX > 500 + #error "Configuration (Config.h): Setting AXIS8_LIMIT_MAX unknown, use value in the range AXIS8_LIMIT_MIN to 500 (mm.)" +#endif + +#if AXIS8_HOME_DEFAULT != MINIMUM && AXIS8_HOME_DEFAULT != MIDDLE && AXIS8_HOME_DEFAULT != MAXIMUM && (AXIS8_HOME_DEFAULT < 0 || AXIS8_HOME_DEFAULT > 500000) + #error "Configuration (Config.h): Setting AXIS8_HOME_DEFAULT unknown, use MINIMUM (zero) or MIDDLE (half travel) or MAXIMUM (full travel) or the position in microns (0 to 500000.)" +#endif + +#if (AXIS8_SENSE_HOME) != OFF && (AXIS8_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS8_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS8_SENSE_LIMIT_MIN) != OFF && (AXIS8_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS8_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS8_SENSE_LIMIT_MAX) != OFF && (AXIS8_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS8_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#ifdef AXIS8_STEP_DIR_PRESENT + #if AXIS8_DRIVER_MICROSTEPS != OFF && (AXIS8_DRIVER_MICROSTEPS < 1 || AXIS8_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS8_DRIVER_MICROSTEPS != 1 && AXIS8_DRIVER_MICROSTEPS != 2 && AXIS8_DRIVER_MICROSTEPS != 4 && \ + AXIS8_DRIVER_MICROSTEPS != 8 && AXIS8_DRIVER_MICROSTEPS != 16 && AXIS8_DRIVER_MICROSTEPS != 32 && \ + AXIS8_DRIVER_MICROSTEPS != 64 && AXIS8_DRIVER_MICROSTEPS != 128 && AXIS8_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS8_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS8_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS8_DRIVER_MICROSTEPS_GOTO < 1 || AXIS8_DRIVER_MICROSTEPS_GOTO >= AXIS8_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS8_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS8_DRIVER_MICROSTEPS_GOTO != 1 && AXIS8_DRIVER_MICROSTEPS_GOTO != 2 && AXIS8_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS8_DRIVER_MICROSTEPS_GOTO != 8 && AXIS8_DRIVER_MICROSTEPS_GOTO != 16 && AXIS8_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS8_DRIVER_MICROSTEPS_GOTO != 64 && AXIS8_DRIVER_MICROSTEPS_GOTO != 128 && AXIS8_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS8_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS8_DRIVER_MICROSTEPS == OFF && AXIS8_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS8_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS8_DRIVER_MICROSTEPS." + #endif + #if AXIS8_DRIVER_DECAY != OFF && (AXIS8_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS8_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS8_DRIVER_DECAY_GOTO != OFF && (AXIS8_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS8_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS8_DRIVER_IRUN != OFF && (AXIS8_DRIVER_IRUN < 0 || AXIS8_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS8_DRIVER_IHOLD != OFF && (AXIS8_DRIVER_IHOLD < 0 || AXIS8_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS8_DRIVER_IGOTO != OFF && (AXIS8_DRIVER_IGOTO < 0 || AXIS8_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS8_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif +#endif + +#ifdef AXIS8_SERVO_PRESENT + #if AXIS8_ENCODER < ENC_FIRST || AXIS8_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS8_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +// AXIS9 FOCUSER +#if AXIS9_DRIVER_MODEL != OFF && \ + (AXIS9_DRIVER_MODEL < STEP_DIR_DRIVER_FIRST || AXIS9_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) && \ + (AXIS9_DRIVER_MODEL < SERVO_DRIVER_FIRST || AXIS9_DRIVER_MODEL > SERVO_DRIVER_LAST) && \ + (AXIS9_DRIVER_MODEL < MOTOR_DRIVER_FIRST || AXIS9_DRIVER_MODEL > MOTOR_DRIVER_LAST) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_MODEL unknown, use OFF or a valid DRIVER (from Constants.h)" +#endif + +#if AXIS9_SLEW_RATE_MINIMUM < 5 || AXIS9_SLEW_RATE_MINIMUM > 200 + #error "Configuration (Config.h): Setting AXIS9_SLEW_RATE_MINIMUM out of range, use a value between 5 and 200" +#endif + +#if AXIS9_SLEW_RATE_BASE_DESIRED < 200 || AXIS9_SLEW_RATE_BASE_DESIRED > 5000 + #error "Configuration (Config.h): Setting AXIS9_SLEW_RATE_BASE_DESIRED out of range, use a value between 200 and 5000" +#endif + +#if AXIS9_DRIVER_STATUS != OFF && AXIS9_DRIVER_STATUS != ON && AXIS9_DRIVER_STATUS != HIGH && AXIS9_DRIVER_STATUS != LOW + #error "Configuration (Config.h): Setting AXIS9_DRIVER_STATUS unknown, use OFF or valid driver status." +#endif + +#if AXIS9_REVERSE != ON && AXIS9_REVERSE != OFF + #error "Configuration (Config.h): Setting AXIS9_REVERSE unknown, use OFF or ON." +#endif + +#if AXIS9_POWER_DOWN != ON && AXIS9_POWER_DOWN != OFF + #error "Configuration (Config.h): Setting AXIS9_POWER_DOWN unknown, use OFF or ON." +#endif +#if AXIS9_POWER_DOWN == ON && (AXIS9_ENABLE_PIN == OFF || AXIS9_ENABLE_PIN == SHARED) && \ + (AXIS9_DRIVER_MODEL < TMC_DRIVER_FIRST || AXIS9_DRIVER_MODEL > STEP_DIR_DRIVER_LAST) + #error "Configuration (Config.h): AXIS9_POWER_DOWN requires a dedicated enable pin or a software-controlled step/dir TMC driver." +#endif + +#if AXIS9_LIMIT_MIN < 0 || AXIS9_LIMIT_MIN > 500 + #error "Configuration (Config.h): Setting AXIS9_LIMIT_MIN unknown, use value in the range 0 to 500 (mm.)" +#endif + +#if AXIS9_LIMIT_MAX < AXIS9_LIMIT_MIN || AXIS9_LIMIT_MAX > 500 + #error "Configuration (Config.h): Setting AXIS9_LIMIT_MAX unknown, use value in the range AXIS9_LIMIT_MIN to 500 (mm.)" +#endif + +#if AXIS9_HOME_DEFAULT != MINIMUM && AXIS9_HOME_DEFAULT != MIDDLE && AXIS9_HOME_DEFAULT != MAXIMUM && (AXIS9_HOME_DEFAULT < 0 || AXIS9_HOME_DEFAULT > 500000) + #error "Configuration (Config.h): Setting AXIS9_HOME_DEFAULT unknown, use MINIMUM (zero) or MIDDLE (half travel) or MAXIMUM (full travel) or the position in microns (0 to 500000.)" +#endif + +#if (AXIS9_SENSE_HOME) != OFF && (AXIS9_SENSE_HOME) < 0 + #error "Configuration (Config.h): Setting AXIS9_SENSE_HOME unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS9_SENSE_LIMIT_MIN) != OFF && (AXIS9_SENSE_LIMIT_MIN) < 0 + #error "Configuration (Config.h): Setting AXIS9_SENSE_LIMIT_MIN unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#if (AXIS9_SENSE_LIMIT_MAX) != OFF && (AXIS9_SENSE_LIMIT_MAX) < 0 + #error "Configuration (Config.h): Setting AXIS9_SENSE_LIMIT_MAX unknown, use OFF or HIGH/LOW and HYST() and/or THLD() as described in comments." +#endif + +#ifdef AXIS9_STEP_DIR_PRESENT + #if AXIS9_DRIVER_MICROSTEPS != OFF && (AXIS9_DRIVER_MICROSTEPS < 1 || AXIS9_DRIVER_MICROSTEPS > 256) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_MICROSTEPS unknown, use OFF or a valid microstep setting (range 1 to 256x and supported by your driver/design.)" + #if AXIS9_DRIVER_MICROSTEPS != 1 && AXIS9_DRIVER_MICROSTEPS != 2 && AXIS9_DRIVER_MICROSTEPS != 4 && \ + AXIS9_DRIVER_MICROSTEPS != 8 && AXIS9_DRIVER_MICROSTEPS != 16 && AXIS9_DRIVER_MICROSTEPS != 32 && \ + AXIS9_DRIVER_MICROSTEPS != 64 && AXIS9_DRIVER_MICROSTEPS != 128 && AXIS9_DRIVER_MICROSTEPS != 256 + #warning "Configuration (Config.h): Setting AXIS9_DRIVER_MICROSTEPS has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS9_DRIVER_MICROSTEPS_GOTO != OFF && (AXIS9_DRIVER_MICROSTEPS_GOTO < 1 || AXIS9_DRIVER_MICROSTEPS_GOTO >= AXIS9_DRIVER_MICROSTEPS) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_MICROSTEPS_GOTO unknown, use OFF or a valid microstep setting (range 1 to < AXIS9_DRIVER_MICROSTEPS and supported by your driver/design.)" + #if AXIS9_DRIVER_MICROSTEPS_GOTO != 1 && AXIS9_DRIVER_MICROSTEPS_GOTO != 2 && AXIS9_DRIVER_MICROSTEPS_GOTO != 4 && \ + AXIS9_DRIVER_MICROSTEPS_GOTO != 8 && AXIS9_DRIVER_MICROSTEPS_GOTO != 16 && AXIS9_DRIVER_MICROSTEPS_GOTO != 32 && \ + AXIS9_DRIVER_MICROSTEPS_GOTO != 64 && AXIS9_DRIVER_MICROSTEPS_GOTO != 128 && AXIS9_DRIVER_MICROSTEPS_GOTO != 256 + #warning "Configuration (Config.h): Setting AXIS9_DRIVER_MICROSTEPS_GOTO has an unusual value (not a power of 2!)" + #endif + #endif + #if AXIS9_DRIVER_MICROSTEPS == OFF && AXIS9_DRIVER_MICROSTEPS_GOTO != OFF + #error "Configuration (Config.h): Setting AXIS9_DRIVER_MICROSTEPS_GOTO can't be enabled without first enabling AXIS9_DRIVER_MICROSTEPS." + #endif + #if AXIS9_DRIVER_DECAY != OFF && (AXIS9_DRIVER_DECAY < DRIVER_DECAY_MODE_FIRST || AXIS9_DRIVER_DECAY > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_DECAY unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS9_DRIVER_DECAY_GOTO != OFF && (AXIS9_DRIVER_DECAY_GOTO < DRIVER_DECAY_MODE_FIRST || AXIS9_DRIVER_DECAY_GOTO > DRIVER_DECAY_MODE_LAST) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_DECAY_GOTO unknown, use a valid DRIVER DECAY MODE (from Constants.h)" + #endif + #if AXIS9_DRIVER_IRUN != OFF && (AXIS9_DRIVER_IRUN < 0 || AXIS9_DRIVER_IRUN > 3000) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_IRUN unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS9_DRIVER_IHOLD != OFF && (AXIS9_DRIVER_IHOLD < 0 || AXIS9_DRIVER_IHOLD > 3000) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_IHOLD unknown, use OFF or a value 0 to 3000 (mA.)" + #endif + #if AXIS9_DRIVER_IGOTO != OFF && (AXIS9_DRIVER_IGOTO < 0 || AXIS9_DRIVER_IGOTO > 3000) + #error "Configuration (Config.h): Setting AXIS9_DRIVER_IGOTO unknown, use OFF or a value 0 to 3000 (mA.)" + #endif +#endif + +#ifdef AXIS9_SERVO_PRESENT + #if AXIS9_ENCODER < ENC_FIRST || AXIS9_ENCODER > ENC_LAST + #error "Configuration (Config.h): Setting AXIS9_ENCODER unknown, use a valid SERVO ENCODER (from Constants.h)" + #endif +#endif + +// GENERAL TEMPERATURE --------------------------- +#if defined(DS1820_DEVICES_PRESENT) && defined(THERMISTOR_DEVICES_PRESENT) + #error "Configuration (Config.h): Setting DS18B20 devices and THERMISTOR devices can not both be used at the same time, use one or the other" +#endif + +// FOCUSER TEMPERATURE --------------------------- +#if FOCUSER_TEMPERATURE != OFF && \ + (FOCUSER_TEMPERATURE & DS_MASK) != DS18B20 && \ + (FOCUSER_TEMPERATURE & DS_MASK) != DS18S20 && \ + (FOCUSER_TEMPERATURE < TEMPERATURE_FIRST || FOCUSER_TEMPERATURE > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FOCUSER_TEMPERATURE unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif + +// AUXILIARY FEATURE ----------------------------- + +#if FEATURE1_PURPOSE != OFF && (FEATURE1_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE1_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE1_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE1_VALUE_DEFAULT != OFF && FEATURE1_VALUE_DEFAULT != ON && (FEATURE1_VALUE_DEFAULT < 0 || FEATURE1_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE1_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE1_VALUE_MEMORY != OFF && FEATURE1_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE1_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE2_PURPOSE != OFF && (FEATURE2_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE2_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE2_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE2_VALUE_DEFAULT != OFF && FEATURE2_VALUE_DEFAULT != ON && (FEATURE2_VALUE_DEFAULT < 0 || FEATURE2_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE2_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE2_VALUE_MEMORY != OFF && FEATURE2_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE2_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE3_PURPOSE != OFF && (FEATURE3_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE3_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE3_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE3_VALUE_DEFAULT != OFF && FEATURE3_VALUE_DEFAULT != ON && (FEATURE3_VALUE_DEFAULT < 0 || FEATURE3_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE3_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE3_VALUE_MEMORY != OFF && FEATURE3_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE3_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE4_PURPOSE != OFF && (FEATURE4_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE4_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE4_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE4_VALUE_DEFAULT != OFF && FEATURE4_VALUE_DEFAULT != ON && (FEATURE4_VALUE_DEFAULT < 0 || FEATURE4_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE4_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE4_VALUE_MEMORY != OFF && FEATURE4_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE4_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE5_PURPOSE != OFF && (FEATURE5_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE5_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE5_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE5_VALUE_DEFAULT != OFF && FEATURE5_VALUE_DEFAULT != ON && (FEATURE5_VALUE_DEFAULT < 0 || FEATURE5_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE5_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE5_VALUE_MEMORY != OFF && FEATURE5_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE5_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE6_PURPOSE != OFF && (FEATURE6_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE6_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE6_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE6_VALUE_DEFAULT != OFF && FEATURE6_VALUE_DEFAULT != ON && (FEATURE6_VALUE_DEFAULT < 0 || FEATURE6_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE6_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE6_VALUE_MEMORY != OFF && FEATURE6_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE6_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE7_PURPOSE != OFF && (FEATURE7_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE7_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE7_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE7_VALUE_DEFAULT != OFF && FEATURE7_VALUE_DEFAULT != ON && (FEATURE7_VALUE_DEFAULT < 0 || FEATURE7_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE7_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE7_VALUE_MEMORY != OFF && FEATURE7_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE7_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE8_PURPOSE != OFF && (FEATURE8_PURPOSE < AUX_FEATURE_PURPOSE_FIRST || FEATURE8_PURPOSE > AUX_FEATURE_PURPOSE_LAST) + #error "Configuration (Config.h): Setting FEATURE8_PURPOSE unknown, use OFF or AUXILIARY FEATURE purpose (from Constants.h)" +#endif +#if FEATURE8_VALUE_DEFAULT != OFF && FEATURE8_VALUE_DEFAULT != ON && (FEATURE8_VALUE_DEFAULT < 0 || FEATURE8_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE8_VALUE_DEFAULT unknown, use OFF or ON or a value 0 to 255" +#endif +#if FEATURE8_VALUE_MEMORY != OFF && FEATURE8_VALUE_MEMORY != ON + #error "Configuration (Config.h): Setting FEATURE8_VALUE_MEMORY unknown, use OFF or ON" +#endif + +#if FEATURE1_TEMP != OFF && \ + (FEATURE1_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE1_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE1_TEMP < TEMPERATURE_FIRST || FEATURE1_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE1_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif +#if FEATURE2_TEMP != OFF && \ + (FEATURE2_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE2_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE2_TEMP < TEMPERATURE_FIRST || FEATURE2_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE2_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif +#if FEATURE3_TEMP != OFF && \ + (FEATURE3_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE3_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE3_TEMP < TEMPERATURE_FIRST || FEATURE3_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE3_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif +#if FEATURE4_TEMP != OFF && \ + (FEATURE4_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE4_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE4_TEMP < TEMPERATURE_FIRST || FEATURE4_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE4_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif +#if FEATURE5_TEMP != OFF && \ + (FEATURE5_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE5_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE5_TEMP < TEMPERATURE_FIRST || FEATURE5_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE5_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif +#if FEATURE6_TEMP != OFF && \ + (FEATURE6_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE6_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE6_TEMP < TEMPERATURE_FIRST || FEATURE6_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE6_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif +#if FEATURE7_TEMP != OFF && \ + (FEATURE7_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE7_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE7_TEMP < TEMPERATURE_FIRST || FEATURE7_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE7_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif +#if FEATURE8_TEMP != OFF && \ + (FEATURE8_TEMP & DS_MASK) != DS18B20 && \ + (FEATURE8_TEMP & DS_MASK) != DS18S20 && \ + (FEATURE8_TEMP < TEMPERATURE_FIRST || FEATURE8_TEMP > TEMPERATURE_LAST) + #error "Configuration (Config.h): Setting FEATURE8_TEMP unknown, use OFF or TEMPERATURE device (from Constants.h)" +#endif + +#if FEATURE1_PIN != OFF && FEATURE1_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE1_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif +#if FEATURE2_PIN != OFF && FEATURE2_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE2_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif +#if FEATURE3_PIN != OFF && FEATURE3_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE3_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif +#if FEATURE4_PIN != OFF && FEATURE4_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE4_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif +#if FEATURE5_PIN != OFF && FEATURE5_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE5_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif +#if FEATURE6_PIN != OFF && FEATURE6_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE6_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif +#if FEATURE7_PIN != OFF && FEATURE7_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE7_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif +#if FEATURE8_PIN != OFF && FEATURE8_PIN < 0 + #error "Configuration (Config.h): Setting FEATURE8_PIN unknown, use OFF or a valid microcontroller pin designation." +#endif + +#if FEATURE1_VALUE_DEFAULT != OFF && FEATURE1_VALUE_DEFAULT != ON && (FEATURE1_VALUE_DEFAULT < 0 || FEATURE1_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE1_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif +#if FEATURE2_VALUE_DEFAULT != OFF && FEATURE2_VALUE_DEFAULT != ON && (FEATURE2_VALUE_DEFAULT < 0 || FEATURE2_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE2_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif +#if FEATURE3_VALUE_DEFAULT != OFF && FEATURE3_VALUE_DEFAULT != ON && (FEATURE3_VALUE_DEFAULT < 0 || FEATURE3_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE3_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif +#if FEATURE4_VALUE_DEFAULT != OFF && FEATURE4_VALUE_DEFAULT != ON && (FEATURE4_VALUE_DEFAULT < 0 || FEATURE4_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE4_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif +#if FEATURE5_VALUE_DEFAULT != OFF && FEATURE5_VALUE_DEFAULT != ON && (FEATURE5_VALUE_DEFAULT < 0 || FEATURE5_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE5_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif +#if FEATURE6_VALUE_DEFAULT != OFF && FEATURE6_VALUE_DEFAULT != ON && (FEATURE6_VALUE_DEFAULT < 0 || FEATURE6_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE6_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif +#if FEATURE7_VALUE_DEFAULT != OFF && FEATURE7_VALUE_DEFAULT != ON && (FEATURE7_VALUE_DEFAULT < 0 || FEATURE7_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE7_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif +#if FEATURE8_VALUE_DEFAULT != OFF && FEATURE8_VALUE_DEFAULT != ON && (FEATURE8_VALUE_DEFAULT < 0 || FEATURE8_VALUE_DEFAULT > 255) + #error "Configuration (Config.h): Setting FEATURE8_VALUE_DEFAULT unknown, use OFF, ON or a value from 0 to 255 (for ANALOG_OUT.)" +#endif + +#if FEATURE1_ON_STATE != HIGH && FEATURE1_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE1_ON_STATE unknown, use HIGH or LOW." +#endif +#if FEATURE2_ON_STATE != HIGH && FEATURE2_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE2_ON_STATE unknown, use HIGH or LOW." +#endif +#if FEATURE3_ON_STATE != HIGH && FEATURE3_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE3_ON_STATE unknown, use HIGH or LOW." +#endif +#if FEATURE4_ON_STATE != HIGH && FEATURE4_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE4_ON_STATE unknown, use HIGH or LOW." +#endif +#if FEATURE5_ON_STATE != HIGH && FEATURE5_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE5_ON_STATE unknown, use HIGH or LOW." +#endif +#if FEATURE6_ON_STATE != HIGH && FEATURE6_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE6_ON_STATE unknown, use HIGH or LOW." +#endif +#if FEATURE7_ON_STATE != HIGH && FEATURE7_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE7_ON_STATE unknown, use HIGH or LOW." +#endif +#if FEATURE8_ON_STATE != HIGH && FEATURE8_ON_STATE != LOW + #error "Configuration (Config.h): Setting FEATURE8_ON_STATE unknown, use HIGH or LOW." +#endif diff --git a/src/lib/1wire/1Wire.cpp b/src/lib/1wire/1Wire.cpp new file mode 100755 index 0000000..dcce178 --- /dev/null +++ b/src/lib/1wire/1Wire.cpp @@ -0,0 +1,10 @@ +// ----------------------------------------------------------------------------------- +// Dallas/Maxim 1-Wire library + +#include "1Wire.h" + +#if defined(DS1820_DEVICES_PRESENT) || (defined(GPIO_DEVICE) && GPIO_DEVICE == DS2413) + +OneWire oneWire(ONE_WIRE_PIN); + +#endif \ No newline at end of file diff --git a/src/lib/1wire/1Wire.h b/src/lib/1wire/1Wire.h new file mode 100755 index 0000000..4d86fbb --- /dev/null +++ b/src/lib/1wire/1Wire.h @@ -0,0 +1,13 @@ +// ----------------------------------------------------------------------------------- +// Dallas/Maxim 1-Wire library +#pragma once + +#include "../../Common.h" + +#if defined(DS1820_DEVICES_PRESENT) || (defined(GPIO_DEVICE) && GPIO_DEVICE == DS2413) + +#include // my OneWire library https://github.com/hjd1964/OneWire + +extern OneWire oneWire; + +#endif diff --git a/src/lib/Constants.h b/src/lib/Constants.h new file mode 100755 index 0000000..8789580 --- /dev/null +++ b/src/lib/Constants.h @@ -0,0 +1,202 @@ +// Library general purpose constants +#pragma once + +// misc. +#define AUTO -1 +#define OFF -1 +#define ON -2 +#define AUX -4 +#define MASTER -5 +#define SLAVE -6 +#define BOTH -7 +#define SoftSerial -8 +#define HardSerial -9 +#define CONSOLE -10 +#define PROFILER -11 +#define VERBOSE -12 +#define REMOTE -13 +#define WIFI_ACCESS_POINT -14 // shorthand for easy WIFI mode settings +#define WIFI_STATION -15 // shorthand for easy WIFI mode settings +#define BLUETOOTH -16 // shorthand for easy bluetooth mode settings +#define SHARED -17 +#define STANDARD -18 +#define PERSISTENT -19 +#define ERRORS_ONLY -20 +#define MINIMUM -21 +#define MIDDLE -22 +#define MAXIMUM -23 +#define INVALID -127 + +// CAN interface options +#define CAN_SAN 0 +#define CAN_ESP32 1 +#define CAN_MCP2515 2 +#define CAN0_TEENSY4 3 +#define CAN1_TEENSY4 4 +#define CAN2_TEENSY4 5 +#define CAN3_TEENSY4 6 + +// network interface options +#define ETHERNET_FIRST 1 +#define ETHERNET_W5100 1 +#define ETHERNET_W5500 2 +#define ETHERNET_TEENSY41 3 +#define ETHERNET_LAST 3 +#define WIFI_FIRST 4 +#define WIFI 4 +#define WIFI_LAST 4 + +// driver (step/dir interface, usually for stepper motors) +// changes here must be reflected in the StepDirDriver.cpp file, etc. +#define STEP_DIR_DRIVER_FIRST 0 +#define A4988 0 // allows M0,M1,M2 bit patterens for 1x,2x,4x,8x,16x +#define DRV8825 1 // allows M0,M1,M2 bit patterens for 1x,2x,4x,8x,16x,32x +#define GENERIC 2 // generic s/d driver allows for 1x,2x,4x,8x,16x,32x,64x,128x,256x (using just the M2 pin) +#define LV8729 3 // allows M0,M1,M2 bit patterens for 1x,2x,4x,8x,16x,32x,64x,128x +#define RAPS128 4 // allows M0,M1,M2 bit patterens for 1x,2x,4x,8x,16x,32x,64x,128x +#define S109 5 // allows M0,M1,M2 bit patterens for 1x,2x,4x,8x,16x,32x +#define ST820 6 // allows M0,M1,M2 bit patterens for 1x,2x,4x,8x,16x,32x,128x,256x +#define TMC2100 7 // allows M0,M1 bit patterens for 1x,2x,4x,16x (spreadCycle only, no 256x intpol) +#define TMC2130S 8 // allows M0,M1 bit patterens for 1x,2x,4x,16x (spreadCycle only, no 256x intpol) +#define TMC2208S 9 // allows M0,M1 bit patterens for 2x,4x,8x,16x (stealthChop default, uses 256x intpol) +#define TMC2225S 10 // allows M0,M1 bit patterens for 4x,8x,16x,32x (M2 sets spreadCycle/stealthChop, uses 256x intpol) +#define TMC2209S 11 // allows M0,M1 bit patterens for 8x,16x,32x,64x (M2 sets spreadCycle/stealthChop, uses 256x intpol) +#define TMC2226S 12 // allows M0,M1 bit patterens for 8x,16x,32x,64x (M2 sets spreadCycle/stealthChop, uses 256x intpol) +#define TMC_DRIVER_FIRST 13 +#define TMC2130 13 // uses TMC software SPI comms for 1x,2x...,256x (SPI sets spreadCycle/stealthChop etc.) +#define TMC2160 14 // uses TMC software SPI comms for 1x,2x...,256x (SPI sets spreadCycle/stealthChop etc.) +#define TMC2660 15 // uses TMC software SPI comms for 1x,2x...,256x (SPI sets spreadCycle/stealthChop etc.) +#define TMC5160 16 // uses TMC software SPI comms for 1x,2x...,256x (SPI sets spreadCycle/stealthChop etc.) +#define TMC5161 17 // uses TMC software SPI comms for 1x,2x...,256x (SPI sets spreadCycle/stealthChop etc.) +#define TMC_UART_DRIVER_FIRST 18 +#define TMC2208 18 // uses TMC software UART comms for 1x,2x...,256x (UART sets spreadCycle/stealthChop etc, dedicated RX/TX UART to each driver using index 0) +#define TMC2225 19 // uses TMC software UART comms for 1x,2x...,256x (UART sets spreadCycle/stealthChop etc, dedicated RX/TX UART to each driver using index 0) +#define TMC2209 20 // uses TMC software UART comms for 1x,2x...,256x (UART sets spreadCycle/stealthChop etc, dedicated RX/TX UART to each driver using index 0) +#define TMC2226 21 // uses TMC software UART comms for 1x,2x...,256x (UART sets spreadCycle/stealthChop etc, dedicated RX/TX UART to each driver using index 0) +#define STEP_DIR_DRIVER_LAST 21 + +// servo driver (usually for DC motors equipped with encoders) +// changes here must be reflected in the ServoDriver.cpp file, etc. +#define SERVO_DRIVER_FIRST 100 +#define SERVO_PE 100 // SERVO, direction (phase) and pwm (enable) connections +#define SERVO_EE 101 // SERVO, dual pwm input (enable/enable) connections +#define SERVO_TMC2130_DC 102 // TMC2130 DC motor control +#define SERVO_TMC5160_DC 103 // TMC5160 DC motor control +#define SERVO_TMC2209 104 // TMC2209 stepper driver using VACTUAL velocity control +#define SERVO_TMC5160 105 // TMC5160 stepper driver using VMAX velocity control +#define SERVO_KTECH 106 // KTech servo motor using velocity control +#define SERVO_DRIVER_LAST 106 + +// integrated motor drivers +#define MOTOR_DRIVER_FIRST 200 +#define ODRIVE 200 // First generation ODrive (axis 1 and 2 only), EXPERIMENTAL! +#define KTECH 201 // KTech CAN driver +#define MKS42D 202 // MKS SERVO 42D, EXPERIMENTAL! +#define MKS57D 202 // MKS SERVO 57D, EXPERIMENTAL! +#define MOTOR_DRIVER_LAST 202 + +// driver (step/dir) decay mode +#define DRIVER_DECAY_MODE_FIRST 1 +#define MIXED 1 +#define FAST 2 +#define SLOW 3 +#define SPREADCYCLE 4 +#define STEALTHCHOP 5 +#define DRIVER_DECAY_MODE_LAST 6 + +// servo encoder (must match Encoder library) +#define ENC_FIRST 1 +#define AB 1 // AB quadrature encoder +#define AB_ESP32 2 // AB quadrature encoder (using fast ESP32 hardware decode) +#define CW_CCW 3 // clockwise/counter-clockwise encoder +#define PULSE_DIR 4 // pulse/direction encoder +#define PULSE_ONLY 5 // pulse only encoder (uses hint for direction) +#define VIRTUAL 6 // virtual encoder (uses hints for velocity and direction) +#define AS37_H39B_B 7 // Broadcom AS37-H39B-B BISS-C interface encoder +#define JTW_24BIT 8 // JTW Trident BISS-C interface 24bit encoder +#define JTW_26BIT 9 // JTW Trident BISS-C interface 26bit encoder +#define LIKA_ASC85 10 // Lika ASC85 BISS-C interface 25bit encoder (experimental) +#define KTECH_IME 11 // KTech in motor encoder +#define SERIAL_BRIDGE 12 // serial bridge to encoders +#define ENC_LAST 12 + +// encoder filter types +#define ENC_FILT_FIRST 1 +#define KALMAN 1 // more advanced, predictive +#define ROLLING 2 // basic, rolling average +#define WINDOWING ROLLING // deprecated compatibility alias for rolling average +#define LEARNING 4 // learning, for RA axis only (experimental may be removed) +#define ENC_FILT_LAST 4 + +// servo feedback (must match Encoder library) +#define SERVO_FEEDBACK_FIRST 1 +#define PID 1 // PID feedback +#define DUAL_PID 2 // Dual PID feedback +#define SERVO_FEEDBACK_LAST 2 + +// driver (step/dir) and servo, misc. +#define ODRIVER -10 // general purpose flag for a ODRIVE driver motor +#define SERVO -11 // general purpose flag for a SERVO driver motor +#define STEP_DIR -12 // general purpose flag for a STEP_DIR driver motor + +// NV/EEPROM +#define NV_KEY_VALUE 111111111UL + +#define NV_DEF 0 +#define NV_DEFAULT 0 // PLATFORM SPECIFIC (HAL) +#define NV_EEPROM 1 // EEPROM.h +#define NV_ESP 2 // ESP8266/ESP32 EEPROM emulation with commits +#define NV_M0 3 // Arduino M0 EEPROM emulation with commits +#define NV_2416 4 // 2KB I2C EEPROM AT DEFAULT ADDRESS 0x50 +#define NV_2432 5 // 4KB I2C EEPROM AT DEFAULT ADDRESS 0x50 +#define NV_2464 6 // 8KB I2C EEPROM AT DEFAULT ADDRESS 0x50 +#define NV_24128 7 // 16KB I2C EEPROM AT DEFAULT ADDRESS 0x50 +#define NV_24256 8 // 32KB I2C EEPROM AT DEFAULT ADDRESS 0x50 +#define NV_AT24C32 9 // 4KB I2C EEPROM AT DEFAULT ADDRESS 0x57 (ZS042 module for instance) +#define NV_MB85RC32 10 // 4KB I2C FRAM AT DEFAULT ADDRESS 0x50 +#define NV_MB85RC64 11 // 8KB I2C FRAM AT DEFAULT ADDRESS 0x50 +#define NV_MB85RC256 12 // 32KB I2C FRAM AT DEFAULT ADDRESS 0x50 + +#define NVE_LOW 0 // low (< 100K writes) +#define NVE_MID 1 // mid (~ 100K writes) +#define NVE_HIGH 2 // high (~ 1M writes) +#define NVE_VHIGH 3 // very high (> 300M writes, FRAM) + +// GPIO devices (pin# 512 up to 543) +// these can work for most digital I/O EXCEPT: STEP/DIR, 1-WIRE/I2C/SPI (CS is ok), the ST4 port, and the PPS pin +#define GPIO_FIRST 1 +#define DS2413 1 // DS2413 2-channel GPIO +#define MCP23008 2 // MCP23008 8-channel GPIO +#define MCP23017 3 // MCP23017 16-channel GPIO +#define X9555 4 // TCA/PCA9555 16-channel GPIO +#define X8574 5 // PCF8574 8-channel GPIO (up to 4) +#define X8575 6 // PCF8575 16-channel GPIO +#define SWS 7 // Smart Web Server 8-channel GPIO (usually on the four Encoder pins, if otherwise unused) +#define SSR74HC595 8 // 74HC595 8,16,24,32-channel GPIO (serial shift register, output only) +#define GPIO_LAST 8 + +// angular +#define RAD_DEG_RATIO 57.29577951308232L +#define RAD_DEG_RATIO_F 57.295780F +#define RAD_HOUR_RATIO 3.819718634205488L +#define RAD_HOUR_RATIO_F 3.8197186F +#define SIDEREAL_RATIO 1.002737909350795L +#define SIDEREAL_RATIO_F 1.0027379F +#define SIDEREAL_PERIOD 15956313.06126534L +#define SIDEREAL_RATE_HZ 60.16427456104770L +#define SOLAR_RATE_HZ 60.0L +#define LUNAR_RATE_HZ 57.9L +#define KING_RATE_HZ 60.136L +#define Deg10 0.174532925199432L +#define Deg20 0.349065850398865L +#define Deg45 0.785398163397448L +#define Deg60 1.047197551196597L +#define Deg85 1.483529864195180L +#define Deg90 1.570796326794896L +#define Deg180 3.141592653589793L +#define Deg360 6.283185307179586L + +// for handling degenerate spherical coordinates near the poles +#define TenthArcSec 0.000000484813681L +#define OneArcSec 0.000004848136811L +#define SmallestFloat 0.0000005F diff --git a/src/lib/Macros.h b/src/lib/Macros.h new file mode 100755 index 0000000..1c13a65 --- /dev/null +++ b/src/lib/Macros.h @@ -0,0 +1,291 @@ +// Library general purpose macros +#pragma once + +// conversion math +#define degToArcsecF(x) ((x)/3600.0F) +#define degToRad(x) ((x)/(double)RAD_DEG_RATIO) +#define degToRadF(x) ((x)/(double)RAD_DEG_RATIO_F) +#define radToDeg(x) ((x)*(double)RAD_DEG_RATIO) +#define radToDegF(x) ((x)*(double)RAD_DEG_RATIO_F) +#define hrsToRad(x) ((x)/(double)RAD_HOUR_RATIO) +#define radToHrs(x) ((x)*(double)RAD_HOUR_RATIO) +#define arcsecToRad(x) ((x)/(double)206264.8062470963L) +#define radToArcsec(x) ((x)*(double)206264.8062470963L) +#define siderealToRad(x) ((x)/(double)13750.98708313975L) +#define siderealToRadF(x) ((x)/13750.987F) +#define radToSidereal(x) ((x)*(double)13750.98708313975L) +#define hzToSubMicros(x) ((x)*(double)266666.666666667L) +#define hzToSidereal(x) ((x)/(double)SIDEREAL_RATE_HZ) +#define siderealToHz(x) ((x)*(double)SIDEREAL_RATE_HZ) +#define fequal(x,y) (fabs((x)-(y))SmallestFloat) +#define flt(x,y) ((y)-(x)>SmallestFloat) +#define NormalizeAzimuth(x) (x<0.0L?x+360.0L:x) + +// strings + +// tests if strings are equal +#define strequ(str1, str2) (strlen(str1) != strlen(str2) ? 0 : strstr(str1, str2)) + +// safe string copy from (char*)source to (char *)dest for a destination array of size bytes +#define sstrcpy(dest, source) do { \ + int16_t _cap = sizeof(dest); \ + if (_cap) { \ + int16_t _i = 0; \ + for (; _i + 1 < _cap && (source)[_i]; ++_i) { \ + (dest)[_i] = (source)[_i]; \ + } \ + (dest)[_i] = '\0'; \ + } \ +} while (0) + +#define sstrcpyex(dest, source, cap) do { \ + if (cap) { \ + int16_t _i = 0; \ + for (; _i + 1 < cap && (source)[_i]; ++_i) { \ + (dest)[_i] = (source)[_i]; \ + } \ + (dest)[_i] = '\0'; \ + } \ +} while (0) + +// safe string append from (char*)source to (char*)dest for a destination array of size bytes +#define sstrcat(dest, source) do { \ + int16_t _cap = sizeof(dest); \ + if (_cap) { \ + int16_t _i = 0; \ + while (_i < _cap && (dest)[_i]) ++_i; \ + if (_i >= _cap) { \ + (dest)[_cap - 1] = '\0'; \ + } else { \ + int16_t _j = 0; \ + for (; _i + 1 < _cap && (source)[_j]; ++_i, ++_j) { \ + (dest)[_i] = (source)[_j]; \ + } \ + (dest)[_i] = '\0'; \ + } \ + } \ +} while (0) + +#define sstrcatex(dest, source, cap) do { \ + int16_t _cap = (cap); \ + if (_cap) { \ + int16_t _i = 0; \ + while (_i < _cap && (dest)[_i]) ++_i; \ + if (_i >= _cap) { \ + (dest)[_cap - 1] = '\0'; \ + } else { \ + int16_t _j = 0; \ + for (; _i + 1 < _cap && (source)[_j]; ++_i, ++_j) { \ + (dest)[_i] = (source)[_j]; \ + } \ + (dest)[_i] = '\0'; \ + } \ + } \ +} while (0) + +// safe single-character append for a destination array of size bytes +#define sstrcatc(dest, value) do { \ + int16_t _cap = sizeof(dest); \ + if (_cap) { \ + int16_t _i = 0; \ + while (_i < _cap && (dest)[_i]) ++_i; \ + if (_i >= _cap) { \ + (dest)[_cap - 1] = '\0'; \ + } else if (_i + 1 < _cap) { \ + (dest)[_i] = (value); \ + (dest)[_i + 1] = '\0'; \ + } \ + } \ +} while (0) + +#define sstrcatcex(dest, value, cap) do { \ + int16_t _cap = (cap); \ + if (_cap) { \ + int16_t _i = 0; \ + while (_i < _cap && (dest)[_i]) ++_i; \ + if (_i >= _cap) { \ + (dest)[_cap - 1] = '\0'; \ + } else if (_i + 1 < _cap) { \ + (dest)[_i] = (value); \ + (dest)[_i + 1] = '\0'; \ + } \ + } \ +} while (0) + +// truncate a string so there is room for reserve bytes plus the null terminator +#define sstrreserve(dest, reserve) do { \ + int16_t _cap = sizeof(dest); \ + int16_t _reserve = (reserve); \ + if (_cap) { \ + if (_reserve >= _cap) { \ + (dest)[0] = '\0'; \ + } else { \ + int16_t _max = _cap - _reserve - 1; \ + int16_t _i = 0; \ + while (_i < _cap && (dest)[_i]) ++_i; \ + if (_i >= _cap) { \ + (dest)[_cap - 1] = '\0'; \ + _i = _cap - 1; \ + } \ + if (_i > _max) (dest)[_max] = '\0'; \ + } \ + } \ +} while (0) + +#define sstrreserveex(dest, reserve, cap) do { \ + int16_t _cap = (cap); \ + int16_t _reserve = (reserve); \ + if (_cap) { \ + if (_reserve >= _cap) { \ + (dest)[0] = '\0'; \ + } else { \ + int16_t _max = _cap - _reserve - 1; \ + int16_t _i = 0; \ + while (_i < _cap && (dest)[_i]) ++_i; \ + if (_i >= _cap) { \ + (dest)[_cap - 1] = '\0'; \ + _i = _cap - 1; \ + } \ + if (_i > _max) (dest)[_max] = '\0'; \ + } \ + } \ +} while (0) + +// convert string to lower case +#define strtolower(source) for (int i = 0; source[i]; i++) source[i] = tolower(source[i]) + +#define _allowed_fqdn "0123456789abcdefghijklmnopqrstuvwxyz" +// convert to lower case and strip away characters that aren't allowed in an hostname +#define strtohostname(source) for (int i = 0; source[i]; i++) { source[i] = tolower(source[i]); if (!strchr(_allowed_fqdn "-", source[i])) { if (i < (int)(strlen(source) - 1)) { memmove(&source[i], &source[i + 1], strlen(&source[i])); i--; } else source[i] = 0;; } } + +// convert to lower case and strip away characters that aren't allowed in an hostname (and '-') +#define strtohostname2(source) for (int i = 0; source[i]; i++) { source[i] = tolower(source[i]); if (!strchr(_allowed_fqdn, source[i])) { if (i < (int)(strlen(source) - 1)) { memmove(&source[i], &source[i + 1], strlen(&source[i])); i--; } else source[i] = 0;; } } + +// embeds a string in a macro +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + +// ip address helper copies from one ip[4] address to another +#define ip4toip4(ip4, ip) for (int i = 0; i < 4; i++) ip4[i] = ip[i] + +// check for an invalid ip[4] address of 0:0:0:0 +#define validip4(ip4) (ip4[0]!=0?true:ip4[1]!=0?true:ip4[2]!=0?true:ip4[3]!=0?true:false) + +// an empty string +#ifndef EmptyStr + #define EmptyStr "" +#endif + +// pins +#define DAC_PIN(v) ((v)+0x100) +#define GPIO_PIN(v) ((v)+0x200) + +// sense +#define THLD(v) ((v)<<1) // 10 bit analog threshold, bits 1 through 10 +#define HYST(v) ((v)<<11) // 10 bit hysteresis, bits 11 through 20 +#define ANLG(v) ((v)<<21) // 10 bit analog value, bits 21 through 30 +#ifndef INPUT_PULLDOWN + #define INPUT_PULLDOWN INPUT +#endif + +#define RANGE_FROM_BITS(bits) ((1 << (bits)) - 1) + +#define BITS_FROM_RANGE(R) ( \ + (R)<=1?1 : (R)<=3?2 : (R)<=7?3 : (R)<=15?4 : (R)<=31?5 : (R)<=63?6 : \ + (R)<=127?7 : (R)<=255?8 : (R)<=511?9 : (R)<=1023?10 : (R)<=2047?11 : \ + (R)<=4095?12 : (R)<=8191?13 : (R)<=16383?14 : (R)<=32767?15 : 16 ) + +#define IS_POW2_MINUS1(R) ((R) > 0 && (R) < 0xFFFFFFFFU && (((R) + 1) & (R)) == 0) + +// Atomic helpers for single ISR-shared scalars. +// Use for single reads/writes/RMW only. For multi-variable invariants, keep cli/sei blocks. +#if defined(__GNUC__) || defined(__clang__) + + #define ATOMIC_LOAD(var) (__atomic_load_n(&(var), __ATOMIC_ACQUIRE)) + #define ATOMIC_STORE(var, val) (__atomic_store_n(&(var), (val), __ATOMIC_RELEASE)) + #define ATOMIC_XCHG(var, val) (__atomic_exchange_n(&(var), (val), __ATOMIC_ACQ_REL)) + + // Read/Modify/Write bit ops (return previous value) + #define ATOMIC_OR(var, mask) (__atomic_fetch_or(&(var), (mask), __ATOMIC_ACQ_REL)) + #define ATOMIC_AND(var, mask) (__atomic_fetch_and(&(var), (mask), __ATOMIC_ACQ_REL)) + +#else + + #define ATOMIC_LOAD(var) \ + ({ __typeof__(var) _v; noInterrupts(); _v = (var); interrupts(); _v; }) + + #define ATOMIC_STORE(var, val) \ + do { noInterrupts(); (var) = (val); interrupts(); } while (0) + + #define ATOMIC_XCHG(var, val) \ + ({ __typeof__(var) _old; noInterrupts(); _old = (var); (var) = (val); interrupts(); _old; }) + + // Read/Modify/Write bit ops (return previous value) + #define ATOMIC_OR(var, mask) \ + ({ __typeof__(var) _old; noInterrupts(); _old = (var); (var) = (__typeof__(var))(_old | (__typeof__(var))(mask)); interrupts(); _old; }) + + #define ATOMIC_AND(var, mask) \ + ({ __typeof__(var) _old; noInterrupts(); _old = (var); (var) = (__typeof__(var))(_old & (__typeof__(var))(mask)); interrupts(); _old; }) + +#endif + +// Fast GPIO read/write for Arduino-ESP32 cores 2.x / 3.x +// Works across ESP32 / ESP32-S2 / ESP32-S3 +#if defined(CONFIG_IDF_TARGET_ESP32) || defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3) + #include "soc/gpio_struct.h" + + // SOC_GPIO_PIN_COUNT exists in ESP-IDF-based cores; core 1.x may be missing it. + #if defined(__has_include) + #if __has_include("soc/soc_caps.h") + #include "soc/soc_caps.h" + #endif + #endif + + static inline IRAM_ATTR void gpioWriteFast(uint32_t pin, uint32_t level) { + if (pin >= (uint32_t)SOC_GPIO_PIN_COUNT) return; + uint32_t mask = 1UL << (pin & 31U); + if (pin < 32U) { if (level) GPIO.out_w1ts = mask; else GPIO.out_w1tc = mask; } + #if SOC_GPIO_PIN_COUNT > 32 + else { if (level) GPIO.out1_w1ts.val = mask; else GPIO.out1_w1tc.val = mask; } + #endif + } + #ifndef digitalWriteF + #define digitalWriteF(pin,value) gpioWriteFast((uint32_t)(pin), (uint32_t)(value)) + #endif + + static inline IRAM_ATTR uint32_t gpioReadFast(uint32_t pin) { + if (pin >= (uint32_t)SOC_GPIO_PIN_COUNT) return 0; + uint32_t mask = 1UL << (pin & 31U); + if (pin < 32U) { return (GPIO.in & mask) ? 1U : 0U; } + #if SOC_GPIO_PIN_COUNT > 32 + else { return (GPIO.in1.data & mask) ? 1U : 0U; } + #else + return 0; + #endif + } + #ifndef digitalReadF + #define digitalReadF(pin) gpioReadFast((uint32_t)(pin)) + #endif +#endif + +// automatically use fast I/O if available +#ifndef digitalReadF + #ifdef HAL_HAS_DIGITAL_FAST + #define digitalReadF(pin) ( digitalReadFast(pin) ) + #else + #define digitalReadF(pin) ( digitalRead(pin) ) + #endif +#endif +#ifndef digitalWriteF + #ifdef HAL_HAS_DIGITAL_FAST + #define digitalWriteF(pin,value) { digitalWriteFast(pin,value); } + #else + #define digitalWriteF(pin,value) { digitalWrite(pin,value); } + #endif +#endif + +// supress compiler warnings for unused parameters +#ifndef UNUSED + #define UNUSED(x) (void)(x) +#endif diff --git a/src/lib/analog/Analog.cpp b/src/lib/analog/Analog.cpp new file mode 100755 index 0000000..128dc5c --- /dev/null +++ b/src/lib/analog/Analog.cpp @@ -0,0 +1,660 @@ +// Analog.cpp +#include + +#include "Analog.h" + +// this was only for pin 38 of a Teensy4.1, now any pin +#if defined(ARDUINO_TEENSY41) && defined(PWM_CUSTOM_PIN) && \ + defined(PWM_CUSTOM_ANALOG_WRITE_FREQUENCY) && defined(PWM_CUSTOM_ANALOG_WRITE_RANGE) + #define HAS_CUSTOM_PIN +#endif + +#ifdef HAS_CUSTOM_PIN + static IntervalTimer itimer4; + static volatile uint16_t _pwm_custom_period = 0; + static volatile uint8_t _pwm_custom_toggle = 0; + static float _base_freq_divider = PWM_CUSTOM_ANALOG_WRITE_FREQUENCY/(1.0F/(PWM_CUSTOM_ANALOG_WRITE_RANGE/1000000.0F)); + + void PWM_CUSTOM_HWTIMER() { + if (_pwm_custom_period == 0 || _pwm_custom_period == PWM_CUSTOM_ANALOG_WRITE_RANGE) { + itimer4.update(PWM_CUSTOM_ANALOG_WRITE_RANGE/_base_freq_divider); + digitalWriteF(PWM_CUSTOM_PIN, _pwm_custom_period == PWM_CUSTOM_ANALOG_WRITE_RANGE); + _pwm_custom_toggle = 0; + return; + } else { + if (!_pwm_custom_toggle) { + itimer4.update((PWM_CUSTOM_ANALOG_WRITE_RANGE - _pwm_custom_period)/_base_freq_divider); + digitalWriteF(PWM_CUSTOM_PIN, LOW); + } else { + itimer4.update(_pwm_custom_period/_base_freq_divider); + digitalWriteF(PWM_CUSTOM_PIN, HIGH); + } + } + _pwm_custom_toggle = !_pwm_custom_toggle; + } + + #define analogWriteCustomPin(x) do { \ + uint32_t _v = (uint32_t)(x); \ + if (_v > (uint32_t)PWM_CUSTOM_ANALOG_WRITE_RANGE) _v = (uint32_t)PWM_CUSTOM_ANALOG_WRITE_RANGE; \ + _pwm_custom_period = (uint16_t)_v; \ + } while (0) +#endif + +// track actual global resolution when platform only supports global reconfig. +// keeps behavior honest (no "virtual range trick"). +static uint16_t gPwmBits = (uint16_t)BITS_FROM_RANGE(ANALOG_WRITE_RANGE); +static uint16_t gAdcBits = (uint16_t)BITS_FROM_RANGE(ANALOG_READ_RANGE); +static volatile bool ready = false; + +static inline bool toCorePin(int16_t pin, uint8_t &out) { + if (!ready) return false; // not initialized + if (pin == OFF) return false; + if (pin < 0) return false; // reject any other negative + if (pin > 0xFF) return false; // prevents wrap on platforms with >255 pins + out = (uint8_t)pin; + return true; +} + +static inline uint32_t clampU32(uint32_t v, uint32_t lo, uint32_t hi) { + if (v < lo) return lo; + if (v > hi) return hi; + return v; +} + +static inline float clamp01(float x) { + if (isnan(x) || isinf(x)) return 0.0F; + if (x < 0.0F) return 0.0F; + if (x > 1.0F) return 1.0F; + return x; +} + +static inline uint32_t rangeFromBits(uint16_t bits) { + if (bits == 0) return 0; + if (bits >= 32) return 0xFFFFFFFFu; + return (1u << bits) - 1u; +} + +int AnalogClass::pwmIndex(int16_t pin) const { + for (int i = 0; i < MAX_TRACKED; i++) if (pwm_[i].pin == pin) return i; + for (int i = 0; i < MAX_TRACKED; i++) if (pwm_[i].pin == OFF) return i; + return -1; +} + +int AnalogClass::adcIndex(int16_t pin) const { + for (int i = 0; i < MAX_TRACKED; i++) if (adc_[i].pin == pin) return i; + for (int i = 0; i < MAX_TRACKED; i++) if (adc_[i].pin == OFF) return i; + return -1; +} + +// prepare global adc read resolution and global pwm write resolution/frequency +void AnalogClass::begin() { + if (ready) return; + + VLF("MSG: Analog, begin"); + + // ADC global default + (void)gAdcBits; + #if HAL_HAS_GLOBAL_ADC_RESOLUTION + const uint16_t adcBits = (uint16_t)BITS_FROM_RANGE(ANALOG_READ_RANGE); + analogReadResolution(adcBits); + gAdcBits = adcBits; + #endif + + // PWM global default (ONLY if your platform truly uses a global resolution) + (void)gPwmBits; + #if HAL_HAS_GLOBAL_PWM_RESOLUTION && !HAL_HAS_PER_PIN_PWM_RESOLUTION + const uint16_t pwmBits = (uint16_t)BITS_FROM_RANGE(ANALOG_WRITE_RANGE); + analogWriteResolution(pwmBits); + gPwmBits = pwmBits; + VF("MSG: Analog, pwmInit global PWM resolution changed to bits="); VL(pwmBits); + #endif + + // PWM global freq (again: only if global is real on that platform) + #if defined(ANALOG_WRITE_FREQUENCY) && HAL_HAS_GLOBAL_PWM_FREQUENCY + analogWriteFrequency(ANALOG_WRITE_FREQUENCY); + VF("MSG: Analog, pwmInit global PWM freq changed to hz="); VL(ANALOG_WRITE_FREQUENCY); + #endif + + ready = true; +} + +// ---------------- PWM ---------------- + +bool AnalogClass::pwmInit(int16_t pin, const AnalogPwmConfig& cfg) { + uint8_t p; + if (!toCorePin(pin, p)) { + DF("ERR: Analog, pwmInit invalid pin="); DL(pin); + return false; + } + + const int idx = pwmIndex(pin); + if (idx < 0) { DF("ERR: Analog, pwmInit table full pin="); DL(pin); return false; } + const uint32_t defRange = (uint32_t)ANALOG_WRITE_RANGE; + uint16_t reqBits = cfg.bits; + uint32_t reqRange = cfg.range; + uint32_t reqHz = cfg.hz; + + // if this is a T4.1 allow one extra custom PWM pin + #ifdef HAS_CUSTOM_PIN + if (pin == PWM_CUSTOM_PIN) { + pinMode(PWM_CUSTOM_PIN, OUTPUT); + pwm_[idx].custom = true; + itimer4.priority(0); + itimer4.begin(PWM_CUSTOM_HWTIMER, 100); + VF("MSG: Analog, pwmInit emulating PWM using Teensy4.1 iTimer4 on pin"); VL(pin); + reqBits = BITS_FROM_RANGE(PWM_CUSTOM_ANALOG_WRITE_RANGE); + reqRange = PWM_CUSTOM_ANALOG_WRITE_RANGE; + reqHz = PWM_CUSTOM_ANALOG_WRITE_FREQUENCY; + } else pwm_[idx].custom = false; + #endif + + if (!pwm_[idx].custom) { + + // derive bits from range if only range provided (range must be 2^n-1) + if (reqBits == 0 && reqRange != 0) { + if (reqRange == 0xFFFFFFFFu) { + DF("ERR: Analog, pwmInit range wrap pin="); D(pin); DF(" range="); DL(reqRange); + return false; + } + uint32_t r = reqRange + 1; + if ((r & (r - 1)) != 0) { + DF("ERR: Analog, pwmInit range not 2^n-1 pin="); D(pin); DF(" range="); DL(reqRange); + return false; + } + uint16_t b = 0; + while (r >>= 1) b++; + reqBits = b; + } + + // derive range from bits or default + if (reqBits == 0 && reqRange == 0) { + reqRange = defRange; + } else if (reqRange == 0 && reqBits != 0) { + reqRange = rangeFromBits(reqBits); + } + + #ifdef ANALOG_WRITE_FREQUENCY + if (reqHz == 0) { + reqHz = ANALOG_WRITE_FREQUENCY; + } + #endif + + // bounds + if (reqBits) { + if (reqBits < HAL_PWM_BITS_MIN || reqBits > HAL_PWM_BITS_MAX) { + DF("ERR: Analog, pwmInit bits out of bounds pin="); D(pin); + DF(" bits="); D(reqBits); DF(" ("); D(HAL_PWM_BITS_MIN); DF(".."); D(HAL_PWM_BITS_MAX); DLF(")"); + return false; + } + } + if (reqHz) { + if (reqHz < HAL_PWM_HZ_MIN || reqHz > HAL_PWM_HZ_MAX) { + DF("ERR: Analog, pwmInit hz out of bounds pin="); D(pin); + DF(" hz="); D(reqHz); DF(" ("); D(HAL_PWM_HZ_MIN); DF(".."); D(HAL_PWM_HZ_MAX); DLF(")"); + return false; + } + } + + // --- APPLY (best-effort) --- + + // warn generally the "per-pin resolutions/frequencies" can still affect other pins if the core + // maps channels to shared timers. This may interfere with tone() or other PWM users. + static bool warned = false; + (void)warned; + + // frequency + if (reqHz) { + #if HAL_HAS_PER_PIN_PWM_FREQUENCY + analogWriteFrequency(p, reqHz); + if (!warned) { + warned = true; + VLF("WRN: Analog, adding new resolutions/frequencies may affect other PWM pins (tone, servo, due to shared timers.)"); + } + #elif HAL_HAS_GLOBAL_PWM_FREQUENCY + if (!HAL_ALLOW_GLOBAL_PWM_RECONFIG) { + DF("ERR: Analog, pwmInit global PWM freq reconfig disallowed pin="); DL(pin); + return false; + } + analogWriteFrequency(reqHz); + VF("WRN: Analog, pwmInit global PWM freq changed hz="); VL(reqHz); + #else + DF("ERR: Analog, pwmInit no PWM frequency control pin="); DL(pin); + return false; + #endif + } + + // resolution + if (reqBits) { + #if HAL_HAS_PER_PIN_PWM_RESOLUTION + analogWriteResolution(p, reqBits); + if (!warned) { + warned = true; + VLF("WRN: Analog, adding new resolutions/frequencies may affect other PWM pins (tone, servo, due to shared timers.)"); + } + #elif HAL_HAS_GLOBAL_PWM_RESOLUTION + if (reqBits != gPwmBits) { + if (!HAL_ALLOW_GLOBAL_PWM_RECONFIG) { + DF("ERR: Analog, pwmInit global PWM resolution reconfig disallowed pin="); D(pin); + DF(" reqBits="); D(reqBits); + DF(" currentBits="); DL(gPwmBits); + return false; + } + analogWriteResolution(reqBits); + gPwmBits = reqBits; + VF("WRN: Analog, pwmInit global PWM bits set bits="); VL(reqBits); + } + #else + DF("ERR: Analog, pwmInit no PWM resolution control pin="); DL(pin); + return false; + #endif + } else reqBits = gPwmBits; + } + + pwm_[idx].pin = pin; + pwm_[idx].bits = reqBits; + pwm_[idx].hz = reqHz; + pwm_[idx].range = reqRange ? reqRange : defRange; + + VF("MSG: Analog, pwmInit ok pin="); V(pin); + VF(" bits="); V(reqBits); + VF(" hz="); V(reqHz); + VF(" range="); VL(pwm_[idx].range); + + return true; +} + +AnalogPwmHandle AnalogClass::pwmInitHandle(int16_t pin, const AnalogPwmConfig& cfg) { + AnalogPwmHandle h; + h.pin = 0; + h.idx = ANALOG_INVALID_IDX; + + uint8_t p; + if (!toCorePin(pin, p)) { + DF("ERR: Analog pwmInitHandle invalid pin="); DL(pin); + return h; + } + if (!pwmInit(pin, cfg)) { + // pwmInit already emits the detailed error path + return h; + } + + // find the committed slot for this pin (no allocation now, should hit first loop) + int idx = -1; + for (int i = 0; i < MAX_TRACKED; i++) if (pwm_[i].pin == pin) { idx = i; break; } + if (idx < 0 || idx > 0xFE) { + DF("ERR: Analog pwmInitHandle slot not found pin="); DL(pin); + return h; + } + + h.pin = p; + h.idx = (uint8_t)idx; + return h; +} + +uint32_t AnalogClass::pwmRange(int16_t pin) const { + const uint32_t fallback = (uint32_t)ANALOG_WRITE_RANGE; + + #ifdef HAS_CUSTOM_PIN + if (pin == (int16_t)PWM_CUSTOM_PIN) { + for (int i = 0; i < MAX_TRACKED; i++) + if (pwm_[i].pin == pin) return pwm_[i].range ? pwm_[i].range : fallback; + return (uint32_t)PWM_CUSTOM_ANALOG_WRITE_RANGE; + } + #endif + + // if only global PWM resolution exists, report the true global range + #if HAL_HAS_GLOBAL_PWM_RESOLUTION && !HAL_HAS_PER_PIN_PWM_RESOLUTION + return rangeFromBits(gPwmBits); + #endif + + for (int i = 0; i < MAX_TRACKED; i++) { + if (pwm_[i].pin == pin) return pwm_[i].range ? pwm_[i].range : fallback; + } + return fallback; +} + +void AnalogClass::write(int16_t pin, uint32_t v) { + uint8_t p; + if (!toCorePin(pin, p)) { + VF("WRN: Analog write(u32) invalid pin="); VL(pin); + return; + } + + const uint32_t r = pwmRange(pin); + v = clampU32(v, 0, r); + + #ifdef HAS_CUSTOM_PIN + if (p == (uint8_t)PWM_CUSTOM_PIN) { analogWriteCustomPin(v); } else + #endif + analogWrite(p, (int)v); +} + +void AnalogClass::write(int16_t pin, float x01) { + uint8_t p; + if (!toCorePin(pin, p)) { + VF("WRN: Analog write(f) invalid pin="); VL(pin); + return; + } + + x01 = clamp01(x01); + const uint32_t r = pwmRange(pin); + const uint32_t v = (uint32_t)lroundf(x01 * (float)r); + + #ifdef HAS_CUSTOM_PIN + if (p == (uint8_t)PWM_CUSTOM_PIN) { analogWriteCustomPin(v); } else + #endif + analogWrite(p, (int)v); +} + +void AnalogClass::writeQ16(int16_t pin, uint16_t q16) { + uint8_t p; + if (!toCorePin(pin, p)) { + VF("WRN: Analog writeQ16 invalid pin="); VL(pin); + return; + } + + const uint32_t r = pwmRange(pin); + const uint32_t v = (uint32_t)((((uint64_t)q16) * (uint64_t)r + 32767ULL) / 65535ULL); + + #ifdef HAS_CUSTOM_PIN + if (p == (uint8_t)PWM_CUSTOM_PIN) { analogWriteCustomPin(v); } else + #endif + analogWrite(p, (int)v); +} + +void AnalogClass::write(const AnalogPwmHandle& h, uint32_t v) { + if (h.idx == ANALOG_INVALID_IDX) return; + + uint32_t r; + #ifdef HAS_CUSTOM_PIN + if (h.pin == (uint8_t)PWM_CUSTOM_PIN) { + r = pwm_[h.idx].range ? pwm_[h.idx].range : (uint32_t)PWM_CUSTOM_ANALOG_WRITE_RANGE; + } else + #endif + #if HAL_HAS_GLOBAL_PWM_RESOLUTION && !HAL_HAS_PER_PIN_PWM_RESOLUTION + r = rangeFromBits(gPwmBits); + #else + r = pwm_[h.idx].range ? pwm_[h.idx].range : (uint32_t)ANALOG_WRITE_RANGE; + #endif + + v = clampU32(v, 0, r); + + #ifdef HAS_CUSTOM_PIN + if (h.pin == (uint8_t)PWM_CUSTOM_PIN) { analogWriteCustomPin(v); } else + #endif + analogWrite(h.pin, (int)v); +} + +void AnalogClass::write(const AnalogPwmHandle& h, float x01) { + if (h.idx == ANALOG_INVALID_IDX) return; + + x01 = clamp01(x01); + + uint32_t r; + #ifdef HAS_CUSTOM_PIN + if (h.pin == (uint8_t)PWM_CUSTOM_PIN) { + r = pwm_[h.idx].range ? pwm_[h.idx].range : (uint32_t)PWM_CUSTOM_ANALOG_WRITE_RANGE; + } else + #endif + #if HAL_HAS_GLOBAL_PWM_RESOLUTION && !HAL_HAS_PER_PIN_PWM_RESOLUTION + r = rangeFromBits(gPwmBits); + #else + r = pwm_[h.idx].range ? pwm_[h.idx].range : (uint32_t)ANALOG_WRITE_RANGE; + #endif + + const uint32_t v = (uint32_t)lroundf(x01 * (float)r); + + #ifdef HAS_CUSTOM_PIN + if (h.pin == (uint8_t)PWM_CUSTOM_PIN) { analogWriteCustomPin(v); } else + #endif + analogWrite(h.pin, (int)v); +} + +void AnalogClass::writeQ16(const AnalogPwmHandle& h, uint16_t q16) { + if (h.idx == ANALOG_INVALID_IDX) return; + + uint32_t r; + #ifdef HAS_CUSTOM_PIN + if (h.pin == (uint8_t)PWM_CUSTOM_PIN) { + r = pwm_[h.idx].range ? pwm_[h.idx].range : (uint32_t)PWM_CUSTOM_ANALOG_WRITE_RANGE; + } else + #endif + #if HAL_HAS_GLOBAL_PWM_RESOLUTION && !HAL_HAS_PER_PIN_PWM_RESOLUTION + r = rangeFromBits(gPwmBits); + #else + r = pwm_[h.idx].range ? pwm_[h.idx].range : (uint32_t)ANALOG_WRITE_RANGE; + #endif + + const uint32_t v = (uint32_t)((((uint64_t)q16) * (uint64_t)r + 32767ULL) / 65535ULL); + + #ifdef HAS_CUSTOM_PIN + if (h.pin == (uint8_t)PWM_CUSTOM_PIN) { analogWriteCustomPin(v); } else + #endif + analogWrite(h.pin, (int)v); +} + +// ---------------- ADC ---------------- + +bool AnalogClass::adcInit(int16_t pin, const AnalogAdcConfig& cfg) { + uint8_t p; + if (!toCorePin(pin, p)) { + DF("ERR: Analog adcInit invalid pin="); DL(pin); + return false; + } + + const int idx = adcIndex(pin); + if (idx < 0) { DF("ERR: Analog adcInit table full pin="); DL(pin); return false; } + const uint32_t defRange = (uint32_t)ANALOG_READ_RANGE; + + uint16_t reqBits = cfg.bits; + uint32_t reqRange = cfg.range; + + // derive bits from range if needed (range must be 2^n-1) + if (reqBits == 0 && reqRange != 0) { + if (!IS_POW2_MINUS1(reqRange)) { + DF("ERR: Analog adcInit range not 2^n-1 pin="); D(pin); DF(" range="); DL(reqRange); + return false; + } + reqBits = (uint16_t)BITS_FROM_RANGE(reqRange); + } + + // derive range from bits or default + if (reqBits == 0 && reqRange == 0) { + reqRange = defRange; + } else if (reqRange == 0 && reqBits != 0) { + reqRange = rangeFromBits(reqBits); + } + + // bounds + if (reqBits) { + if (reqBits < HAL_ADC_BITS_MIN || reqBits > HAL_ADC_BITS_MAX) { + DF("ERR: Analog, adcInit bits out of bounds pin="); D(pin); + DF(" bits="); D(reqBits); DF(" ("); D(HAL_ADC_BITS_MIN); DF(".."); D(HAL_ADC_BITS_MAX); DLF(")"); + return false; + } + } + + // apply resolution only if allowed + if (reqBits) { + #if HAL_HAS_PER_PIN_ADC_RESOLUTION + analogReadResolution(p, reqBits); + #elif HAL_HAS_GLOBAL_ADC_RESOLUTION + if (!HAL_ALLOW_GLOBAL_ADC_RECONFIG) { + DF("ERR: Analog, adcInit global ADC reconfig disallowed pin="); DL(pin); + return false; + } + analogReadResolution(reqBits); + gAdcBits = reqBits; // reflect actual global state + VF("WRN: Analog, adcInit global ADC bits set bits="); VL(reqBits); + #else + DF("ERR: Analog, adcInit no ADC resolution control pin="); DL(pin); + return false; + #endif + } + + adc_[idx].pin = pin; + adc_[idx].bits = reqBits; + adc_[idx].range = reqRange ? reqRange : defRange; + adc_[idx].oversample = cfg.oversample ? cfg.oversample : 1; + + VF("MSG: Analog, adcInit ok pin="); V(pin); + VF(" bits="); V(reqBits); + VF(" range="); V(adc_[idx].range); + VF(" os="); VL(adc_[idx].oversample); + + return true; +} + +AnalogAdcHandle AnalogClass::adcInitHandle(int16_t pin, const AnalogAdcConfig& cfg) { + AnalogAdcHandle h; + h.pin = 0; + h.oversample = 1; + h.idx = ANALOG_INVALID_IDX; + + uint8_t p; + if (!toCorePin(pin, p)) { + DF("ERR: Analog, adcInitHandle invalid pin="); DL(pin); + return h; + } + if (!adcInit(pin, cfg)) { + // adcInit already emits the detailed error path + return h; + } + + int idx = -1; + for (int i = 0; i < MAX_TRACKED; i++) if (adc_[i].pin == pin) { idx = i; break; } + if (idx < 0 || idx > 0xFE) { + DF("ERR: Analog, adcInitHandle slot not found pin="); DL(pin); + return h; + } + + h.pin = p; + h.idx = (uint8_t)idx; + h.oversample = adc_[idx].oversample ? adc_[idx].oversample : 1; + return h; +} + +uint32_t AnalogClass::adcRange(int16_t pin) const { + const uint32_t fallback = (uint32_t)ANALOG_READ_RANGE; + + // If only global ADC resolution exists, report the true global range + #if HAL_HAS_GLOBAL_ADC_RESOLUTION && !HAL_HAS_PER_PIN_ADC_RESOLUTION + (void)pin; + return rangeFromBits(gAdcBits); + #endif + + for (int i = 0; i < MAX_TRACKED; i++) { + if (adc_[i].pin == pin) return adc_[i].range ? adc_[i].range : fallback; + } + return fallback; +} + +uint32_t AnalogClass::read(int16_t pin) { + uint8_t p; + if (!toCorePin(pin, p)) { + VF("WRN: Analog, read invalid pin="); VL(pin); + return 0; + } + + uint8_t os = 1; + for (int i = 0; i < MAX_TRACKED; i++) { + if (adc_[i].pin == pin) { os = adc_[i].oversample; break; } + } + + if (os <= 1) return (uint32_t)analogRead(p); + + uint32_t acc = 0; + for (uint8_t i = 0; i < os; i++) acc += (uint32_t)analogRead(p); + return (acc + (os / 2)) / os; +} + +float AnalogClass::readf(int16_t pin) { + uint8_t p; + if (!toCorePin(pin, p)) { + VF("WRN: Analog, readf invalid pin="); VL(pin); + return 0.0f; + } + (void)p; + + const uint32_t r = adcRange(pin); + const uint32_t v = read(pin); + if (r == 0) return 0.0f; + return clamp01((float)v / (float)r); +} + +float AnalogClass::readV(int16_t pin) { + uint8_t p; + if (!toCorePin(pin, p)) { + VF("WRN: Analog, readV invalid pin="); VL(pin); + return 0.0f; + } + +#if defined(ESP32) + return (float)analogReadMilliVolts(p) * 0.001F; +#else + const uint32_t r = adcRange(pin); + if (r == 0) return 0.0F; + const uint32_t raw = read(pin); + return (float)raw * (HAL_VCC / (float)r); +#endif +} + +uint16_t AnalogClass::readQ16(int16_t pin) { + uint8_t p; + if (!toCorePin(pin, p)) { + VF("WRN: Analog, readQ16 invalid pin="); VL(pin); + return 0; + } + (void)p; + + const uint32_t r = adcRange(pin); + const uint32_t v = read(pin); + if (r == 0) return 0; + + const uint32_t q = (uint32_t)((((uint64_t)v) * 65535ULL + (r / 2)) / (uint64_t)r); + return (uint16_t)clampU32(q, 0, 65535U); +} + +uint32_t AnalogClass::read(const AnalogAdcHandle& h) { + if (h.idx == ANALOG_INVALID_IDX) return 0; + + const uint8_t os = (h.oversample <= 1) ? 1 : h.oversample; + if (os == 1) return (uint32_t)analogRead(h.pin); + + uint32_t acc = 0; + for (uint8_t i = 0; i < os; i++) acc += (uint32_t)analogRead(h.pin); + return (acc + (os / 2)) / os; +} + +float AnalogClass::readf(const AnalogAdcHandle& h) { + if (h.idx == ANALOG_INVALID_IDX) return 0.0f; + + uint32_t r; + #if HAL_HAS_GLOBAL_ADC_RESOLUTION && !HAL_HAS_PER_PIN_ADC_RESOLUTION + r = rangeFromBits(gAdcBits); + #else + r = adc_[h.idx].range ? adc_[h.idx].range : (uint32_t)ANALOG_READ_RANGE; + #endif + + if (r == 0) return 0.0f; + const uint32_t v = read(h); + return clamp01((float)v / (float)r); +} + +uint16_t AnalogClass::readQ16(const AnalogAdcHandle& h) { + if (h.idx == ANALOG_INVALID_IDX) return 0; + + uint32_t r; + #if HAL_HAS_GLOBAL_ADC_RESOLUTION && !HAL_HAS_PER_PIN_ADC_RESOLUTION + r = rangeFromBits(gAdcBits); + #else + r = adc_[h.idx].range ? adc_[h.idx].range : (uint32_t)ANALOG_READ_RANGE; + #endif + + if (r == 0) return 0; + const uint32_t v = read(h); + const uint32_t q = (uint32_t)((((uint64_t)v) * 65535ULL + (r / 2)) / (uint64_t)r); + return (uint16_t)clampU32(q, 0, 65535U); +} + +AnalogClass analog; diff --git a/src/lib/analog/Analog.h b/src/lib/analog/Analog.h new file mode 100755 index 0000000..c8e27b6 --- /dev/null +++ b/src/lib/analog/Analog.h @@ -0,0 +1,112 @@ +// Analog.h +#pragma once +#include +#include +#include "../../Common.h" + +// --- Pin sentinel --- +#ifndef OFF + #define OFF (-1) +#endif + +// --- Some error checking --- +#if !IS_POW2_MINUS1(ANALOG_WRITE_RANGE) + #error "ANALOG_WRITE_RANGE must be (2^n - 1)." +#endif + +#if !IS_POW2_MINUS1(ANALOG_WRITE_RANGE) + #error "ANALOG_WRITE_RANGE must be (2^n - 1)." +#endif +#if ANALOG_WRITE_RANGE < RANGE_FROM_BITS(HAL_PWM_BITS_MIN) + #error "ANALOG_WRITE_RANGE below platform PWM resolution limits." +#endif +#if ANALOG_WRITE_RANGE > RANGE_FROM_BITS(HAL_PWM_BITS_MAX) + #error "ANALOG_WRITE_RANGE exceeds platform PWM resolution limits." +#endif + +#if !IS_POW2_MINUS1(ANALOG_READ_RANGE) + #error "ANALOG_READ_RANGE must be (2^n - 1)." +#endif +#if ANALOG_READ_RANGE < RANGE_FROM_BITS(HAL_ADC_BITS_MIN) + #error "ANALOG_READ_RANGE below platform ADC resolution limits." +#endif +#if ANALOG_READ_RANGE > RANGE_FROM_BITS(HAL_ADC_BITS_MAX) + #error "ANALOG_READ_RANGE exceeds platform ADC resolution limits." +#endif + +// Optional: Servo PWM range only allowed to differ when per-pin PWM resolution exists +#ifdef SERVO_ANALOG_WRITE_RANGE + #if !IS_POW2_MINUS1(SERVO_ANALOG_WRITE_RANGE) + #error "SERVO_ANALOG_WRITE_RANGE must be (2^n - 1)." + #endif + #if !HAL_HAS_PER_PIN_PWM_RESOLUTION && SERVO_ANALOG_WRITE_RANGE != ANALOG_WRITE_RANGE + #error "Global-only PWM: SERVO_ANALOG_WRITE_RANGE must equal ANALOG_WRITE_RANGE." + #endif +#endif + +static constexpr uint8_t ANALOG_INVALID_IDX = 0xFF; + +struct AnalogPwmConfig { + uint32_t hz = 0; // 0 = leave default + uint16_t bits = 0; // 0 = leave default + uint32_t range = 0; // 0 = auto (2^bits-1) or leave +}; + +struct AnalogPwmHandle { + uint8_t pin = 0; // core pin for analogWrite() + uint8_t idx = ANALOG_INVALID_IDX; // index into pwm_[] cache +}; + +struct AnalogAdcConfig { + uint16_t bits = 0; // 0 = leave default + uint32_t range = 0; // 0 = auto (2^bits-1) or leave + uint8_t oversample = 1; +}; + +struct AnalogAdcHandle { + uint8_t pin = 0; // core pin for analogRead() + uint8_t oversample = 1; // cached oversample (avoid adc_[] lookup each read) + uint8_t idx = ANALOG_INVALID_IDX; // index into adc_[] cache +}; + +class AnalogClass { +public: + // prepare global adc read resolution and global pwm write resolution/frequency + void begin(); + + // pwm write, with per pin frequency/resolution where possible + bool pwmInit(int16_t pin, const AnalogPwmConfig& cfg = {}); + AnalogPwmHandle pwmInitHandle(int16_t pin, const AnalogPwmConfig& cfg); + void write(int16_t pin, uint32_t value); // native 0..range + void write(int16_t pin, float x01); // normalized 0..1 + void writeQ16(int16_t pin, uint16_t q16); // normalized 0..65535 + void write(const AnalogPwmHandle& h, uint32_t v); + void write(const AnalogPwmHandle& h, float x01); + void writeQ16(const AnalogPwmHandle& h, uint16_t q16); + uint32_t pwmRange(int16_t pin) const; + + // adc read, with per pin resolution where possible + bool adcInit(int16_t pin, const AnalogAdcConfig& cfg = {}); + AnalogAdcHandle adcInitHandle(int16_t pin, const AnalogAdcConfig& cfg); + uint32_t read(int16_t pin); // native 0..range + float readf(int16_t pin); // normalized 0..1 + float readV(int16_t pin); // volts (best-effort) + uint16_t readQ16(int16_t pin); // normalized 0..65535 + uint32_t read(const AnalogAdcHandle& h); + float readf(const AnalogAdcHandle& h); + uint16_t readQ16(const AnalogAdcHandle& h); + uint32_t adcRange(int16_t pin) const; + +private: + struct PwmState { int16_t pin = OFF; uint16_t bits = 0; uint32_t hz = 0; uint32_t range = 0; bool custom = false; }; + struct AdcState { int16_t pin = OFF; uint16_t bits = 0; uint8_t oversample = 1; uint32_t range = 0; }; + + static constexpr uint8_t MAX_TRACKED = 32; // must be <= 254 + mutable PwmState pwm_[MAX_TRACKED]{}; + mutable AdcState adc_[MAX_TRACKED]{}; + + int pwmIndex(int16_t pin) const; + int adcIndex(int16_t pin) const; +}; + +extern AnalogClass analog; diff --git a/src/lib/axis/Axis.command.cpp b/src/lib/axis/Axis.command.cpp new file mode 100755 index 0000000..b6cc2e4 --- /dev/null +++ b/src/lib/axis/Axis.command.cpp @@ -0,0 +1,195 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, commands + +#include "Axis.h" + +#ifdef MOTOR_PRESENT + +#include "../../lib/nv/Nv.h" + +#include "../convert/Convert.h" + +extern AxesRevert revert; + +bool Axis::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + *suppressFrame = false; + + if (command[0] == 'G' && command[1] == 'X') { + + // :GXA[n],[p]# Get axis/motor/driver parameter + // Returns: Value + if (parameter[0] == 'A' && parameter[1] >= '1' && parameter[1] <= '9' && parameter[2] == ',') { + if (parameter[1] - '0' != axisNumber) return false; + + // return motor/driver name + if (parameter[3] == 'M' && parameter[4] == 0) { + strncpy(reply, motor->name(), 40); + *numericReply = false; + return true; + } + + int parameterNumber = atoi(¶meter[3]); + if (parameterNumber < 0 || parameterNumber > getParameterCount()) { *commandError = CE_PARAM_RANGE; return true; } + + // check if all axes are set to revert + if (revert.all) { *commandError = CE_0; return true; } + + // check if this axis is set to revert + if (getAxisRevert()) { *commandError = CE_0; return true; } + + // requesting parameter 0 returns the parameter count + if (parameterNumber == 0) { + sprintf(reply, "%d", (int)getParameterCount()); + *numericReply = false; + return true; + } + + // get parameter values + double value = ((parameterNumber == 1) ? stepsPerMeasureValueNv : getParameter(parameterNumber)->valueNv); + float min = getParameter(parameterNumber)->min; + float max = getParameter(parameterNumber)->max; + AxisParameterType type = getParameter(parameterNumber)->type; + + // convert to degrees if necessary + if (getParameter(parameterNumber)->type == AXP_FLOAT_RAD) { + value = radToDeg(value); + min = radToDeg(min); + max = radToDeg(max); + type = AXP_FLOAT; + } + if (getParameter(parameterNumber)->type == AXP_FLOAT_RAD_INV) { + value = degToRad(value); + min = degToRad(min); + max = degToRad(max); + type = AXP_FLOAT; + } + + char valueStr[24]; + sprintF(valueStr, "%1.3f", value); + convert.stripNumericStr(valueStr); + + sprintf(reply, "%s,%ld,%ld,%d,%s", valueStr, lround(min), lround(max), (int)type, getParameter(parameterNumber)->name); + + *numericReply = false; + } + + #ifdef SERVO_MOTOR_PRESENT + // :GXS[n]# Get axis servo delta (in counts) and velocity + // Returns: Values + if (parameter[0] == 'S' && parameter[2] == 0) { + int index = parameter[1] - '1'; + if (index > 8) { *commandError = CE_PARAM_RANGE; return true; } + if (index + 1 != axisNumber) return false; // command wasn't processed + if (motor->driverType != SERVO) { *commandError = CE_CMD_UNKNOWN; return true; } // not a servo + + char temp[20]; + sprintF(temp, "%0.3f", ((ServoMotor*)motor)->velocityPercent); + sprintf(reply, "%ld,%s", ((ServoMotor*)motor)->delta, temp); + *numericReply = false; + } else + #endif + + // :GXU[n]# Get stepper driver statUs for axis [n] + // Returns: Value + if (parameter[0] == 'U' && parameter[2] == 0) { + int index = parameter[1] - '1'; + if (index < 0 || index > 8) { *commandError = CE_PARAM_RANGE; return true; } + if (index + 1 != axisNumber) return false; // command wasn't processed + DriverStatus status = getStatus(); + if (status.active) { + strcat(reply, status.standstill ? "ST," : ","); + strcat(reply, status.outputA.openLoad ? "OA," : ","); + strcat(reply, status.outputB.openLoad ? "OB," : ","); + strcat(reply, status.outputA.shortToGround ? "GA," : ","); + strcat(reply, status.outputB.shortToGround ? "GB," : ","); + strcat(reply, status.overTemperature ? "OT," : ","); // > 150C + strcat(reply, status.overTemperatureWarning ? "PW," : ","); // > 120C + strcat(reply, status.fault ? "GF" : ""); + } else { *commandError = CE_0; return true; } + *numericReply = false; + } else return false; + } else + + if (command[0] == 'S' && command[1] == 'X') { + + // :SXA[n],R# Revert axis/motor/driver parameters to default + // :SXA[n],[p],nnnn.n# Set axis/motor/driver parameter value + if (parameter[0] == 'A' && parameter[2] == ',') { + + // check for axis settings enable/disable + // :SXAC,0# for run-time NV (EEPROM) axis settings + // :SXAC,1# for compile-time Config.h axis settings + // Return: 0 failure, 1 success + if (parameter[1] == 'C') { + if ((parameter[3] == '0' || parameter[3] == '1') && parameter[4] == 0) { + + if (parameter[3] == '0') { + VLF("MSG: Using Axes settings from NV (EEPROM)"); + revert.all = false; + } else { + VLF("MSG: Using Axes settings from Config.h"); + revert.all = true; + } + nv().kv().put(nvRevertKey, revert); + + return true; + } else { *commandError = CE_PARAM_RANGE; return false; } + } + + // check for a valid axisNumber + int index = parameter[1] - '1'; + if (index + 1 != axisNumber) return false; + + // check if all axes are set to revert + if (revert.all) { *commandError = CE_0; return true; } + + // check if this axis is set to revert + if (getAxisRevert()) { *commandError = CE_0; return true; } + + // :SXA[n],R# reverts this axis to defaults on next boot + if (parameter[3] == 'R' && parameter[4] == 0) { + setAxisRevert(true); + nv().kv().put(nvRevertKey, revert); + return true; + } + + // :SXA[n],[p],nnn.n# Set axis/motor/driver parameter value + // :SXA[n],[p],nnn.n# :SXA1,14,12.5# + int parameterNumber = atoi(¶meter[3]); + char* valueStr = strchr(¶meter[3], ','); + if (valueStr == NULL) { *commandError = CE_PARAM_FORM; return true; } + char* conv_end; + double value = strtod(++valueStr, &conv_end); + if (&valueStr[0] == conv_end) { *commandError = CE_PARAM_FORM; return true; } + + // convert to radians if necessary + if (getParameter(parameterNumber)->type == AXP_FLOAT_RAD) value = degToRad(value); + if (getParameter(parameterNumber)->type == AXP_FLOAT_RAD_INV) value = radToDeg(value); + + if (parameterIsValid(getParameter(parameterNumber), value, true)) { + // save parameter values + getParameter(parameterNumber)->valueNv = value; + if (parameterNumber == 1) stepsPerMeasureValueNv = value; + + // get ready to write to NV + AxisStoredSettings nvAxisSettings; + nvAxisSettings.stepsPerMeasure = stepsPerMeasureValueNv; + for (int i = 1; i <= getParameterCount(); i++) nvAxisSettings.value[i - 1] = getParameter(i)->valueNv; + + // write the settings to NV + nv().kv().put(nvKey, nvAxisSettings); + + // update immediate parameters now + AxisParameterType type = getParameter(parameterNumber)->type; + if (type == AXP_BOOLEAN_IMMEDIATE || type == AXP_INTEGER_IMMEDIATE || type == AXP_FLOAT_IMMEDIATE) { + getParameter(parameterNumber)->value = value; + } + + } else *commandError = CE_PARAM_RANGE; + } else return false; + } else return false; + + return true; +} + +#endif \ No newline at end of file diff --git a/src/lib/axis/Axis.cpp b/src/lib/axis/Axis.cpp new file mode 100755 index 0000000..c1c9254 --- /dev/null +++ b/src/lib/axis/Axis.cpp @@ -0,0 +1,765 @@ +// ----------------------------------------------------------------------------------- +// Axis motion control + +#include "Axis.h" + +#include "../tasks/OnTask.h" +#include "../nv/Nv.h" + +#ifdef MOTOR_PRESENT + +AxesRevert revert = {true, false, false, false, false, false, false, false, false, false, false}; + +Axis *axisWrapper[9]; +IRAM_ATTR void axisWrapper1() { axisWrapper[0]->poll(); } +IRAM_ATTR void axisWrapper2() { axisWrapper[1]->poll(); } +IRAM_ATTR void axisWrapper3() { axisWrapper[2]->poll(); } +IRAM_ATTR void axisWrapper4() { axisWrapper[3]->poll(); } +IRAM_ATTR void axisWrapper5() { axisWrapper[4]->poll(); } +IRAM_ATTR void axisWrapper6() { axisWrapper[5]->poll(); } +IRAM_ATTR void axisWrapper7() { axisWrapper[6]->poll(); } +IRAM_ATTR void axisWrapper8() { axisWrapper[7]->poll(); } +IRAM_ATTR void axisWrapper9() { axisWrapper[8]->poll(); } + +Axis::Axis(uint8_t axisNumber, const AxisPins *pins, const AxisSettings *Settings, const AxisMeasure axisMeasure, float targetTolerance) { + axisPrefix[5] = '0' + axisNumber; + this->axisNumber = axisNumber; + + this->pins = pins; + + // attach the function pointers to the callbacks + axisWrapper[axisNumber - 1] = this; + switch (axisNumber) { + case 1: callback = axisWrapper1; break; + case 2: callback = axisWrapper2; break; + case 3: callback = axisWrapper3; break; + case 4: callback = axisWrapper4; break; + case 5: callback = axisWrapper5; break; + case 6: callback = axisWrapper6; break; + case 7: callback = axisWrapper7; break; + case 8: callback = axisWrapper8; break; + case 9: callback = axisWrapper9; break; + } + + unitsRadians = false; + switch (axisMeasure) { + case AXIS_MEASURE_MICRONS: + strcpy(unitsStr, "um"); + stepsPerMeasure = &stepsPerMicron; + minMeasure = &minMicrons; + maxMeasure = &maxMicrons; + break; + case AXIS_MEASURE_RADIANS: + strcpy(unitsStr, " deg"); + stepsPerMeasure = &stepsPerRadian; + minMeasure = &minRadians; + maxMeasure = &maxRadians; + unitsRadians = true; + break; + case AXIS_MEASURE_UNKNOWN: + case AXIS_MEASURE_DEGREES: + default: + strcpy(unitsStr, " deg"); + stepsPerMeasure = &stepsPerDegree; + minMeasure = &minDegrees; + maxMeasure = &maxDegrees; + break; + } + + if (stepsPerMeasure != NULL) { + stepsPerMeasure->valueDefault = Settings->stepsPerMeasure; + stepsPerMeasureDefault = Settings->stepsPerMeasure; + minMeasure->valueDefault = Settings->limits.min; + maxMeasure->valueDefault = Settings->limits.max; + } + + backlashFreq = Settings->backlashFreq; + + this->targetTolerance = targetTolerance; +} + +bool Axis::init(Motor *motor) { + this->motor = motor; + + // get default parameter values + AxisStoredSettings nvAxisSettings; + nvAxisSettings.revert = true; + nvAxisSettings.stepsPerMeasure = stepsPerMeasureDefault; + for (int i = 1; i <= getParameterCount(); i++) { + #if DEBUG == VERBOSE + const char* axpn[26] = {AXPN_1, AXPN_2, AXPN_3, AXPN_4, AXPN_5, AXPN_6, AXPN_7, AXPN_8, AXPN_9, AXPN_10, + AXPN_11, AXPN_12, AXPN_13, AXPN_14, AXPN_15, AXPN_16, AXPN_17, AXPN_18, AXPN_19, AXPN_20, AXPN_21, AXPN_22, + AXPN_23, AXPN_24, AXPN_25, AXPN_26}; + VF("MSG:"); V(axisPrefix); VF("found parameter \""); + if (getParameter(i)->name[0] == '$') { + char number[3] = ""; + number[0] = getParameter(i)->name[1]; + number[1] = getParameter(i)->name[2]; + number[2] = 0; + V(axpn[atoi(number) - 1]); + } else { V(getParameter(i)->name); } + VF("\" with default value "); VL(getParameter(i)->valueDefault); + #endif + getParameter(i)->value = getParameter(i)->valueDefault; + getParameter(i)->valueNv = getParameter(i)->valueDefault; + nvAxisSettings.value[i - 1] = getParameter(i)->valueDefault; + } + + // check default parameters + for (int i = 1; i <= getParameterCount(); i++) { + if (!parameterIsValid(getParameter(i))) { + DF("ERR:"); D(axisPrefix); DF("default parameter# "); D(i); DLF(" validation failed!"); + return false; + } + } + + // prepare the revert key + nvRevertKey = nv().kv().computeKey("AXIS_SETTINGS_REVERT"); + + // get the revert setting + if (!nv().kv().getOrInit(nvRevertKey, revert)) { DLF("WRN: init failed for AXIS_SETTINGS_REVERT"); }; + + // prepare the axis settings key for this axis + char keyStr[26]; + snprintf(keyStr, sizeof(keyStr), "AXIS%u_SETTINGS", (unsigned)axisNumber); + nvKey = nv().kv().computeKey(keyStr); + + // read axis settings from NV, unless we are reverting to defaults + if (!revert.all) { + if (getAxisRevert()) { + VF("MSG:"); V(axisPrefix); VLF("reverting settings on NV"); + nv().kv().put(nvKey, nvAxisSettings); + setAxisRevert(false); + nv().kv().put(nvRevertKey, revert); + } else { + VF("MSG:"); V(axisPrefix); VLF("reading settings from NV"); + if (!nv().kv().getOrInit(nvKey, nvAxisSettings)) { DF("WRN: init failed for "); DL(keyStr); } + } + } else { + nv().kv().del(nvKey); + VF("MSG:"); V(axisPrefix); VLF("revert all ignoring settings on NV"); + } + + // move NV stepsPerMeasure into parameter (special case of using a double) + stepsPerMeasureValue = nvAxisSettings.stepsPerMeasure; + stepsPerMeasureValueNv = nvAxisSettings.stepsPerMeasure; + getParameter(1)->value = nvAxisSettings.value[0]; + getParameter(1)->valueNv = nvAxisSettings.value[0]; + if (!parameterIsValid(getParameter(1))) { + DF("WRN:"); D(axisPrefix); DLF("parameter validation failed, reverting to default"); + stepsPerMeasureValue = stepsPerMeasureDefault; + stepsPerMeasureValueNv = stepsPerMeasureDefault; + getParameter(1)->value = getParameter(1)->valueDefault; + getParameter(1)->valueNv = getParameter(1)->valueDefault; + } + + // move other NV values into parameters + for (int i = 2; i <= getParameterCount(); i++) { + getParameter(i)->value = nvAxisSettings.value[i - 1]; + getParameter(i)->valueNv = nvAxisSettings.value[i - 1]; + if (!parameterIsValid(getParameter(i))) { + DF("WRN:"); D(axisPrefix); DLF("reverting parameter to default"); + getParameter(i)->value = getParameter(i)->valueDefault; + getParameter(i)->valueNv = getParameter(i)->valueDefault; + } + } + + #if DEBUG == VERBOSE + VF("MSG:"); V(axisPrefix); VF("stepsPerMeasure="); VL(stepsPerMeasureValue); + VF("MSG:"); V(axisPrefix); VF("backlash takeup frequency set to "); + if (unitsRadians) V(radToDegF(backlashFreq)); else V(backlashFreq); + V(unitsStr); VLF("/s"); + #endif + + // activate home and limit sense + VF("MSG:"); V(axisPrefix); VLF("adding any home and/or limit senses"); + homeSenseHandle = sense.add(pins->home, pins->axisSense.homeInit, pins->axisSense.homeTrigger); + minSenseHandle = sense.add(pins->min, pins->axisSense.minMaxInit, pins->axisSense.minTrigger); + maxSenseHandle = sense.add(pins->max, pins->axisSense.minMaxInit, pins->axisSense.maxTrigger); + #if LIMIT_SENSE_STRICT != ON + commonMinMaxSense = pins->min != OFF && pins->min == pins->max; + #endif + + // setup motor + if (!motor->init()) { DF("ERR:"); D(axisPrefix); DLF("no motor driver!"); return false; } + + motor->setBacklashFrequencySteps(backlashFreq*stepsPerMeasureValue); + + // start monitor + VF("MSG:"); V(axisPrefix); VF("start motion controller task (rate "); V(FRACTIONAL_SEC_US); VF("us priority 1)... "); + uint8_t taskHandle = 0; + char taskName[] = "Ax_Motn"; + taskName[2] = axisNumber + '0'; + taskHandle = tasks.add(0, 0, true, 1, callback, taskName); + tasks.setPeriodMicros(taskHandle, FRACTIONAL_SEC_US); + if (taskHandle) { VLF("success"); } else { + VLF("FAILED!"); + DF("ERR:"); D(axisPrefix); DLF("no motion controller task!"); return false; + } + motor->monitorHandle = taskHandle; + + return true; +} + +AxisParameter* Axis::getParameter(uint8_t number) { + if (number == 0) { return motor->getParameter(number); } else + if (number == 1) { return stepsPerMeasure; } else + if (number == 2) { return minMeasure; } else + if (number == 3) { return maxMeasure; } else + if (number >= 4) { return motor->getParameter(number - 3); } else return NULL; +} + +void Axis::enable(bool state) { + enabled = state; + motor->enable(enabled & !poweredDown); +} + +void Axis::setPowerDownTime(int time) { + if (time == 0) powerDownStandstill = false; else { powerDownStandstill = true; powerDownDelay = time; } +} + +void Axis::setPowerDownOverrideTime(int time) { + if (time == 0) powerDownOverride = false; else { + if (poweredDown) { + poweredDown = false; + motor->enable(true); + } + powerDownOverride = true; + powerDownOverrideEnds = millis() + time; + } +} + +void Axis::setBacklash(float value) { + if (autoRate == AR_NONE) motor->setBacklashSteps(round(value*stepsPerMeasureValue)); +} + +float Axis::getBacklash() { + return motor->getBacklashSteps()/stepsPerMeasureValue; +} + +CommandError Axis::resetPosition(double value) { + return resetPositionSteps(lround(value*stepsPerMeasureValue)); +} + +CommandError Axis::resetPositionSteps(long value) { + if (autoRate != AR_NONE) return CE_SLEW_IN_MOTION; + if (motor->getFrequencySteps() != 0) return CE_SLEW_IN_MOTION; + motor->resetPositionSteps(value); + return CE_NONE; +} + +double Axis::getMotorPosition() { + return motor->getMotorPositionSteps()/stepsPerMeasureValue; +} + +double Axis::getIndexPosition() { + return motor->getIndexPositionSteps()/stepsPerMeasureValue; +} + +double distance(double c1, double c2) { + double d1 = fabs(c1 - c2); + double d2 = fabs(c2 - c1); + if (d1 <= d2) return d1; else return d2; +} + +double Axis::wrap(double value) { + if (wrapEnabled) { + while (value > maxMeasure->value) value -= wrapAmount; + while (value < minMeasure->value) value += wrapAmount; + } + return value; +} + +double Axis::unwrap(double value) { + if (wrapEnabled) { + double position = motor->getInstrumentCoordinateSteps()/stepsPerMeasureValue; + while (value > position + wrapAmount/2.0L) value -= wrapAmount; + while (value < position - wrapAmount/2.0L) value += wrapAmount; + } + return value; +} + +double Axis::unwrapNearest(double value) { + if (wrapEnabled) { + value = unwrap(value); + double instr = motor->getInstrumentCoordinateSteps()/stepsPerMeasureValue; +// VF("MSG:"); V(axisPrefix); +// VF("unwrapNearest instr "); V(radToDeg(instr)); +// VF(", before "); V(radToDeg(value)); + double dist = distance(value, instr); + if (distance(value + wrapAmount, instr) < dist) value += wrapAmount; else + if (distance(value - wrapAmount, instr) < dist) value -= wrapAmount; +// VF(", after "); VL(radToDeg(value)); + } + return value; +} + +void Axis::setInstrumentCoordinate(double value) { + setInstrumentCoordinateSteps(lround(unwrap(value)*stepsPerMeasureValue)); +} + +double Axis::getInstrumentCoordinate() { + return wrap(motor->getInstrumentCoordinateSteps()/stepsPerMeasureValue); +} + +void Axis::setInstrumentCoordinatePark(double value) { + motor->setInstrumentCoordinateParkSteps(lround(unwrapNearest(value)*stepsPerMeasureValue), motor->getSequencerSteps()); +} + +void Axis::setTargetCoordinatePark(double value) { + motor->setFrequencySteps(0); + motor->setTargetCoordinateParkSteps(lround(unwrapNearest(value)*stepsPerMeasureValue), motor->getSequencerSteps()); +} + +void Axis::setTargetCoordinate(double value) { + setTargetCoordinateSteps(lround(unwrapNearest(value)*stepsPerMeasureValue)); +} + +double Axis::getTargetCoordinate() { + return wrap(motor->getTargetCoordinateSteps()/stepsPerMeasureValue); +} + +bool Axis::atTarget() { + return labs(motor->getTargetDistanceSteps()) <= targetTolerance*stepsPerMeasureValue; +} + +bool Axis::nearTarget() { + return labs(motor->getTargetDistanceSteps()) < backlashFreq*stepsPerMeasureValue; +} + +double Axis::getTargetDistance() { + return labs(motor->getTargetDistanceSteps())/stepsPerMeasureValue; +} + +double Axis::getOriginOrTargetDistance() { + return motor->getOriginOrTargetDistanceSteps()/stepsPerMeasureValue; +} + +void Axis::setSlewAccelerationRate(float mpsps) { + if (autoRate == AR_NONE) { + slewAccelRateFs = mpsps/FRACTIONAL_SEC; + if (slewAccelRateFs > backlashFreq) slewAccelRateFs = backlashFreq; + slewAccelTime = NAN; + } +} + +void Axis::setSlewAccelerationTime(float seconds) { + if (autoRate == AR_NONE) { + if (seconds < 0.1F) seconds = 0.1F; + slewAccelTime = seconds; + } +} + +void Axis::setSlewAccelerationRateAbort(float mpsps) { + if (autoRate == AR_NONE) { + abortAccelRateFs = mpsps/FRACTIONAL_SEC; + if (abortAccelRateFs > backlashFreq) abortAccelRateFs = backlashFreq; + abortAccelTime = NAN; + } +} + +void Axis::setSlewAccelerationTimeAbort(float seconds) { + if (autoRate == AR_NONE) abortAccelTime = seconds; +} + +CommandError Axis::autoGoto(float frequency) { + if (!enabled) return CE_SLEW_ERR_IN_STANDBY; + if (autoRate != AR_NONE) return CE_SLEW_IN_SLEW; + if (motionError(DIR_BOTH)) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + + if (!isnan(frequency)) setFrequencySlew(frequency); + + VF("MSG:"); V(axisPrefix); + VF("autoGoto start "); + + motor->markOriginCoordinateSteps(); + motor->setSynchronized(false); + motor->setSlewing(true); + autoRate = AR_RATE_BY_DISTANCE; + + #if DEBUG == VERBOSE + if (unitsRadians) V(radToDeg(slewFreq)); else V(slewFreq); + V(unitsStr); VF("/s, accel "); + if (unitsRadians) SERIAL_DEBUG.print(radToDeg(slewAccelRateFs)*FRACTIONAL_SEC, 3); else SERIAL_DEBUG.print(slewAccelRateFs*FRACTIONAL_SEC, 3); + V(unitsStr); VLF("/s/s"); + #endif + + return CE_NONE; +} + +CommandError Axis::autoSlew(Direction direction, float frequency) { + if (!enabled) return CE_SLEW_ERR_IN_STANDBY; + if (autoRate == AR_RATE_BY_DISTANCE) return CE_SLEW_IN_SLEW; + if (direction != DIR_FORWARD && direction != DIR_REVERSE) return CE_SLEW_ERR_UNSPECIFIED; + if (motionError(direction)) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + + if (!isnan(frequency)) setFrequencySlew(frequency); + + VF("MSG:"); V(axisPrefix); + if (autoRate == AR_NONE) { + motor->setSynchronized(true); + motor->setSlewing(true); + VF("MSG:"); V(axisPrefix); VF("autoSlew start "); + } else { VF("autoSlew resum "); } + + if (direction == DIR_FORWARD) { + autoRate = AR_RATE_BY_TIME_FORWARD; + VF("fwd@ "); + } else { + autoRate = AR_RATE_BY_TIME_REVERSE; + VF("rev@ "); + } + + #if DEBUG == VERBOSE + if (unitsRadians) { + if (radToDeg(slewFreq) >= 0.01F ) { + V(radToDeg(slewFreq)); V(unitsStr); + } else { + V(radToDeg(slewFreq)*3600.0F); V(" arc-sec"); + } + } else { V(slewFreq); V(unitsStr); } + + VF("/s, accel "); + if (unitsRadians) SERIAL_DEBUG.print(radToDeg(slewAccelRateFs)*FRACTIONAL_SEC, 3); else SERIAL_DEBUG.print(slewAccelRateFs*FRACTIONAL_SEC, 3); + V(unitsStr); VLF("/s/s"); + #endif + + return CE_NONE; +} + +CommandError Axis::autoSlewHome(unsigned long timeout) { + if (!enabled) return CE_SLEW_ERR_IN_STANDBY; + if (autoRate != AR_NONE) return CE_SLEW_IN_SLEW; + if (motionError(DIR_BOTH)) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + + if (pins->axisSense.homeTrigger != OFF) { + motor->setSynchronized(true); + if (homingStage == HOME_NONE) homingStage = HOME_FAST; + if (autoRate == AR_NONE) { + motor->setSlewing(true); + VF("MSG:"); V(axisPrefix); VF("autoSlewHome "); + switch (homingStage) { + case HOME_FAST: VF("fast "); break; + case HOME_SLOW: VF("slow "); break; + case HOME_FINE: VF("fine "); break; + default: break; + } + } + if (sense.isOn(homeSenseHandle)) { + VF("fwd@ "); + autoRate = AR_RATE_BY_TIME_FORWARD; + } else { + VF("rev@ "); + autoRate = AR_RATE_BY_TIME_REVERSE; + } + + // automatically set timeout if not specified + if (timeout == 0) timeout = (pins->axisSense.homeDistLimit/slewFreq)*1.2F*1000.0F; + + #if DEBUG == VERBOSE + if (unitsRadians) V(radToDeg(slewFreq)); else V(slewFreq); + V(unitsStr); VF("/s, accel "); + if (unitsRadians) SERIAL_DEBUG.print(radToDeg(slewAccelRateFs)*FRACTIONAL_SEC, 3); else SERIAL_DEBUG.print(slewAccelRateFs*FRACTIONAL_SEC, 3); + V(unitsStr); VF("/s/s, timeout "); + #endif + VL(timeout); + homeTimeoutTime = millis() + timeout; + } + return CE_NONE; +} + +void Axis::autoSlewStop() { + if (autoRate <= AR_RATE_BY_TIME_END) return; + + motor->setSynchronized(true); + + VF("MSG:"); V(axisPrefix); VLF("slew stopping"); + autoRate = AR_RATE_BY_TIME_END; + poll(); +} + +void Axis::autoSlewAbort() { + if (autoRate <= AR_RATE_BY_TIME_ABORT) return; + + motor->setSynchronized(true); + + VF("MSG:"); V(axisPrefix); VLF("slew aborting"); + autoRate = AR_RATE_BY_TIME_ABORT; + homingStage = HOME_NONE; + poll(); +} + +bool Axis::isSlewing() { + return autoRate != AR_NONE; +} + +void Axis::poll() { + // make sure we're ready + if (axisNumber == 0) return; + + // check physical limit switches + errors.minLimitSensed = sense.isOn(minSenseHandle); + errors.maxLimitSensed = sense.isOn(maxSenseHandle); + bool commonMinMaxSensed = commonMinMaxSense && (errors.minLimitSensed || errors.maxLimitSensed); + + // let the user know if the associated senses change state + #if DEBUG == VERBOSE + bool senseMin = sense.isOn(minSenseHandle); + bool senseMax = sense.isOn(maxSenseHandle); + if (lastSenseMin != senseMin || lastSenseMax != senseMax) { + VF("MSG:"); V(axisPrefix); VF("limit sense state: "); + V("A"); V(axisNumber); V("S-"); + if (senseMin) VF("< "); else VF(" "); + V("A"); V(axisNumber); V("S+"); + if (senseMax) VLF("< "); else VLF(" "); + lastSenseMin = senseMin; + lastSenseMax = senseMax; + } + bool senseHome = sense.isOn(homeSenseHandle); + if (lastSenseHome != senseHome) { + VF("MSG:"); V(axisPrefix); VF("home sense state: "); + if (senseHome) VLF("ON"); else VLF("OFF"); + lastSenseHome = senseHome; + } + #endif + + // stop homing as we pass by the switch or times out + if (homingStage != HOME_NONE && (autoRate == AR_RATE_BY_TIME_FORWARD || autoRate == AR_RATE_BY_TIME_REVERSE)) { + if (autoRate == AR_RATE_BY_TIME_FORWARD && !sense.isOn(homeSenseHandle)) autoSlewStop(); + if (autoRate == AR_RATE_BY_TIME_REVERSE && sense.isOn(homeSenseHandle)) autoSlewStop(); + if ((long)(millis() - homeTimeoutTime) > 0) { + VF("MSG:"); V(axisPrefix); VLF("autoSlewHome timed out"); + autoSlewAbort(); + } + } + Y; + + // slewing + if (autoRate != AR_NONE && !motor->inBacklash) { + + if (autoRate != AR_RATE_BY_TIME_ABORT) { + if (motionError(motor->getDirection())) { + VF("MSG:"); V(axisPrefix); VLF("motion error"); + autoSlewAbort(); + return; + } + if (motorFault()) { + VF("MSG:"); V(axisPrefix); VLF("motor fault"); + autoSlewAbort(); + return; + } + } + if (autoRate == AR_RATE_BY_DISTANCE) { + if (commonMinMaxSensed) { + VF("MSG:"); V(axisPrefix); VLF("commonMinMaxSensed"); + autoSlewAbort(); + return; + } + if (atTarget()) { + motor->setSlewing(false); + autoRate = AR_NONE; + freq = 0.0F; + motor->setSynchronized(true); + VF("MSG:"); V(axisPrefix); VLF("slew stopped"); + } else { + freq = sqrtf(2.0F*(slewAccelRateFs*FRACTIONAL_SEC)*getOriginOrTargetDistance()); + if (freq < backlashFreq) freq = backlashFreq; + if (freq > slewFreq) freq = slewFreq; + if (motor->getTargetDistanceSteps() < 0) freq = -freq; + } + } else + if (autoRate == AR_RATE_BY_TIME_FORWARD) { + freq += slewAccelRateFs; + if (freq > slewFreq) freq = slewFreq; + } else + if (autoRate == AR_RATE_BY_TIME_REVERSE) { + freq -= slewAccelRateFs; + if (freq < -slewFreq) freq = -slewFreq; + } else + if (autoRate == AR_RATE_BY_TIME_END) { + if (commonMinMaxSensed) { + VF("MSG:"); V(axisPrefix); VLF("commonMinMaxSensed"); + autoSlewAbort(); + return; + } + + if (freq > slewAccelRateFs) freq -= slewAccelRateFs; else if (freq < -slewAccelRateFs) freq += slewAccelRateFs; else freq = 0.0F; + if (fabs(freq) <= slewAccelRateFs) { + motor->setSlewing(false); + autoRate = AR_NONE; + freq = 0.0F; + if (homingStage == HOME_FAST) homingStage = HOME_SLOW; else + if (homingStage == HOME_SLOW) { + if (!sense.isOn(homeSenseHandle)) homingStage = HOME_FINE; else { + slewFreq *= 6.0F; + VF("MSG:"); V(axisPrefix); VLF("autoSlewHome approach correction"); + } + } else + if (homingStage == HOME_FINE) homingStage = HOME_NONE; + if (homingStage != HOME_NONE) { + float f = fabs(slewFreq)/6.0F; + if (f < 0.0003F) f = 0.0003F; + setFrequencySlew(f); + autoSlewHome(SLEW_HOME_REFINE_TIME_LIMIT * 1000); + } else { + VF("MSG:"); V(axisPrefix); VLF("slew stopped"); + } + } + } else + if (autoRate == AR_RATE_BY_TIME_ABORT) { + if (freq > abortAccelRateFs) freq -= abortAccelRateFs; else if (freq < -abortAccelRateFs) freq += abortAccelRateFs; else freq = 0.0F; + if (fabs(freq) <= abortAccelRateFs) { + motor->setSlewing(false); + autoRate = AR_NONE; + freq = 0.0F; + VF("MSG:"); V(axisPrefix); VLF("slew aborted"); + } + } else freq = 0.0F; + } else { + freq = 0.0F; + if (commonMinMaxSensed || motionError(DIR_BOTH) || motorFault()) baseFreq = 0.0F; + } + Y; + + setFrequency(freq); + + // keep associated motor updated + motor->poll(); + + // respond to the motor disabling itself + if (autoRate != AR_NONE && !motor->enabled) { + autoRate = AR_NONE; + freq = 0.0F; + VF("MSG:"); V(axisPrefix); VLF("motion stopped, motor disabled!"); + } +} + +void Axis::setFrequencyMin(float frequency) { + minFreq = frequency; +} + +void Axis::setFrequencyMax(float frequency) { + maxFreq = frequency; + motor->setFrequencyMax(frequency*stepsPerMeasure->value); +} + +void Axis::setFrequencySlew(float frequency) { + if (minFreq != 0.0F && frequency < minFreq) frequency = minFreq; + if (maxFreq != 0.0F && frequency > maxFreq) frequency = maxFreq; + slewFreq = frequency; + + // adjust acceleration rates if they depend on slewFreq + if (!isnan(slewAccelTime)) slewAccelRateFs = (slewFreq/slewAccelTime)/FRACTIONAL_SEC; + if (!isnan(abortAccelTime)) abortAccelRateFs = (slewFreq/abortAccelTime)/FRACTIONAL_SEC; +} + +void Axis::setFrequency(float frequency) { + const unsigned long now = millis(); + + frequency *= scaleFreq; + if (powerDownStandstill && frequency == 0.0F && baseFreq == 0.0F) { + if (!poweredDown) { + if (!powerDownOverride || (long)(now - powerDownOverrideEnds) > 0) { + powerDownOverride = false; + if ((long)(now - powerDownTime) > 0) { + poweredDown = true; + motor->enable(false); + } + } + } + } else { + if (poweredDown) { + poweredDown = false; + motor->enable(true); + } + powerDownTime = now + powerDownDelay; + } + + if (frequency != 0.0F) { + if (frequency < 0.0F) { + if (maxFreq != 0.0F && frequency < -maxFreq) frequency = -maxFreq; + if (minFreq != 0.0F && frequency > -minFreq) frequency = -minFreq; + } else { + if (minFreq != 0.0F && frequency < minFreq) frequency = minFreq; + if (maxFreq != 0.0F && frequency > maxFreq) frequency = maxFreq; + } + } + + // apply base frequency as required + if (enabled) { + if (autoRate == AR_NONE) { + motor->setFrequencySteps((frequency + baseFreq)*stepsPerMeasureValue); + } else { + motor->setFrequencySteps(frequency*stepsPerMeasureValue); + } + } else { + motor->setFrequencySteps(0.0F); + } +} + +float Axis::getFrequency() { + return motor->getFrequencySteps()/stepsPerMeasureValue; +} + +float Axis::getBacklashFrequency() { + return backlashFreq; +} + +DriverStatus Axis::getStatus() { + return motor->getDriverStatus(); +} + +void Axis::setMotionLimitsCheck(bool state) { + limitsCheck = state; +} + +bool Axis::motionError(Direction direction) { + bool result = false; + + if (motor->isStalled()) return true; + + if (direction == DIR_FORWARD || direction == DIR_BOTH) { + result = getInstrumentCoordinateSteps() > lroundf(0.9F*INT32_MAX) || + (limitsCheck && homingStage == HOME_NONE && getInstrumentCoordinate() > maxMeasure->value + 1.0F/stepsPerMeasureValue) || + (!commonMinMaxSense && errors.maxLimitSensed); + if (result == true && result != lastErrorResult) { VF("MSG:"); V(axisPrefix); VLF("motion error forward limit"); } + } else + + if (direction == DIR_REVERSE || direction == DIR_BOTH) { + result = getInstrumentCoordinateSteps() < lroundf(0.9F*INT32_MIN) || + (limitsCheck && homingStage == HOME_NONE && getInstrumentCoordinate() < minMeasure->value - 1.0F/stepsPerMeasureValue) || + (!commonMinMaxSense && errors.minLimitSensed); + if (result == true && result != lastErrorResult) { VF("MSG:"); V(axisPrefix); VLF("motion error reverse limit"); } + } + + lastErrorResult = result; + + return lastErrorResult; +} + +bool Axis::motionErrorSensed(Direction direction) { + if ((direction == DIR_REVERSE || direction == DIR_BOTH) && errors.minLimitSensed) return true; else + if ((direction == DIR_FORWARD || direction == DIR_BOTH) && errors.maxLimitSensed) return true; else return false; +} + +void Axis::setAxisRevert(bool state) { + switch (axisNumber) { + case 1: revert.axis1 = state; break; case 2: revert.axis2 = state; break; case 3: revert.axis3 = state; break; + case 4: revert.axis4 = state; break; case 5: revert.axis5 = state; break; case 6: revert.axis6 = state; break; + case 7: revert.axis7 = state; break; case 8: revert.axis8 = state; break; case 9: revert.axis9 = state; break; + } +} + +bool Axis::getAxisRevert() { + switch (axisNumber) { + case 1: return revert.axis1; case 2: return revert.axis2; case 3: return revert.axis3; + case 4: return revert.axis4; case 5: return revert.axis5; case 6: return revert.axis6; + case 7: return revert.axis7; case 8: return revert.axis8; case 9: return revert.axis9; + default: return false; + } +} + +#endif diff --git a/src/lib/axis/Axis.h b/src/lib/axis/Axis.h new file mode 100755 index 0000000..17ac228 --- /dev/null +++ b/src/lib/axis/Axis.h @@ -0,0 +1,552 @@ +// ----------------------------------------------------------------------------------- +// Axis motion control +#pragma once + +#include "../../Common.h" +#include "../sense/Sense.h" + +#ifdef MOTOR_PRESENT + +// default start of axis class hardware timers +#ifndef AXIS_HARDWARE_TIMER_BASE +#define AXIS_HARDWARE_TIMER_BASE 1 +#endif + +// polling frequency for monitoring axis motion (default 100X/second) +#ifndef FRACTIONAL_SEC +#define FRACTIONAL_SEC 100.0F +#endif +#define FRACTIONAL_SEC_US (lround(1000000.0F / FRACTIONAL_SEC)) + +// time limit in seconds for slew home phases +#ifndef SLEW_HOME_REFINE_TIME_LIMIT +#define SLEW_HOME_REFINE_TIME_LIMIT 120 +#endif + +// ON blocks all motion when min/max are on the same pin, applies to all axes (mount/rotator/focusers) +#ifndef LIMIT_SENSE_STRICT +#define LIMIT_SENSE_STRICT OFF +#endif + +#include "../../libApp/commands/ProcessCmds.h" +#include "motor/Motor.h" +#include "motor/stepDir/StepDir.h" +#include "motor/servo/Servo.h" +#include "motor/oDrive/ODriveNew.h" +#include "motor/oDrive/ODrive.h" +#include "motor/kTech/KTech.h" + +#pragma pack(1) + +typedef struct AxesRevert +{ + uint16_t all : 1; + uint16_t axis1 : 1; + uint16_t axis2 : 1; + uint16_t axis3 : 1; + uint16_t axis4 : 1; + uint16_t axis5 : 1; + uint16_t axis6 : 1; + uint16_t axis7 : 1; + uint16_t axis8 : 1; + uint16_t axis9 : 1; + uint16_t reserved : 5; +} AxesRevert; + +typedef struct AxisLimits +{ + float min; // unit (radians, microns, etc.) depends on axis settings + float max; // unit (radians, microns, etc.) depends on axis settings +} AxisLimits; + +typedef struct AxisSettings +{ + double stepsPerMeasure; // unit (radians, microns, etc.) depends on axis settings + AxisLimits limits; + float backlashFreq; // in measures/s +} AxisSettings; + +// axis parameters +#define MOTOR_PARAMETER_MAX_COUNT 20 + +#define AxisStoredSettingsSize 89 +typedef struct AxisStoredSettings +{ + bool revert; + double stepsPerMeasure; // unit (radians, microns, etc.) depends on axis settings + float value[MOTOR_PARAMETER_MAX_COUNT]; +} AxisStoredSettings; +#pragma pack() + +typedef struct AxisSense +{ + int32_t homeTrigger; + int8_t homeInit; + float homeDistLimit; + int32_t minTrigger; + int32_t maxTrigger; + int8_t minMaxInit; +} AxisSense; + +typedef struct AxisPins +{ + int16_t min; + int16_t home; + int16_t max; + AxisSense axisSense; +} AxisPins; + +typedef struct AxisErrors +{ + uint8_t minLimitSensed : 1; + uint8_t maxLimitSensed : 1; +} AxisErrors; + +enum AutoRate : uint8_t +{ + AR_NONE, + AR_RATE_BY_TIME_ABORT, + AR_RATE_BY_TIME_END, + AR_RATE_BY_DISTANCE, + AR_RATE_BY_TIME_FORWARD, + AR_RATE_BY_TIME_REVERSE +}; +enum HomingStage : uint8_t +{ + HOME_NONE, + HOME_FINE, + HOME_SLOW, + HOME_FAST +}; +enum AxisMeasure : uint8_t +{ + AXIS_MEASURE_UNKNOWN, + AXIS_MEASURE_MICRONS, + AXIS_MEASURE_DEGREES, + AXIS_MEASURE_RADIANS +}; + +class Axis +{ +public: + // constructor for this motion controller + // \param axisNumber: from 1 to 9 + // \param *pins: provides pins and states for h/w limits and homing + // \param *settings: provides axis scale (steps per measure,) s/w limits, reversal, and backlash frequency + // \param axisMeasure: one of AXIS_MEASURE_MICRONS, _DEGREES, or _RADIANS for conversion to/from steps + // \param targetTolerance: in "measures" for detecting when slews arrive at their target + Axis(uint8_t axisNumber, const AxisPins *pins, const AxisSettings *settings, const AxisMeasure axisMeasure, float targetTolerance = 0.0F); + + // process axis commands + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // init prepares this motion controller and the provided motor for operation + // \param motor: associated motor to be controlled + bool init(Motor *motor); + + // returns the maximum number of parameters possible + uint8_t getParameterMaxCount() { return MOTOR_PARAMETER_MAX_COUNT; } + + // returns the number of parameters for this axis + uint8_t getParameterCount() { return 3 + motor->getParameterCount(); } + + // returns the specified axis parameter + AxisParameter *getParameter(uint8_t number); + + // check if parameter is valid + bool parameterIsValid(AxisParameter *parameter, bool next = false) { return motor->parameterIsValid(parameter, next); } + bool parameterIsValid(AxisParameter *parameter, float value, bool next = false) { + bool result; + float temp = parameter->value; + float tempNv = parameter->valueNv; + if (next) parameter->valueNv = value; else parameter->value = value; + result = motor->parameterIsValid(parameter, next); + parameter->valueNv = tempNv; + parameter->value = temp; + return result; + } + + // sets the enabled state (and for its associated motor if supported) + // \param state: true to enable or false to disable + void enable(bool state); + + // gets the enabled state + inline bool isEnabled() { return enabled && !motor->calibrating; } + + // sets the automatic power down time + // \param time: allowed standstill time in milliseconds before power down, or 0 to disable + void setPowerDownTime(int time); + + // sets the automatic power down override time + // \param time: time in milliseconds to disable automatic power down, or 0 to disable + void setPowerDownOverrideTime(int time); + + // get steps per "measure" (radian, micron, etc.) + inline double getStepsPerMeasure() { return stepsPerMeasureValue; } + + // get tracking mode steps per slewing mode step + inline int getStepsPerStepSlewing() { return motor->getStepsPerStepSlewing(); } + + // reset position, also zeros backlash and index + // \param value: position of motor and target in "measures" (radians, microns, etc.) + CommandError resetPosition(double value); + + // reset position, also zeros backlash and index + // \param value: position of motor and target in steps + CommandError resetPositionSteps(long value); + + // resets the target position to the motor position + inline void resetTargetToMotorPosition() { motor->resetTargetToMotorPosition(); } + + // get motor position, in "measures" (radians, microns, etc.) + // \note motor: position based on actual motion taken since startup/reset (including backlash) + double getMotorPosition(); + + // get motor position, in steps + // \note motor: position based on actual motion taken since startup/reset (including backlash) + inline long getMotorPositionSteps() { return motor->getMotorPositionSteps(); } + + // get index position, in "measures" (radians, microns, etc.) + // \note indexPosition: added to motorPositon (-backlash) yeilds an instrumentCoordinate + double getIndexPosition(); + + // get index position, in steps + // \note indexPosition: added to motorPositon (-backlash) yeilds an instrumentCoordinate + inline long getIndexPositionSteps() { return motor->getIndexPositionSteps(); } + + // coordinate wrap + // \param value: one complete rotation in "measures" (radians, microns, etc.), 0.0 disables + // \note wrap occurs when the coordinate exceeds the min or max limits + inline void coordinateWrap(double value) + { + wrapAmount = value; + wrapEnabled = fabs(value) > 0.0F; + } + + // set instrument coordinate + // \param value: position in "measures" (radians, microns, etc.) + void setInstrumentCoordinate(double value); + + // set instrument coordinate + // \param value: position in steps + inline void setInstrumentCoordinateSteps(long value) { motor->setInstrumentCoordinateSteps(value); } + + // get instrument coordinate in "measures" (radians, microns, etc.) + double getInstrumentCoordinate(); + + // get instrument coordinate, in steps + inline long getInstrumentCoordinateSteps() { return motor->getInstrumentCoordinateSteps(); } + + // set instrument coordinate to park at + // \param value: position in "measures" (radians, microns, etc.) + // \note with backlash disabled this indexes to the nearest position where the motor wouldn't cog + void setInstrumentCoordinatePark(double value); + + // set the target coordinate for park + // \param value: postion in "measures" (degrees, microns, etc.) + // \note with backlash disabled this moves to the nearest position where the motor doesn't cog + void setTargetCoordinatePark(double value); + + // set target coordinate for autoGoto() + // \param value: postion in "measures" (degrees, microns, etc.) + void setTargetCoordinate(double value); + + // set target coordinate for autoGoto() + // \param value: postion in steps + inline void setTargetCoordinateSteps(long value) { motor->setTargetCoordinateSteps(value); } + + // get target coordinate, in "measures" (degrees, microns, etc.) + double getTargetCoordinate(); + + // get target coordinate, in steps + inline long getTargetCoordinateSteps() { return motor->getTargetCoordinateSteps(); } + + // distance to target in "measures" (degrees, microns, etc.) + double getTargetDistance(); + + // returns true if at the target coordinate (+/- targetTolerance) + bool atTarget(); + + // returns true if within one second of the target coordinate at the backlash takeup rate + bool nearTarget(); + + // sets backlash amount + // \param value: backlash distance in "measures" (degrees, microns, etc.) + void setBacklash(float value); + + // sets backlash amount + // \param value: backlash distance in steps + inline void setBacklashSteps(long value) + { + if (autoRate == AR_NONE) + motor->setBacklashSteps(value); + } + + // get backlash amount in "measures" (radians, microns, etc.) + float getBacklash(); + + // get frequency in "measures" (degrees, microns, etc.) per second + float getFrequency(); + + // get frequency in steps per second + float getFrequencySteps() { return motor->getFrequencySteps(); } + + // get direction + Direction getDirection() { return motor->getDirection(); } + + // gets backlash frequency in "measures" (degrees, microns, etc.) per second + float getBacklashFrequency(); + + // sets reversal of axis directions + // \param state: true reverses the direction behavior specified in settings + inline void setReverse(bool state) { motor->setReverse(state); } + + // sets reversal of homing directions + // \param state: true reverses the normal sense direction behavior + inline void setHomeReverse(bool state) { sense.reverse(homeSenseHandle, state); } + + // sets maximum frequency for next slew + // \param frequency: rate of motion in "measures" (radians, microns, etc.) per second + void setFrequencySlew(float frequency); + + // sets overall minimum frequency + // \param frequency: rate of motion in "measures" (radians, microns, etc.) per second + void setFrequencyMin(float frequency); + + // sets overall maximum frequency + // \param frequency: rate of motion in "measures" (radians, microns, etc.) per second + void setFrequencyMax(float frequency); + + // sets frequency scaling factor for slews + // \param frequency: scaling factor for this axis ranging from 0.0 (0%) to 1.0 (100%) of normal + void setFrequencyScale(float frequency) + { + if (frequency >= 0.0F && frequency <= 1.0F) + scaleFreq = frequency; + } + + // sets acceleration rate for slews + // \param mpsps: acceleration rate in "measures" per second per second + void setSlewAccelerationRate(float mpsps); + + // set acceleration rate for slews + // \param seconds: time during which acceleration to/from the last specified slew frequency occurs + void setSlewAccelerationTime(float seconds); + + // sets acceleration rate for emergency stop of movement + // \param mpsps: deceleration rate in "measures" per second per second + void setSlewAccelerationRateAbort(float mpsps); + + // set acceleration rate for emergency stop of movement + // \param seconds: time during which deceleration from the last specified slew frequency occurs + void setSlewAccelerationTimeAbort(float seconds); + + // auto goto to destination target coordinate + // \param frequency: optional frequency of slew in "measures" (radians, microns, etc.) per second + CommandError autoGoto(float frequency = NAN); + + // auto slew + // \param direction: direction of motion, DIR_FORWARD or DIR_REVERSE + // \param frequency: optional frequency of slew in "measures" (radians, microns, etc.) per second + CommandError autoSlew(Direction direction, float frequency = NAN); + + // automatic slew to home + // \param timeout: time in seconds before aborting the operation + // \note requires a home sensor (switch) + CommandError autoSlewHome(unsigned long timeout = 0); + + // returns true homing is in progress + bool isHoming() { return homingStage != HOME_NONE; } + + // returns true if a home sensor is available + inline bool hasHomeSense() { return pins->axisSense.homeTrigger != OFF; } + + // stops any autoGoto() or autoSlew() or autoSlewHome() with deacceleration by time + void autoSlewStop(); + + // stops any autoGoto() or autoSlew() or autoSlewHome() with deacceleration by time using the abort rate + void autoSlewAbort(); + + // returns true if a slew is active on this axis + bool isSlewing(); + + // returns 1 if departing origin or -1 if approaching target + inline int getRampDirection() { return motor->getRampDirection(); } + + // sets synchronized automatic target movement frequency + // \param frequency: rate of motion in "measures" (radians, microns, etc.) per second + inline void setSynchronizedFrequency(float frequency) { baseFreq = frequency; } + + // set synchronized automatic target movement + // \param state: true to force target synchronized movement + // \note movement at the setSynchronizedFrequency() rate + inline void setSynchronized(bool state) { motor->setSynchronized(state); } + + // returns true if synchronized automatic target movement is active + // \note movement at the setSynchronizedFrequency() rate + inline bool getSynchronized() { return motor->getSynchronized(); } + + // returns true if the associated motor reports a fault + inline bool motorFault() { return motor->getDriverStatus().fault; }; + + // returns the associated motors driver status + DriverStatus getStatus(); + + // enables or disables software based position range limits + // \param state: true to enable or false to disable + void setMotionLimitsCheck(bool state); + + // check for all errors that would disallow motion in the given direction + // \param direction: one of DIR_FORWARD, DIR_REVERSE, or DIR_BOTH + bool motionError(Direction direction); + + // gets the minimum coordinate in "measures" (radians, microns, etc.) + float getLimitMin() { return minMeasure->value; } + + // sets the minimum coordinate in "measures" (radians, microns, etc.) + void setLimitMin(float value) { if (parameterIsValid(minMeasure, value)) minMeasure->value = value; } + + // gets the maximum coordinate in "measures" (radians, microns, etc.) + float getLimitMax() { return maxMeasure->value; } + + // sets the maximum coordinate in "measures" (radians, microns, etc.) + void setLimitMax(float value) { if (parameterIsValid(maxMeasure, value)) maxMeasure->value = value; } + + // check for sense (h/w limit) errors that would disallow motion in the given direction + // \param direction: one of DIR_FORWARD, DIR_REVERSE, or DIR_BOTH + bool motionErrorSensed(Direction direction); + + // calibrate the associated motor + void calibrate(float value) { motor->calibrate(value); } + + // calibrate the associated motor's driver + void calibrateDriver() { motor->calibrateDriver(); } + + // monitor movement + void poll(); + + // associated motor + Motor *motor; + + // hint that min and max pins are the same (no directional limit indication) + bool commonMinMaxSense = false; + +private: + // set frequency in "measures" (degrees, microns, etc.) per second (0 stops motion) + void setFrequency(float frequency); + + // sets driver power on/off + void powered(bool value); + + // distance to origin or target, whichever is closer, in "measures" (degrees, microns, etc.) + double getOriginOrTargetDistance(); + + // returns true if traveling through backlash + bool inBacklash(); + + // convert from unwrapped (full range) to normal (+/- wrapAmount) coordinate + double wrap(double value); + + // convert from normal (+/- wrapAmount) to an unwrapped (full range) coordinate + double unwrap(double value); + + // convert from normal (+/- wrapAmount) to an unwrapped (full range) coordinate + // nearest the instrument coordinate + double unwrapNearest(double value); + + // get or set the axis revert flags + void setAxisRevert(bool state); + bool getAxisRevert(); + + AxisErrors errors; + bool lastErrorResult = false; + + uint8_t axisNumber = 0; + char axisPrefix[9] = " Axis_, "; + + char unitsStr[5] = "?"; + bool unitsRadians = false; + + bool enabled = false; // enabled or disabled axis and motor power + bool limitsCheck = true; // enabled or disabled software range limits check + + uint8_t homeSenseHandle = 0; // home sensor handle + uint8_t minSenseHandle = 0; // min sensor handle + uint8_t maxSenseHandle = 0; // max sensor handle + +#if DEBUG == VERBOSE + bool lastSenseMin = false; + bool lastSenseMax = false; + bool lastSenseHome = false; +#endif + + uint16_t backlashStepsStore; + volatile uint16_t backlashSteps = 0; + volatile uint16_t backlashAmountSteps = 0; + + volatile bool takeStep = false; + + // coordinate handling + double wrapAmount = 0.0L; + bool wrapEnabled = false; + + // power down standstill control + bool powerDownStandstill = false; + bool powerDownOverride = false; + bool poweredDown = false; + unsigned long powerDownDelay; // in milliseconds + unsigned long powerDownOverrideEnds; // in milliseconds + unsigned long powerDownTime = 0; // in milliseconds + + // timeout for home switch detection + unsigned long homeTimeoutTime = 0; // in milliseconds + + // rates (in measures per second) to control motor movement + float freq = 0.0F; // momentary frequency in measures/s + float baseFreq = 0.0F; // base frequency for continuous movement in measures/s + float minFreq = 0.0F; // minimum frequency in measures/s + float slewFreq = 0.0F; // slewing frequency in measures/s + float maxFreq = 0.0F; // maximum frequency in measures/s + float backlashFreq = 0.0F; // in measures/s + + float scaleFreq = 1.0F; + + double stepsPerMeasureValue = NAN; + double stepsPerMeasureValueNv = NAN; + double stepsPerMeasureDefault = NAN; + + float targetTolerance = 0.0F; + + AutoRate autoRate = AR_NONE; // auto slew mode + float slewAccelRateFs; // auto slew rate in measures per second per frac-sec + float abortAccelRateFs; // abort slew rate in measures per second per frac-sec + float slewAccelTime = NAN; // auto slew acceleration time in seconds + float abortAccelTime = NAN; // abort slew acceleration time in seconds + + HomingStage homingStage = HOME_NONE; + + const AxisPins *pins; + + void (*volatile callback)() = NULL; + + uint16_t nvKey, nvRevertKey; + + AxisParameter stepsPerDegree = {NAN, NAN, NAN, 1.0, 360000.0, AXP_FLOAT, AXPN_STEPS_PER_DEG}; + AxisParameter minDegrees = {NAN, NAN, NAN, -360.0, 360.0, AXP_FLOAT, AXPN_LIMIT_DEGS_MIN}; + AxisParameter maxDegrees = {NAN, NAN, NAN, -360.0, 360.0, AXP_FLOAT, AXPN_LIMIT_DEGS_MAX}; + + AxisParameter stepsPerRadian = {NAN, NAN, NAN, radToDegF(1.0), radToDegF(360000.0), AXP_FLOAT_RAD_INV, AXPN_STEPS_PER_DEG}; + AxisParameter minRadians = {NAN, NAN, NAN, -Deg360, Deg360, AXP_FLOAT_RAD, AXPN_LIMIT_DEGS_MIN}; + AxisParameter maxRadians = {NAN, NAN, NAN, -Deg360, Deg360, AXP_FLOAT_RAD, AXPN_LIMIT_DEGS_MAX}; + + AxisParameter stepsPerMicron = {NAN, NAN, NAN, 0.001, 1000.0, AXP_FLOAT, AXPN_STEPS_PER_UM}; + AxisParameter minMicrons = {NAN, NAN, NAN, 0.0, 500000.0, AXP_FLOAT, AXPN_LIMIT_UM_MIN}; + AxisParameter maxMicrons = {NAN, NAN, NAN, 0.0, 500000.0, AXP_FLOAT, AXPN_LIMIT_UM_MAX}; + + AxisParameter *stepsPerMeasure = NULL; + AxisParameter *minMeasure = NULL; + AxisParameter *maxMeasure = NULL; + +}; + +#endif diff --git a/src/lib/axis/motor/Drivers.h b/src/lib/axis/motor/Drivers.h new file mode 100755 index 0000000..6f40438 --- /dev/null +++ b/src/lib/axis/motor/Drivers.h @@ -0,0 +1,93 @@ +#pragma once + +#include + +typedef struct DriverOutputStatus { + bool shortToGround; + bool openLoad; +} DriverOutputStatus; + +typedef struct DriverStatus { + bool active; + DriverOutputStatus outputA; + DriverOutputStatus outputB; + bool overTemperatureWarning; + bool overTemperature; + bool standstill; + bool fault; +} DriverStatus; + +// codes for locale specific name generation of certain terms in SWS, etc. +// any AxisParameter can instead just pass a string of up to 18 chars directly +#define AXPN_STEPS_PER_DEG "$1" +#define AXPN_LIMIT_DEGS_MIN "$2" +#define AXPN_LIMIT_DEGS_MAX "$3" +#define AXPN_STEPS_PER_UM "$4" +#define AXPN_LIMIT_UM_MIN "$5" +#define AXPN_LIMIT_UM_MAX "$6" +#define AXPN_REVERSE "$7" +#define AXPN_MICROSTEPS "$8" +#define AXPN_MICROSTEPS_GOTO "$9" +#define AXPN_DECAY_MODE "$10" +#define AXPN_DECAY_MODE_GOTO "$11" +#define AXPN_CURRENT_HOLD "$12" +#define AXPN_CURRENT_RUN "$13" +#define AXPN_CURRENT_GOTO "$14" +#define AXPN_INTERPOLATE "$15" +#define AXPN_PID_P "$16" +#define AXPN_PID_I "$17" +#define AXPN_PID_D "$18" +#define AXPN_PID_SLEWING_P "$19" +#define AXPN_PID_SLEWING_I "$20" +#define AXPN_PID_SLEWING_D "$21" +#define AXPN_COUNTS_PER_RAD "$22" +#define AXPN_STEPS_TO_COUNTS "$23" +#define AXPN_MAX_ACCEL "$24" +#define AXPN_MIN_PWR "$25" +#define AXPN_MAX_PWR "$26" + +#define AXPN_1 "Steps/degree" +#define AXPN_2 "Min limit, degs" +#define AXPN_3 "Max limit, degs" +#define AXPN_4 "Steps/um" +#define AXPN_5 "Min limit, um" +#define AXPN_6 "Max limit, um" +#define AXPN_7 "Reverse" +#define AXPN_8 "Microsteps" +#define AXPN_9 "Microsteps Goto" +#define AXPN_10 "Decay mode" +#define AXPN_11 "Decay mode Goto" +#define AXPN_12 "mA Hold" +#define AXPN_13 "mA Run" +#define AXPN_14 "mA Goto" +#define AXPN_15 "256x Interpolate" +#define AXPN_16 "P tracking" +#define AXPN_17 "I tracking" +#define AXPN_18 "D tracking" +#define AXPN_19 "P slewing" +#define AXPN_20 "I slewing" +#define AXPN_21 "D slewing" +#define AXPN_22 "Rads/count" +#define AXPN_23 "Steps/count ratio" +#define AXPN_24 "Max accel, %/s/s" +#define AXPN_25 "Min power, %" +#define AXPN_26 "Max power, %" + +// AXP_FLOAT_RAD and AXP_FLOAT_RAD_INV automatically translate to/from AXP_FLOAT (in degrees) for commands +enum AxisParameterType: uint8_t {AXP_INVALID, + AXP_BOOLEAN, AXP_BOOLEAN_IMMEDIATE, + AXP_INTEGER, AXP_INTEGER_IMMEDIATE, + AXP_FLOAT, AXP_FLOAT_IMMEDIATE, + AXP_FLOAT_RAD, AXP_FLOAT_RAD_INV, + AXP_POW2, + AXP_DECAY}; + +typedef struct AxisParameter { + float value; + float valueNv; + float valueDefault; + float min; + float max; + AxisParameterType type; + const char name[18]; +} AxisParameter; diff --git a/src/lib/axis/motor/Motor.cpp b/src/lib/axis/motor/Motor.cpp new file mode 100755 index 0000000..6128b4c --- /dev/null +++ b/src/lib/axis/motor/Motor.cpp @@ -0,0 +1,212 @@ +// ----------------------------------------------------------------------------------- +// Generic motor controller + +#include "Motor.h" + +#ifdef MOTOR_PRESENT + +Motor::Motor(uint8_t axisNumber, int8_t reverse) { + this->axisNumber = axisNumber; + this->reverse.valueDefault = reverse == ON; +} + +bool Motor::init() { + VF("MSG:"); V(axisPrefix); VLF("motor init"); + normalizedReverse = (bool)lround(reverse.value); + return true; +} + +// returns the specified axis parameter by name +AxisParameter* Motor::getParameterByName(const char* name) { + for (int i = 1; i <= getParameterCount(); i++) { + if (strcmp(getParameter(i)->name, name) == 0) { return getParameter(i); } + } + return &invalid; +} + +// check if parameter is valid +bool Motor::parameterIsValid(AxisParameter* parameter, bool next) { + const float value = next ? parameter->valueNv : parameter->value; + + if (isnan(value) || isinf(value)) return false; + + // reject nonsense bounds + const float pmin = parameter->min; + const float pmax = parameter->max; + if (isnan(pmin) || isnan(pmax) || isinf(pmin) || isinf(pmax) || (pmax < pmin)) return false; + + if (value < pmin) return false; + if (value > pmax) return false; + + if (parameter->type == AXP_POW2) { + static constexpr float kIntEps = 1E-3F; + + // Value must be effectively integral + const float vf = value; + const int32_t vi = (int32_t)lroundf(vf); + if (fabsf(vf - (float)vi) > kIntEps) return false; + + if (vi < 1 || vi > 256) return false; + if ((vi & (vi - 1)) != 0) return false; // power of two check + } + + return true; +} + +// resets motor and target angular position in steps, also zeros backlash and index +void Motor::resetPositionSteps(long value) { + indexSteps = 0; + noInterrupts(); + motorSteps = value; + targetSteps = value; + backlashSteps = 0; + interrupts(); +} + +// resets target position to the motor position +void Motor::resetTargetToMotorPosition() { + noInterrupts(); + targetSteps = motorSteps; + interrupts(); +} + +// get motor position in steps (including backlash) +long Motor::getMotorPositionSteps() { + noInterrupts(); + long steps = motorSteps + backlashSteps; + interrupts(); + return steps; +} + +// get instrument coordinate, in steps +long Motor::getInstrumentCoordinateSteps() { + long steps = ATOMIC_LOAD(motorSteps); + return steps + indexSteps; +} + +// set instrument coordinate, in steps +void Motor::setInstrumentCoordinateSteps(long value) { + indexSteps = value - ATOMIC_LOAD(motorSteps); +} + +// set instrument park coordinate, in steps +// should only be called when the axis is not moving +// nudge index so park lands on a stable cogging grid (multiple of 4*modulo steps) +void Motor::setInstrumentCoordinateParkSteps(long value, int modulo) { + long priorLocationOffset = value - ATOMIC_LOAD(motorSteps); + if (driverType == STEP_DIR) { + if (modulo == OFF) modulo = 1; + long steps = priorLocationOffset; + steps -= modulo*2L; + for (int l = 0; l < modulo*4; l++) { if (steps % (modulo*4L) == 0) break; steps++; } + indexSteps = steps; + } else setInstrumentCoordinateSteps(value); + VF("MSG:"); V(axisPrefix); VF("setInstrumentCoordinateParkSteps at "); V(indexSteps); VF(" (was "); V(priorLocationOffset); VL(")"); +} + +// get target coordinate (with index), in steps +long Motor::getTargetCoordinateSteps() { + long steps = ATOMIC_LOAD(targetSteps) + indexSteps; + return steps; +} + +// set target coordinate (with index), in steps +void Motor::setTargetCoordinateSteps(long value) { + noInterrupts(); + targetSteps = value - indexSteps; + interrupts(); +} + +// set target park coordinate, in steps (taking into account stepper motor cogging when powered off) +// should only be called when the axis is not moving +void Motor::setTargetCoordinateParkSteps(long value, int modulo) { + if (driverType == STEP_DIR) { + if (modulo == OFF) modulo = 1; + long steps = value - indexSteps; + steps -= modulo*2L; + for (int l = 0; l < modulo*4; l++) { if (steps % (modulo*4L) == 0) break; steps++; } + noInterrupts(); + targetSteps = steps; + interrupts(); + } else setTargetCoordinateSteps(value); + VF("MSG:"); V(axisPrefix); VF("setTargetCoordinateParkSteps at "); V(targetSteps); VF(" (was "); V(value - indexSteps); VL(")"); +} + +// get backlash amount in steps +long Motor::getBacklashSteps() { + uint16_t backlash = ATOMIC_LOAD(backlashAmountSteps); + return (long)backlash; +} + +// set backlash amount in steps +void Motor::setBacklashSteps(long value) { + if (value < 0) value = 0; + if (value > 65535) value = 65535; + ATOMIC_STORE(backlashAmountSteps, (uint16_t)value); +} + +// mark origin coordinate for autoGoto as current location +void Motor::markOriginCoordinateSteps() { + noInterrupts(); + originSteps = motorSteps; + interrupts(); +} + +// distance to target in steps (+/-) +long Motor::getTargetDistanceSteps() { + noInterrupts(); + long dist = targetSteps - motorSteps; + interrupts(); + return dist; +} + +// distance to origin or target, whichever is closer, in steps +long Motor::getOriginOrTargetDistanceSteps() { + long steps = ATOMIC_LOAD(motorSteps); + long distanceOrigin = labs(originSteps - steps); + long distanceTarget = labs(targetSteps - steps); + if (distanceOrigin < distanceTarget) return distanceOrigin; else return distanceTarget; +} + +// returns 1 if distance to origin is closer else -1 if target is closer +int Motor::getRampDirection() { + long steps = ATOMIC_LOAD(motorSteps); + long distanceOrigin = labs(originSteps - steps); + long distanceTarget = labs(targetSteps - steps); + if (distanceOrigin < distanceTarget) return 1; else return -1; +} + +// set backlash frequency in steps per second +void Motor::setBacklashFrequencySteps(float frequency) { + backlashFrequency = frequency; +} + +// get the current direction of motion +Direction Motor::getDirection() { + if (getFrequencySteps() != 0.0F) { + if (step >= 1) return DIR_FORWARD; + if (step <= -1) return DIR_REVERSE; + } + return DIR_NONE; +} + +void Motor::disableBacklash() { + noInterrupts(); + backlashAmountStepsStore = backlashAmountSteps; + motorSteps += backlashSteps; + backlashStepsStore = backlashSteps; + backlashSteps = 0; + interrupts(); +} + +void Motor::enableBacklash() { + noInterrupts(); + backlashSteps = backlashStepsStore; + motorSteps -= backlashSteps; + backlashAmountSteps = backlashAmountStepsStore; + interrupts(); + backlashStepsStore = 0; + backlashAmountStepsStore = 0; +} + +#endif diff --git a/src/lib/axis/motor/Motor.h b/src/lib/axis/motor/Motor.h new file mode 100755 index 0000000..41e399d --- /dev/null +++ b/src/lib/axis/motor/Motor.h @@ -0,0 +1,219 @@ +// ----------------------------------------------------------------------------------- +// Generic motor controller +#pragma once + +#include +#include "../../../Common.h" +#include "Drivers.h" + +#ifdef MOTOR_PRESENT + +enum Direction: uint8_t {DIR_NONE, DIR_FORWARD, DIR_REVERSE, DIR_BOTH}; + +#ifndef STEP_WAVE_FORM +#define STEP_WAVE_FORM SQUARE +#endif + +class Motor { + public: + Motor(uint8_t axisNumber, int8_t reverse); + + // sets up the motor identification + virtual bool init(); + + // returns the number of parameters from the motor and motor driver + virtual uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + virtual AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // check if parameter is valid + virtual bool parameterIsValid(AxisParameter* parameter, bool next = false); + + // returns the specified axis parameter by name + AxisParameter* getParameterByName(const char* name); + + // sets reversal of axis directions + // \param state: true reverses the direction behavior specified in settings + virtual void setReverse(bool state) { reversed = state; } + + // sets motor enable on/off (if possible) + virtual void enable(bool value) { UNUSED(value); } + + // get the associated stepper motor driver status + virtual DriverStatus getDriverStatus() { return errorStatus; } + + // resets motor and target angular position in steps, also zeros backlash and index + virtual void resetPositionSteps(long value); + + // resets target position to the motor position + void resetTargetToMotorPosition(); + + // get motor position in steps (including backlash) + long getMotorPositionSteps(); + + // get index position in steps + inline long getIndexPositionSteps() { return indexSteps; } + + // get instrument coordinate, in steps + virtual long getInstrumentCoordinateSteps(); + + // set instrument coordinate, in steps + virtual void setInstrumentCoordinateSteps(long value); + + // set instrument park coordinate, in steps + // should only be called when the axis is not moving + void setInstrumentCoordinateParkSteps(long value, int modulo); + + // get target coordinate (with index), in steps + long getTargetCoordinateSteps(); + + // set target coordinate (with index), in steps + void setTargetCoordinateSteps(long value); + + // set target park coordinate, in steps (taking into account stepper motor cogging when powered off) + // should only be called when the axis is not moving + void setTargetCoordinateParkSteps(long value, int modulo); + + // get backlash amount in steps + long getBacklashSteps(); + + // set backlash amount in steps + void setBacklashSteps(long value); + + // mark origin coordinate at current location + void markOriginCoordinateSteps(); + + // distance to target in steps (+/-) + virtual long getTargetDistanceSteps(); + + // distance to origin or target, whichever is closer, in steps + long getOriginOrTargetDistanceSteps(); + + // returns 1 if distance to origin is closer else -1 if target is closer + int getRampDirection(); + + // sets overall maximum frequency + // \param frequency: rate of motion in steps per second + virtual void setFrequencyMax(float frequency) { UNUSED(frequency); } + + // get movement frequency in steps per second + virtual float getFrequencySteps() { return 0; } + + // set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) + virtual void setFrequencySteps(float frequency) { UNUSED(frequency); } + + // set backlash frequency in steps per second + virtual void setBacklashFrequencySteps(float frequency); + + // get tracking mode steps per slewing mode step + virtual int getStepsPerStepSlewing() { return 1; } + + // get synchronized state (automatic movement of target at setFrequencySteps() rate) + inline bool getSynchronized() { return sync; } + + // set synchronized state (automatic movement of target at setFrequencySteps() rate) + virtual inline void setSynchronized(bool state) { + if (state) { + noInterrupts(); + sync = state; + targetSteps = motorSteps; + interrupts(); + } else sync = state; + } + + // get the current direction of motion + Direction getDirection(); + + // microstep sequencer steps + int getSequencerSteps() { return 1; }; + + // set slewing state (hint that we are about to slew or are done slewing) + virtual void setSlewing(bool state) { UNUSED(state); } + + // signal that the motor load has exceeded its threshold + virtual bool isStalled() { return false; } + + // calibrate the motor if required + virtual void calibrate(float value) { UNUSED(value); } + + // calibrate the motor driver if required + virtual void calibrateDriver() {} + + // set zero of absolute encoders + virtual uint32_t encoderZero() { return 0; } + + // return the encoder count, if present + virtual int32_t getEncoderCount() { return 0; } + + // set origin of absolute encoders + virtual void encoderSetOrigin(uint32_t origin) { UNUSED(origin); } + + // true when this motor has an absolute position source after boot + virtual bool hasAbsoluteEncoder() const { return false; } + + // get the motor name + virtual const char* name() { return NULL; } + + // monitor and respond to motor state as required + virtual void poll() {} + + int driverType = OFF; + volatile bool inBacklash = false; // must be true if within the backlash travel + + volatile uint8_t monitorHandle = 0; // handle to the axis task monitor + + bool enabled = false; // enable/disable logical state + + bool ready = false; // set to true after successful init + + bool calibrating = false; // shadow disable when calibrating + + protected: + // disable backlash compensation, to work properly there must be an enable call to match + void disableBacklash(); + + // enable backlash compensation, to work properly this must be proceeded by a disable call + void enableBacklash(); + + volatile uint8_t axisNumber = 0; // axis number for this motor (1 to 9 in OnStepX) + char axisPrefix[32]; // prefix for debug messages + char nameStr[40]; // name of this motor/driver + + volatile bool sync = true; // locks movement of axis target with timer rate + bool limitsCheck = true; // enable/disable numeric range limits (doesn't apply to limit switches) + + uint8_t homeSenseHandle = 0; // home sensor handle + uint8_t minSenseHandle = 0; // limit min sensor handle + uint8_t maxSenseHandle = 0; // limit max sensor handle + + float backlashFrequency = 0.0F; + volatile uint16_t backlashSteps = 0; // count of motor steps taken for backlash + uint16_t backlashStepsStore; // temporary storage for the position in backlash + volatile uint16_t backlashAmountSteps = 0; // the amount of backlash travel + uint16_t backlashAmountStepsStore; // temporary storage for the amount of backlash travel + + long originSteps = 0; // start position for an autoGoto() + volatile long targetSteps = 0; // where we want the motor + volatile long motorSteps = 0; // where the motor is not counting backlash + volatile long indexSteps = 0; // for absolute motor position to axis position + volatile long step = 1; // step size, and for direction control + + // a fault return status + const DriverStatus errorStatus = {false, {false, false}, {false, false}, false, false, false, true}; + + // reverses the set direction of motion + bool reversed = false; + + // set direction of motion + bool normalizedReverse = false; + + // runtime adjustable settings + const int numParameters = 1; + AxisParameter invalid = {NAN, NAN, NAN, NAN, NAN, AXP_INVALID, ""}; + AxisParameter reverse = {NAN, NAN, NAN, 0, 1, AXP_BOOLEAN, AXPN_REVERSE}; + + AxisParameter* parameter[2] = {&invalid, &reverse}; +}; + +#endif diff --git a/src/lib/axis/motor/kTech/KTech.cpp b/src/lib/axis/motor/kTech/KTech.cpp new file mode 100755 index 0000000..6e1db50 --- /dev/null +++ b/src/lib/axis/motor/kTech/KTech.cpp @@ -0,0 +1,377 @@ +// ----------------------------------------------------------------------------------- +// axis ktech servo motor (designed for the MS4010v3) + +#include "KTech.h" + +#ifdef KTECH_MOTOR_PRESENT + +#include "../../../tasks/OnTask.h" +#include "../../../canPlus/CanPlus.h" + +static KTechMotor *ktechMotorInstance[9]; +IRAM_ATTR void moveKTechMotorAxis1() { ktechMotorInstance[0]->move(); } +IRAM_ATTR void moveKTechMotorAxis2() { ktechMotorInstance[1]->move(); } +IRAM_ATTR void moveKTechMotorAxis3() { ktechMotorInstance[2]->move(); } +IRAM_ATTR void moveKTechMotorAxis4() { ktechMotorInstance[3]->move(); } +IRAM_ATTR void moveKTechMotorAxis5() { ktechMotorInstance[4]->move(); } +IRAM_ATTR void moveKTechMotorAxis6() { ktechMotorInstance[5]->move(); } +IRAM_ATTR void moveKTechMotorAxis7() { ktechMotorInstance[6]->move(); } +IRAM_ATTR void moveKTechMotorAxis8() { ktechMotorInstance[7]->move(); } +IRAM_ATTR void moveKTechMotorAxis9() { ktechMotorInstance[8]->move(); } + +void statusKTechMotorAxis1(uint8_t data[8]) { ktechMotorInstance[0]->requestStatusCallback(data); } +void statusKTechMotorAxis2(uint8_t data[8]) { ktechMotorInstance[1]->requestStatusCallback(data); } +void statusKTechMotorAxis3(uint8_t data[8]) { ktechMotorInstance[2]->requestStatusCallback(data); } +void statusKTechMotorAxis4(uint8_t data[8]) { ktechMotorInstance[3]->requestStatusCallback(data); } +void statusKTechMotorAxis5(uint8_t data[8]) { ktechMotorInstance[4]->requestStatusCallback(data); } +void statusKTechMotorAxis6(uint8_t data[8]) { ktechMotorInstance[5]->requestStatusCallback(data); } +void statusKTechMotorAxis7(uint8_t data[8]) { ktechMotorInstance[6]->requestStatusCallback(data); } +void statusKTechMotorAxis8(uint8_t data[8]) { ktechMotorInstance[7]->requestStatusCallback(data); } +void statusKTechMotorAxis9(uint8_t data[8]) { ktechMotorInstance[8]->requestStatusCallback(data); } + +// constructor +KTechMotor::KTechMotor(uint8_t axisNumber, int8_t reverse, const KTechDriverSettings *Settings, bool useFastHardwareTimers) + :Motor(axisNumber, reverse) { + if (axisNumber < 1 || axisNumber > 9) return; + + driverType = ODRIVER; + + strcpy(axisPrefix, " Axis_KTech, "; + axisPrefix[5] = '0' + axisNumber; + + // the motor CAN ID is the axis number! + canID = 0x140 + axisNumber; + + statusMode = Settings->status; + + if (axisNumber > 2) useFastHardwareTimers = false; + this->useFastHardwareTimers = useFastHardwareTimers; + + // attach the function pointers to the callbacks + ktechMotorInstance[this->axisNumber - 1] = this; + switch (this->axisNumber) { + case 1: callback = moveKTechMotorAxis1; break; + case 2: callback = moveKTechMotorAxis2; break; + case 3: callback = moveKTechMotorAxis3; break; + case 4: callback = moveKTechMotorAxis4; break; + case 5: callback = moveKTechMotorAxis5; break; + case 6: callback = moveKTechMotorAxis6; break; + case 7: callback = moveKTechMotorAxis7; break; + case 8: callback = moveKTechMotorAxis8; break; + case 9: callback = moveKTechMotorAxis9; break; + } +} + +bool KTechMotor::init() { + if (!Motor::init()) return false; + + if (!canPlus.ready) { + V("ERR:"); V(axisPrefix); VLF("No CAN interface!"); + return false; + } + + ready = true; + enable(false); + + // start the motion timer + VF("MSG:"); V(axisPrefix); VF("start task to synthesize motion... "); + char timerName[] = "Ax_KTec"; + timerName[2] = axisNumber + '0'; + taskHandle = tasks.add(0, 0, true, 0, callback, timerName); + if (taskHandle) { + VF("success"); + if (useFastHardwareTimers) { + if (!tasks.requestHardwareTimer(taskHandle, 0)) { + VF(" (no hardware timer!)"); + } else { + maxFrequency = (1000000.0F/HAL_MAXRATE_LOWER_LIMIT)/2.0F; + }; + } + VLF(""); + } else { + VLF("FAILED!"); + ready = false; + return false; + } + + stopSyntheticMotion(); + + // get ready for status messages + if (statusMode == ON) { + switch (this->axisNumber) { + case 1: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis1); break; + case 2: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis2); break; + case 3: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis3); break; + case 4: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis4); break; + case 5: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis5); break; + case 6: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis6); break; + case 7: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis7); break; + case 8: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis8); break; + case 9: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechMotorAxis9); break; + } + } + + return true; +} + +// low level reversal of axis directions +// \param state: OFF normal or ON to reverse +void KTechMotor::setReverse(int8_t state) { + if (!ready) return; + + if (state == ON) { + V("WRN:"); V(axisPrefix); VLF("axis reversal must be accomplished with hardware or KTech setup!"); + } +} + +// sets motor enable on/off (if possible) +void KTechMotor::enable(bool state) { + if (!ready || state == enabled) return; + + if (!state) enabled = false; + + stopSyntheticMotion(); + resetToTrackingBaseline(); + + VF("MSG:"); V(axisPrefix); VF("driver powered "); VLF(state ? "up" : "down"); + + // best-effort command (no ACK assumed) + if (state) { + static const uint8_t cmdOn[] = "\x88\x00\x00\x00\x00\x00\x00\x00"; + canPlus.writePacket(canID, cmdOn, 8); + enabled = true; // open window only if we intend to be enabled + } else { + static const uint8_t cmdOff[] = "\x80\x00\x00\x00\x00\x00\x00\x00"; + canPlus.writePacket(canID, cmdOff, 8); + // enabled already false + } +} + +void KTechMotor::setInstrumentCoordinateSteps(long value) { + Motor::setInstrumentCoordinateSteps(value); +} + +// resets motor and target angular position in steps, also zeros backlash and index +void KTechMotor::resetPositionSteps(long value) { + if (!ready) return; + + static const uint8_t cmd[] = "\x95\x00\x00\x00"; + canPlus.txWait(); + canPlus.beginPacket(canID); + canPlus.write(cmd, 4); + canPlus.write((uint8_t*)&value, 4); + canPlus.endPacket(); + + noInterrupts(); + motorSteps = value; + targetSteps = value; + backlashSteps = 0; + interrupts(); +} + +// set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) +void KTechMotor::setFrequencySteps(float frequency) { + if (!ready) return; + + if (!enabled) { stopSyntheticMotion(); return; } + + // negative frequency, convert to positive and reverse the direction + int dir = 0; + if (frequency > 0.0F) dir = 1; else if (frequency < 0.0F) { frequency = -frequency; dir = -1; } + + // if in backlash override the frequency + if (inBacklash) frequency = backlashFrequency; + + if (frequency != currentFrequency) { + // compensate for performace limitations by taking larger steps as needed + if (frequency < maxFrequency) stepSize = 1; else + if (frequency < maxFrequency*2) stepSize = 2; else + if (frequency < maxFrequency*4) stepSize = 4; else + if (frequency < maxFrequency*8) stepSize = 8; else + if (frequency < maxFrequency*16) stepSize = 16; else + if (frequency < maxFrequency*32) stepSize = 32; else + if (frequency < maxFrequency*64) stepSize = 64; else + if (frequency < maxFrequency*128) stepSize = 128; else stepSize = 256; + + // timer period in microseconds + float period = (1000000.0F*stepSize)/frequency; + + // range is 0 to 134 seconds/step + if (!isnan(period) && period <= 130000000.0F) { + period *= 16.0F; + lastPeriod = (unsigned long)lroundf(period); + } else { + lastPeriod = 0; + frequency = 0.0F; + dir = 0; + } + + currentFrequency = frequency; + + // change the motor rate/direction + noInterrupts(); + step = 0; + interrupts(); + tasks.setPeriodSubMicros(taskHandle, lastPeriod); + } + + noInterrupts(); + step = dir * stepSize; + absStep = (int)labs(step); + interrupts(); +} + +float KTechMotor::getFrequencySteps() { + if (!ready) return 0.0F; + + if (lastPeriod == 0) return 0; + return (16000000.0F / lastPeriod) * absStep; +} + +// set slewing state (hint that we are about to slew or are done slewing) +void KTechMotor::setSlewing(bool state) { + if (!ready) return; + + isSlewing = state; +} + +// updates PID and sets ktech position +void KTechMotor::poll() { + const unsigned long now = millis(); + + if (statusMode == ON && (now - lastStatusRequestTime) >= KTECH_STATUS_MS) { + lastStatusRequestTime = now; + static const uint8_t cmd[] = "\x9a\x00\x00\x00\x00\x00\x00\x00"; + canPlus.writePacket(canID, cmd, 8); + } + + // hard faults on loss of comms during motion + if (statusMode == ON) { + if (!hasHeartbeat(KTECH_STATUS_MS * 4U) || status.fault) { + enabled = false; + stopSyntheticMotion(); + resetToTrackingBaseline(); + return; + } + } + + if (!enabled) return; + + if (now - lastSetPositionTime < CAN_SEND_RATE_MS) return; + lastSetPositionTime = now; + + noInterrupts(); + #if KTECH_SLEW_DIRECT == ON + long target = targetSteps + backlashSteps; + #else + long target = motorSteps + backlashSteps; + #endif + interrupts(); + + if (lastTarget != target) { + static const uint8_t cmd[] = "\xa3\x00\x00\x00"; + canPlus.txWait(); + canPlus.beginPacket(canID); + canPlus.write(cmd, 4); + canPlus.write((uint8_t*)&target, 4); + canPlus.endPacket(); + lastTarget = target; + } +} + +void KTechMotor::stopSyntheticMotion() { + if (lastPeriod == 0 && step == 0 && absStep == 0) return; + currentFrequency = 0.0F; + lastPeriod = 0; + noInterrupts(); + step = 0; + absStep = 0; + interrupts(); + if (taskHandle) tasks.setPeriodSubMicros(taskHandle, 0); +} + +void KTechMotor::resetToTrackingBaseline() { + noInterrupts(); + step = 0; + absStep = 0; + inBacklash = false; + backlashSteps = 0; + interrupts(); + + currentFrequency = 0.0F; + lastPeriod = 0; + stepSize = 1; + + // force next poll() to send target + lastTarget = LONG_MIN; +} + +// sets dir as required and moves coord toward target at setFrequencySteps() rate +IRAM_ATTR void KTechMotor::move() { + if (sync && !inBacklash) targetSteps += step; + + if (motorSteps > targetSteps) { + if (backlashSteps > 0) { + backlashSteps -= absStep; + inBacklash = backlashSteps > 0; + } else { + motorSteps -= absStep; + inBacklash = false; + } + } else + + if (motorSteps < targetSteps || inBacklash) { + if (backlashSteps < backlashAmountSteps) { + backlashSteps += absStep; + inBacklash = backlashSteps < backlashAmountSteps; + } else { + motorSteps += absStep; + inBacklash = false; + } + } +} + +void KTechMotor::updateStatus() { + if (statusMode == OFF) return; + if (millis() - lastStatusUpdateTime >= KTECH_STATUS_MS*4U) { + status.outputA.shortToGround = true; + status.outputA.openLoad = true; + status.outputB.shortToGround = true; + status.outputB.openLoad = true; + status.overTemperatureWarning = true; + status.overTemperature = true; + status.standstill = true; + status.fault = true; + status.active = true; + } +} + +// update the associated driver status from CAN +void KTechMotor::requestStatusCallback(uint8_t data[8]) { + if (statusMode == OFF) return; + + uint8_t errorState = data[7]; + status.outputA.shortToGround = false; + status.outputA.openLoad = false; + status.outputB.shortToGround = false; + status.outputB.openLoad = false; + status.overTemperatureWarning = false; + status.overTemperature = bitRead(errorState, 3); + status.standstill = false; + status.fault = bitRead(errorState, 0) || bitRead(errorState, 3); + status.active = true; + + lastStatusUpdateTime = millis(); + statusValid = true; +} + +bool KTechMotor::hasHeartbeat(uint32_t maxAgeMs) const { + if (!ready || statusMode == OFF) return false; + + const unsigned long now = millis(); + const unsigned long age = now - lastStatusUpdateTime; + + return statusValid && (age < maxAgeMs); +} + +#endif diff --git a/src/lib/axis/motor/kTech/KTech.h b/src/lib/axis/motor/kTech/KTech.h new file mode 100755 index 0000000..27d472b --- /dev/null +++ b/src/lib/axis/motor/kTech/KTech.h @@ -0,0 +1,118 @@ +// ----------------------------------------------------------------------------------- +// axis ktech servo motor (designed for the MS4010v3) +#pragma once +#include "../../../../Common.h" + +#ifdef KTECH_MOTOR_PRESENT + +#if !defined(CAN_PLUS) || CAN_PLUS == OFF + #error "No KTECH motor CAN interface!" +#endif + +#include "../Motor.h" +#include "../../../convert/Convert.h" + +// KTECH status rate default 1Hz +#ifndef KTECH_STATUS_MS + #define KTECH_STATUS_MS 1000 +#endif + +// KTECH direct slewing ON or OFF (KTech handles acceleration) +#ifndef KTECH_SLEW_DIRECT + #define KTECH_SLEW_DIRECT OFF +#endif + +typedef struct KTechDriverSettings { + int16_t model; + int8_t status; +} KTechDriverSettings; + +class KTechMotor : public Motor { + public: + // constructor + KTechMotor(uint8_t axisNumber, int8_t reverse, const KTechDriverSettings *Settings, bool useFastHardwareTimers = true); + + // sets up the ktech motor + bool init(); + + // low level reversal of axis directions + // \param state: OFF normal or ON to reverse + void setReverse(int8_t state); + + // sets motor enable on/off (if possible) + void enable(bool value); + + // set instrument coordinate, in steps + void setInstrumentCoordinateSteps(long value); + + // get the associated driver status + DriverStatus getDriverStatus() { if (ready) updateStatus(); else status = errorStatus; return status; } + bool hasHeartbeat(uint32_t maxAgeMs) const; + + // resets motor and target angular position in steps, also zeros backlash and index + void resetPositionSteps(long value); + + // get tracking mode steps per slewing mode step + inline int getStepsPerStepSlewing() { return 256; } + + // get movement frequency in steps per second + float getFrequencySteps(); + + // set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) + void setFrequencySteps(float frequency); + + // set slewing state (hint that we are about to slew or are done slewing) + void setSlewing(bool state); + + // updates PID and sets servo motor power/direction + void poll(); + + // sets dir as required and moves coord toward target at setFrequencySteps() rate + void move(); + + // update the associated driver status + void updateStatus(); + + // update the associated driver status from CAN + void requestStatusCallback(uint8_t data[8]); + + // get the motor name + const char* name() { return "KTECH"; } + + private: + void stopSyntheticMotion(); + void resetToTrackingBaseline(); + + int canID; + + unsigned long lastSetPositionTime = 0; + unsigned long lastStatusRequestTime = 0; + unsigned long lastStatusUpdateTime = 0; + bool statusValid = false; + + uint8_t taskHandle = 0; + + int stepSize = 1; // step size + volatile bool takeStep = false; // should we take a step + long lastTarget = 0; + + float currentFrequency = 0.0F; // last frequency set + unsigned long lastPeriod = 0; // last timer period (in sub-micros) + float maxFrequency = HAL_FRACTIONAL_SEC; // fastest timer rate + + volatile int absStep = 0; // absolute step size (unsigned) + + void (*callback)() = NULL; + + bool useFastHardwareTimers = true; + + bool isSlewing = false; + + int16_t statusMode = OFF; + DriverStatus status = { false, {false, false}, {false, false}, false, false, false, false }; + bool statusRequestSent = false; + + float stepsPerMeasure = 0.0F; +}; + +#endif \ No newline at end of file diff --git a/src/lib/axis/motor/mksServo/MksServo.cpp b/src/lib/axis/motor/mksServo/MksServo.cpp new file mode 100755 index 0000000..b0a910e --- /dev/null +++ b/src/lib/axis/motor/mksServo/MksServo.cpp @@ -0,0 +1,454 @@ +// ----------------------------------------------------------------------------------- +// axis MKS SERVO42D/57D motor driver via CAN, using 0xFE absolute position updates +// EXPERIMENTAL!!! + +#include "MksServo.h" + +#ifdef MKS42D_MOTOR_PRESENT + +#include "../../../tasks/OnTask.h" +#include "../../../canPlus/CanPlus.h" + +static Mks42DMotor *mksMotorInstance[9] = {nullptr}; + +IRAM_ATTR void moveMksAxis1() { mksMotorInstance[0]->move(); } +IRAM_ATTR void moveMksAxis2() { mksMotorInstance[1]->move(); } +IRAM_ATTR void moveMksAxis3() { mksMotorInstance[2]->move(); } +IRAM_ATTR void moveMksAxis4() { mksMotorInstance[3]->move(); } +IRAM_ATTR void moveMksAxis5() { mksMotorInstance[4]->move(); } +IRAM_ATTR void moveMksAxis6() { mksMotorInstance[5]->move(); } +IRAM_ATTR void moveMksAxis7() { mksMotorInstance[6]->move(); } +IRAM_ATTR void moveMksAxis8() { mksMotorInstance[7]->move(); } +IRAM_ATTR void moveMksAxis9() { mksMotorInstance[8]->move(); } + +// Status callbacks (0xF1 and 0x3E both funnel into requestStatusCallback) +static void statusMksAxis1(uint8_t data[8]) { if (mksMotorInstance[0]) mksMotorInstance[0]->requestStatusCallback(data); } +static void statusMksAxis2(uint8_t data[8]) { if (mksMotorInstance[1]) mksMotorInstance[1]->requestStatusCallback(data); } +static void statusMksAxis3(uint8_t data[8]) { if (mksMotorInstance[2]) mksMotorInstance[2]->requestStatusCallback(data); } +static void statusMksAxis4(uint8_t data[8]) { if (mksMotorInstance[3]) mksMotorInstance[3]->requestStatusCallback(data); } +static void statusMksAxis5(uint8_t data[8]) { if (mksMotorInstance[4]) mksMotorInstance[4]->requestStatusCallback(data); } +static void statusMksAxis6(uint8_t data[8]) { if (mksMotorInstance[5]) mksMotorInstance[5]->requestStatusCallback(data); } +static void statusMksAxis7(uint8_t data[8]) { if (mksMotorInstance[6]) mksMotorInstance[6]->requestStatusCallback(data); } +static void statusMksAxis8(uint8_t data[8]) { if (mksMotorInstance[7]) mksMotorInstance[7]->requestStatusCallback(data); } +static void statusMksAxis9(uint8_t data[8]) { if (mksMotorInstance[8]) mksMotorInstance[8]->requestStatusCallback(data); } + +Mks42DMotor::Mks42DMotor(uint8_t axisNumber, + int8_t reverse, + const MksDriverSettings *Settings, + bool useFastHardwareTimers) + : Motor(axisNumber, reverse) { + + if (axisNumber < 1 || axisNumber > 9) return; + + driverType = ODRIVER; + + strcpy(axisPrefix, " Axis_Mks42D, "); + axisPrefix[5] = '0' + axisNumber; + + // Mapping policy: CAN ID == axis number (1..9) + canID = axisNumber; + + // reserve settings for future protocol/status differences + if (Settings) { + model = Settings->model; + statusMode = Settings->status; + } else { + model = 0; + statusMode = OFF; + } + + // AxisParameter defaults (must be assigned here) + // stepsPerSecondToRpm is intentionally *not* used in the simple/robust mode below. + stepsPerSecondToRpm.valueDefault = 0.01F; + maxRpm.valueDefault = 60.0F; + + if (axisNumber > 2) useFastHardwareTimers = false; + this->useFastHardwareTimers = useFastHardwareTimers; + + mksMotorInstance[axisNumber - 1] = this; + switch (axisNumber) { + case 1: callback = moveMksAxis1; break; + case 2: callback = moveMksAxis2; break; + case 3: callback = moveMksAxis3; break; + case 4: callback = moveMksAxis4; break; + case 5: callback = moveMksAxis5; break; + case 6: callback = moveMksAxis6; break; + case 7: callback = moveMksAxis7; break; + case 8: callback = moveMksAxis8; break; + case 9: callback = moveMksAxis9; break; + } +} + +bool Mks42DMotor::init() { + if (!Motor::init()) return false; + + if (!canPlus.ready) { + V("ERR:"); V(axisPrefix); VLF("No CAN interface!"); + return false; + } + + ready = true; + enable(false); + + VF("MSG:"); V(axisPrefix); VF("start task to synthesize motion... "); + char timerName[] = "Ax_Mks_"; + timerName[2] = axisNumber + '0'; + + taskHandle = tasks.add(0, 0, true, 0, callback, timerName); + if (taskHandle) { + VF("success"); + if (useFastHardwareTimers) { + if (!tasks.requestHardwareTimer(taskHandle, 0)) { + VF(" (no hardware timer!)"); + } else { + maxFrequency = (1000000.0F / HAL_MAXRATE_LOWER_LIMIT) / 2.0F; + } + } + VLF(""); + } else { + VLF("FAILED!"); + ready = false; + return false; + } + + stopSyntheticMotion(); + resetToTrackingBaseline(); + + // Always-on heartbeat + protect polling (RX callbacks) + if (statusMode == ON) { + switch (axisNumber) { + case 1: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis1); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis1); + break; + case 2: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis2); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis2); + break; + case 3: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis3); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis3); + break; + case 4: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis4); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis4); + break; + case 5: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis5); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis5); + break; + case 6: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis6); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis6); + break; + case 7: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis7); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis7); + break; + case 8: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis8); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis8); + break; + case 9: + canPlus.callbackRegisterMessage(canID, 0xF1, statusMksAxis9); + canPlus.callbackRegisterMessage(canID, 0x3E, statusMksAxis9); + break; + } + + statusValid = false; + protectActive = false; + status.fault = false; + lastStatusUpdateTime = 0; + lastStatusRequestTime = 0; + lastProtectRequestTime = 0; + } + + return true; +} + +void Mks42DMotor::setReverse(int8_t state) { + if (!ready) return; + if (state == ON) { + V("WRN:"); V(axisPrefix); VLF("axis reversal must be done in MKS setup or wiring!"); + } +} + +void Mks42DMotor::setReverse(bool state) { + if (state) { + V("WRN:"); V(axisPrefix); VLF("software reverse requested; prefer MKS setup or wiring!"); + } + Motor::setReverse(state); +} + +void Mks42DMotor::enable(bool state) { + if (!ready || state == enabled) return; + + if (!state) enabled = false; + + stopSyntheticMotion(); + resetToTrackingBaseline(); + + VF("MSG:"); V(axisPrefix); VF("driver powered "); VLF(state ? "up" : "down"); + + canPlus.txWait(); + sendF3(state); + + if (state) { + enabled = true; + lastEnableTime = millis(); + + // clear latched faults on enable attempt (simple policy for v1) + status.fault = false; + protectActive = false; + // statusValid remains as-is; always-on polling will refresh it soon + } + + // force next poll() to send target + lastTarget = LONG_MIN; +} + +void Mks42DMotor::resetPositionSteps(long value) { + if (!ready) return; + Motor::resetPositionSteps(value); + + // force next poll() to send target + lastTarget = LONG_MIN; +} + +float Mks42DMotor::getFrequencySteps() { + if (!ready) return 0.0F; + if (lastPeriod == 0) return 0.0F; + return (16000000.0F / lastPeriod) * absStep; +} + +void Mks42DMotor::setFrequencySteps(float frequency) { + if (!ready) return; + if (!enabled) { stopSyntheticMotion(); return; } + + int dir = 0; + if (frequency > 0.0F) dir = 1; + else if (frequency < 0.0F) { frequency = -frequency; dir = -1; } + + if (inBacklash) frequency = backlashFrequency; + + if (frequency != currentFrequency) { + // compensate for performance limitations by taking larger steps as needed + if (frequency < maxFrequency) stepSize = 1; else + if (frequency < maxFrequency*2) stepSize = 2; else + if (frequency < maxFrequency*4) stepSize = 4; else + if (frequency < maxFrequency*8) stepSize = 8; else + if (frequency < maxFrequency*16) stepSize = 16; else + if (frequency < maxFrequency*32) stepSize = 32; else + if (frequency < maxFrequency*64) stepSize = 64; else + if (frequency < maxFrequency*128) stepSize = 128; else stepSize = 256; + + // timer period in microseconds + float period = (1000000.0F * stepSize) / frequency; + + // range is 0 to 134 seconds/step + if (!isnan(period) && period <= 130000000.0F) { + period *= 16.0F; + lastPeriod = (unsigned long)lroundf(period); + } else { + lastPeriod = 0; + frequency = 0.0F; + dir = 0; + } + + currentFrequency = frequency; + + // change the motor rate/direction + noInterrupts(); + step = 0; + interrupts(); + tasks.setPeriodSubMicros(taskHandle, lastPeriod); + } + + noInterrupts(); + step = dir * stepSize; + absStep = (int)labs(step); + interrupts(); +} + +void Mks42DMotor::setSlewing(bool state) { + if (!ready) return; + isSlewing = state; +} + +// ---- CAN helpers ---- + +inline void Mks42DMotor::sendWithCrc(const uint8_t *payload, size_t n) { + if (!payload || n < 1 || n > 7) return; + + uint8_t buf[8] = {0}; + memcpy(buf, payload, n); + buf[n] = crc8((uint16_t)canID, buf, n); + + canPlus.writePacket(canID, buf, n + 1); +} + +inline void Mks42DMotor::sendF3(bool en) { + const uint8_t p[2] = { 0xF3, (uint8_t)(en ? 1 : 0) }; + sendWithCrc(p, 2); +} + +inline void Mks42DMotor::sendFE(int32_t pos, uint16_t speed) { + const int32_t p = clamp24(pos); + const uint32_t u = (uint32_t)p & 0x00FFFFFFUL; // signed 24-bit in low 24 bits + + const uint8_t payload[7] = { + 0xFE, + (uint8_t)(speed >> 8), + (uint8_t)(speed & 0xFF), + (uint8_t)MKS_FE_ACCEL_CONST, + (uint8_t)((u >> 16) & 0xFF), + (uint8_t)((u >> 8) & 0xFF), + (uint8_t)((u >> 0) & 0xFF), + }; + + sendWithCrc(payload, 7); +} + +// Always-on polling commands +inline void Mks42DMotor::sendF1() { const uint8_t p[1] = { 0xF1 }; sendWithCrc(p, 1); } +inline void Mks42DMotor::send3E() { const uint8_t p[1] = { 0x3E }; sendWithCrc(p, 1); } + +// RX: treat 0xF1 and 0x3E as heartbeat sources; latch fault on protect active. +void Mks42DMotor::requestStatusCallback(uint8_t data[8]) { + if (statusMode == OFF) return; + + const uint8_t code = data[0]; + if (code != 0xF1 && code != 0x3E) return; + + lastStatusUpdateTime = millis(); + statusValid = true; + + if (code == 0x3E) { + protectActive = (data[1] != 0); + if (protectActive) status.fault = true; // latch + } + + // Optional extension hook: decode 0xF1 state from data[1] later if desired. +} + +bool Mks42DMotor::hasHeartbeat(uint32_t maxAgeMs) const { + if (!ready || statusMode == OFF) return false; + if (!statusValid) return false; + + const unsigned long now = millis(); + return (now - lastStatusUpdateTime) < maxAgeMs; +} + +// Absolute position streaming (simple/robust): +// - Stream 0xFE target position +// - Use a fixed speed cap (maxRpm) rather than a dynamic feed-forward mapping +void Mks42DMotor::poll() { + if (!ready) return; + + const unsigned long now = millis(); + + // Always-on heartbeat + protect polling + if (statusMode == ON) { + if ((now - lastStatusRequestTime) >= MKS_STATUS_MS) { + lastStatusRequestTime = now; + if (canPlus.txTryLock()) sendF1(); // best-effort, non-blocking + } + + if ((now - lastProtectRequestTime) >= MKS_PROTECT_MS) { + lastProtectRequestTime = now; + if (canPlus.txTryLock()) send3E(); // best-effort, non-blocking + } + + // Hard faults only when enabled, with grace window after enabling + if (enabled) { + const bool inGrace = (now - lastEnableTime) < (unsigned long)MKS_HEARTBEAT_GRACE_MS; + + const bool ok = statusValid ? hasHeartbeat(MKS_STATUS_MS * 4U) : inGrace; + + if (!ok || status.fault) { + enabled = false; + stopSyntheticMotion(); + resetToTrackingBaseline(); + return; + } + } + } + + if (!enabled) return; + + noInterrupts(); + #if MKS_SLEW_DIRECT == ON + long target = targetSteps + backlashSteps; + #else + long target = motorSteps + backlashSteps; + #endif + interrupts(); + + // signed 24-bit domain enforced here + const int32_t pos = clamp24((int32_t)target); + + // simple: speed is a cap (RPM), not a feed-forward term + uint16_t spd = (uint16_t)lroundf(pval(maxRpm)); + if (spd > 3000) spd = 3000; + + // send only on position change (avoid any "speed jitter" surprises) + if (lastTarget == (long)pos) return; + + if (!canPlus.txTryLock()) return; + + sendFE(pos, spd); + + lastTarget = (long)pos; +} + +void Mks42DMotor::stopSyntheticMotion() { + if (lastPeriod == 0 && step == 0 && absStep == 0) return; + currentFrequency = 0.0F; + lastPeriod = 0; + noInterrupts(); + step = 0; + absStep = 0; + interrupts(); + if (taskHandle) tasks.setPeriodSubMicros(taskHandle, 0); +} + +void Mks42DMotor::resetToTrackingBaseline() { + noInterrupts(); + step = 0; + absStep = 0; + inBacklash = false; + backlashSteps = 0; + interrupts(); + + currentFrequency = 0.0F; + lastPeriod = 0; + stepSize = 1; + + // force next poll() to send target + lastTarget = LONG_MIN; +} + +// sets dir as required and moves coord toward target at setFrequencySteps() rate +IRAM_ATTR void Mks42DMotor::move() { + if (sync && !inBacklash) targetSteps += step; + + if (motorSteps > targetSteps) { + if (backlashSteps > 0) { + backlashSteps -= absStep; + inBacklash = backlashSteps > 0; + } else { + motorSteps -= absStep; + inBacklash = false; + } + } else if (motorSteps < targetSteps || inBacklash) { + if (backlashSteps < backlashAmountSteps) { + backlashSteps += absStep; + inBacklash = backlashSteps < backlashAmountSteps; + } else { + motorSteps += absStep; + inBacklash = false; + } + } +} + +#endif diff --git a/src/lib/axis/motor/mksServo/MksServo.h b/src/lib/axis/motor/mksServo/MksServo.h new file mode 100755 index 0000000..5a74d40 --- /dev/null +++ b/src/lib/axis/motor/mksServo/MksServo.h @@ -0,0 +1,184 @@ +// ----------------------------------------------------------------------------------- +// axis MKS SERVO42D/57D motor driver via CAN, using 0xFE absolute position updates +// EXPERIMENTAL!!! +#pragma once +#include "../../../../Common.h" + +#ifdef MKS42D_MOTOR_PRESENT + +#if !defined(CAN_PLUS) || CAN_PLUS == OFF + #error "No MKS motor CAN interface!" +#endif + +#include "../Motor.h" + +#ifndef MKS_SLEW_DIRECT + #define MKS_SLEW_DIRECT OFF +#endif + +#ifndef MKS_FE_ACCEL_CONST + #define MKS_FE_ACCEL_CONST 10 +#endif + +// always-on status request (0xF1) rate +#ifndef MKS_STATUS_MS + #define MKS_STATUS_MS 250 +#endif + +// always-on protect-state request (0x3E) rate +#ifndef MKS_PROTECT_MS + #define MKS_PROTECT_MS 1000 +#endif + +// grace period after enabling before enforcing heartbeat +#ifndef MKS_HEARTBEAT_GRACE_MS + #define MKS_HEARTBEAT_GRACE_MS 1500 +#endif + +typedef struct MksDriverSettings { + int16_t model; + int8_t status; // ON/OFF (enables always-on heartbeat + protect polling) +} MksDriverSettings; + +class Mks42DMotor : public Motor { + public: + Mks42DMotor(uint8_t axisNumber, + int8_t reverse, + const MksDriverSettings *Settings, + bool useFastHardwareTimers = true); + + bool init() override; + + uint8_t getParameterCount() override { return Motor::getParameterCount() + numParameters; } + + AxisParameter* getParameter(uint8_t number) override { + const uint8_t base = Motor::getParameterCount(); + if (number >= 1 && number <= base) return Motor::getParameter(number); + + const uint8_t i = number - base; + if (i == 1) return &stepsPerSecondToRpm; + if (i == 2) return &maxRpm; + return &invalid; + } + + // Integrated-driver style reverse control (kept for parity with other integrated CAN motors) + void setReverse(int8_t state); + + // Base-class reverse (prevents silent bypass if someone calls Motor::setReverse(bool)) + void setReverse(bool state) override; + + void enable(bool state) override; + + void resetPositionSteps(long value) override; + + int getStepsPerStepSlewing() override { return 256; } + + float getFrequencySteps() override; + void setFrequencySteps(float frequency) override; + + void setSlewing(bool state) override; + + void poll() override; + + const char* name() override { return "MKS_FE"; } + + // ISR uses this; not a Motor virtual + void move(); + + // minimal heartbeat API (updated by 0xF1 or 0x3E callbacks) + bool hasHeartbeat(uint32_t maxAgeMs) const; + + // driver status (minimal; only "fault" is currently meaningful) + DriverStatus getDriverStatus() { return ready ? status : errorStatus; } + + // RX callback (wired via CanPlus callbackRegisterMessage) + void requestStatusCallback(uint8_t data[8]); + + private: + static inline uint8_t crc8(uint16_t id11, const uint8_t *data, size_t n) { + uint32_t sum = (uint32_t)(id11 & 0x7FF); + for (size_t i = 0; i < n; i++) sum += data[i]; + return (uint8_t)(sum & 0xFF); + } + + static inline int32_t clamp24(int32_t v) { + if (v > 8388607L) return 8388607L; + if (v < -8388608L) return -8388608L; + return v; + } + + inline float pval(const AxisParameter &p) const { + const float v = p.value; + return isnan(v) ? p.valueDefault : v; + } + + // stepsPerSecondToRpm unit: RPM per (steps/second) + inline uint16_t speedFromStepRate(float stepsPerSec) const { + if (stepsPerSec < 0.0F) stepsPerSec = -stepsPerSec; + + const float scale = pval(stepsPerSecondToRpm); + float sF = stepsPerSec * scale; + if (sF < 0.0F) sF = 0.0F; + + // MKS protocol speed range is 0..3000 RPM; keep the clamp aligned to that. + float capF = pval(maxRpm); + if (capF < 0.0F) capF = 0.0F; + if (capF > 3000.0F) capF = 3000.0F; + + if (sF > capF) sF = capF; + return (uint16_t)lroundf(sF); + } + + inline void sendWithCrc(const uint8_t *payload, size_t n); // n<=7 + inline void sendF3(bool en); + inline void sendFE(int32_t pos, uint16_t speed); + + // Always-on polling helpers + inline void sendF1(); // queryStatus (0xF1) + inline void send3E(); // readProtectState (0x3E) + + void stopSyntheticMotion(); + void resetToTrackingBaseline(); + + int canID = 0; + + // reserved / future + int16_t model = 0; + int8_t statusMode = OFF; + + uint8_t taskHandle = 0; + + int stepSize = 1; + float currentFrequency = 0.0F; + unsigned long lastPeriod = 0; + float maxFrequency = HAL_FRACTIONAL_SEC; + + volatile int absStep = 0; + + void (*callback)() = nullptr; + + bool useFastHardwareTimers = true; + bool isSlewing = false; + + long lastTarget = LONG_MIN; + + // --- always-on heartbeat / protect state --- + unsigned long lastStatusRequestTime = 0; + unsigned long lastProtectRequestTime = 0; + unsigned long lastStatusUpdateTime = 0; + unsigned long lastEnableTime = 0; + bool statusValid = false; + + bool protectActive = false; // latched via 0x3E + + // Minimal status model: only "fault" is currently meaningful + DriverStatus status = { false, {false, false}, {false, false}, false, false, false, false }; + + // runtime adjustable settings + AxisParameter stepsPerSecondToRpm = {NAN, NAN, NAN, 0.0F, 10000.0F, AXP_FLOAT, "Steps/s to RPM"}; + AxisParameter maxRpm = {NAN, NAN, NAN, 0.0F, 3000.0F, AXP_INTEGER, "Max Speed RPM"}; + + const int numParameters = 2; +}; + +#endif diff --git a/src/lib/axis/motor/oDrive/ODrive.cpp b/src/lib/axis/motor/oDrive/ODrive.cpp new file mode 100755 index 0000000..71bb71f --- /dev/null +++ b/src/lib/axis/motor/oDrive/ODrive.cpp @@ -0,0 +1,288 @@ +// ----------------------------------------------------------------------------------- +// axis odrive servo motor + +#include "ODrive.h" + +#ifndef ODRIVE_NEW_MOTOR_PRESENT +#ifdef ODRIVE_MOTOR_PRESENT + +#include "../../../tasks/OnTask.h" +#include "../../../gpioEx/GpioEx.h" + +#include "ODriveEnums.h" + +ODriveMotor *odriveMotorInstance[2]; +IRAM_ATTR void moveODriveMotorAxis1() { odriveMotorInstance[0]->move(); } +IRAM_ATTR void moveODriveMotorAxis2() { odriveMotorInstance[1]->move(); } + +// ODrive servo motor driver object pointer +#if ODRIVE_COMM_MODE == OD_UART + ODriveArduino *_oDriveDriver; +#elif ODRIVE_COMM_MODE == OD_CAN + ODriveTeensyCAN *_oDriveDriver; +#endif + +// constructor +ODriveMotor::ODriveMotor(uint8_t axisNumber, int8_t reverse, const ODriveDriverSettings *Settings, float radsPerCount, bool useFastHardwareTimers) + :Motor(axisNumber, reverse) { + if (axisNumber < 1 || axisNumber > 2) return; + + driverType = ODRIVER; + + #if ODRIVE_SWAP_AXES == ON + this->axisNumber = 3 - axisNumber; + #else + this->axisNumber = axisNumber; + #endif + + strcpy(axisPrefix, " Axis_ODrive, "); + axisPrefix[5] = '0' + axisNumber; + + if (axisNumber > 2) useFastHardwareTimers = false; + this->useFastHardwareTimers = useFastHardwareTimers; + + if (axisNumber == 1) { // only do once since motor 2 object creation could + #if ODRIVE_COMM_MODE == OD_UART + _oDriveDriver = new ODriveArduino(ODRIVE_SERIAL); + #elif ODRIVE_COMM_MODE == OD_CAN + _oDriveDriver = new ODriveTeensyCAN(250000); + #endif + } + + // set parameter default values during object creation + radsPerCount.valueDefualt = radsPerCount; + + // attach the function pointers to the callbacks + odriveMotorInstance[this->axisNumber - 1] = this; + switch (this->axisNumber) { + case 1: callback = moveODriveMotorAxis1; break; + case 2: callback = moveODriveMotorAxis2; break; + } +} + +bool ODriveMotor::init() { + if (!Motor::init()) return false; + + if (axisNumber == 1) { + pinModeEx(ODRIVE_RST_PIN, OUTPUT); + digitalWriteEx(ODRIVE_RST_PIN, HIGH); // bring ODrive out of Reset + delay(1000); // allow time for ODrive to boot + + #if ODRIVE_COMM_MODE == OD_UART + ODRIVE_SERIAL.begin(ODRIVE_SERIAL_BAUD); + VF("MSG:"); V(axisPrefix); VLF("SERIAL channel init"); + #elif ODRIVE_COMM_MODE == OD_CAN + // .begin is done by the constructor + VF("MSG:"); V(axisPrefix); VLF("CAN channel init"); + #endif + } + + enable(false); + + // start the motion timer + VF("MSG:"); V(axisPrefix); VF("start task to synthesize motion... "); + char timerName[] = "Target_"; + timerName[6] = '0' + axisNumber; + taskHandle = tasks.add(0, 0, true, 0, callback, timerName); + if (taskHandle) { + V("success"); + if (useFastHardwareTimers && !tasks.requestHardwareTimer(taskHandle, 0)) { VLF(" (no hardware timer!)"); } else { VLF(""); } + } else { + VLF("FAILED!"); + return false; + } + + ready = true; + return true; +} + +// set motor reverse state +void ODriveMotor::setReverse(int8_t state) { + if (!ready) return; + + if (state == ON) { + VF("MSG:"); V(axisPrefix); VLF("axis reversal must be accomplished with hardware or ODrive setup!"); + } +} + +// sets motor enable on/off (if possible) +void ODriveMotor::enable(bool state) { + if (!ready) return; + + VF("MSG:"); V(axisPrefix); VF("driver powered "); if (state) { VLF("up"); } else { VLF("down"); } + + int requestedState = AXIS_STATE_IDLE; + if (state) requestedState = AXIS_STATE_CLOSED_LOOP_CONTROL; + + #if ODRIVE_COMM_MODE == OD_UART + float timeout = 0.5; + if(!_oDriveDriver->run_state(axisNumber - 1, requestedState, false, timeout)) { + VF("MSG:"); V(axisPrefix); VLF(" Power, closed loop control - command timeout!"); + return; + } + #elif ODRIVE_COMM_MODE == OD_CAN + if(!_oDriveDriver->RunState(axisNumber - 1, requestedState)) { //currently, always returns true...need to add timeout + VF("MSG:"); V(axisPrefix); VLF(" Power, closed loop control - command timeout!"); + return; + } + #endif + + VF("MSG:"); V(axisPrefix); VF("closed loop control - "); if (state) { VLF("Active"); } else { VLF("Idle"); } + + enabled = state; +} + +void ODriveMotor::setInstrumentCoordinateSteps(long value) { + if (!ready) return; + + #if ODRIVE_ABSOLUTE == ON && ODRIVE_SYNC_LIMIT != OFF + noInterrupts(); + long index = value - motorSteps; + interrupts(); + float indexDeg = index/radsPerCount.value; + if (indexDeg >= -degToRadF(ODRIVE_SYNC_LIMIT/3600.0F) && indexDeg <= degToRadF(ODRIVE_SYNC_LIMIT/3600.0F)) + #endif + Motor::setInstrumentCoordinateSteps(value); +} + +// resets motor and target angular position in steps, also zeros backlash and index +void ODriveMotor::resetPositionSteps(long value) { + if (!ready) return; + + // this is where the initial odrive position in "steps" is brought into agreement with the motor position in steps + // not sure on this... but code below ignores (value,) gets the odrive position convert to steps and resets the motor + // there (as the odrive encoders are absolute.) + + long oPosition; + // if (axisNumber - 1 == 0) oPosition = o_position0; + // if (axisNumber - 1 == 1) oPosition = o_position1; + + // get ODrive position in fractionial Turns + #if ODRIVE_COMM_MODE == OD_UART + oPosition = _oDriveDriver->GetPosition(axisNumber - 1)*TWO_PI*radsPerCount.value; // axis1/2 are in steps per radian + #elif ODRIVE_COMM_MODE == OD_CAN + oPosition = _oDriveDriver->GetPosition(axisNumber - 1)*TWO_PI*radsPerCount.value; // axis1/2 are in steps per radian + #endif + + noInterrupts(); + motorSteps = oPosition; + targetSteps = motorSteps; + #if ODRIVE_ABSOLUTE == OFF + // but what if the odrive encoders are incremental? how to tell the odrive what its angular position is? + // here thinking we'll ignore it... sync OnStepX there and let the offset handle it + indexSteps = value - motorSteps; + #else + UNUSED(value); + indexSteps = 0; + #endif + backlashSteps = 0; + interrupts(); +} + +// set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) +void ODriveMotor::setFrequencySteps(float frequency) { + if (!ready) return; + + // negative frequency, convert to positive and reverse the direction + int dir = 0; + if (frequency > 0.0F) dir = 1; else if (frequency < 0.0F) { frequency = -frequency; dir = -1; } + + // if in backlash override the frequency + if (inBacklash) + frequency = backlashFrequency; + + if (frequency != currentFrequency) { + lastFrequency = frequency; + + // if slewing has a larger step size divide the frequency to account for it + if (lastFrequency <= backlashFrequency * 2.0F) stepSize = 1; else { if (!inBacklash) stepSize = 64; } + frequency /= stepSize; + + // timer period in microseconds + float period = 1000000.0F / frequency; + + // range is 0 to 134 seconds/step + if (!isnan(period) && period <= 130000000.0F) { + period *= 16.0F; + lastPeriod = (unsigned long)lroundf(period); + } else { + lastPeriod = 0; + frequency = 0.0F; + dir = 0; + } + + currentFrequency = frequency; + + // change the motor rate/direction + noInterrupts(); + step = 0; + interrupts(); + tasks.setPeriodSubMicros(taskHandle, lastPeriod); + } + + noInterrupts(); + step = dir * stepSize; + absStep = abs(step); + interrupts(); +} + +float ODriveMotor::getFrequencySteps() { + if (!ready) return 0.0F; + + if (lastPeriod == 0) return 0.0F; + return (16000000.0F / lastPeriod) * absStep; +} + +// set slewing state (hint that we are about to slew or are done slewing) +void ODriveMotor::setSlewing(bool state) { + if (!ready) return; + + isSlewing = state; +} + +// updates PID and sets odrive position +void ODriveMotor::poll() { + if ((long)(millis() - lastSetPositionTime) < ODRIVE_UPDATE_MS) return; + lastSetPositionTime = millis(); + + noInterrupts(); + #if ODRIVE_SLEW_DIRECT == ON + long target = targetSteps + backlashSteps; + #else + long target = motorSteps + backlashSteps; + #endif + interrupts(); + #if ODRIVE_COMM_MODE == OD_UART + setPosition(axisNumber -1, target/(TWO_PI*radsPerCount.value)); + #elif ODRIVE_COMM_MODE == OD_CAN + _oDriveDriver->SetPosition(axisNumber -1, target/(TWO_PI*radsPerCount.value)); + #endif +} + +// sets dir as required and moves coord toward target at setFrequencySteps() rate +IRAM_ATTR void ODriveMotor::move() { + if (sync && !inBacklash) targetSteps += step; + + if (motorSteps > targetSteps) { + if (backlashSteps > 0) { + backlashSteps -= absStep; + inBacklash = backlashSteps > 0; + } else { + motorSteps -= absStep; + inBacklash = false; + } + } else + + if (motorSteps < targetSteps || inBacklash) { + if (backlashSteps < backlashAmountSteps) { + backlashSteps += absStep; + inBacklash = backlashSteps < backlashAmountSteps; + } else { + motorSteps += absStep; + inBacklash = false; + } + } +} + +#endif +#endif diff --git a/src/lib/axis/motor/oDrive/ODrive.h b/src/lib/axis/motor/oDrive/ODrive.h new file mode 100755 index 0000000..cba9388 --- /dev/null +++ b/src/lib/axis/motor/oDrive/ODrive.h @@ -0,0 +1,181 @@ +// ----------------------------------------------------------------------------------- +// axis oDrive servo motor +#pragma once +#include "../../../../Common.h" + +#ifndef ODRIVE_NEW_MOTOR_PRESENT +#ifdef ODRIVE_MOTOR_PRESENT + +#include "../Motor.h" +#include "../../../convert/Convert.h" + +// the following would be in the pinmap normally and should trigger #error on compile here when not present +// for now, they fit your hardware as best as I can tell... +#ifndef ODRIVE_SERIAL + #define ODRIVE_SERIAL Serial3 // Teensy HW Serial3 (if used,) for example +#endif +#ifndef ODRIVE_SERIAL_BAUD + #define ODRIVE_SERIAL_BAUD 115200 // was 19200 +#endif + +#ifndef ODRIVE_RST_PIN + #define ODRIVE_RST_PIN 3 +#endif + +// odrive update rate default 10Hz +#ifndef ODRIVE_UPDATE_MS + #define ODRIVE_UPDATE_MS 100 // was 3000 +#endif + +// odrive direct slewing ON or OFF (ODrive handles acceleration) +// ON to use ODrive trapezoidal move profile, OFF for OnStep move profile +#ifndef ODRIVE_SLEW_DIRECT + #define ODRIVE_SLEW_DIRECT OFF +#endif + +// odrive if using absolute encoders set to ON +#ifndef ODRIVE_ABSOLUTE + #define ODRIVE_ABSOLUTE ON // was OFF +#endif + +// odrive sync limit (for absolute encoders) OFF or specify the sync limit in arc-seconds +// this should, I hope, allow you to limit just how far from the encoders you can sync. (+/-) to fine-tune the origin +// with absolute encoders this protects you from exceeding the software min/max limits by > the amount specified +// in arc seconds..one encoder tick encoder resolution = 2^14 = 16380 +// 16380/360=45.5 ticks/deg; 45.5/60=0.7583 ticks/min; 0.7583/60 = .00126 ticks/sec +// or 1/0.7583 = 1.32 arc-min/tick; 1.32*60 sec = 79.2 arc sec per encoder tick +#ifndef ODRIVE_SYNC_LIMIT + #define ODRIVE_SYNC_LIMIT 80 // was OFF +#endif + +// Use OD_UART or OD_CAN...I2C may be added later +#ifndef ODRIVE_COMM_MODE + #define ODRIVE_COMM_MODE OD_CAN +#endif + +// ODrive axis 0 = OnStep Axis2 = DEC or ALT +// ODrive axis 1 = OnStep Axis1 = RA or AZM +#ifndef ODRIVE_SWAP_AXES + #define ODRIVE_SWAP_AXES ON +#endif + +#if ODRIVE_COMM_MODE == OD_UART + #include // https://github.com/odriverobotics/ODrive/tree/master/Arduino/ODriveArduino + // ODrive servo motor serial driver + extern ODriveArduino *_oDriveDriver; +#elif ODRIVE_COMM_MODE == OD_CAN + #include // https://github.com/tonton81/FlexCAN_T4.git + // changes were required to this CAN library so it is local now + #include "src/plugins/DDScope/ODriveTeensyCAN/ODriveTeensyCAN.h" //https://github.com/Malaphor/ODriveTeensyCAN.git + extern ODriveTeensyCAN *_oDriveDriver; +#endif + +typedef struct ODriveDriverSettings { + int16_t model; + int8_t status; +} ODriveDriverSettings; + +class ODriveMotor : public Motor { + public: + // constructor + ODriveMotor(uint8_t axisNumber, int8_t reverse, const ODriveDriverSettings *Settings, float radsPerCount, bool useFastHardwareTimers = true); + + // sets up the odrive motor + bool init(); + + // returns the number of parameters from the motor + uint8_t getParameterCount() { return Motor::getParameterCount() + numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { + if (number == Motor::getParameterCount() + 1) return &radsPerCount; else + if (number >= 1 && number <= Motor::getParameterCount()) return Motor::getParameter(number); else + return &invalid; + } + + // set motor reverse state + void setReverse(int8_t state); + + // sets motor enable on/off (if possible) + void enable(bool value); + + bool hasAbsoluteEncoder() const override { return ODRIVE_ABSOLUTE == ON; } + + // set instrument coordinate, in steps + void setInstrumentCoordinateSteps(long value); + + // get the associated driver status + inline DriverStatus getDriverStatus() { if (ready) return status; else return errorStatus; } + + // resets motor and target angular position in steps, also zeros backlash and index + void resetPositionSteps(long value); + + // get tracking mode steps per slewing mode step + inline int getStepsPerStepSlewing() { return 64; } + + // get movement frequency in steps per second + float getFrequencySteps(); + + // set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) + void setFrequencySteps(float frequency); + + // set slewing state (hint that we are about to slew or are done slewing) + void setSlewing(bool state); + + // get the motor name + const char* name() { return "ODRIVE"; } + + // updates PID and sets servo motor power/direction + void poll(); + + // sets dir as required and moves coord toward target at setFrequencySteps() rate + void move(); + + private: + +// float o_position0 = 0; +// float o_position1 = 0; + + #if ODRIVE_COMM_MODE == OD_UART + // special command to send high resolution position to odrive + void setPosition(int motor_number, float position) { +// if (motor_number == 0) o_position0 = position; +// if (motor_number == 1) o_position1 = position; + char command[32]; + sprintF(command, "p n %1.8f\n", position); + command[2] = '0' + motor_number; + ODRIVE_SERIAL.print(command); + } + #endif + + unsigned long lastSetPositionTime = 0; + uint8_t oDriveMonitorHandle = 0; + uint8_t taskHandle = 0; + + int stepSize = 1; // step size + volatile int homeSteps = 1; // step count for microstep sequence between home positions (driver indexer) + volatile bool takeStep = false; // should we take a step + + float currentFrequency = 0.0F; // last frequency set + float lastFrequency = 0.0F; // last frequency requested + unsigned long lastPeriod = 0; // last timer period (in sub-micros) + + volatile int absStep = 1; // absolute step size (unsigned) + + void (*callback)() = NULL; + + bool useFastHardwareTimers = true; + + bool isSlewing = false; + + DriverStatus status = { false, {false, false}, {false, false}, false, false, false, false }; + + // runtime adjustable settings + AxisParameter radsPerCount = {NAN, NAN, NAN, degToRadF(300.0), degToRadF(360000.0), AXP_FLOAT, AXPN_RADS_PER_COUNT}; + + const int numParameters = 1; + AxisParameter* parameter[2] = {&invalid, &radsPerCount}; +}; + +#endif +#endif diff --git a/src/lib/axis/motor/oDrive/ODriveCanPlus.cpp b/src/lib/axis/motor/oDrive/ODriveCanPlus.cpp new file mode 100755 index 0000000..55708a2 --- /dev/null +++ b/src/lib/axis/motor/oDrive/ODriveCanPlus.cpp @@ -0,0 +1,220 @@ +// ----------------------------------------------------------------------------------- +// axis oDrive servo motor CanPlus interface + +#include "ODriveCanPlus.h" + +#ifdef ODRIVE_NEW_MOTOR_PRESENT +#ifdef ODRIVE_MOTOR_PRESENT + +#include "../../../tasks/OnTask.h" +#include "../../../canPlus/CanPlus.h" + +static void odHb0(uint8_t d[8]) { odriveCan.onHeartbeat(0, d); } +static void odEnc0(uint8_t d[8]) { odriveCan.onFeedbackPosVel(0, d); } +static void odHb1(uint8_t d[8]) { odriveCan.onHeartbeat(1, d); } +static void odEnc1(uint8_t d[8]) { odriveCan.onFeedbackPosVel(1, d); } +static void odVer0(uint8_t d[8]) { odriveCan.onGetVersion(0, d); } +static void odVer1(uint8_t d[8]) { odriveCan.onGetVersion(1, d); } + +bool ODriveCanPlus::init(uint8_t node) { + if (!canPlus.ready) return false; + switch (node) { + case 0: + canPlus.callbackRegisterId(makeId(0, CMD_HEARTBEAT), odHb0); + canPlus.callbackRegisterId(makeId(0, CMD_GET_ENCODER_EST), odEnc0); + canPlus.callbackRegisterId(makeId(0, CMD_GET_VERSION), odVer0); + return true; + case 1: + canPlus.callbackRegisterId(makeId(1, CMD_HEARTBEAT), odHb1); + canPlus.callbackRegisterId(makeId(1, CMD_GET_ENCODER_EST), odEnc1); + canPlus.callbackRegisterId(makeId(1, CMD_GET_VERSION), odVer1); + return true; + default: + return false; + } +} + +bool ODriveCanPlus::getVersionSnapshot(uint8_t node, + uint8_t raw[8], + uint16_t& proto, + uint8_t& fw_major, + uint8_t& fw_minor, + uint8_t& fw_rev, + uint8_t& hw_variant, + uint8_t& hw_version) const { + if (node >= MaxNodes || raw == nullptr) return false; + + uint8_t seen; + noInterrupts(); + seen = cache_[node].seen; + if (!(seen & SEEN_VER)) { interrupts(); return false; } + + proto = cache_[node].proto_ver; + fw_major = cache_[node].fw_major; + fw_minor = cache_[node].fw_minor; + fw_rev = cache_[node].fw_rev; + hw_variant = cache_[node].hw_variant; + hw_version = cache_[node].hw_version; + for (int i = 0; i < 8; i++) raw[i] = cache_[node].ver_raw[i]; + interrupts(); + + return true; +} + +bool ODriveCanPlus::hasFreshVersion(uint8_t node, uint32_t max_age_ms) const { + if (node >= MaxNodes) return false; + uint8_t seen; + uint32_t t; + noInterrupts(); + seen = cache_[node].seen; + t = cache_[node].last_ver_ms; + interrupts(); + if (!(seen & SEEN_VER)) return false; + return (uint32_t)(millis() - t) <= max_age_ms; +} + +bool ODriveCanPlus::setAxisState(uint8_t node, uint32_t requested_state) { + if (!canPlus.ready || node >= MaxNodes) return false; + + uint8_t d[8] = {0}; + put_le32(&d[0], requested_state); + + canPlus.txWait(); + return canPlus.writePacket(makeId(node, CMD_SET_AXIS_STATE), d, 8) == 1; +} + +uint8_t ODriveCanPlus::axisState(uint8_t node) const { + if (node >= MaxNodes) return 0; + noInterrupts(); + uint8_t s = cache_[node].axis_state; + interrupts(); + return s; +} + +void ODriveCanPlus::setInputPos(uint8_t node, float pos_turns, float vel_ff_turns_s , float tq_ff_nm) { + if (!canPlus.ready || node >= MaxNodes) return; + + uint8_t d[8] = {0}; + + const int32_t vel_i = lroundf(vel_ff_turns_s * 1000.0F); + const int32_t tq_i = lroundf(tq_ff_nm * 1000.0F); + + put_f32(&d[0], pos_turns); + put_le16(&d[4], clamp_i16(vel_i)); + put_le16(&d[6], clamp_i16(tq_i)); + + if (canPlus.txTryLock()) { + canPlus.writePacket(makeId(node, CMD_SET_INPUT_POS), d, 8); + } +} + +float ODriveCanPlus::getPosTurns(uint8_t node) const { + if (node >= MaxNodes) return 0.0F; + noInterrupts(); + float p = cache_[node].pos_turns; + interrupts(); + return p; +} + +bool ODriveCanPlus::hasFreshTurns(uint8_t node, uint32_t max_age_ms) const { + if (node >= MaxNodes) return false; + uint8_t seen; + uint32_t t; + noInterrupts(); + seen = cache_[node].seen; + t = cache_[node].last_enc_ms; + interrupts(); + if (!(seen & SEEN_ENC)) return false; + return (uint32_t)(millis() - t) <= max_age_ms; +} + +bool ODriveCanPlus::hasHeartbeat(uint8_t node, uint32_t max_age_ms) const { + if (node >= MaxNodes) return false; + uint8_t seen; + uint32_t t; + noInterrupts(); + seen = cache_[node].seen; + t = cache_[node].last_hb_ms; + interrupts(); + if (!(seen & SEEN_HB)) return false; + return (uint32_t)(millis() - t) <= max_age_ms; +} + +uint32_t ODriveCanPlus::lastHeartbeatMs(uint8_t node) const { + if (node >= MaxNodes) return 0; + noInterrupts(); + uint32_t t = cache_[node].last_hb_ms; + interrupts(); + return t; +} + +uint32_t ODriveCanPlus::lastError(uint8_t node) const { + if (node >= MaxNodes) return 0; + noInterrupts(); + uint32_t e = cache_[node].last_err; + interrupts(); + return e; +} + +void ODriveCanPlus::onHeartbeat(uint8_t node, const uint8_t* d, uint8_t len) { + if (node >= MaxNodes || len < 7) return; + + cache_[node].last_hb_ms = millis(); + cache_[node].last_err = + (uint32_t(d[0]) ) | + (uint32_t(d[1]) << 8) | + (uint32_t(d[2]) << 16) | + (uint32_t(d[3]) << 24); + cache_[node].axis_state = d[4]; + + ATOMIC_OR(cache_[node].seen, SEEN_HB); + + // optionally store: + // cache_[node].procedure_result = d[5]; + // cache_[node].traj_done = d[6]; +} + +bool ODriveCanPlus::requestVersion(uint8_t node) { + if (!canPlus.ready || node >= MaxNodes) return false; + return canPlus.writePacketRtr(makeId(node, CMD_GET_VERSION), 8) == 1; +} + +bool ODriveCanPlus::requestTurns(uint8_t node) { + if (!canPlus.ready || node >= MaxNodes) return false; + return canPlus.writePacketRtr(makeId(node, CMD_GET_ENCODER_EST), 8) == 1; +} + +void ODriveCanPlus::onGetVersion(uint8_t node, const uint8_t* d, uint8_t len) { + if (node >= MaxNodes || len < 8) return; + + const uint32_t now = millis(); + + for (int i = 0; i < 8; i++) cache_[node].ver_raw[i] = d[i]; + + cache_[node].proto_ver = get_u16_le(&d[0]); + cache_[node].fw_major = d[2]; + cache_[node].fw_minor = d[3]; + cache_[node].fw_rev = d[4]; + cache_[node].hw_variant = d[5]; + cache_[node].hw_version = d[6]; + + cache_[node].last_ver_ms = now; // write time first + + ATOMIC_OR(cache_[node].seen, SEEN_VER); +} + +void ODriveCanPlus::onFeedbackPosVel(uint8_t node, const uint8_t* d, uint8_t len) { + if (node >= MaxNodes || len < 8) return; + + // Example: pos_turns (f32) + vel_turns/s (f32) + cache_[node].pos_turns = get_f32(d + 0); + cache_[node].vel_turns_s = get_f32(d + 4); + cache_[node].last_enc_ms = millis(); + + ATOMIC_OR(cache_[node].seen, SEEN_ENC); +} + +ODriveCanPlus odriveCan; + +#endif +#endif diff --git a/src/lib/axis/motor/oDrive/ODriveCanPlus.h b/src/lib/axis/motor/oDrive/ODriveCanPlus.h new file mode 100755 index 0000000..b4b119d --- /dev/null +++ b/src/lib/axis/motor/oDrive/ODriveCanPlus.h @@ -0,0 +1,113 @@ +// ----------------------------------------------------------------------------------- +// axis oDrive servo motor CanPlus interface +#pragma once +#include "../../../../Common.h" + +#ifdef ODRIVE_NEW_MOTOR_PRESENT +#ifdef ODRIVE_MOTOR_PRESENT + +class ODriveCanPlus { +public: + bool init(uint8_t node); + + bool getVersionSnapshot(uint8_t node, + uint8_t raw[8], + uint16_t& proto, + uint8_t& fw_major, + uint8_t& fw_minor, + uint8_t& fw_rev, + uint8_t& hw_variant, + uint8_t& hw_version) const; + + bool setAxisState(uint8_t node, uint32_t requested_state); + uint8_t axisState(uint8_t node) const; + + void setInputPos(uint8_t node, float pos_turns, float vel_ff_turns_s = 0.0F, float tq_ff_nm = 0.0F); + float getPosTurns(uint8_t node) const; // cached feedback + bool hasFreshTurns(uint8_t node, uint32_t max_age_ms = 250U) const; + + void onGetVersion(uint8_t node, const uint8_t* d, uint8_t len = 8); + bool hasFreshVersion(uint8_t node, uint32_t max_age_ms = 1000U) const; + + bool hasHeartbeat(uint8_t node, uint32_t max_age_ms = 250U) const; + uint32_t lastHeartbeatMs(uint8_t node) const; + uint32_t lastError(uint8_t node) const; + + void onHeartbeat(uint8_t node, const uint8_t* d, uint8_t len = 8); + void onFeedbackPosVel(uint8_t node, const uint8_t* d, uint8_t len = 8); + + bool requestVersion(uint8_t node); + bool requestTurns(uint8_t node); + +private: + + // helpers + constexpr uint8_t CmdBits = 5; + static inline uint16_t makeId(uint8_t node, uint8_t cmd) { return (uint16_t(node) << CmdBits) | (cmd & 0x1F); } + + static inline void put_le16(uint8_t* p, int16_t v) { p[0] = uint8_t(v & 0xFF); p[1] = uint8_t((v >> 8) & 0xFF); } + static inline void put_le32(uint8_t* p, uint32_t v) { + p[0]=(uint8_t)v; + p[1]=(uint8_t)(v>>8); + p[2]=(uint8_t)(v>>16); + p[3]=(uint8_t)(v>>24); + } + static inline void put_f32(uint8_t* p, float f) { + static_assert(sizeof(float) == 4, "float must be 32-bit"); + uint32_t u; + memcpy(&u, &f, 4); + put_le32(p, u); + } + + static inline float get_f32(const uint8_t* p) { + uint32_t u = (uint32_t(p[0])) | + (uint32_t(p[1]) << 8) | + (uint32_t(p[2]) << 16) | + (uint32_t(p[3]) << 24); + float f; + memcpy(&f, &u, 4); + return f; + } + static inline uint16_t get_u16_le(const uint8_t* p) { return (uint16_t)p[0] | ((uint16_t)p[1] << 8); } + + static inline int16_t clamp_i16(int32_t x) { + if (x > 32767) return 32767; + if (x < -32768) return -32768; + return (int16_t)x; + } + + // ODrive CMD IDs + static constexpr uint8_t CMD_GET_VERSION = 0x00; + static constexpr uint8_t CMD_HEARTBEAT = 0x01; + static constexpr uint8_t CMD_SET_AXIS_STATE = 0x07; + static constexpr uint8_t CMD_SET_INPUT_POS = 0x0C; + static constexpr uint8_t CMD_GET_ENCODER_EST = 0x09; + + // startup state + static constexpr uint8_t SEEN_HB = 0x01; + static constexpr uint8_t SEEN_ENC = 0x02; + static constexpr uint8_t SEEN_VER = 0x04; + + struct AxisCache { + volatile float pos_turns = 0.0F; + volatile float vel_turns_s = 0.0F; + volatile uint32_t last_hb_ms = 0; + volatile uint32_t last_err = 0; + volatile uint8_t axis_state = 0; + volatile uint32_t last_enc_ms = 0; + volatile uint32_t last_ver_ms = 0; + volatile uint8_t ver_raw[8] = {0}; + volatile uint16_t proto_ver = 0; + volatile uint8_t fw_major = 0, fw_minor = 0, fw_rev = 0; + volatile uint8_t hw_variant = 0, hw_version = 0; + volatile uint8_t seen = 0; + }; + + static constexpr uint8_t MaxNodes = 2; + AxisCache cache_[MaxNodes]; +}; + +extern ODriveCanPlus odriveCan; + +#endif +#endif diff --git a/src/lib/axis/motor/oDrive/ODriveNew.cpp b/src/lib/axis/motor/oDrive/ODriveNew.cpp new file mode 100755 index 0000000..db6217b --- /dev/null +++ b/src/lib/axis/motor/oDrive/ODriveNew.cpp @@ -0,0 +1,437 @@ +// ----------------------------------------------------------------------------------- +// oDrive servo motor +// EXPERIMENTAL!!! + +#include "ODriveNew.h" + +#ifdef ODRIVE_NEW_MOTOR_PRESENT +#ifdef ODRIVE_MOTOR_PRESENT + +#include "../../../tasks/OnTask.h" +#include "../../../gpioEx/GpioEx.h" + +#include "ODriveCanPlus.h" + +// constructor +ODriveMotor::ODriveMotor(uint8_t axisNumber, int8_t reverse, const ODriveDriverSettings *Settings, float countsPerRad, bool useFastHardwareTimers) + :Motor(axisNumber, reverse) { + if (axisNumber < 1 || axisNumber > 2) return; + + driverType = ODRIVER; + + this->axisNumber = axisNumber; + this->useFastHardwareTimers = useFastHardwareTimers; + + #if ODRIVE_SWAP_AXES == ON + oDriveNode = 2 - axisNumber; + #else + oDriveNode = axisNumber - 1; + #endif + + strcpy(axisPrefix, " Axis_oDrive, "); + axisPrefix[5] = '0' + this->axisNumber; + + // set parameter default values during object creation + this->countsPerRad.valueDefault = countsPerRad; + + // attach the function pointers to the callbacks + odriveMotorInstance[axisNumber - 1] = this; + switch (this->axisNumber) { + case 1: callback = moveODriveMotorAxis1; break; + case 2: callback = moveODriveMotorAxis2; break; + } +} + +bool ODriveMotor::init() { + if (!Motor::init()) return false; + + // reset the ODrive(s) via h/w pin + if (axisNumber == 1) { + pinModeEx(ODRIVE_RST_PIN, OUTPUT); + digitalWriteEx(ODRIVE_RST_PIN, LOW); + tasks.yield(10); + digitalWriteEx(ODRIVE_RST_PIN, HIGH); + + // allow time for boot + tasks.yield(1000); + } + + if (!odriveCan.init(oDriveNode)) return false; + + // request version info and + // let CAN poll task (priority 3) run to deliver the reply + odriveCan.requestVersion(oDriveNode); + const uint32_t t0 = millis(); + while (!odriveCan.hasFreshVersion(oDriveNode, 1000) && (uint32_t)(millis() - t0) < 1000U) { tasks.yield(10); } + + if (odriveCan.hasFreshVersion(oDriveNode, 1000)) { + uint8_t raw[8]; + uint16_t proto; + uint8_t maj, min, rev, hwv, hww; + + if (odriveCan.getVersionSnapshot(oDriveNode, raw, proto, maj, min, rev, hwv, hww)) { + VF("MSG:"); V(axisPrefix); VF("ODrive version: node "); V(oDriveNode); + VF(" proto "); V(proto); + VF(" fw "); V(maj); VF("."); V(min); VF("."); V(rev); + VF(" hwVar "); V(hwv); + VF(" hwVer "); V(hww); + VLF(""); + + VF("MSG:"); V(axisPrefix); VF("ODrive version raw: "); + for (int i = 0; i < 8; i++) { + if (raw[i] < 16) V("0"); + VH(raw[i]); + if (i != 7) V(" "); + } + VLF(""); + } + } + + // idle the ODrive + odriveCan.setAxisState(oDriveNode, AXIS_STATE_IDLE); + + // request an encoder position update and + // let CAN poll task (priority 3) run to deliver the reply + odriveCan.requestTurns(oDriveNode); + lastEncoderUpdateRequestTime = millis(); + tasks.yield(250); + + // require an encoder sample + if (!odriveCan.hasFreshTurns(oDriveNode, 250)) { + VF("MSG:"); V(axisPrefix); VLF("no encoder reply at startup"); + ready = false; + return false; + } + + // local state: definitely disabled, no transition pending + ready = true; + enabled = false; + enablePending = false; + enableTarget = false; + enableDeadlineMs = 0; + + // start the motion timer + VF("MSG:"); V(axisPrefix); VF("start task to synthesize motion... "); + char timerName[] = "Ax_ODr"; + timerName[2] = axisNumber + '0'; + taskHandle = tasks.add(0, 0, true, 0, callback, timerName); + if (taskHandle) { + VF("success"); + if (useFastHardwareTimers) { + if (!tasks.requestHardwareTimer(taskHandle, 0)) { + VF(" (no hardware timer!)"); + } else { + maxFrequency = (1000000.0F/HAL_MAXRATE_LOWER_LIMIT)/2.0F; + }; + } + VLF(""); + } else { + VLF("FAILED!"); + ready = false; + return false; + } + + stopSyntheticMotion(); + + return true; +} + +// set motor reverse state +void ODriveMotor::setReverse(bool state) { + if (!ready) return; + + if (state) { + VF("MSG:"); V(axisPrefix); VLF("axis reversal must be accomplished with hardware or ODrive setup!"); + } +} + +// sets motor enable on/off (if possible) +void ODriveMotor::enable(bool state) { + if (!ready) return; + + // If we're already in the requested steady-state AND no transition is pending, do nothing. + if (!enablePending && (state == enabled)) return; + + // Always reset local motion first + if (!state) enabled = false; + stopSyntheticMotion(); + resetToTrackingBaseline(); + + const uint32_t requestedState = state ? AXIS_STATE_CLOSED_LOOP_CONTROL + : AXIS_STATE_IDLE; + + if (!odriveCan.setAxisState(oDriveNode, requestedState)) { + enabled = false; + enablePending = false; + return; + } + + if (!state) { + // disable: treat as immediate locally; no need to wait for heartbeats/encoder + enabled = false; + enablePending = false; + enableTarget = false; + enableDeadlineMs = 0; + return; + } + + // Enable: wait for confirmation + enablePending = true; + enableTarget = true; + enableDeadlineMs = millis() + 500; +} + +void ODriveMotor::setInstrumentCoordinateSteps(long value) { + if (!ready) return; + + #if ODRIVE_ABSOLUTE == ON && ODRIVE_SYNC_LIMIT != OFF + long index = value - ATOMIC_LOAD(motorSteps); + float indexRad = index/countsPerRad.value; + if (indexRad >= -degToRadF(ODRIVE_SYNC_LIMIT/3600.0F) && indexRad <= degToRadF(ODRIVE_SYNC_LIMIT/3600.0F)) + #endif + + Motor::setInstrumentCoordinateSteps(value); +} + +// resets motor and target angular position in steps, also zeros backlash and index +void ODriveMotor::resetPositionSteps(long value) { + if (!ready) return; + + // make sure we have a recent encoder reading + if (!odriveCan.hasFreshTurns(oDriveNode, 250)) { + status.fault = true; + odriveCan.requestTurns(oDriveNode); + return; + } else { + status.fault = false; + } + + // Convert turns -> steps (countsPerRad is counts-per-radian) + const float posTurns = odriveCan.getPosTurns(oDriveNode); + const long oPositionSteps = lroundf(posTurns * TWO_PI * countsPerRad.value); + + noInterrupts(); + motorSteps = oPositionSteps; + targetSteps = motorSteps; + + #if ODRIVE_ABSOLUTE == OFF + // For incremental encoders, keep an index offset so OnStepX can still align to "value". + indexSteps = value - motorSteps; + #else + UNUSED(value); + indexSteps = 0; + #endif + + backlashSteps = 0; + interrupts(); +} + +// set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) +void ODriveMotor::setFrequencySteps(float frequency) { + if (!ready) return; + + if (!enabled) { stopSyntheticMotion(); return; } + + // negative frequency, convert to positive and reverse the direction + int dir = 0; + if (frequency > 0.0F) dir = 1; else if (frequency < 0.0F) { frequency = -frequency; dir = -1; } + + // if in backlash override the frequency + if (inBacklash) frequency = backlashFrequency; + + if (frequency != currentFrequency) { + if (frequency < maxFrequency) stepSize = 1; else + if (frequency < maxFrequency*2) stepSize = 2; else + if (frequency < maxFrequency*4) stepSize = 4; else + if (frequency < maxFrequency*8) stepSize = 8; else + if (frequency < maxFrequency*16) stepSize = 16; else + if (frequency < maxFrequency*32) stepSize = 32; else + if (frequency < maxFrequency*64) stepSize = 64; else + if (frequency < maxFrequency*128) stepSize = 128; else stepSize = 256; + + // timer period in microseconds + float period = (1000000.0F*stepSize)/frequency; + + // range is 0 to 134 seconds/step + if (!isnan(period) && period <= 130000000.0F) { + period *= 16.0F; + lastPeriod = (unsigned long)lroundf(period); + } else { + lastPeriod = 0; + frequency = 0.0F; + dir = 0; + } + + currentFrequency = frequency; + + // change the motor rate/direction + noInterrupts(); + step = 0; + interrupts(); + tasks.setPeriodSubMicros(taskHandle, lastPeriod); + } + + noInterrupts(); + step = dir * stepSize; + absStep = (int)labs(step); + interrupts(); + + if (dir == 0 || lastPeriod == 0 || frequency == 0.0F) { + feedForwardVelocity = 0.0F; + } else { + feedForwardVelocity = (dir > 0) ? frequency : -frequency; + } +} + +float ODriveMotor::getFrequencySteps() { + if (!ready) return 0.0F; + + if (lastPeriod == 0) return 0.0F; + return (16000000.0F / lastPeriod) * absStep; +} + +// set slewing state (hint that we are about to slew or are done slewing) +void ODriveMotor::setSlewing(bool state) { + if (!ready) return; + + isSlewing = state; +} + +// updates PID and sets odrive position +void ODriveMotor::poll() { + const uint32_t now = millis(); + + // grab a new encoder position update at 10Hz + if ((uint32_t)(now - lastEncoderUpdateRequestTime) >= 100U) { + odriveCan.requestTurns(oDriveNode); + lastEncoderUpdateRequestTime = now; + } + + // ------------------------- + // handle enable/disable handshake (async confirmation) + // ------------------------- + if (enablePending) { + // snapshot ODrive status + const uint8_t st = odriveCan.axisState(oDriveNode); + const uint32_t err = odriveCan.lastError(oDriveNode); + + // hard faults during transition: lose heartbeat or error latched + if (!odriveCan.hasHeartbeat(oDriveNode, 250) || (err != 0)) { + enabled = false; + enablePending = false; + stopSyntheticMotion(); + resetToTrackingBaseline(); + return; + } + + // commit when we observe the desired steady state + if (enableTarget) { + if (st == AXIS_STATE_CLOSED_LOOP_CONTROL) { + enabled = true; + enablePending = false; + } + } else { + if (st == AXIS_STATE_IDLE) { + enabled = false; + enablePending = false; + } + } + + // deadline: if still pending after the window, fail closed + if (enablePending && (int32_t)(now - enableDeadlineMs) >= 0) { + enabled = false; + enablePending = false; + stopSyntheticMotion(); + resetToTrackingBaseline(); + return; + } + } + + // ------------------------- + // normal operation (only when truly enabled) + // ------------------------- + if (!ready || !enabled) return; + + // fail closed if we lose comms or an error appears while running + // (no encoder freshness requirement in this variant) + if (!odriveCan.hasHeartbeat(oDriveNode, 250) || (odriveCan.lastError(oDriveNode) != 0)) { + enabled = false; + enablePending = false; + stopSyntheticMotion(); + resetToTrackingBaseline(); + return; + } + + // rate-limit position commands + if (now - lastSetPositionTime < ODRIVE_UPDATE_MS) return; + lastSetPositionTime = now; + + long target; + noInterrupts(); + #if ODRIVE_SLEW_DIRECT == ON + target = targetSteps + backlashSteps; + #else + target = motorSteps + backlashSteps; + #endif + interrupts(); + + float vel_ff_turns_s = feedForwardVelocity/(TWO_PI*countsPerRad.value); + + odriveCan.setInputPos(oDriveNode, target/(TWO_PI*countsPerRad.value), vel_ff_turns_s); +} + +void ODriveMotor::stopSyntheticMotion() { + feedForwardVelocity = 0.0F; + + if (lastPeriod == 0 && step == 0 && absStep == 0) return; + currentFrequency = 0.0F; + lastPeriod = 0; + feedForwardVelocity = 0.0F; + noInterrupts(); + step = 0; + absStep = 0; + interrupts(); + if (taskHandle) tasks.setPeriodSubMicros(taskHandle, 0); +} + +void ODriveMotor::resetToTrackingBaseline() { + noInterrupts(); + step = 0; + absStep = 0; + inBacklash = false; + backlashSteps = 0; + interrupts(); + + currentFrequency = 0.0F; + lastPeriod = 0; + stepSize = 1; + feedForwardVelocity = 0.0F; +} + +// sets dir as required and moves coord toward target at setFrequencySteps() rate +IRAM_ATTR void ODriveMotor::move() { + if (sync && !inBacklash) targetSteps += step; + + if (motorSteps > targetSteps) { + if (backlashSteps > 0) { + backlashSteps -= absStep; + inBacklash = backlashSteps > 0; + } else { + motorSteps -= absStep; + inBacklash = false; + } + } else + + if (motorSteps < targetSteps || inBacklash) { + if (backlashSteps < backlashAmountSteps) { + backlashSteps += absStep; + inBacklash = backlashSteps < backlashAmountSteps; + } else { + motorSteps += absStep; + inBacklash = false; + } + } +} + +#endif +#endif \ No newline at end of file diff --git a/src/lib/axis/motor/oDrive/ODriveNew.h b/src/lib/axis/motor/oDrive/ODriveNew.h new file mode 100755 index 0000000..058b7ed --- /dev/null +++ b/src/lib/axis/motor/oDrive/ODriveNew.h @@ -0,0 +1,151 @@ +// ----------------------------------------------------------------------------------- +// oDrive servo motor +// EXPERIMENTAL!!! +#pragma once +#include "../../../../Common.h" + +#ifdef ODRIVE_NEW_MOTOR_PRESENT +#ifdef ODRIVE_MOTOR_PRESENT + +#include "../Motor.h" +#include "../../../convert/Convert.h" + +#ifndef ODRIVE_RST_PIN + #define ODRIVE_RST_PIN 3 +#endif + +// odrive update rate default 40Hz +#ifndef ODRIVE_UPDATE_MS + #define ODRIVE_UPDATE_MS CAN_SEND_RATE_MS // normally > about 5ms and < 100ms +#endif + +// odrive direct slewing ON or OFF (ODrive handles acceleration) +// ON to use ODrive trapezoidal move profile, OFF for OnStep move profile +#ifndef ODRIVE_SLEW_DIRECT + #define ODRIVE_SLEW_DIRECT OFF +#endif + +// odrive if using absolute encoders set to ON +#ifndef ODRIVE_ABSOLUTE + #define ODRIVE_ABSOLUTE ON // was OFF +#endif + +// odrive sync limit (for absolute encoders) OFF or specify the sync limit in arc-seconds +// this should, I hope, allow you to limit just how far from the encoders you can sync. (+/-) to fine-tune the origin +// with absolute encoders this protects you from exceeding the software min/max limits by > the amount specified +// in arc seconds..one encoder tick encoder resolution = 2^14 = 16384 +// 16384/360=45.5 ticks/deg; 45.5/60=0.7583 ticks/min; 0.7583/60 = .00126 ticks/sec +// or 1/0.7583 = 1.32 arc-min/tick; 1.32*60 sec = 79.2 arc sec per encoder tick +#ifndef ODRIVE_SYNC_LIMIT + #define ODRIVE_SYNC_LIMIT 80 // was OFF +#endif + +// Use OD_UART or OD_CAN...I2C may be added later +#ifndef ODRIVE_COMM_MODE + #define ODRIVE_COMM_MODE OD_CAN +#endif + +// ODrive axis 0 = OnStep Axis2 = DEC or ALT +// ODrive axis 1 = OnStep Axis1 = RA or AZM +#ifndef ODRIVE_SWAP_AXES + #define ODRIVE_SWAP_AXES ON +#endif + +typedef struct ODriveDriverSettings { + int16_t model; + int8_t status; +} ODriveDriverSettings; + +class ODriveMotor : public Motor { + public: + // constructor + ODriveMotor(uint8_t axisNumber, int8_t reverse, const ODriveDriverSettings *Settings, float countsPerRad, bool useFastHardwareTimers = true); + + // sets up the odrive motor + bool init(); + + // returns the number of parameters from the motor + uint8_t getParameterCount() { return Motor::getParameterCount() + numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { + if (number == Motor::getParameterCount() + 1) return &countsPerRad; else + if (number >= 1 && number <= Motor::getParameterCount()) return Motor::getParameter(number); else + return &invalid; + } + + // set motor reverse state + void setReverse(bool state); + + // sets motor enable on/off (if possible) + void enable(bool value); + + bool hasAbsoluteEncoder() const override { return ODRIVE_ABSOLUTE == ON; } + + // set instrument coordinate, in steps + void setInstrumentCoordinateSteps(long value); + + // get the associated driver status + inline DriverStatus getDriverStatus() { if (ready) return status; else return errorStatus; } + + // resets motor and target angular position in steps, also zeros backlash and index + void resetPositionSteps(long value); + + // get tracking mode steps per slewing mode step + inline int getStepsPerStepSlewing() { return 64; } + + // get movement frequency in steps per second + float getFrequencySteps(); + + // set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) + void setFrequencySteps(float frequency); + + // set slewing state (hint that we are about to slew or are done slewing) + void setSlewing(bool state); + + // get the motor name + const char* name() { return "ODRIVE"; } + + // updates PID and sets servo motor power/direction + void poll(); + + // sets dir as required and moves coord toward target at setFrequencySteps() rate + void move(); + + private: + void stopSyntheticMotion(); + void resetToTrackingBaseline(); + + float feedForwardVelocity = 0.0F; + unsigned long lastSetPositionTime = 0; + unsigned long lastEncoderUpdateRequestTime = 0; + uint8_t oDriveNode = 0; // ODrive node for this mount axis (swapped if ODRIVE_SWAP_AXES==ON) + uint8_t taskHandle = 0; + + int stepSize = 1; // step size + + unsigned long lastPeriod = 0; // last timer period (in sub-micros) + float maxFrequency = HAL_FRACTIONAL_SEC; // fastest timer rate + + volatile int absStep = 0; // absolute step size (unsigned) + + void (*callback)() = NULL; + + bool useFastHardwareTimers = true; + + bool enablePending = false; + bool enableTarget = false; + uint32_t enableDeadlineMs = 0; + bool isSlewing = false; + + DriverStatus status = { false, {false, false}, {false, false}, false, false, false, false }; + + // runtime adjustable settings + AxisParameter countsPerRad = {NAN, NAN, NAN, 300.0*RAD_DEG_RATIO_F, 360000.0*RAD_DEG_RATIO_F, AXP_FLOAT, AXPN_COUNTS_PER_RAD}; + + const int numParameters = 1; + AxisParameter* parameter[2] = {&invalid, &countsPerRad}; +}; + +#endif +#endif diff --git a/src/lib/axis/motor/servo/Servo.cpp b/src/lib/axis/motor/servo/Servo.cpp new file mode 100755 index 0000000..818477a --- /dev/null +++ b/src/lib/axis/motor/servo/Servo.cpp @@ -0,0 +1,525 @@ +// ----------------------------------------------------------------------------------- +// axis servo motor + +#include "Servo.h" + +#ifdef SERVO_MOTOR_PRESENT + +#include "../../../tasks/OnTask.h" +#include "../Motor.h" + +static ServoMotor *servoMotorInstance[9]; +IRAM_ATTR void moveServoMotorAxis1() { servoMotorInstance[0]->move(); } +IRAM_ATTR void moveServoMotorAxis2() { servoMotorInstance[1]->move(); } +IRAM_ATTR void moveServoMotorAxis3() { servoMotorInstance[2]->move(); } +IRAM_ATTR void moveServoMotorAxis4() { servoMotorInstance[3]->move(); } +IRAM_ATTR void moveServoMotorAxis5() { servoMotorInstance[4]->move(); } +IRAM_ATTR void moveServoMotorAxis6() { servoMotorInstance[5]->move(); } +IRAM_ATTR void moveServoMotorAxis7() { servoMotorInstance[6]->move(); } +IRAM_ATTR void moveServoMotorAxis8() { servoMotorInstance[7]->move(); } +IRAM_ATTR void moveServoMotorAxis9() { servoMotorInstance[8]->move(); } + +// constructor +ServoMotor::ServoMotor(uint8_t axisNumber, int8_t reverse, + ServoDriver *Driver, Filter *filter, + Encoder *encoder, uint32_t encoderOrigin, bool encoderReverse, + Feedback *feedback, ServoControl *control, + bool useFastHardwareTimers) + :Motor(axisNumber, reverse) { + if (axisNumber < 1 || axisNumber > 9) return; + + driverType = SERVO; + + strcpy(axisPrefix, " Axis_Servo, "); + axisPrefix[5] = '0' + axisNumber; + + this->filter = filter; + this->encoder = encoder; + this->feedback = feedback; + this->control = control; + + if (axisNumber > 2) useFastHardwareTimers = false; + this->useFastHardwareTimers = useFastHardwareTimers; + + this->driver = Driver; + + this->encoderOrigin = encoderOrigin; + this->encoderReverse = encoderReverse; + this->encoderReverseDefault = encoderReverse; + + // attach the function pointers to the callbacks + servoMotorInstance[axisNumber - 1] = this; + switch (axisNumber) { + case 1: callback = moveServoMotorAxis1; break; + case 2: callback = moveServoMotorAxis2; break; + case 3: callback = moveServoMotorAxis3; break; + case 4: callback = moveServoMotorAxis4; break; + case 5: callback = moveServoMotorAxis5; break; + case 6: callback = moveServoMotorAxis6; break; + case 7: callback = moveServoMotorAxis7; break; + case 8: callback = moveServoMotorAxis8; break; + case 9: callback = moveServoMotorAxis9; break; + } +} + +bool ServoMotor::init() { + if (!Motor::init()) return false; + + #ifdef CALIBRATE_SERVO_DC + calibrateVelocity = new ServoCalibrateTrackingVelocity(axisNumber); + driver->setTrackingMode(true); + driver->setBypassAccelOnTracking(true); + slewing = false; + #endif + + if (encoder->supportsTimeAlignedMotorSteps()) { encoder->setMotorStepsPtr(&motorSteps); } + + if (!encoder->init()) { DF("ERR:"); D(axisPrefix); DLF("no encoder!"); return false; } + + encoder->setOrigin(encoderOrigin); + + if (!driver->init(normalizedReverse)) { DF("ERR:"); D(axisPrefix); DLF("no motor driver!"); return false; } + + driver->enable(false); + + // get the feedback control loop ready + feedback->init(axisNumber, control); + feedback->reset(); + trackingFrequency = (AXIS1_STEPS_PER_DEGREE/240.0F)*SIDEREAL_RATIO_F; + + // start the motion timer + VF("MSG:"); V(axisPrefix); VF("start task to synthesize motion... "); + char timerName[] = "Ax_Svo"; + timerName[2] = '0' + axisNumber; + taskHandle = tasks.add(0, 0, true, 0, callback, timerName); + if (taskHandle) { + VF("success"); + if (useFastHardwareTimers) { + if (!tasks.requestHardwareTimer(taskHandle, 0)) { + VF(" (no hardware timer!)"); + } else { + maxFrequency = (1000000.0F/HAL_MAXRATE_LOWER_LIMIT)/2.0F; + }; + } + VLF(""); + } else { + VLF("FAILED!"); + return false; + } + + ready = true; + return true; +} + +void ServoMotor::setReverse(int8_t state) { + if (!ready) return; + + feedback->setControlDirection(state); + if (state == ON) encoderReverse = encoderReverseDefault; else encoderReverse = !encoderReverseDefault; +} + +void ServoMotor::enable(bool state) { + if (!ready || state == enabled) return; + + if (state) { + driver->enable(true); // power up first + feedback->reset(); // clean start (PID state) + safetyShutdown = false; + + stopSyntheticMotion(); // stop ISR/task + resetToTrackingBaseline(); // clear backlash/step state + enabled = true; + } else { + enabled = false; // close window + stopSyntheticMotion(); // stop ISR/task + resetToTrackingBaseline(); // clear backlash/step state + + feedback->reset(); // clear PID state + driver->enable(false); // then power down + } + + #ifdef CALIBRATE_SERVO_DC + if (enabled && !encoder->isVirtual) { + calibrateVelocity->start(trackingFrequency, getInstrumentCoordinateSteps()); + } + #endif +} + +DriverStatus ServoMotor::getDriverStatus() { + if (!ready) return errorStatus; + + DriverStatus driverStatus; + if (ready) { driver->updateStatus(); driverStatus = driver->getStatus(); } else driverStatus.fault = true; + if (encoder->errorThresholdExceeded()) driverStatus.fault = true; + if (safetyShutdown) driverStatus.fault = true; + return driverStatus; +} + +// resets motor and target angular position in steps, also zeros backlash and index +void ServoMotor::resetPositionSteps(long value) { + if (!ready) return; + + Motor::resetPositionSteps(value); + if (!encoder->isAbsolute()) { + encoder->write(value); + } else { + VF("MSG:"); V(axisPrefix); VL("absolute encoder ignored reset position"); + } +} + +// get instrument coordinate, in steps +long ServoMotor::getInstrumentCoordinateSteps() { + if (!ready) return 0; + + return encoderRead() + indexSteps; +} + +// set instrument coordinate, in steps +void ServoMotor::setInstrumentCoordinateSteps(long value) { + bool startupOriginState = indexSteps == 0 && motorSteps == 0 && targetSteps == 0 && backlashSteps == 0; + + noInterrupts(); + indexSteps = value - motorSteps; + interrupts(); + + if (encoder->isAbsolute() && startupOriginState) homeSet = true; +} + +// distance to target in steps (+/-) +long ServoMotor::getTargetDistanceSteps() { + if (!ready) return 0; + + long encoderCounts = encoderRead(); + noInterrupts(); + long dist = targetSteps - encoderCounts; + interrupts(); + return dist; +} + +// set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) +void ServoMotor::setFrequencySteps(float frequency) { + if (!ready) return; + + if (!enabled) { stopSyntheticMotion(); return; } + + // negative frequency, convert to positive and reverse the direction + int dir = 0; + if (frequency > 0.0F) dir = 1; else if (frequency < 0.0F) { frequency = -frequency; dir = -1; } + + // if in backlash override the frequency + if (inBacklash) frequency = backlashFrequency; + + if (frequency != currentFrequency) { + // compensate for performace limitations by taking larger steps as needed + if (frequency < maxFrequency) stepSize = 1; else + if (frequency < maxFrequency*2) stepSize = 2; else + if (frequency < maxFrequency*4) stepSize = 4; else + if (frequency < maxFrequency*8) stepSize = 8; else + if (frequency < maxFrequency*16) stepSize = 16; else + if (frequency < maxFrequency*32) stepSize = 32; else + if (frequency < maxFrequency*64) stepSize = 64; else + if (frequency < maxFrequency*128) stepSize = 128; else stepSize = 256; + + // timer period in microseconds + float period = (1000000.0F*stepSize)/frequency; + + // range is 0 to 134 seconds/step + if (!isnan(period) && period <= 130000000.0F) { + period *= 16.0F; + lastPeriod = (unsigned long)lroundf(period); + } else { + lastPeriod = 0; + frequency = 0.0F; + dir = 0; + } + + currentFrequency = frequency; + currentDirection = dir; + + // change the motor rate/direction + noInterrupts(); + step = 0; + interrupts(); + tasks.setPeriodSubMicros(taskHandle, lastPeriod); + } + + noInterrupts(); + step = dir * stepSize; + absStep = abs(step); + interrupts(); +} + +float ServoMotor::getFrequencySteps() { + if (!ready) return 0; + + if (lastPeriod == 0) return 0; + return (16000000.0F / lastPeriod) * absStep; +} + +// set slewing state (hint that we are about to slew or are done slewing) +void ServoMotor::setSlewing(bool state) { + if (!ready) return; + + slewing = state; +} + +// set zero/origin of absolute encoders +uint32_t ServoMotor::encoderZero() { + if (!ready) return 0; + + encoder->origin = 0; + encoder->index = 0; + + uint32_t zero = (uint32_t)(-encoder->read()); + encoder->origin = zero; + + return zero; +} + +int32_t ServoMotor::encoderRead() { + int32_t encoderCounts = encoder->read(); + if (encoderReverse) encoderCounts = -encoderCounts; + return encoderCounts; +} + +// updates PID and sets servo motor power/direction +void ServoMotor::poll() { + int32_t encoderCounts = encoder->read(); + if (encoderReverse) encoderCounts = -encoderCounts; + + // for absolute encoders initialize the motor position at startup + if (encoder->isAbsolute()) { + if (!motorStepsInitDone && homeSet) { + noInterrupts(); + motorSteps = encoderCounts; + targetSteps = encoderCounts; + backlashSteps = 0; + interrupts(); + motorStepsInitDone = true; + } + } + + long motorCounts; + if (encoder->hasMotorStepsAtLastRead()) { + motorCounts = encoder->motorStepsAtLastRead(); + } else { + noInterrupts(); + motorCounts = motorSteps; + interrupts(); + } + + long unfilteredEncoderCounts = encoderCounts; + UNUSED(unfilteredEncoderCounts); + + // find encoder velocity + float encoderVelocity = encoder->readVelocityCps(); + if (encoderReverse) encoderVelocity = -encoderVelocity; + if (isnan(encoderVelocity)) { + const uint32_t nowUs = micros(); + + if (lastUs == 0) { + lastUs = nowUs; + lastEnc = unfilteredEncoderCounts; + encoderVelocity = 0.0F; + } else { + uint32_t dtUs = nowUs - lastUs; + if (dtUs == 0) dtUs = 1; + encoderVelocity = (unfilteredEncoderCounts - lastEnc)*(1000000.0F/(float)dtUs); + lastEnc = unfilteredEncoderCounts; + lastUs = nowUs; + } + } + + // Eq mount tracking? + bool isTracking = (axisNumber == 1) && (fabsf(currentFrequency - trackingFrequency) < trackingFrequency*0.1F); + + encoderCounts = filter->update(encoderCounts, motorCounts, isTracking); + + control->set = motorCounts; + control->in = encoderCounts; + float velocity; + if (enabled) { + // directly use fixed PWM value during calibration + #ifdef CALIBRATE_SERVO_DC + if (calibrateVelocity->experimentMode) { + // Get unfiltered counts + calibrateVelocity->updateState(unfilteredEncoderCounts); + // experimentVel is in PERCENT of max velocity, convert to cps + velocity = (calibrateVelocity->experimentVelocity/100.0F)*velocityMax; + // disable the PID // or feedback->zeroOutputs() + control->out = 0.0f; // ensure PID output doesn't leak in + } else { + // your normal control path; keep whatever you used before + velocity = control->out + currentDirection*currentFrequency; + } + #else + feedback->poll(); + velocity = control->out + currentDirection*currentFrequency; + #endif + + } else velocity = 0.0F; + + // for virtual encoders set the velocity and direction + if (encoder->isVirtual) { + encoderDirection = velocity < 0.0F ? -1 : 1; + encoder->setVelocity(abs(velocity)); + encoder->setDirection(&encoderDirection); + } + +// const float vmax = velocityMax; +// v_eff = driver->setMotorVelocity(velocity, encoderVelocity)*100.0F; +// velocityPercent = (vmax > 0.0F) ? (v_eff/vmax)*100.0F : 0.0F; + velocityPercent = (driver->setMotorVelocity(velocity, encoderVelocity)/velocityMax)*100.0F; + if (driver->getMotorDirection() == DIR_FORWARD) control->directionHint = 1; else control->directionHint = -1; + + const unsigned long now = millis(); + + if (feedback->manuallySwitchParameters) { + if (!slewing && enabled) { + if (now - lastSlewingTime >= SERVO_SLEWING_TO_TRACKING_DELAY) feedback->selectTrackingParameters(); else feedback->selectSlewingParameters(); + } else { + lastSlewingTime = now; + feedback->selectSlewingParameters(); + } + } else { + feedback->variableParameters(fabs(velocityPercent)); + } + + if (velocityPercent < -33) wasBelow33 = true; + if (velocityPercent > 33) wasAbove33 = true; + + if (now - lastCheckTime >= 1000U) { + delta = motorCounts - encoderCounts; + + #ifndef SERVO_SAFETY_DISABLE + // if above SERVO_SAFETY_STALL_POWER (33% default) and we're not moving something is seriously wrong, so shut it down + if (labs(encoderCounts - lastEncoderCounts) < 10 && abs(velocityPercent) >= SERVO_SAFETY_STALL_POWER) { + DF("WRN:"); D(axisPrefix); DF("stall detected!"); + DF(" control->in = "); D(control->in); DF(", control->set = "); D(control->set); + DF(", control->out = "); D(control->out); DF(", velocity % = "); DL(velocityPercent); + enable(false); + safetyShutdown = true; + } + + // if above 90% power for > three seconds and the distance to the target is increasing + // something is seriously wrong so shut it down + if (labs(delta - lastDelta) > lastTargetDistance && abs(velocityPercent) >= 90) { + movingAwaySeconds++; + if (movingAwaySeconds >= 3) { + DF("WRN:"); D(axisPrefix); DF("runaway detected!"); + DLF(" > 90% power while moving away from the target!"); + enable(false); + safetyShutdown = true; + } + } else movingAwaySeconds = 0; + lastTargetDistance = labs(delta - lastDelta); + + // if we were below -33% and above 33% power in a one second period something is seriously wrong, so shut it down + if (wasBelow33 && wasAbove33) { + DF("WRN:"); D(axisPrefix); DF("oscillation detected!"); + DLF(" below -33% and above 33% power in a 2 second period!"); + enable(false); + safetyShutdown = true; + } + #endif + + wasAbove33 = false; + wasBelow33 = false; + lastEncoderCounts = encoderCounts; + lastDelta = delta; + lastCheckTime = now; + } + + #if DEBUG != OFF && defined(DEBUG_AXIS) && DEBUG_AXIS != OFF + if (axisNumber == DEBUG_AXIS) { + static uint16_t count = 0; + count++; + if (count % 10 == 0) { + char s[256]; + + float spas = 0; + if (axisNumber == 1) spas = AXIS1_STEPS_PER_DEGREE/3600.0F; else if (axisNumber == 2) spas = AXIS2_STEPS_PER_DEGREE/3600.0F; + +// snprintf(s, sizeof(s), "Ax%dSvo: Delta %6ld, Motor %6ld, Encoder %6ld, Ax%dSvo_Power: %6.3f%%\r\n", (int)axisNumber, (motorCounts - encoderCounts), motorCounts, (long)encoderCounts, (int)axisNumber, velocityPercent); +// snprintf(s, sizeof(s), "Ax%dSvo: Motor %6ld, Encoder %6ld\r\n", (int)axisNumber, motorCounts, (long)encoderCounts); +// snprintf(s, sizeof(s), "Ax%dSvo: Delta %0.2f\r\n", (int)axisNumber, (motorCounts - (long)encoderCounts)/12.9425); +// snprintf(s, sizeof(s), "Ax%dSvo: DeltaASf: %0.2f, DeltaAS: %0.2f, Ax%dSvo_Power: %6.3f%%\r\n", (int)axisNumber, (motorCounts - encoderCounts)/spas, (motorCounts - unfilteredEncoderCounts)/spas, (int)axisNumber, velocityPercent); + snprintf(s, sizeof(s), "%0.2f, %6.3f%%\r\n", (motorCounts - unfilteredEncoderCounts)/spas, velocityPercent); + + D(s); + UNUSED(spas); + } + } + #endif +} + +void ServoMotor::stopSyntheticMotion() { + if (lastPeriod == 0 && step == 0 && absStep == 0) return; + currentFrequency = 0.0F; + currentDirection = 0; + lastPeriod = 0; + + noInterrupts(); + step = 0; + absStep = 0; + interrupts(); + if (taskHandle) tasks.setPeriodSubMicros(taskHandle, 0); +} + +void ServoMotor::resetToTrackingBaseline() { + noInterrupts(); + step = 0; + absStep = 0; + inBacklash = false; + backlashSteps = 0; + interrupts(); + + currentFrequency = 0.0F; + currentDirection = 0; + lastPeriod = 0; + + lastEnc = 0; + lastUs = 0; +} + +// sets dir as required and moves coord toward target at setFrequencySteps() rate +IRAM_ATTR void ServoMotor::move() { + + #if SERVO_SLEW_DIRECT == ON + if (sync && !inBacklash) targetSteps += step; + + if (motorSteps > targetSteps) { + motorSteps = targetSteps; + } else + + if (motorSteps < targetSteps + backlashAmountSteps) { + motorSteps = targetSteps + backlashAmountSteps; + } + + #else + if (sync && !inBacklash) targetSteps += step; + + if (motorSteps > targetSteps) { + if (backlashSteps > 0) { + backlashSteps -= absStep; + inBacklash = backlashSteps > 0; + } else { + motorSteps -= absStep; + inBacklash = false; + } + } else + + if (motorSteps < targetSteps || inBacklash) { + if (backlashSteps < backlashAmountSteps) { + backlashSteps += absStep; + inBacklash = backlashSteps < backlashAmountSteps; + } else { + motorSteps += absStep; + inBacklash = false; + } + } + + #endif +} + +#endif diff --git a/src/lib/axis/motor/servo/Servo.h b/src/lib/axis/motor/servo/Servo.h new file mode 100755 index 0000000..94d1572 --- /dev/null +++ b/src/lib/axis/motor/servo/Servo.h @@ -0,0 +1,190 @@ +// ----------------------------------------------------------------------------------- +// axis servo motor +#pragma once +#include "../../../../Common.h" + +#ifdef SERVO_MOTOR_PRESENT + +#include "../../../encoder/Encoder.h" +#include "filter/Filter.h" +#include "feedback/Feedback.h" +#include "dc/calibration/TrackingVelocity.h" + +#include "dc/eE/EE.h" +#include "dc/pE/PE.h" +#include "dc/tmc/tmc2130/Tmc2130.h" +#include "dc/tmc/tmc5160/Tmc5160.h" +#include "kTech/KTech.h" +#include "tmc/tmc2209/Tmc2209.h" +#include "tmc/tmc5160/Tmc5160.h" + +#ifndef SERVO_SLEW_DIRECT + #define SERVO_SLEW_DIRECT OFF +#endif + +#ifndef SERVO_SLEWING_TO_TRACKING_DELAY + #define SERVO_SLEWING_TO_TRACKING_DELAY 3000 // in milliseconds +#endif + +#ifndef SERVO_SAFETY_STALL_POWER + #define SERVO_SAFETY_STALL_POWER 33 // in percent +#endif + +class ServoMotor : public Motor { + public: + // constructor + ServoMotor(uint8_t axisNumber, int8_t reverse, ServoDriver *Driver, Filter *filter, Encoder *encoder, uint32_t encoderOrigin, bool encoderReverse, Feedback *feedback, ServoControl *control, bool useFastHardwareTimers = true); + + // sets up the servo motor + bool init(); + + // returns the number of parameters from the motor/driver + uint8_t getParameterCount() { return Motor::getParameterCount() + driver->getParameterCount() + feedback->getParameterCount(); } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { + if (number > Motor::getParameterCount() + driver->getParameterCount()) return feedback->getParameter(number - (Motor::getParameterCount() + driver->getParameterCount())); else + if (number > Motor::getParameterCount()) return driver->getParameter(number - Motor::getParameterCount()); else + if (number >= 1 && number <= Motor::getParameterCount()) return Motor::getParameter(number); else + return &invalid; + } + + // check if parameter is valid + bool parameterIsValid(AxisParameter* parameter, bool next = false) { if (!Motor::parameterIsValid(parameter, next)) return false; else return driver->parameterIsValid(parameter, next); } + + // sets reversal of axis directions + // \param state: true reverses the direction behavior specified in settings + void setReverse(int8_t state); + + // sets motor enable on/off (if possible) + void enable(bool value); + + // get the associated motor driver status + DriverStatus getDriverStatus(); + + // resets motor and target angular position in steps, also zeros backlash and index + void resetPositionSteps(long value); + + // get instrument coordinate, in steps + long getInstrumentCoordinateSteps(); + + // set instrument coordinate, in steps + void setInstrumentCoordinateSteps(long value); + + // distance to target in steps (+/-) + long getTargetDistanceSteps(); + + // get tracking mode steps per slewing mode step + inline int getStepsPerStepSlewing() { return 256; } + + // sets overall maximum frequency (also allows for 10% over) + // \param frequency: rate of motion in steps (counts) per second + void setFrequencyMax(float frequency) { + velocityMax = frequency; + if (velocityMax < 1.0F) velocityMax = 1.0F; + driver->setFrequencyMax(frequency*1.1F); + feedback->setControlRange(frequency*1.1F); + } + + // get movement frequency in steps per second + float getFrequencySteps(); + + // set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) + void setFrequencySteps(float frequency); + + // set slewing state (hint that we are about to slew or are done slewing) + void setSlewing(bool state); + + // calibrate the motor driver + void calibrateDriver() { if (ready) driver->calibrateDriver(); } + + // get encoder count + int32_t getEncoderCount() { if (ready) return encoder->count; else return 0; } + + // set zero of absolute encoders + uint32_t encoderZero(); + + // set origin of absolute encoders + void encoderSetOrigin(uint32_t origin) { if (ready) encoder->setOrigin(origin); } + + bool hasAbsoluteEncoder() const override { return encoder != nullptr && encoder->isAbsolute(); } + + // read encoder + int32_t encoderRead(); + + // get the motor name + const char* name() { strcpy(nameStr, "Servo, "); strcat(nameStr, driver->name()); return nameStr; } + + // updates PID and sets servo motor power/direction + void poll(); + + // sets dir as required and moves coord toward target at setFrequencySteps() rate + void move(); + + // servo motor driver + ServoDriver *driver; + + // servo encoder + Encoder *encoder; + + float velocityPercent = 0.0F; + long delta = 0; + + private: + void stopSyntheticMotion(); + void resetToTrackingBaseline(); + + Filter *filter; + + void (*callback)() = NULL; + + Feedback *feedback; + ServoControl *control; + #ifdef CALIBRATE_SERVO_DC + ServoCalibrateTrackingVelocity *calibrateVelocity; + #endif + + uint8_t servoMonitorHandle = 0; + uint8_t taskHandle = 0; + + float maxFrequency = HAL_FRACTIONAL_SEC; // fastest timer rate + float maxFrequency2, maxFrequency4, maxFrequency8, maxFrequency16, maxFrequency32, maxFrequency64, maxFrequency128; + bool useFastHardwareTimers = true; + + bool encoderReverse = false; + bool encoderReverseDefault = false; + volatile int8_t encoderDirection = 0; + int32_t lastEnc = 0; + uint32_t lastUs = 0; + + bool motorStepsInitDone = false; // help determining when absolute position is initialized + bool homeSet = false; // help determining when startup home/origin was established + uint32_t encoderOrigin = 0; // the starting position + + int stepSize = 1; // step size + volatile int homeSteps = 1; // step count for microstep sequence between home positions (driver indexer) + volatile bool takeStep = false; // should we take a step + float trackingFrequency = 0; // help figure out if equatorial mount is tracking + bool slewing = false; + + float currentDirection = 0.0F; // last direction + float currentFrequency = 0.0F; // last frequency set in encoder counts per second + float lastFrequency = 0.0F; // last frequency requested + unsigned long lastPeriod = 0; // last timer period (in sub-micros) + float velocityMax = 0.0F; // the maximum velocity allowed + + volatile int absStep = 1; // absolute step size (unsigned) + // servo safety checks + unsigned long lastCheckTime = 0; // time since the last encoder position was checked + unsigned long startTime = 0; // time at start of servo polling + unsigned long lastSlewingTime = 0; // time when last slewing + long lastEncoderCounts = 0; // the last encoder position for stall check + long movingAwaySeconds = 0; // amount of time for runaway check + long lastTargetDistance = 0; // check if moving away from target for runaway check + long lastDelta = 0; // the last distance from target for runaway check + bool wasAbove33 = false; // check for oscillation + bool wasBelow33 = false; // check for oscillation + bool safetyShutdown = false; +}; + +#endif diff --git a/src/lib/axis/motor/servo/ServoDriver.cpp b/src/lib/axis/motor/servo/ServoDriver.cpp new file mode 100755 index 0000000..4c30bdc --- /dev/null +++ b/src/lib/axis/motor/servo/ServoDriver.cpp @@ -0,0 +1,170 @@ +// ----------------------------------------------------------------------------------- +// axis servo motor driver + +#include "ServoDriver.h" + +#ifdef SERVO_MOTOR_PRESENT + +// the various servo driver models +#define SERVO_DRIVER_MODEL_COUNT (SERVO_DRIVER_LAST - SERVO_DRIVER_FIRST + 1) + +#include "../../../gpioEx/GpioEx.h" + +#include "../Motor.h" + +#if DEBUG != OFF + const char* SERVO_DRIVER_NAME[SERVO_DRIVER_MODEL_COUNT] = + { + "SERVO_PE", // Phase (Direction)/Enable + "SERVO_EE", // Enable/Enable + "SERVO_TMC2130_DC", // TMC2130 w/ DC motor + "SERVO_TMC5160_DC", // TMC5160 w/ DC motor + "SERVO_TMC2209", // TMC2209 w/ stepper motor + "SERVO_TMC5160", // TMC5160 w/ stepper motor + "SERVO_KTECH" // KTech servo motor using velocity control + }; +#endif + +ServoDriver::ServoDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings) { + if (axisNumber < 1 || axisNumber > 9) return; + + this->axisNumber = axisNumber; + + this->Pins = Pins; + if (Pins != NULL) { + enablePin = Pins->enable; + enabledState = Pins->enabledState; + faultPin = Pins->fault; + } + + this->Settings = Settings; + driverModel = Settings->model; + statusMode = Settings->status; + + acceleration.valueDefault = Settings->acceleration; + zeroDeadband.valueDefault = 0.0F; +} + +bool ServoDriver::init(bool reverse) { + #if DEBUG == VERBOSE + VF("MSG:"); V(axisPrefix); VF("init model "); VL(SERVO_DRIVER_NAME[driverModel - SERVO_DRIVER_FIRST]); + VF("MSG:"); V(axisPrefix); VF("en="); + if (enablePin == OFF) { VLF("OFF"); } else + if (enablePin == SHARED) { VLF("SHARED"); } else { VL(enablePin); } + #endif + + // init default driver control pins + if (enablePin != SHARED) { + pinModeEx(enablePin, OUTPUT); + digitalWriteEx(enablePin, !enabledState); + } + + // automatically set fault status for known drivers + status.active = statusMode != OFF; + if (statusMode == ON) statusMode = LOW; + if (statusMode == LOW) pinModeEx(faultPin, INPUT_PULLUP); + #ifdef PULLDOWN + if (statusMode == HIGH) pinModeEx(faultPin, INPUT_PULLDOWN); + #else + if (statusMode == HIGH) pinModeEx(faultPin, INPUT); + #endif + + reversed = reverse; + + return true; +} + +void ServoDriver::enable(bool state) { + enabled = state; + + // Only touch a real, dedicated enable pin + if (enablePin != OFF && enablePin != SHARED) { + digitalWriteEx(enablePin, state ? enabledState : !enabledState); + } + + // Default behavior: if disabled, force ramp to 0 so outputs converge quickly + if (!state) velocityRamp = 0.0F; +} + +void ServoDriver::setFrequencyMax(float frequency) { + velocityMax = frequency; + InvVelocityMax = 1.0F/frequency; + + normalizedAcceleration = (acceleration.value/100.0F)*velocityMax; + accelerationFs = normalizedAcceleration/FRACTIONAL_SEC; + + // show velocity control settings + VF("MSG:"); V(axisPrefix); VF("Vmax="); V(velocityMax); VLF(" steps/s"); + VF("MSG:"); V(axisPrefix); VF("Acceleration="); V(acceleration.value); VF("%/s/s ("); V(accelerationFs); VLF(" steps/s/fs)"); +} + +float ServoDriver::setMotorVelocity(float velocity) { + if (!enabled) velocity = 0.0F; + + if (zeroDeadband.value > 0.0F && fabsf(velocity) < zeroDeadband.value) velocity = 0.0F; + + if (velocity > velocityMax) velocity = velocityMax; else + if (velocity < -velocityMax) velocity = -velocityMax; + + if(bypassAccelOnTracking) + velocityRamp = velocity; + else { + // ramp velocity + if (velocity > velocityRamp) { + velocityRamp += accelerationFs; + if (velocityRamp > velocity) velocityRamp = velocity; + } else + if (velocity < velocityRamp) { + velocityRamp -= accelerationFs; + if (velocityRamp < velocity) velocityRamp = velocity; + } + } + + if (velocityRamp >= 0.0F) motorDirection = DIR_FORWARD; else motorDirection = DIR_REVERSE; + + return velocityRamp; +} + +// update status info. for driver +void ServoDriver::updateStatus() { + if (statusMode == ON) { + const unsigned long now = millis(); + + if ((long)(now - timeLastStatusUpdate) > 200L) { + readStatus(); + + // open load indication is not reliable in standstill + if (status.outputA.shortToGround || + status.outputB.shortToGround || + status.overTemperatureWarning || + status.overTemperature) status.fault = true; else status.fault = false; + + timeLastStatusUpdate = now; + } + } + + #if DEBUG == VERBOSE + if (status.standstill) { status.outputA.openLoad = false; status.outputB.openLoad = false; status.standstill = false; } + if ((status.outputA.shortToGround != lastStatus.outputA.shortToGround) || + (status.outputA.openLoad != lastStatus.outputA.openLoad) || + (status.outputB.shortToGround != lastStatus.outputB.shortToGround) || + (status.outputB.openLoad != lastStatus.outputB.openLoad) || + (status.overTemperatureWarning != lastStatus.overTemperatureWarning) || + (status.overTemperature != lastStatus.overTemperature) || + (status.standstill != lastStatus.standstill) || + (status.fault != lastStatus.fault)) { + VF("MSG:"); V(axisPrefix); VF("status change "); + VF("SGA"); if (status.outputA.shortToGround) VF("< "); else VF(". "); + VF("OLA"); if (status.outputA.openLoad) VF("< "); else VF(". "); + VF("SGB"); if (status.outputB.shortToGround) VF("< "); else VF(". "); + VF("OLB"); if (status.outputB.openLoad) VF("< "); else VF(". "); + VF("OTP"); if (status.overTemperatureWarning) VF("< "); else VF(". "); + VF("OTE"); if (status.overTemperature) VF("< "); else VF(". "); + VF("SST"); if (status.standstill) VF("< "); else VF(". "); + VF("FLT"); if (status.fault) VLF("<"); else VLF("."); + } + lastStatus = status; + #endif +}; + +#endif diff --git a/src/lib/axis/motor/servo/ServoDriver.h b/src/lib/axis/motor/servo/ServoDriver.h new file mode 100755 index 0000000..c785413 --- /dev/null +++ b/src/lib/axis/motor/servo/ServoDriver.h @@ -0,0 +1,135 @@ +// ----------------------------------------------------------------------------------- +// axis servo motor driver +#pragma once + +#include +#include "../../../../Common.h" + +#include "../Drivers.h" +#include "../Motor.h" + +#ifdef SERVO_MOTOR_PRESENT + +typedef struct ServoPins { + int16_t ph1; // step + int16_t ph1State; + int16_t ph2; // dir + int16_t ph2State; + int16_t enable; + uint8_t enabledState; + int16_t m0; + int16_t m1; + int16_t m2; + int16_t m3; + int16_t fault; +} ServoPins; + +typedef struct ServoSettings { + int16_t model; + int8_t status; + int32_t acceleration; // acceleration in encoder %/s/s +} ServoSettings; + +class ServoDriver { + public: + ServoDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings); + + // decodes driver model and sets up the pin modes + virtual bool init(bool reverse); + + // returns the number of axis parameters + virtual uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + virtual AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // check if axis parameter is valid + virtual bool parameterIsValid(AxisParameter* parameter, bool next = false) { return true; } + + // alternate mode for movement + virtual void alternateMode(bool state) { UNUSED(state); } + + // enable or disable the driver using the enable pin or other method + virtual void enable(bool state); + + // sets frequency corrosponding to the fastest allowed slew rate + // \param frequency: rate of motion in steps (counts) per second + void setFrequencyMax(float frequency); + + // set motor velocity + // \param velocity as needed to reach the target position, in signed encoder counts per second + virtual float setMotorVelocity(float velocity); + + // set motor velocity + // \param velocity as needed to reach the target position, in signed encoder counts per second + // \param encoderVelocity in signed encoder counts per second + // \returns velocity in effect, in signed encoder counts per second + virtual float setMotorVelocity(float velocity, float encoderVelocity) { + UNUSED(encoderVelocity); + return setMotorVelocity(velocity); + } + + // returns motor direction (DIR_FORWARD or DIR_REVERSE) + Direction getMotorDirection() { return motorDirection; }; + + // update status info. for driver + // this is a required method for the Axis class + virtual void updateStatus(); + + // get status info. + // this is a required method for the Axis class + DriverStatus getStatus() { return status; } + + // do not use acceleration while trackings + void setBypassAccelOnTracking(bool on) { bypassAccelOnTracking = on; } + + // calibrate the motor if required + virtual void calibrateDriver() {} + + // get the driver name + virtual const char* name() { return NULL; } + + protected: + virtual void readStatus() {} + + int axisNumber; + char axisPrefix[32]; // prefix for debug messages + + int16_t driverModel; + int16_t statusMode; + + DriverStatus status = { false, {false, false}, {false, false}, false, false, false, false }; + #if DEBUG != OFF + DriverStatus lastStatus = {false, {false, false}, {false, false}, false, false, false, false}; + #endif + unsigned long timeLastStatusUpdate = 0; + + float normalizedAcceleration; // in encoder counts/s/s + float accelerationFs; // in encoder counts/s/fs + float velocityRamp = 1.0F; // regulate velocity changes + float velocityMax = 1.0F; // frequency corrosponding to the fastest allowed slew rate, in encoder counts/s + float InvVelocityMax = 1.0F; + + Direction motorDirection = DIR_FORWARD; + bool reversed = false; + + int16_t enablePin = OFF; + uint8_t enabledState = LOW; + bool enabled = false; + int16_t faultPin = OFF; + bool bypassAccelOnTracking = false; + + const ServoPins *Pins; + const ServoSettings *Settings; + + // runtime adjustable settings + AxisParameter invalid = {NAN, NAN, NAN, NAN, NAN, AXP_INVALID, ""}; + AxisParameter acceleration = {NAN, NAN, NAN, 0, 100000, AXP_FLOAT, AXPN_MAX_ACCEL}; + AxisParameter zeroDeadband = {NAN, NAN, NAN, 0.0, 500.0, AXP_FLOAT_IMMEDIATE, "Deadband, cps"}; + + // this array list isn't used if overridden, just a place holder! + const int numParameters = 2; + AxisParameter* parameter[3] = {&invalid, &acceleration, &zeroDeadband}; +}; + +#endif diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.cpp b/src/lib/axis/motor/servo/dc/DcServoDriver.cpp new file mode 100755 index 0000000..d445092 --- /dev/null +++ b/src/lib/axis/motor/servo/dc/DcServoDriver.cpp @@ -0,0 +1,167 @@ +// ----------------------------------------------------------------------------------- +// axis servo DC motor driver + +#include "DcServoDriver.h" + +#if defined(SERVO_PE_PRESENT) || defined(SERVO_EE_PRESENT) || \ + defined(SERVO_TMC2130_DC_PRESENT) || defined(SERVO_TMC5160_DC_PRESENT) + +#include "../../../../gpioEx/GpioEx.h" + +// constructor +ServoDcDriver::ServoDcDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, + float pwmMinimum, float pwmMaximum) + :ServoDriver(axisNumber, Pins, Settings) { + if (axisNumber < 1 || axisNumber > 9) return; + + this->pwmMinimum.valueDefault = pwmMinimum; + this->pwmMaximum.valueDefault = pwmMaximum; + + dirDeadTimeMs.valueDefault = 0.0F; + #if SERVO_DC_VPID == ON + velKp.valueDefault = 0.2F; + velKi.valueDefault = 0.5F; + velIMax.valueDefault = 0.2F; + #endif +} + +#if SERVO_DC_VPID == ON + + float ServoDcDriver::setMotorVelocity(float velocity, float encoderVelocity) { + velocity = ServoDriver::setMotorVelocity(velocity); + + if (velocity == 0.0F) { + pwmUpdate(0.0F); + vI = 0.0F; + velLastUs = 0; + lastEffectiveDirection = 0; + return 0.0F; + } + + const float vmag = fabsf(velocity); + const int8_t dir = (velocity > 0.0F) ? 1 : -1; + + const unsigned long nowMs = millis(); + if (lastEffectiveDirection != 0 && dir != lastEffectiveDirection) { + const unsigned long dtMs = (unsigned long)lroundf(dirDeadTimeMs.value); + if (dtMs) dirHoldUntilMs = nowMs + dtMs; + vI = 0.0F; + velLastUs = 0; + } + lastEffectiveDirection = dir; + + if ((long)(dirHoldUntilMs - nowMs) > 0) { + pwmUpdate(0.0F); + return 0.0F; + } + + const float dMin = pwmMinimum.value * 0.01F; + const float dMax0 = pwmMaximum.value * 0.01F; + const float dMax = (dMax0 >= dMin) ? dMax0 : dMin; + const float dSpan = dMax - dMin; + + float vcmd01 = vmag * InvVelocityMax; + if (vcmd01 > 1.0F) vcmd01 = 1.0F; + + const uint32_t nowUs = micros(); + uint32_t dUs = nowUs - velLastUs; + if (velLastUs == 0) dUs = 0; + velLastUs = nowUs; + + // measured velocity projected onto commanded direction and normalized + encoderVelocity *= (float)dir; + const float vmeas01 = encoderVelocity * InvVelocityMax; + const float error01 = vcmd01 - vmeas01; + + float u01 = vcmd01 + (velKp.value * error01) + vI; + + float uSat = u01; + if (uSat < 0.0F) uSat = 0.0F; + else if (uSat > 1.0F) uSat = 1.0F; + + if (velKi.value > 0.0F && dUs > 0 && dUs <= 200000U) { + const bool atLo = (uSat <= 0.0F); + const bool atHi = (uSat >= 1.0F); + const bool allowI = + (!atLo && !atHi) || + (atLo && error01 > 0.0F) || + (atHi && error01 < 0.0F); + + if (allowI) { + vI += velKi.value * error01 * (dUs * 0.000001F); + + const float imax = velIMax.value; + if (vI > imax) vI = imax; + else if (vI < -imax) vI = -imax; + + u01 = vcmd01 + (velKp.value * error01) + vI; + uSat = u01; + if (uSat < 0.0F) uSat = 0.0F; + else if (uSat > 1.0F) uSat = 1.0F; + } + } else if (dUs > 200000U) { + vI = 0.0F; + } + + float dutyMag01 = 0.0F; + if (uSat > 0.0F) { + dutyMag01 = dMin + dSpan * uSat; + if (dutyMag01 > dMax) dutyMag01 = dMax; + } + + pwmUpdate(dutyMag01 * (float)dir); + return velocity; + } + +#else + + float ServoDcDriver::setMotorVelocity(float velocity, float /*encoderVelocity*/) { + velocity = ServoDriver::setMotorVelocity(velocity); + + if (velocity == 0.0F) { + pwmUpdate(0.0F); + velLastUs = 0; + lastEffectiveDirection = 0; + return 0.0F; + } + + const float vmag = fabsf(velocity); + const int8_t dir = (velocity > 0.0F) ? 1 : -1; + + const unsigned long nowMs = millis(); + if (lastEffectiveDirection != 0 && dir != lastEffectiveDirection) { + const unsigned long dtMs = (unsigned long)lroundf(dirDeadTimeMs.value); + if (dtMs) dirHoldUntilMs = nowMs + dtMs; + velLastUs = 0; + } + lastEffectiveDirection = dir; + + if ((long)(dirHoldUntilMs - nowMs) > 0) { + pwmUpdate(0.0F); + return 0.0F; + } + + const float dMin = pwmMinimum.value * 0.01F; + const float dMax0 = pwmMaximum.value * 0.01F; + const float dMax = (dMax0 >= dMin) ? dMax0 : dMin; + const float dSpan = dMax - dMin; + + float vcmd01 = vmag * InvVelocityMax; + if (vcmd01 > 1.0F) vcmd01 = 1.0F; + + // feedforward-only: uSat = vcmd01 + const float uSat = vcmd01; + + float dutyMag01 = 0.0F; + if (uSat > 0.0F) { + dutyMag01 = dMin + dSpan * uSat; + if (dutyMag01 > dMax) dutyMag01 = dMax; + } + + pwmUpdate(dutyMag01 * (float)dir); + return velocity; + } + +#endif + +#endif diff --git a/src/lib/axis/motor/servo/dc/DcServoDriver.h b/src/lib/axis/motor/servo/dc/DcServoDriver.h new file mode 100755 index 0000000..74c8c5e --- /dev/null +++ b/src/lib/axis/motor/servo/dc/DcServoDriver.h @@ -0,0 +1,65 @@ +// ----------------------------------------------------------------------------------- +// axis servo DC motor driver +#pragma once + +#include +#include "../../../../../Common.h" + +#if defined(SERVO_PE_PRESENT) || defined(SERVO_EE_PRESENT) || defined(SERVO_TMC2130_DC_PRESENT) || defined(SERVO_TMC5160_DC_PRESENT) + +#include "../ServoDriver.h" + +#ifndef SERVO_DC_VPID + #if ENCODER_VELOCITY == ON + #define SERVO_DC_VPID ON + #else + #define SERVO_DC_VPID OFF + #endif +#endif + +class ServoDcDriver : public ServoDriver { + public: + // constructor + ServoDcDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum); + + // returns the number of axis parameters + uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // set motor velocity + // \param velocity as needed to reach the target position, in signed encoder counts per second + // \param encoderVelocity in signed encoder counts per second + // \returns velocity in effect, in signed encoder counts per second + float setMotorVelocity(float velocity, float encoderVelocity) override; + + protected: + + // motor control pwm update + // \param power01 in -1.0 to 1.0 units + virtual void pwmUpdate(float power01) { } + + // runtime adjustable settings + AxisParameter pwmMinimum = {NAN, NAN, NAN, 0.0, 100.0, AXP_FLOAT_IMMEDIATE, AXPN_MIN_PWR}; + AxisParameter pwmMaximum = {NAN, NAN, NAN, 1.0, 100.0, AXP_FLOAT_IMMEDIATE, AXPN_MAX_PWR}; + AxisParameter dirDeadTimeMs = {NAN, NAN, NAN, 0.0, 50.0, AXP_FLOAT_IMMEDIATE, "Deadtime, ms"}; + #if SERVO_DC_VPID == ON + AxisParameter velKp = {NAN, NAN, NAN, 0.0, 10.0, AXP_FLOAT_IMMEDIATE, "Vel Kp"}; + AxisParameter velKi = {NAN, NAN, NAN, 0.0, 10.0, AXP_FLOAT_IMMEDIATE, "Vel Ki"}; + AxisParameter velIMax = {NAN, NAN, NAN, 0.0, 1.0, AXP_FLOAT_IMMEDIATE, "Vel Imax"}; + const int numParameters = 8; + AxisParameter* parameter[9] = {&invalid, &acceleration, &zeroDeadband, &pwmMinimum, &pwmMaximum, &dirDeadTimeMs, &velKp, &velKi, &velIMax}; + #else + const int numParameters = 5; + AxisParameter* parameter[6] = {&invalid, &acceleration, &zeroDeadband, &pwmMinimum, &pwmMaximum, &dirDeadTimeMs}; + #endif + + private: + float vI = 0.0F; + uint32_t velLastUs = 0; + uint32_t dirHoldUntilMs = 0; + int8_t lastEffectiveDirection = 0; // -1,0,+1 +}; + +#endif diff --git a/src/lib/axis/motor/servo/dc/calibration/TrackingVelocity.cpp b/src/lib/axis/motor/servo/dc/calibration/TrackingVelocity.cpp new file mode 100755 index 0000000..7b49532 --- /dev/null +++ b/src/lib/axis/motor/servo/dc/calibration/TrackingVelocity.cpp @@ -0,0 +1,746 @@ +// ============================== TrackingVelocity.cpp =============================== +// ----------------------------------------------------------------------------------- +// Servo Tracking Velocity Calibration — Overview +// +// Goal +// Calibrate, per direction, the minimum open-loop *driver command percent* (“u”, % of +// the driver’s velocity range) that: +// • breaks stiction from rest (u_break) +// • sustains motion after a short kick (u_hold) +// +// Mode (Open-loop) +// While calibration is active (experimentMode=true) the axis runs open-loop: +// • Tracking/PID/hysteresis/nonlinear helpers are disabled in this TU +// • The driver is fed a fixed *command %* (experimentVelocity), interpreted as a +// fraction of the driver’s max velocity capability (not a PWM duty cycle) +// • “Velocity” in the logs means encoder-derived steps/s (not a PID setpoint) +// +// Per-direction phases +// 1) Stiction Break (u_break) +// - Staircase upward from SERVO_CALIBRATION_START_VELOCITY_PERCENT. +// - Between steps, STOP and wait SERVO_CALIBRATION_MOTOR_SETTLE_TIME ms to kill carry-over. +// - Acceptance rule: must meet the SAME consecutive-moving requirement as u_hold +// (SERVO_CALIBRATION_REQUIRED_MOVING_SAMPLES). UB additionally enforces a hard +// per-candidate sample cap (SERVO_CAL_UB_MAX_SAMPLES) to avoid indefinite waiting. +// 2) Velocity Hold (u_hold) +// - From u_break, staircase downward to find the minimum command % that keeps moving. +// - Each candidate is kick-started at u_break, then held at the candidate %. +// - Acceptance rule: SAME consecutive-moving requirement. Any failed sample +// immediately stalls the candidate. +// +// Reporting/imbalance +// After both directions finish, an imbalance warning is logged if FWD/REV u_hold +// differ beyond SERVO_CALIBRATION_IMBALANCE_ERROR_THRESHOLD. +// +// ------------------------------ How the state machine works ------------------------- +// There are TWO interleaved state machines: +// +// (A) High-level PHASE machine (per axis) [CalibrationState]: +// IDLE → U_BREAK → U_HOLD → (flip direction) → U_BREAK → U_HOLD → CHECK_IMBALANCE +// ↘ (if both axes enabled) WAIT_PEER +// If both axes are enabled, we serialize execution RA first, DEC second via WAIT_TURN. +// +// (B) Motor SUB-STATE machine (per axis) [MotorState]: +// STOPPED ↔ SETTLING → (start test) → KICKING → HOLDING → RUNNING_STEADY +// +// - SETTLING: enforced pause after STOP to let motion decay. +// - KICKING: briefly command u_break to overcome stiction for a u_hold test. +// - HOLDING: hold candidate % and sample velocity at fixed cadence. +// - RUNNING_STEADY: means “accepted as moving” (name kept for API compatibility). +// +// ------------------------------ “Motor moved?” decision ----------------------------- +// At each HOLDING sample (every SERVO_CALIBRATION_VELOCITY_SETTLE_CHECK_INTERVAL ms): +// - Compute instantaneous velocity from encoder/step deltas. +// - Accept a single sample as MOVING if BOTH are true: +// • |velocity| > SERVO_CALIBRATION_MIN_DETECTABLE_VELOCITY (noise floor guard) +// • |deltaCounts| ≥ SERVO_CALIBRATION_MIN_COUNTS_PER_SAMPLE (minimum progress) +// +// Acceptance rule (shared by UB & UH): +// - Require SERVO_CALIBRATION_REQUIRED_MOVING_SAMPLES consecutive MOVING samples +// to accept the candidate as RUNNING_STEADY. +// - For u_hold (UH): any non-moving sample immediately fails the candidate. +// - For u_break (UB): keep sampling until a small cap (SERVO_CAL_UB_MAX_SAMPLES); +// if we still haven’t reached the consecutive requirement, fail the candidate. +// +// ------------------------------ Two-axis coordination ------------------------------ +// - Selection mask: CALIBRATE_SERVO_AXIS_SELECT (bit0=Axis1, bit1=Axis2). In this .cpp, +// 0 expands to “both” via SERVO_CAL_ALL_AXES_MASK (0x03). +// - Turn-taking: If both axes are enabled, Axis 1 runs first while Axis 2 waits +// in WAIT_TURN. When Axis 1 ends, it grants the turn to Axis 2. +// - Joint report: First finisher waits (WAIT_PEER) for the other; then a single +// combined report is printed. +// ----------------------------------------------------------------------------------- + +#include "TrackingVelocity.h" + +#if defined(SERVO_MOTOR_PRESENT) && defined(CALIBRATE_SERVO_DC) + +// ---------- Compact logging helpers (one-line logs) ---------- +static inline void LOG_HDR(const char* axisPrefix) { V(axisPrefix); VF(" "); } +static inline void LOG_DIR(bool fwd){ VF(fwd ? "F " : "R "); } // F=forward, R=reverse +static inline const char* PHASE_UB(){ return "UB"; } // u_break +static inline const char* PHASE_UH(){ return "UH"; } // u_hold + +// Disable servo helpers during calibration (we want raw behavior) +#ifdef SERVO_HYSTERESIS_ENABLE + #undef SERVO_HYSTERESIS_ENABLE +#endif +#ifdef SERVO_STICTION_KICK + #undef SERVO_STICTION_KICK +#endif +#ifdef SERVO_NONLINEAR_ENABLE + #undef SERVO_NONLINEAR_ENABLE +#endif + +#ifdef TRACK_AUTOSTART + #undef TRACK_AUTOSTART +#endif + +// ------------------------ UB sampling cap (avoid infinite wait) --------------------- +// UB uses the same consecutive requirement as UH, but we also enforce a hard cap on the +// total number of samples we will take for a single UB candidate before declaring STALLED. +#ifndef SERVO_CAL_UB_MAX_SAMPLES + #define SERVO_CAL_UB_MAX_SAMPLES 6 +#endif + +// -------------------- Joint calibration coordination (for 2 axes) ------------------- +struct AxisCalibSnapshot { + bool valid = false; + ServoCalibrateTrackingVelocity::DirectionData fwd; + ServoCalibrateTrackingVelocity::DirectionData rev; + char axisPrefix[32] = {0}; +}; + +// 1-based index: [1] and [2] used; [0] unused +static volatile bool g_axisEnabledForCal[3] = {false,false,false}; +static volatile bool g_axisDone[3] = {false,false,false}; +static AxisCalibSnapshot g_snapshot[3]; +static volatile bool g_jointReportPrinted = false; + +// Turn-taking: 0 = none yet, 1 = Axis 1's turn, 2 = Axis 2's turn +static volatile uint8_t g_currentAxisTurn = 0; + +static inline uint8_t otherAxis(uint8_t axis) { return axis == 1 ? 2 : 1; } + +// Save final per-axis results for the joint report +static void snapshotAxisData(uint8_t axis, + const char* axisPrefix, + const ServoCalibrateTrackingVelocity::DirectionData& fwd, + const ServoCalibrateTrackingVelocity::DirectionData& rev) { + AxisCalibSnapshot &s = g_snapshot[axis]; + s.valid = true; + s.fwd = fwd; + s.rev = rev; + snprintf(s.axisPrefix, sizeof(s.axisPrefix), "%s", axisPrefix ? axisPrefix : ""); +} + +// Print one axis table +static void printOneAxisTable(const AxisCalibSnapshot& s) { + ServoCalibrateTrackingVelocity::printReport(s.axisPrefix, s.fwd, s.rev); +} + +// Print the combined report for both axes (if present) +static void printJointReport() { + VLF("\n===== Joint Servo Calibration ====="); + if (g_snapshot[1].valid) printOneAxisTable(g_snapshot[1]); + if (g_snapshot[2].valid) printOneAxisTable(g_snapshot[2]); + + if (g_snapshot[1].valid && g_snapshot[2].valid) { + float a1 = (g_snapshot[1].fwd.u_hold + g_snapshot[1].rev.u_hold) / 2.0f; + float a2 = (g_snapshot[2].fwd.u_hold + g_snapshot[2].rev.u_hold) / 2.0f; + float delta = fabsf(a1 - a2); + VF("Cross-axis avg u_hold delta: "); V(delta); VLF("%"); + (void)delta; // silence if logs trimmed + } + VLF("==================================\n"); +} + +// ------------------------------------------------------------------------------------ +// Class methods +// ------------------------------------------------------------------------------------ + +ServoCalibrateTrackingVelocity::ServoCalibrateTrackingVelocity(uint8_t axisNumber) { + this->axisNumber = axisNumber; + snprintf(axisPrefix, sizeof(axisPrefix), "Axis%d ServoCalibrate", axisNumber); +} + +void ServoCalibrateTrackingVelocity::init() { + experimentVelocity = 0.0f; + experimentMode = false; + calibrationState = CALIBRATION_IDLE; + motorState = MOTOR_STOPPED; + + resetCalibrationValues(); + + lastStateChangeTime = lastVelocityTime = settleStartTime = + calibrationStepStartTime = kickStartTime = 0; + currentTicks = calibrationStepStartTicks = lastTicks = 0; + lastVelocityMeasurement = 0.0f; + lastDeltaCounts = 0; + steadySinceMs = 0; + steadyStartTicks = 0; + lastCheckTime = 0; + + staircaseIters = 0; + currentSearchVelocity = 0.0f; + foundValidMotion = false; + + hasQueuedTest = false; + queuedState = CALIBRATION_IDLE; + queuedVelocity = 0.0f; + + consecutiveMovingSamples = 0; + candidateSampleCount = 0; + + if (axisNumber == 1) { + g_axisDone[1] = g_axisDone[2] = false; + g_axisEnabledForCal[1] = g_axisEnabledForCal[2] = false; // set in start() + g_snapshot[1] = AxisCalibSnapshot(); + g_snapshot[2] = AxisCalibSnapshot(); + g_jointReportPrinted = false; + g_currentAxisTurn = 0; + } + + setVelocity(0); + startSettling(); +} + +void ServoCalibrateTrackingVelocity::start(float trackingFrequency, long /*instrumentCoordinateSteps*/) { + // Expand 0 → all-axes mask, otherwise use the provided mask + const uint8_t effectiveMask = + (CALIBRATE_SERVO_AXIS_SELECT == 0) ? SERVO_CAL_ALL_AXES_MASK + : CALIBRATE_SERVO_AXIS_SELECT; + + // Respect axis selection bitmask (skip if not requested) + if (!(effectiveMask & (1U << (axisNumber - 1)))) { + LOG_HDR(axisPrefix); VLF("skip"); + g_axisEnabledForCal[axisNumber] = false; + return; + } + + g_axisEnabledForCal[axisNumber] = true; + + LOG_HDR(axisPrefix); VLF("start"); + + enabled = true; + experimentMode = true; + calibrationDirectionIsForward = true; + calibrationState = CALIBRATION_U_BREAK; // may be overridden to WAIT_TURN + motorState = MOTOR_STOPPED; + calibrationVelocity = SERVO_CALIBRATION_START_VELOCITY_PERCENT; + targetVelocity = trackingFrequency; + lastStateChangeTime = millis(); + + staircaseIters = 0; + currentSearchVelocity = calibrationVelocity; + foundValidMotion = false; + + const bool bothEnabled = (effectiveMask == SERVO_CAL_ALL_AXES_MASK); + if (bothEnabled) { + if (g_currentAxisTurn == 0) { + g_currentAxisTurn = 1; // Axis 1 starts + } + if (axisNumber != g_currentAxisTurn) { + LOG_HDR(axisPrefix); VLF("wait-turn"); + calibrationState = CALIBRATION_WAIT_TURN; + setVelocity(0); + startSettling(); + return; + } + } + + setVelocity(0); + startSettling(); +} + +void ServoCalibrateTrackingVelocity::updateState(long instrumentCoordinateSteps) { + if(!enabled || !experimentMode) return; + + currentTime = millis(); + currentTicks = instrumentCoordinateSteps; + + handleMotorState(); + + if (currentTime - lastStateChangeTime > SERVO_CALIBRATION_TIMEOUT) { + LOG_HDR(axisPrefix); VLF("timeout → reset"); + handleCalibrationFailure(); + return; + } + + if (calibrationState == CALIBRATION_WAIT_TURN) { + if (g_currentAxisTurn == axisNumber) { + calibrationState = CALIBRATION_U_BREAK; + hasQueuedTest = false; + queueNextTest(CALIBRATION_U_BREAK, calibrationDirectionIsForward + ? SERVO_CALIBRATION_START_VELOCITY_PERCENT + : -SERVO_CALIBRATION_START_VELOCITY_PERCENT); + if (motorState != MOTOR_SETTLING) { + setVelocity(0); + startSettling(); + } + } + return; + } + + if (calibrationState == CALIBRATION_WAIT_PEER) { + uint8_t other = otherAxis(axisNumber); + if (!g_axisEnabledForCal[other] || g_axisDone[other]) { + if (!g_jointReportPrinted) { + g_jointReportPrinted = true; + printJointReport(); + } + experimentMode = false; + calibrationState = CALIBRATION_IDLE; + setVelocity(0); + LOG_HDR(axisPrefix); VLF("done"); + } + return; + } + + // Only run phase logic when a candidate outcome is resolvable + if (motorState != MOTOR_RUNNING_STEADY && motorState != MOTOR_STOPPED) return; + + switch (calibrationState) { + case CALIBRATION_U_BREAK: processUBreak(); break; + case CALIBRATION_U_HOLD: processUHold(); break; + case CALIBRATION_CHECK_IMBALANCE: processImbalanceCheck(); break; + default: break; + } +} + +// -------------------------------- Motor sub-state machine --------------------------- +void ServoCalibrateTrackingVelocity::handleMotorState() { + switch (motorState) { + case MOTOR_SETTLING: { + float currentVelocity = calculateInstantaneousVelocity(); + if (currentTime - settleStartTime >= SERVO_CALIBRATION_MOTOR_SETTLE_TIME) { + motorState = MOTOR_STOPPED; + LOG_HDR(axisPrefix); VF("settled v="); V(currentVelocity); VLF(" sps"); + if (hasQueuedTest) { + calibrationState = queuedState; + hasQueuedTest = false; + startTest(queuedVelocity); + } + } + lastVelocityMeasurement = currentVelocity; + lastVelocityTime = currentTime; + break; + } + + case MOTOR_KICKING: + if (currentTime - kickStartTime >= SERVO_CALIBRATION_KICK_DURATION_MS) { + setVelocity(calibrationVelocity); + motorState = MOTOR_HOLDING; + calibrationStepStartTime = currentTime; + calibrationStepStartTicks = currentTicks; + lastVelocityTime = currentTime; + lastVelocityMeasurement = 0; + consecutiveMovingSamples = 0; + candidateSampleCount = 0; + + LOG_HDR(axisPrefix); LOG_DIR(calibrationDirectionIsForward); + VF("kick→hold @"); V(calibrationVelocity); VLF("%"); + } + break; + + case MOTOR_HOLDING: + // Both UB and UH require N consecutive moving samples. + // UB additionally has a hard sample cap to avoid endless waiting. + if (currentTime - lastVelocityTime >= SERVO_CALIBRATION_VELOCITY_SETTLE_CHECK_INTERVAL) { + float v_now = calculateInstantaneousVelocity(); + long dCounts = lastDeltaCounts; + + const bool isUB = (calibrationState == CALIBRATION_U_BREAK); + const int requiredConsec = SERVO_CALIBRATION_REQUIRED_MOVING_SAMPLES; + + const bool movingNow = + (fabsf(v_now) > SERVO_CALIBRATION_MIN_DETECTABLE_VELOCITY) && + (labs(dCounts) >= SERVO_CALIBRATION_MIN_COUNTS_PER_SAMPLE); + + // count total samples in this candidate (UB and UH) + candidateSampleCount++; + + if (movingNow) { + consecutiveMovingSamples++; + LOG_HDR(axisPrefix); LOG_DIR(calibrationDirectionIsForward); + VF(isUB ? PHASE_UB() : PHASE_UH()); + VF(" hold@"); V(calibrationVelocity); VF("% sps="); V(v_now); + VF(" cnt="); V(dCounts); VF(" ok "); V(consecutiveMovingSamples); + VF("/"); V(requiredConsec); + if (consecutiveMovingSamples >= requiredConsec) VF(" → MOVE"); + VL(""); + + if (consecutiveMovingSamples >= requiredConsec) { + motorState = MOTOR_RUNNING_STEADY; // accept candidate + steadySinceMs = currentTime; + steadyStartTicks = currentTicks; + } + } else { + // failed sample → reset the consecutive counter for BOTH UB and UH + consecutiveMovingSamples = 0; + + if (isUB) { + // UB: keep sampling until cap, then fail if we never reached requiredConsec + if (candidateSampleCount >= SERVO_CAL_UB_MAX_SAMPLES) { + motorState = MOTOR_STOPPED; + LOG_HDR(axisPrefix); VLF("HOLD_STALLED (UB: no consecutive move within cap)"); + } + // else: keep waiting for next sample + } else { + // UH: strict — any failure cancels this candidate immediately + motorState = MOTOR_STOPPED; + LOG_HDR(axisPrefix); VLF("HOLD_STALLED"); + } + } + + lastVelocityMeasurement = v_now; + lastVelocityTime = currentTime; + } + break; + + case MOTOR_RUNNING_STEADY: + // Nothing to do; phase logic will treat as "moved" + break; + + case MOTOR_STOPPED: + // Idle; phase logic will interpret as "not moved" + break; + } +} + +// Compute instantaneous velocity in steps/s using elapsed time. +float ServoCalibrateTrackingVelocity::calculateInstantaneousVelocity() { + if (lastCheckTime == 0) { + lastCheckTime = currentTime; + lastTicks = currentTicks; + lastDeltaCounts = 0; + return 0.0f; + } + + float elapsed = (currentTime - lastCheckTime) / 1000.0f; + if (elapsed <= 0) return 0.0f; + + long dCounts = (currentTicks - lastTicks); + float velocity = dCounts / elapsed; + +#ifdef SERVO_CAL_DEBUG + VF("DBG: "); V(axisPrefix); VF(" vel%="); V(calibrationVelocity); + VF(", dTicks="); V(dCounts); VF(", Vel="); V(velocity); VL(" steps/s"); +#endif + + lastTicks = currentTicks; + lastCheckTime = currentTime; + lastDeltaCounts = dCounts; + + return velocity; +} + +// ------------------------------- Helpers & Phase handlers --------------------------- + +ServoCalibrateTrackingVelocity::DirectionData& +ServoCalibrateTrackingVelocity::currentDirectionData() { + return calibrationDirectionIsForward ? fwdData : revData; +} + +const ServoCalibrateTrackingVelocity::DirectionData& +ServoCalibrateTrackingVelocity::currentDirectionData() const { + return calibrationDirectionIsForward ? fwdData : revData; +} + +void ServoCalibrateTrackingVelocity::recordMovementData(float velocity, long counts) { + DirectionData& data = currentDirectionData(); + if (calibrationState == CALIBRATION_U_BREAK) { + data.breakVel = velocity; + data.breakCounts = counts; + } else { + data.holdVel = velocity; + data.holdCounts = counts; + } +} + +void ServoCalibrateTrackingVelocity::logTestResult(const char* phase, bool moved) { + LOG_HDR(axisPrefix); LOG_DIR(calibrationDirectionIsForward); + V(phase); VF(": "); VF(moved ? "MOVE" : "STALL"); + VF(" @"); V(calibrationVelocity); VF("%"); + if (moved) { VF(" v≈"); V(lastVelocityMeasurement); VF(" sps"); } + VL(""); +} + +// u_break: staircase upward from rest until we get a "moved" or hit limits +void ServoCalibrateTrackingVelocity::processUBreak() { + if (++staircaseIters > SERVO_CALIBRATION_REFINE_MAX_ITERATIONS) { + LOG_HDR(axisPrefix); VLF("UB iter-limit"); + if (!foundValidMotion) { + currentDirectionData().u_break = SERVO_CALIBRATION_STOP_VELOCITY_PERCENT; + LOG_HDR(axisPrefix); VF("UB none→max="); V(currentDirectionData().u_break); VLF("%"); + } + proceedToUHold(); + return; + } + + if (motorState != MOTOR_RUNNING_STEADY && motorState != MOTOR_STOPPED) return; + + const bool moved = (motorState == MOTOR_RUNNING_STEADY); + const float currentAbsVelocity = fabsf(calibrationVelocity); + + LOG_HDR(axisPrefix); LOG_DIR(calibrationDirectionIsForward); + VF("UB "); VF(moved ? "OK" : "NO"); VF(" @"); V(currentAbsVelocity); VLF("%"); + + if (moved) { + foundValidMotion = true; + recordMovementData(lastVelocityMeasurement, lastDeltaCounts); + currentDirectionData().u_break = currentAbsVelocity; + currentDirectionData().everMoved = true; + + LOG_HDR(axisPrefix); LOG_DIR(calibrationDirectionIsForward); + VF("UB set="); V(currentAbsVelocity); VF("% v≈"); V(lastVelocityMeasurement); VLF(" sps"); + proceedToUHold(); + } else { + const float nextAbsVelocity = currentAbsVelocity + SERVO_CALIBRATION_STAIRCASE_STEP_PERCENT; + + if (nextAbsVelocity > SERVO_CALIBRATION_STOP_VELOCITY_PERCENT) { + if (!foundValidMotion) { + currentDirectionData().u_break = SERVO_CALIBRATION_STOP_VELOCITY_PERCENT; + LOG_HDR(axisPrefix); VF("UB none→max="); V(currentDirectionData().u_break); VLF("%"); + } + proceedToUHold(); + } else { + calibrationVelocity = calibrationDirectionIsForward ? nextAbsVelocity : -nextAbsVelocity; + startTest(calibrationVelocity); + } + } +} + +// u_hold: staircase downward; each candidate must meet the N-consecutive-moving rule +void ServoCalibrateTrackingVelocity::processUHold() { + if (++staircaseIters > SERVO_CALIBRATION_REFINE_MAX_ITERATIONS) { + LOG_HDR(axisPrefix); VLF("UH iter-limit"); + finishUHold(); + return; + } + + if (motorState != MOTOR_RUNNING_STEADY && motorState != MOTOR_STOPPED) return; + + const bool moved = (motorState == MOTOR_RUNNING_STEADY); + const float currentAbsVelocity = fabsf(calibrationVelocity); + const float u_break = currentDirectionData().u_break; + + if (moved) { + recordMovementData(lastVelocityMeasurement, lastDeltaCounts); + currentDirectionData().u_hold = currentAbsVelocity; + + logTestResult("u_hold", true); + + const float nextAbsVelocity = currentAbsVelocity - SERVO_CALIBRATION_STAIRCASE_STEP_PERCENT; + + if (nextAbsVelocity < SERVO_CALIBRATION_START_VELOCITY_PERCENT) { + LOG_HDR(axisPrefix); VF("UH min="); V(currentDirectionData().u_hold); VLF("%"); + finishUHold(); + } else { + // Kick at u_break to overcome stiction, then test the next-lower candidate + calibrationVelocity = calibrationDirectionIsForward ? nextAbsVelocity : -nextAbsVelocity; + setVelocity(calibrationDirectionIsForward ? u_break : -u_break); + motorState = MOTOR_KICKING; + kickStartTime = currentTime; + lastStateChangeTime = currentTime; + } + } else { + logTestResult("u_hold", false); + + if (currentDirectionData().u_hold == 0.0f) { + currentDirectionData().u_hold = u_break; // fall back + LOG_HDR(axisPrefix); VF("UH use UB="); V(currentDirectionData().u_hold); VLF("%"); + } + finishUHold(); + } +} + +void ServoCalibrateTrackingVelocity::proceedToUHold() { + // Transition to u_hold: stop, settle, then queue first test below u_break + setVelocity(0); + startSettling(); + + calibrationState = CALIBRATION_U_HOLD; + staircaseIters = 0; + + const float u_break = currentDirectionData().u_break; + currentSearchVelocity = max(u_break - SERVO_CALIBRATION_STAIRCASE_STEP_PERCENT, + SERVO_CALIBRATION_START_VELOCITY_PERCENT); + + LOG_HDR(axisPrefix); VF("UH first="); V(currentSearchVelocity); VLF("%"); + + queueNextTest(CALIBRATION_U_HOLD, calibrationDirectionIsForward ? currentSearchVelocity : -currentSearchVelocity); +} + +void ServoCalibrateTrackingVelocity::finishUHold() { + setVelocity(0); + startSettling(); + switchDirectionOrComplete(); +} + +void ServoCalibrateTrackingVelocity::switchDirectionOrComplete() { + if (calibrationDirectionIsForward) { + calibrationDirectionIsForward = false; + calibrationState = CALIBRATION_U_BREAK; + staircaseIters = 0; + calibrationVelocity = -SERVO_CALIBRATION_START_VELOCITY_PERCENT; + currentSearchVelocity = fabsf(calibrationVelocity); + foundValidMotion = false; + + queueNextTest(CALIBRATION_U_BREAK, calibrationVelocity); + LOG_HDR(axisPrefix); VLF("→ REVERSE"); + } else { + calibrationState = CALIBRATION_CHECK_IMBALANCE; + processImbalanceCheck(); + } +} + +void ServoCalibrateTrackingVelocity::processImbalanceCheck() { + snapshotAxisData(axisNumber, axisPrefix, fwdData, revData); + + if (fwdData.u_hold > 0 && revData.u_hold > 0) { + float avgTracking = (fwdData.u_hold + revData.u_hold) / 2.0f; + float imbalance = fabsf(fwdData.u_hold - revData.u_hold) / avgTracking * 100.0f; + + if (imbalance > SERVO_CALIBRATION_IMBALANCE_ERROR_THRESHOLD) { + LOG_HDR(axisPrefix); VF("imbalance "); V(imbalance); VLF("%"); + } + } + + g_axisDone[axisNumber] = true; + + uint8_t other = otherAxis(axisNumber); + if (g_axisEnabledForCal[other] && !g_axisDone[other]) { + g_currentAxisTurn = other; // pass the turn + } + + const bool peerEnabled = g_axisEnabledForCal[other]; + const bool peerDone = g_axisDone[other]; + + if (peerEnabled && !peerDone) { + calibrationState = CALIBRATION_WAIT_PEER; + setVelocity(0); + startSettling(); + return; + } + + if (!g_jointReportPrinted) { + g_jointReportPrinted = true; + printJointReport(); + } + + experimentMode = false; + calibrationState = CALIBRATION_IDLE; + setVelocity(0); + LOG_HDR(axisPrefix); VLF("done"); +} + +void ServoCalibrateTrackingVelocity::handleCalibrationFailure() { + LOG_HDR(axisPrefix); VLF("ERR reset"); + experimentMode = false; + calibrationState = CALIBRATION_IDLE; + setVelocity(0); + resetCalibrationValues(); + hasQueuedTest = false; +} + +void ServoCalibrateTrackingVelocity::startSettling() { + settleStartTime = currentTime; + motorState = MOTOR_SETTLING; +} + +void ServoCalibrateTrackingVelocity::startTest(float velocity) { + calibrationVelocity = velocity; + lastVelocityTime = currentTime; + lastVelocityMeasurement = 0; + lastStateChangeTime = currentTime; + + consecutiveMovingSamples = 0; + candidateSampleCount = 0; + + if (calibrationState == CALIBRATION_U_BREAK) { + setVelocity(calibrationVelocity); + motorState = MOTOR_HOLDING; + calibrationStepStartTime = currentTime; + calibrationStepStartTicks = currentTicks; + + LOG_HDR(axisPrefix); LOG_DIR(calibrationDirectionIsForward); + VF(PHASE_UB()); VF(" test @"); V(velocity); VLF("%"); + } else { + float u_break = currentDirectionData().u_break; + setVelocity(calibrationDirectionIsForward ? u_break : -u_break); + motorState = MOTOR_KICKING; + kickStartTime = currentTime; + + LOG_HDR(axisPrefix); LOG_DIR(calibrationDirectionIsForward); + VF(PHASE_UH()); VF(" kick@"); V(u_break); VF("% → hold@"); V(velocity); VLF("%"); + } +} + +void ServoCalibrateTrackingVelocity::setVelocity(float velocityPercent) { + experimentVelocity = constrain(velocityPercent, + -SERVO_CALIBRATION_STOP_VELOCITY_PERCENT, + +SERVO_CALIBRATION_STOP_VELOCITY_PERCENT); +} + +// -------- Getters ------------------------------------------------------------------- +float ServoCalibrateTrackingVelocity::getU_break(bool forward) { + return forward ? fwdData.u_break : revData.u_break; +} + +float ServoCalibrateTrackingVelocity::getU_hold(bool forward) { + return forward ? fwdData.u_hold : revData.u_hold; +} + +// -------- Reports ------------------------------------------------------------------- +void ServoCalibrateTrackingVelocity::printReport(const char* prefix, + const DirectionData& fwd, + const DirectionData& rev) { + VF("\n=== Calibration Report: "); V(prefix); VLF(" ==="); + VLF("Dir | u_break | u_hold"); + VLF("-----+---------------------------------+---------------------------------"); + + auto printRow = [&](const char* dir, const DirectionData& data) { + VF(dir); VF(" | "); + VF("pct "); V(data.u_break); VF("%, v "); V(data.breakVel); VF(" steps/s, cnt "); V(data.breakCounts); VF(" | "); + VF("pct "); V(data.u_hold); VF("%, v "); V(data.holdVel); VF(" steps/s, cnt "); V(data.holdCounts); VL(""); + }; + + printRow("FWD", fwd); + printRow("REV", rev); + + float breakDeltaAbs = fabsf(fwd.u_break - rev.u_break); + float holdDeltaAbs = fabsf(rev.u_hold - fwd.u_hold); + + VF("Δ u_break: "); V(breakDeltaAbs); VF("%, "); + VF("Δ u_hold: "); V(holdDeltaAbs); VLF("%"); + + if (fwd.u_hold > 0.0f && rev.u_hold > 0.0f) { + float avgTracking = (fwd.u_hold + rev.u_hold) / 2.0f; + float holdImbalancePct = (avgTracking > 0.0f) + ? (fabsf(fwd.u_hold - rev.u_hold) / avgTracking * 100.0f) + : 0.0f; + + VF("u_hold imbalance vs avg: "); V(holdImbalancePct); VLF("%"); + + if (holdImbalancePct > SERVO_CALIBRATION_IMBALANCE_ERROR_THRESHOLD) { + VF("WARN: "); V(prefix); VF(" Imbalance: "); V(holdImbalancePct); VL("%"); + } + } + + if (fwd.u_hold >= SERVO_CALIBRATION_STOP_VELOCITY_PERCENT - 0.1f) VLF("WARN: FWD velocity at calibration limit"); + if (rev.u_hold >= SERVO_CALIBRATION_STOP_VELOCITY_PERCENT - 0.1f) VLF("WARN: REV velocity at calibration limit"); + + VF("=== End Report ===\n"); +} + +void ServoCalibrateTrackingVelocity::printReport() { + printReport(axisPrefix, fwdData, revData); +} + +void ServoCalibrateTrackingVelocity::resetCalibrationValues() { + fwdData = DirectionData(); + revData = DirectionData(); +} + +#endif // SERVO_MOTOR_PRESENT && CALIBRATE_SERVO_DC diff --git a/src/lib/axis/motor/servo/dc/calibration/TrackingVelocity.h b/src/lib/axis/motor/servo/dc/calibration/TrackingVelocity.h new file mode 100755 index 0000000..c1cf4e6 --- /dev/null +++ b/src/lib/axis/motor/servo/dc/calibration/TrackingVelocity.h @@ -0,0 +1,192 @@ +// ============================== TrackingVelocity.h ============================== +#pragma once + +#include +#include "../../../../../../Common.h" + +#if defined(SERVO_MOTOR_PRESENT) && defined(CALIBRATE_SERVO_DC) + +#include "../DcServoDriver.h" + +// ----------------------------------------------------------------------------- +// Axis selection mask: +// - Bit 0 -> Axis 1, Bit 1 -> Axis 2 (i.e., 1=Axis1, 2=Axis2, 3=both). +// - 0 also means all axes using SERVO_CAL_ALL_AXES_MASK +// ----------------------------------------------------------------------------- +#ifndef CALIBRATE_SERVO_AXIS_SELECT + #define CALIBRATE_SERVO_AXIS_SELECT 0 +#endif +// When CALIBRATE_SERVO_AXIS_SELECT==0, use it as both axes +#define SERVO_CAL_ALL_AXES_MASK 0x03 // bit0=axis1, bit1=axis2 + +// ----------------------------------------------------------------------------- +// Configuration constants (tuning and safety limits) +// All units are either percentages of PWM duty (for velocities) or milliseconds +// for timing, unless stated otherwise. +// ----------------------------------------------------------------------------- +#define SERVO_CALIBRATION_START_VELOCITY_PERCENT 2.5f // Initial staircase % for u_break (per direction) +#define SERVO_CALIBRATION_STOP_VELOCITY_PERCENT 12.0f // Safety cap for commanded % during tests +#define SERVO_CALIBRATION_STAIRCASE_STEP_PERCENT 0.1f // Step size for staircase up/down +#define SERVO_CALIBRATION_MOTOR_SETTLE_TIME 300 // ms to wait after STOP to remove transients +#define SERVO_CALIBRATION_VELOCITY_SETTLE_CHECK_INTERVAL 200 // ms between HOLD samples +#define SERVO_CALIBRATION_REQUIRED_MOVING_SAMPLES 1 // need this many consecutive moving samples to accept +#define SERVO_CALIBRATION_MIN_COUNTS_PER_SAMPLE 2L // min raw encoder/step counts per sample to accept "moving" (tune for encoder) +#define SERVO_CALIBRATION_MIN_DETECTABLE_VELOCITY 0.1f // steps/s—below this we treat as "not moving" +#define SERVO_CALIBRATION_REFINE_MAX_ITERATIONS 200 // Max steps per phase (u_break/u_hold) to avoid endless loops +#define SERVO_CALIBRATION_KICK_DURATION_MS 300 // ms to apply kick at u_break before HOLD +#define SERVO_CALIBRATION_IMBALANCE_ERROR_THRESHOLD 2.0f // % threshold (vs. avg) to warn about FWD/REV imbalance +#define SERVO_CALIBRATION_TIMEOUT 10000000 // ms guard for state timing (very generous) + +// ----------------------------------------------------------------------------- +// High-level calibration states (per-axis) +// ----------------------------------------------------------------------------- +enum CalibrationState { + CALIBRATION_IDLE, // Not running + CALIBRATION_U_BREAK, // Staircase upward to overcome stiction from rest + CALIBRATION_U_HOLD, // Staircase downward to find minimum % that sustains motion + CALIBRATION_CHECK_IMBALANCE, // Compute/report per-axis FWD/REV imbalance and snapshot data + CALIBRATION_WAIT_PEER, // Finished this axis; waiting for the other axis to finish (joint report) + CALIBRATION_WAIT_TURN // Both axes selected; wait here until it's this axis' turn to start +}; + +// ----------------------------------------------------------------------------- +// Motor sub-states (lifecycle around each test step) +// ----------------------------------------------------------------------------- +enum MotorState { + MOTOR_STOPPED, // At rest; used to delimit tests or indicate failure to move (after HOLD sample) + MOTOR_SETTLING, // Waiting for transients to decay after STOP/changes (enforced pause) + MOTOR_KICKING, // Brief kick at u_break to overcome stiction for u_hold tests + MOTOR_HOLDING, // Holding candidate % and sampling velocity periodically + MOTOR_RUNNING_STEADY // ACCEPTED MOVING (kept name for compatibility) +}; + +// ----------------------------------------------------------------------------- +// Calibrator class (per axis instance) +// ----------------------------------------------------------------------------- +class ServoCalibrateTrackingVelocity { +public: + // --------------------------------------------------------------------------- + // Per-direction compact record of results/telemetry for reporting + // --------------------------------------------------------------------------- + struct DirectionData { + float u_break = 0.0f; // % that first produced motion from rest (staircase up) + float u_hold = 0.0f; // % that minimally sustains motion (after kick; staircase down) + float breakVel = 0.0f; // measured steps/s at u_break + float holdVel = 0.0f; // measured steps/s at u_hold + long breakCounts= 0; // raw delta counts measured at u_break (for diagnostics) + long holdCounts = 0; // raw delta counts measured at u_hold (for diagnostics) + bool everMoved = false;// whether any motion was detected in this direction + }; + + // axisNumber is 1-based (1=RA, 2=DEC in common mounts) + ServoCalibrateTrackingVelocity(uint8_t axisNumber); + + // One-time per-run init: clears state, resets local accumulators + void init(); + + // Start the calibration sequence for this axis. + // - trackingFrequency: target sidereal steps/s reference (informational). + // - instrumentCoordinateSteps: current encoder/step count baseline (unused here, caller passes live counts in updateState). + void start(float trackingFrequency, long instrumentCoordinateSteps); + + // Drive the state machine; call frequently from the main control loop. + void updateState(long instrumentCoordinateSteps); + + // Readouts for the final tuned duty % values + float getU_break(bool forward); // forward=true -> FWD, false -> REV + float getU_hold(bool forward); // forward=true -> FWD, false -> REV + + // Exposed experiment flags/telemetry (read-only from outside in normal use) + bool experimentMode = false; // true while calibration is running + float experimentVelocity = 0.0f; // last commanded velocity % (signed) + bool enabled = false; // axis was selected and started + + // Reports + static void printReport(const char* axisPrefix, + const struct DirectionData& fwd, + const struct DirectionData& rev); + void printReport(); + +private: + // --------------------------- Phase handlers -------------------------------- + void handleMotorState(); // sub-state machine for settle/kick/hold + void processUBreak(); // staircase upward search + void processUHold(); // staircase downward refinement + void processImbalanceCheck(); // per-axis FWD/REV imbalance + joint sync/report + void handleCalibrationFailure(); // timeout/fault recovery + + // --------------------------- Utilities ------------------------------------- + float calculateInstantaneousVelocity(); // steps/s from delta counts/time + void startSettling(); // enter MOTOR_SETTLING and timestamp + void startTest(float velocityPercent); // begin a test step (kick/hold based on phase) + void setVelocity(float velocityPercent); // clamp+store experimentVelocity (signed %) + void resetCalibrationValues(); // clear DirectionData (FWD/REV) + void proceedToUHold(); // transition helper (after u_break success) + void finishUHold(); // transition helper (after u_hold completes) + void switchDirectionOrComplete(); // flip FWD↔REV or exit to imbalance stage + + // --------------------------- Helpers --------------------------------------- + DirectionData& currentDirectionData(); // FWD or REV ref (mutable) + const DirectionData& currentDirectionData() const; // FWD or REV ref (const) + void recordMovementData(float velocity, long counts); // write last measurement + void logTestResult(const char* phase, bool moved); // compact progress log + + // Queue the next test to start right after settling + void queueNextTest(CalibrationState st, float velocity) { + queuedState = st; + queuedVelocity = velocity; + hasQueuedTest = true; + } + + // --------------------------- Hardware/context ------------------------------ + ServoDcDriver* driver = nullptr; // optional driver hook if needed by platform + uint8_t axisNumber; // 1-based axis id + char axisPrefix[32]; // used in logs (e.g., "Axis1 ServoCalibration") + + // --------------------------- State ----------------------------------------- + CalibrationState calibrationState = CALIBRATION_IDLE; + MotorState motorState = MOTOR_STOPPED; + bool calibrationDirectionIsForward = true; // FWD=true, REV=false + int candidateSampleCount = 0; // samples taken in current HOLDING candidate (UB/UH) + + + // --------------------------- Timing/telemetry ------------------------------- + unsigned long currentTime = 0; + unsigned long lastStateChangeTime = 0; + unsigned long settleStartTime = 0; + unsigned long calibrationStepStartTime = 0; + unsigned long lastVelocityTime = 0; + unsigned long kickStartTime = 0; + + long currentTicks = 0; // caller-provided instrument steps + long calibrationStepStartTicks = 0; // ticks at the start of a test step + float lastVelocityMeasurement = 0.0f; // steps/s + + unsigned long steadySinceMs = 0; // time we accepted the candidate + long steadyStartTicks = 0; // ticks when we accepted the candidate + + long lastDeltaCounts = 0; // raw delta counts from last sample + + float calibrationVelocity = 0.0f; // signed % (positive FWD, negative REV) + float targetVelocity = 0.0f; // sidereal target (steps/s), informational + + // --------------------------- Results ---------------------------------------- + DirectionData fwdData; // accumulated forward-direction results + DirectionData revData; // accumulated reverse-direction results + + // --------------------------- Search bookkeeping ---------------------------- + int staircaseIters = 0; // steps taken within current phase + float currentSearchVelocity = 0.0f; // helper for proposed % values + bool foundValidMotion = false; // latched once motion is first observed + + long lastTicks = 0; // sample baseline + unsigned long lastCheckTime = 0; // sample timestamp + int consecutiveMovingSamples = 0; // counts consecutive moving samples for a candidate + + // --------------------------- Deferred start of next test -------------------- + bool hasQueuedTest = false; // true if next test is queued + CalibrationState queuedState = CALIBRATION_IDLE; // queued phase + float queuedVelocity = 0.0f; // queued signed % +}; + +#endif // SERVO_MOTOR_PRESENT && CALIBRATE_SERVO_DC diff --git a/src/lib/axis/motor/servo/dc/eE/EE.cpp b/src/lib/axis/motor/servo/dc/eE/EE.cpp new file mode 100755 index 0000000..946af1d --- /dev/null +++ b/src/lib/axis/motor/servo/dc/eE/EE.cpp @@ -0,0 +1,92 @@ +// ----------------------------------------------------------------------------------- +// axis servo DC EE motor driver + +#include "EE.h" + +#ifdef SERVO_EE_PRESENT + +#include "../../../../../gpioEx/GpioEx.h" +#include "../../../../../analog/Analog.h" + +ServoEE::ServoEE(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum) + :ServoDcDriver(axisNumber, Pins, Settings, pwmMinimum, pwmMaximum) { + if (axisNumber < 1 || axisNumber > 9) return; + + strcpy(axisPrefix, " Axis_ServoEE, "); + axisPrefix[5] = '0' + axisNumber; +} + +bool ServoEE::init(bool reverse) { +if (!ServoDcDriver::init(reverse)) return false; + + #if DEBUG == VERBOSE + VF("MSG:"); V(axisPrefix); + V("pins pwm1="); if (Pins->ph1 == OFF) VF("OFF"); else V(Pins->ph1); + V(", pwm2="); if (Pins->ph2 == OFF) VLF("OFF"); else VL(Pins->ph2); + #endif + + // init default driver control pins + pinModeEx(Pins->ph1, OUTPUT); + digitalWriteF(Pins->ph1, Pins->ph1State); // either ph1 or direction, state should default to inactive + pinModeEx(Pins->ph2, OUTPUT); + digitalWriteF(Pins->ph2, Pins->ph2State); // either ph2 or step (PWM,) state should default to inactive + + // set PWM range & frequency + AnalogPwmConfig scfg{}; + #ifdef SERVO_ANALOG_WRITE_FREQUENCY + scfg.hz = SERVO_ANALOG_WRITE_FREQUENCY; + #endif + #ifdef SERVO_ANALOG_WRITE_RANGE + scfg.range = SERVO_ANALOG_WRITE_RANGE; + #endif + if (!analog.pwmInit(Pins->ph1, scfg)) return false; + if (!analog.pwmInit(Pins->ph2, scfg)) return false; + + return true; +} + +// enable or disable the driver using the enable pin or other method +void ServoEE::enable(bool state) { + ServoDriver::enable(state); + + VF("MSG:"); V(axisPrefix); + if (!enabled) { + VF("EE outputs off"); + + // force both outputs inactive + analog.write(Pins->ph1, off1()); + analog.write(Pins->ph2, off2()); + + VLF(""); + } else { + VLF("EE enabled"); + } + + ServoDriver::updateStatus(); +} + +void ServoEE::pwmUpdate(float power01) { + if (!enabled) return; + + if (reversed) power01 = -power01; + float duty01 = fabsf(power01); + + if (power01 >= 0.0F) { + if (Pins->ph2State == HIGH) duty01 = 1.0F - duty01; + analog.write(Pins->ph1, off1()); + analog.write(Pins->ph2, duty01); + } else { + if (Pins->ph1State == HIGH) duty01 = 1.0F - duty01; + analog.write(Pins->ph1, duty01); + analog.write(Pins->ph2, off2()); + } +} + +// update status info. for driver +void ServoEE::updateStatus() { + if (statusMode == LOW || statusMode == HIGH) { + status.fault = digitalReadEx(faultPin) == statusMode; + } +} + +#endif diff --git a/src/lib/axis/motor/servo/dc/eE/EE.h b/src/lib/axis/motor/servo/dc/eE/EE.h new file mode 100755 index 0000000..828e1da --- /dev/null +++ b/src/lib/axis/motor/servo/dc/eE/EE.h @@ -0,0 +1,41 @@ +// ----------------------------------------------------------------------------------- +// axis servo DC EE motor driver +#pragma once + +#include +#include "../../../../../../Common.h" + +#ifdef SERVO_EE_PRESENT + +#include "../DcServoDriver.h" + +class ServoEE : public ServoDcDriver { + public: + // constructor + ServoEE(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum); + + // decodes driver model and sets up the pin modes + bool init(bool reverse); + + // enable or disable the driver using the enable pin or other method + void enable(bool state); + + // update status info. for driver + void updateStatus(); + + // get the driver name + const char* name() { + if (driverModel == SERVO_EE) return "DC Enable/Enable"; else + return "?"; + } + + private: + // motor control pwm update + // \param power01 in -1.0 to 1.0 units + void pwmUpdate(float power01) override; + + inline float off1() { return (Pins->ph1State == HIGH) ? 1.0F : 0.0F; } + inline float off2() { return (Pins->ph2State == HIGH) ? 1.0F : 0.0F; } +}; + +#endif diff --git a/src/lib/axis/motor/servo/dc/pE/PE.cpp b/src/lib/axis/motor/servo/dc/pE/PE.cpp new file mode 100755 index 0000000..53566d2 --- /dev/null +++ b/src/lib/axis/motor/servo/dc/pE/PE.cpp @@ -0,0 +1,95 @@ +// ----------------------------------------------------------------------------------- +// axis servo DC PE motor driver + +#include "PE.h" + +#ifdef SERVO_PE_PRESENT + +// for a Teensy4.1 you can have one additional PWM channel on a pin (for example pin 38) +// for example in Config.h: +// +// #define PWM_CUSTOM_PIN 38 +// #define PWM_CUSTOM_ANALOG_WRITE_RANGE 4095 +// #define PWM_CUSTOM_ANALOG_WRITE_FREQUENCY 10000 +// +// then use the AXISn_SERVO_PHn_PIN as you normally would (AXIS1_SERVO_PH2_PIN 38, for example) + +#include "../../../../../gpioEx/GpioEx.h" +#include "../../../../../analog/Analog.h" + +ServoPE::ServoPE(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum) + :ServoDcDriver(axisNumber, Pins, Settings, pwmMinimum, pwmMaximum) { + if (axisNumber < 1 || axisNumber > 9) return; + + strcpy(axisPrefix, " Axis_ServoPE, "); + axisPrefix[5] = '0' + axisNumber; +} + +bool ServoPE::init(bool reverse) { + if (!ServoDcDriver::init(reverse)) return false; + + #if DEBUG == VERBOSE + VF("MSG:"); V(axisPrefix); + V("pins dir="); if (Pins->ph1 == OFF) VF("OFF"); else V(Pins->ph1); + V(", pwm="); if (Pins->ph2 == OFF) VLF("OFF"); else VL(Pins->ph2); + #endif + + // init default driver control pins + pinModeEx(Pins->ph1, OUTPUT); + digitalWriteF(Pins->ph1, Pins->ph1State); // either ph1 or direction, state should default to inactive + pinModeEx(Pins->ph2, OUTPUT); + digitalWriteF(Pins->ph2, Pins->ph2State); // either ph2 or step (PWM,) state should default to inactive + + // set PWM range & frequency + AnalogPwmConfig scfg{}; + #ifdef SERVO_ANALOG_WRITE_FREQUENCY + scfg.hz = SERVO_ANALOG_WRITE_FREQUENCY; + #endif + #ifdef SERVO_ANALOG_WRITE_RANGE + scfg.range = SERVO_ANALOG_WRITE_RANGE; + #endif + if (!analog.pwmInit(Pins->ph2, scfg)) return false; + + return true; +} + +// enable or disable the driver using the enable pin or other method +void ServoPE::enable(bool state) { + ServoDriver::enable(state); + + VF("MSG:"); V(axisPrefix); + if (!enabled) { + VF("PE outputs off"); + + // force outputs inactive + digitalWriteF(Pins->ph1, Pins->ph1State); + analog.write(Pins->ph2, off2()); + + VLF(""); + } else { + VLF("PE enabled"); + } + + ServoDriver::updateStatus(); +} + +void ServoPE::pwmUpdate(float power01) { + if (!enabled) return; + + if (reversed) power01 = -power01; + float duty01 = fabsf(power01); + + digitalWriteF(Pins->ph1, power01 >= 0.0F ? Pins->ph1State : !Pins->ph1State); + + if (Pins->ph2State == HIGH) duty01 = 1.0F - duty01; + analog.write(Pins->ph2, duty01); +} + +// update status info. for driver +void ServoPE::updateStatus() { + if (statusMode == LOW || statusMode == HIGH) { + status.fault = digitalReadEx(faultPin) == statusMode; + } +} + +#endif diff --git a/src/lib/axis/motor/servo/dc/pE/PE.h b/src/lib/axis/motor/servo/dc/pE/PE.h new file mode 100755 index 0000000..0be4ebc --- /dev/null +++ b/src/lib/axis/motor/servo/dc/pE/PE.h @@ -0,0 +1,40 @@ +// ----------------------------------------------------------------------------------- +// axis servo DC motor driver +#pragma once + +#include +#include "../../../../../../Common.h" + +#ifdef SERVO_PE_PRESENT + +#include "../DcServoDriver.h" + +class ServoPE : public ServoDcDriver { + public: + // constructor + ServoPE(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum); + + // decodes driver model and sets up the pin modes + bool init(bool reverse); + + // enable or disable the driver using the enable pin or other method + void enable(bool state); + + // update status info. for driver + void updateStatus(); + + // get the driver name + const char* name() { + if (driverModel == SERVO_PE) return "DC Phase/Enable" ; else + return "?"; + } + + private: + // motor control pwm update + // \param power01 -1.0 to 1.0 + void pwmUpdate(float power01) override; + + inline float off2() { return (Pins->ph2State == HIGH) ? 1.0F : 0.0F; } +}; + +#endif diff --git a/src/lib/axis/motor/servo/dc/tmc/tmc2130/Tmc2130.cpp b/src/lib/axis/motor/servo/dc/tmc/tmc2130/Tmc2130.cpp new file mode 100755 index 0000000..6d3ed84 --- /dev/null +++ b/src/lib/axis/motor/servo/dc/tmc/tmc2130/Tmc2130.cpp @@ -0,0 +1,103 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC2130 DC motor driver + +#include "Tmc2130.h" + +#ifdef SERVO_TMC2130_DC_PRESENT + +#include "../../../../../tasks/OnTask.h" +#include "../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +ServoTmc2130DC::ServoTmc2130DC(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum) + :ServoDcDriver(axisNumber, Pins, Settings, pwmMinimum, pwmMaximum) { + if (axisNumber < 1 || axisNumber > 9) return; + + strcpy(axisPrefix, " Axis_ServoTmc2130DC, "); + axisPrefix[5] = '0' + axisNumber; +} + +bool ServoTmc2130DC::init(bool reverse) { + if (!ServoDcDriver::init(reverse)) return false; + + // get TMC SPI driver ready + pinModeEx(Pins->ph1, OUTPUT); // step + pinModeEx(Pins->ph2, OUTPUT); // dir + + // set S/D motion pins to known, fixed state + digitalWriteEx(Pins->ph1, LOW); // step + digitalWriteEx(Pins->ph2, LOW); // dir + + VF("MSG:"); V(axisPrefix); VLF("TMC current control at max (IHOLD, IRUN, and IGOTO ignored)"); + + driver = new TMC2130Stepper(Pins->cs, Pins->mosi, Pins->miso, Pins->sck); + driver->begin(); + driver->direct_mode(true); + driver->en_pwm_mode(true); + driver->pwm_autoscale(false); + driver->pwm_ampl(255); + driver->pwm_grad(4); + driver->ihold(0); + driver->irun(31); + driver->toff(5); + + driver->XDIRECT((uint32_t)(lround(0) & 0b111111111)); + + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { DF("ERR:"); D(axisPrefix); DLF("no driver detected!"); return false; } + } + #endif + + return true; +} + +// enable or disable the driver using the enable pin or other method +void ServoTmc2130DC::enable(bool state) { + ServoDriver::enable(state); + + VF("MSG:"); V(axisPrefix); VF("powered "); if (state) { VF("up"); } else { VF("down"); } VLF(" using SPI"); + + if (state) { driver->ihold(31); } else { driver->ihold(0); } + + ServoDriver::updateStatus(); +} + +void ServoTmc2130DC::pwmUpdate(float power01) { + + // signed 9-bit value: -255..+255 (avoid -256) + int32_t power = (int32_t)lroundf(power01 * 255.0F); + if (power > 255) power = 255; else + if (power < -255) power = -255; + + if (reversed) power = -power; + + driver->XDIRECT((uint32_t)(power & 0x1FF)); // 9-bit two's complement +} + +void ServoTmc2130DC::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = 0; + + status_result.sr = driver->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = false; + status.outputB.openLoad = false; + status.overTemperatureWarning= status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +#endif diff --git a/src/lib/axis/motor/servo/dc/tmc/tmc2130/Tmc2130.h b/src/lib/axis/motor/servo/dc/tmc/tmc2130/Tmc2130.h new file mode 100755 index 0000000..f4b704a --- /dev/null +++ b/src/lib/axis/motor/servo/dc/tmc/tmc2130/Tmc2130.h @@ -0,0 +1,39 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC2130 DC motor driver +#pragma once + +#include +#include "../../../../../../../Common.h" + +#ifdef SERVO_TMC2130_DC_PRESENT + +#include "../../DcServoDriver.h" + +#include // https://github.com/teemuatlut/TMCStepper + +class ServoTmc2130DC : public ServoDcDriver { + public: + // constructor + ServoTmc2130DC(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum); + + // decodes driver model and sets up the pin modes + bool init(bool reverse); + + // enable or disable the driver using the enable pin or other method + void enable(bool state); + + // get the driver name + const char* name() { return "DC TMC2130 (SPI)"; } + + private: + // motor control pwm update + // \param power01 in -1.0 to 1.0 units + void pwmUpdate(float power01); + + // read status info. from driver + void readStatus(); + + TMC2130Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/servo/dc/tmc/tmc5160/Tmc5160.cpp b/src/lib/axis/motor/servo/dc/tmc/tmc5160/Tmc5160.cpp new file mode 100755 index 0000000..bf32917 --- /dev/null +++ b/src/lib/axis/motor/servo/dc/tmc/tmc5160/Tmc5160.cpp @@ -0,0 +1,103 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC5160 DC motor driver + +#include "Tmc5160.h" + +#ifdef SERVO_TMC5160_DC_PRESENT + +#include "../../../../../tasks/OnTask.h" +#include "../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +ServoTmc5160DC::ServoTmc5160DC(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum) + :ServoDcDriver(axisNumber, Pins, Settings, pwmMinimum, pwmMaximum) { + if (axisNumber < 1 || axisNumber > 9) return; + + strcpy(axisPrefix, " Axis_ServoTmc5160DC, "); + axisPrefix[5] = '0' + axisNumber; + } + +bool ServoTmc5160DC::init(bool reverse) { + if (!ServoDcDriver::init(reverse)) return false; + + // get TMC SPI driver ready + pinModeEx(Pins->ph1, OUTPUT); // step + pinModeEx(Pins->ph2, OUTPUT); // dir + + // set S/D motion pins to known, fixed state + digitalWriteEx(Pins->ph1, LOW); // step + digitalWriteEx(Pins->ph2, LOW); // dir + + VF("MSG:"); V(axisPrefix); VLF("TMC current control at max (IHOLD, IRUN, and IGOTO ignored)"); + + driver = new TMC5160Stepper(Pins->cs, Pins->mosi, Pins->miso, Pins->sck); + driver->begin(); + driver->direct_mode(true); + driver->en_pwm_mode(true); + driver->pwm_autoscale(false); + driver->pwm_ofs(255); + driver->pwm_grad(4); + driver->ihold(0); + driver->irun(31); + driver->toff(5); + + driver->XTARGET((uint32_t)(lround(0) & 0b111111111)); + + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { DF("ERR:"); D(axisPrefix); DLF("no driver detected!"); return false; } + } + #endif + + return true; +} + +// enable or disable the driver using the enable pin or other method +void ServoTmc5160DC::enable(bool state) { + ServoDriver::enable(state); + + VF("MSG:"); V(axisPrefix); VF("powered "); if (state) { VF("up"); } else { VF("down"); } VLF(" using SPI"); + + if (state) { driver->ihold(31); } else { driver->ihold(0); } + + ServoDriver::updateStatus(); +} + +void ServoTmc5160DC::pwmUpdate(float power01) { + + // signed 9-bit value: -255..+255 + int32_t power = (int32_t)lroundf(power01 * 255.0F); + if (power > 255) power = 255; else + if (power < -255) power = -255; + if (reversed) power = -power; + + driver->XTARGET((uint32_t)(power & 0x1FF)); // 9-bit two's complement +} + +// read status info. from driver +void ServoTmc5160DC::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = 0; + + status_result.sr = driver->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = false; + status.outputB.openLoad = false; + status.overTemperatureWarning= status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +#endif diff --git a/src/lib/axis/motor/servo/dc/tmc/tmc5160/Tmc5160.h b/src/lib/axis/motor/servo/dc/tmc/tmc5160/Tmc5160.h new file mode 100755 index 0000000..94280fd --- /dev/null +++ b/src/lib/axis/motor/servo/dc/tmc/tmc5160/Tmc5160.h @@ -0,0 +1,39 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC5160 DC motor driver +#pragma once + +#include +#include "../../../../../../../Common.h" + +#ifdef SERVO_TMC5160_DC_PRESENT + +#include "../../DcServoDriver.h" + +#include // https://github.com/teemuatlut/TMCStepper + +class ServoTmc5160DC : public ServoDcDriver { + public: + // constructor + ServoTmc5160DC(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float pwmMinimum, float pwmMaximum); + + // decodes driver model and sets up the pin modes + bool init(bool reverse); + + // enable or disable the driver using the enable pin or other method + void enable(bool state); + + // get the driver name + const char* name() {return "DC TMC5160 (SPI)"; } + + private: + // motor control pwm update + // \param power01 in -1.0 to 1.0 units + void pwmUpdate(float power01); + + // read status info. from driver + void readStatus(); + + TMC5160Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/servo/feedback/DualPid/DualPid.cpp b/src/lib/axis/motor/servo/feedback/DualPid/DualPid.cpp new file mode 100755 index 0000000..d9d1dfa --- /dev/null +++ b/src/lib/axis/motor/servo/feedback/DualPid/DualPid.cpp @@ -0,0 +1,118 @@ +// ----------------------------------------------------------------------------------- +// servo motor PID feedback + +#include "DualPid.h" + +#ifdef SERVO_MOTOR_PRESENT + +#if AXIS1_SERVO_FEEDBACK == DUAL_PID || AXIS2_SERVO_FEEDBACK == DUAL_PID || AXIS3_SERVO_FEEDBACK == DUAL_PID || \ + AXIS4_SERVO_FEEDBACK == DUAL_PID || AXIS5_SERVO_FEEDBACK == DUAL_PID || AXIS6_SERVO_FEEDBACK == DUAL_PID || \ + AXIS7_SERVO_FEEDBACK == DUAL_PID || AXIS8_SERVO_FEEDBACK == DUAL_PID || AXIS9_SERVO_FEEDBACK == DUAL_PID + +DualPid::DualPid(const float P, const float I, const float D, const float P_goto, const float I_goto, const float D_goto, const float sensitivity) { + trackingP.valueDefault = P; + trackingI.valueDefault = I; + trackingD.valueDefault = D; + + slewingP.valueDefault = P_goto; + slewingI.valueDefault = I_goto; + slewingD.valueDefault = D_goto; + + manuallySwitchParameters = (sensitivity == 0); + if (manuallySwitchParameters) this->sensitivity = 100; else this->sensitivity = sensitivity; +} + +// initialize PID control and parameters +void DualPid::init(uint8_t axisNumber, ServoControl *control) { + if (ready) return; + + Feedback::init(axisNumber, control); + + axisPrefix[5] = '0' + axisNumber; + + VF("MSG:"); V(axisPrefix); if (manuallySwitchParameters) { VL("using manual parameter switching"); } else { VL("using auto parameter scaling"); } + + pid = new QuickPID(&control->in, &control->out, &control->set, + 0, 0, 0, + QuickPID::pMode::PID_PMODE, QuickPID::dMode::PID_DMODE, QuickPID::iAwMode::PID_IMODE, + QuickPID::Action::direct); + pid->SetSampleTimeUs(PID_SAMPLE_TIME_US); + pid->Initialize(); + pid->SetMode(QuickPID::Control::automatic); +} + +// reset feedback control and parameters +void DualPid::reset() { + if (!ready) return; + + VF("MSG:"); V(axisPrefix); VLF("reset"); + control->in = 0; + control->set = 0; + control->out = 0; + trackingSelected = false; + selectTrackingParameters(); +} + +void DualPid::setControlDirection(int8_t state) { + if (!ready) return; + + if (state == ON) pid->SetControllerDirection(QuickPID::Action::reverse); else pid->SetControllerDirection(QuickPID::Action::direct); +} + +void DualPid::setControlRange(float controlRange) { + if (!ready) return; + + VF("MSG:"); V(axisPrefix); VF("setting feedback range +/-"); VL(controlRange); + pid->SetOutputLimits(-controlRange, controlRange); +} + +// select PID param set for tracking +void DualPid::selectTrackingParameters() { + if (!trackingSelected) { + VF("MSG:"); V(axisPrefix); VL("tracking selected"); + trackingSelected = true; + parameterSelectPercent = 0; + pid->Reset(); + pid->SetTunings(trackingP.value, trackingI.value, trackingD.value); + lastP = trackingP.value; + lastI = trackingI.value; + lastD = trackingD.value; + } +} + +// select PID param set for slewing +void DualPid::selectSlewingParameters() { + if (trackingSelected) { + VF("MSG:"); V(axisPrefix); VL("slewing selected"); + trackingSelected = false; + parameterSelectPercent = 100; + pid->Reset(); + pid->SetTunings(slewingP.value, slewingI.value, slewingD.value); + lastP = slewingP.value; + lastI = slewingI.value; + lastD = slewingD.value; + } +} + +// manage feedback, variable PID params +void DualPid::variableParameters(float percent) { + float s = percent/sensitivity; + if (s < 0.0F) s = 0.0F; + if (s > 1.0F) s = 1.0F; + float p = trackingP.value + (slewingP.value - trackingP.value)*s; + float i = trackingI.value + (slewingI.value - trackingI.value)*s; + float d = trackingD.value + (slewingD.value - trackingD.value)*s; + if (lastP != p || lastI != i || lastD != d) { +// VF("MSG:"); V(axisPrefix); +// V("variable parameters"); +// V(" P="); V(p); V(", I="); V(i); V(", D="); VL(d); + pid->SetTunings(p, i, d); + lastP = p; + lastI = i; + lastD = d; + } +} + +#endif + +#endif diff --git a/src/lib/axis/motor/servo/feedback/DualPid/DualPid.h b/src/lib/axis/motor/servo/feedback/DualPid/DualPid.h new file mode 100755 index 0000000..ab2a171 --- /dev/null +++ b/src/lib/axis/motor/servo/feedback/DualPid/DualPid.h @@ -0,0 +1,107 @@ +// ----------------------------------------------------------------------------------- +// servo motor PID feedback +#pragma once + +#include "../FeedbackBase.h" + +#ifdef SERVO_MOTOR_PRESENT + +#if AXIS1_SERVO_FEEDBACK == DUAL_PID || AXIS2_SERVO_FEEDBACK == DUAL_PID || AXIS3_SERVO_FEEDBACK == DUAL_PID || \ + AXIS4_SERVO_FEEDBACK == DUAL_PID || AXIS5_SERVO_FEEDBACK == DUAL_PID || AXIS6_SERVO_FEEDBACK == DUAL_PID || \ + AXIS7_SERVO_FEEDBACK == DUAL_PID || AXIS8_SERVO_FEEDBACK == DUAL_PID || AXIS9_SERVO_FEEDBACK == DUAL_PID + +#include // https://github.com/Dlloydev/QuickPID + +// various options for QuickPID read about them here https://github.com/Dlloydev/QuickPID +#ifndef PID_SLEWING_TO_TRACKING_TIME_MS + #define PID_SLEWING_TO_TRACKING_TIME_MS 1000 // time to switch from PID slewing to tracking parameters in milliseconds +#endif +#ifndef PID_SAMPLE_TIME_US + #define PID_SAMPLE_TIME_US 10000 // PID sample time in microseconds (defaults to 10 milliseconds) +#endif +#ifndef PID_PMODE + #define PID_PMODE pOnError // http://brettbeauregard.com/blog/2017/06/introducing-proportional-on-measurement/ +#endif +#ifndef PID_IMODE + #define PID_IMODE iAwCondition +#endif +#ifndef PID_DMODE + #define PID_DMODE dOnMeas +#endif + +class DualPid : public Feedback { + public: + DualPid(const float P, const float I, const float D, const float P_goto, const float I_goto, const float D_goto, const float sensitivity = 0); + + // initialize PID control and parameters + void init(uint8_t axisNumber, ServoControl *control); + + // reset feedback control and parameters + void reset(); + + // returns the number of axis parameters + uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // set feedback control direction + void setControlDirection(int8_t state); + + // set feedback control range + void setControlRange(float controlRange); + + // select PID param set for tracking + void selectTrackingParameters(); + + // select PID param set for slewing + void selectSlewingParameters(); + + // variable feedback, variable PID params + void variableParameters(float percent); + + inline void poll() { + pid->Compute(); + + if (manuallySwitchParameters) { + if ((long)(millis() - nextSelectIncrementTime) > 0) { + if (trackingSelected) parameterSelectPercent--; + if (parameterSelectPercent < 0) parameterSelectPercent = 0; + variableParameters(parameterSelectPercent); + nextSelectIncrementTime = millis() + round(PID_SLEWING_TO_TRACKING_TIME_MS/100.0F); + } + } + } + + private: + QuickPID *pid; + + char axisPrefix[26] = " Axis_ServoFeedbackDPID, "; // prefix for debug messages + + float sensitivity; + float lastP = 0; + float lastI = 0; + float lastD = 0; + + unsigned long timeSinceLastUpdate = 0; // for varaible pid update + + int parameterSelectPercent = 0; + bool trackingSelected = true; + unsigned long nextSelectIncrementTime = 0; + + // runtime adjustable settings + AxisParameter trackingP = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, AXPN_PID_P}; + AxisParameter trackingI = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, AXPN_PID_I}; + AxisParameter trackingD = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, AXPN_PID_D}; + + AxisParameter slewingP = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, AXPN_PID_SLEWING_P}; + AxisParameter slewingI = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, AXPN_PID_SLEWING_I}; + AxisParameter slewingD = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, AXPN_PID_SLEWING_D}; + + const int numParameters = 6; + AxisParameter* parameter[7] = {&invalid, &trackingP, &trackingI, &trackingD, &slewingP, &slewingI, &slewingD}; +}; + +#endif + +#endif diff --git a/src/lib/axis/motor/servo/feedback/Feedback.h b/src/lib/axis/motor/servo/feedback/Feedback.h new file mode 100755 index 0000000..38ef59d --- /dev/null +++ b/src/lib/axis/motor/servo/feedback/Feedback.h @@ -0,0 +1,5 @@ +// ----------------------------------------------------------------------------------- +// servo motor feedback +#pragma once + +#include "DualPid/DualPid.h" diff --git a/src/lib/axis/motor/servo/feedback/FeedbackBase.cpp b/src/lib/axis/motor/servo/feedback/FeedbackBase.cpp new file mode 100755 index 0000000..813209c --- /dev/null +++ b/src/lib/axis/motor/servo/feedback/FeedbackBase.cpp @@ -0,0 +1,19 @@ +// ----------------------------------------------------------------------------------- +// servo motor feedback + +#include "FeedbackBase.h" + +#ifdef SERVO_MOTOR_PRESENT + +// initialize feedback control and parameters +void Feedback::init(uint8_t axisNumber, ServoControl *control) { + this->axisNumber = axisNumber; + + this->control = control; + control->in = 0; + control->set = 0; + + ready = true; +} + +#endif \ No newline at end of file diff --git a/src/lib/axis/motor/servo/feedback/FeedbackBase.h b/src/lib/axis/motor/servo/feedback/FeedbackBase.h new file mode 100755 index 0000000..90e24ab --- /dev/null +++ b/src/lib/axis/motor/servo/feedback/FeedbackBase.h @@ -0,0 +1,90 @@ +// ----------------------------------------------------------------------------------- +// servo motor feedback +#pragma once +#include "../../../../../Common.h" + +#ifdef SERVO_MOTOR_PRESENT + +#include "../../Drivers.h" + +#ifndef AXIS1_SERVO_FEEDBACK + #define AXIS1_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS2_SERVO_FEEDBACK + #define AXIS2_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS3_SERVO_FEEDBACK + #define AXIS3_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS4_SERVO_FEEDBACK + #define AXIS4_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS5_SERVO_FEEDBACK + #define AXIS5_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS6_SERVO_FEEDBACK + #define AXIS6_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS7_SERVO_FEEDBACK + #define AXIS7_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS8_SERVO_FEEDBACK + #define AXIS8_SERVO_FEEDBACK DUAL_PID +#endif +#ifndef AXIS9_SERVO_FEEDBACK + #define AXIS9_SERVO_FEEDBACK DUAL_PID +#endif + +typedef struct ServoControl { + float in; + float out; + float set; + volatile int8_t directionHint; +} ServoControl; + +class Feedback { + public: + // initialize feedback control and parameters + virtual void init(uint8_t axisNumber, ServoControl *control); + + // reset feedback control and parameters + virtual void reset(); + + // returns the number of axis parameters + virtual uint8_t getParameterCount() { return 0; } + + // returns the specified axis parameter + virtual AxisParameter* getParameter(uint8_t number) { return &invalid; } + + // select feedback PID param set for tracking + virtual void selectTrackingParameters(); + + // select feedback PID param set for slewing + virtual void selectSlewingParameters(); + + // variable feedback PID params + virtual void variableParameters(float percent); + + // set feedback control direction + virtual void setControlDirection(int8_t state); + + // set feedback control range controlRange, +/- the maximum encoder counts/s + virtual void setControlRange(float controlRange); + + virtual void poll(); + + bool manuallySwitchParameters = true; + + // true if the feedback instance is ready to use + bool ready = false; + + protected: + uint8_t axisNumber = 0; + + ServoControl *control; + + // runtime adjustable settings + AxisParameter invalid = {NAN, NAN, NAN, NAN, NAN, AXP_INVALID, ""}; +}; + +#endif \ No newline at end of file diff --git a/src/lib/axis/motor/servo/feedback/Pid/Pid.cpp b/src/lib/axis/motor/servo/feedback/Pid/Pid.cpp new file mode 100755 index 0000000..92b247c --- /dev/null +++ b/src/lib/axis/motor/servo/feedback/Pid/Pid.cpp @@ -0,0 +1,81 @@ +// ----------------------------------------------------------------------------------- +// servo motor PID feedback + +#include "Pid.h" + +#ifdef SERVO_MOTOR_PRESENT + +#if AXIS1_SERVO_FEEDBACK == PID || AXIS2_SERVO_FEEDBACK == PID || AXIS3_SERVO_FEEDBACK == PID || \ + AXIS4_SERVO_FEEDBACK == PID || AXIS5_SERVO_FEEDBACK == PID || AXIS6_SERVO_FEEDBACK == PID || \ + AXIS7_SERVO_FEEDBACK == PID || AXIS8_SERVO_FEEDBACK == PID || AXIS9_SERVO_FEEDBACK == PID + +Pid::Pid(const float P, const float I, const float D) { + trackingP.valueDefault = P; + trackingI.valueDefault = I; + trackingD.valueDefault = D; +} + +// initialize PID control and parameters +void DualPid::init(uint8_t axisNumber, ServoControl *control) { + if (ready) return; + + Feedback::init(axisNumber, control); + + axisPrefix[5] = '0' + axisNumber; + + pid = new QuickPID(&control->in, &control->out, &control->set, + 0, 0, 0, + QuickPID::pMode::PID_PMODE, QuickPID::dMode::PID_DMODE, QuickPID::iAwMode::PID_IMODE, + QuickPID::Action::direct); + pid->SetSampleTimeUs(PID_SAMPLE_TIME_US); + pid->Initialize(); + pid->SetMode(QuickPID::Control::automatic); +} + +// reset feedback control and parameters +void Pid::reset() { + VF("MSG:"); V(axisPrefix); VLF("reset"); + control->in = 0; + control->set = 0; + control->out = 0; + trackingSelected = false; + selectTrackingParameters(); +} + +void DualPid::setControlDirection(int8_t state) { + if (state == ON) pid->SetControllerDirection(QuickPID::Action::reverse); else pid->SetControllerDirection(QuickPID::Action::direct); +} + +void Pid::setControlRange(float controlRange) { + VF("MSG:"); V(axisPrefix); VF("setting feedback range +/-"); VL(controlRange); + pid->SetOutputLimits(-controlRange, controlRange); +} + +// select PID param set for tracking +void Pid::selectTrackingParameters() { + if (!trackingSelected) { + VF("MSG:"); V(axisPrefix); VL("tracking selected"); + trackingSelected = true; + pid->Reset(); + pid->SetTunings(P.value, I.value, D.value); + } +} + +// select PID param set for slewing +void Pid::selectSlewingParameters() { + if (trackingSelected) { + VF("MSG:"); V(axisPrefix); VL("slewing selected"); + trackingSelected = false; + pid->Reset(); + pid->SetTunings(P.value, I.value, D.value); + } +} + +// variable PID params isn't used +void Pid::variableParameters(float percent) { + UNUSED(percent); +} + +#endif + +#endif diff --git a/src/lib/axis/motor/servo/feedback/Pid/Pid.h b/src/lib/axis/motor/servo/feedback/Pid/Pid.h new file mode 100755 index 0000000..dd937ae --- /dev/null +++ b/src/lib/axis/motor/servo/feedback/Pid/Pid.h @@ -0,0 +1,80 @@ +// ----------------------------------------------------------------------------------- +// servo motor PID feedback +#pragma once + +#include "../FeedbackBase.h" + +#ifdef SERVO_MOTOR_PRESENT + +#if AXIS1_SERVO_FEEDBACK == PID || AXIS2_SERVO_FEEDBACK == PID || AXIS3_SERVO_FEEDBACK == PID || \ + AXIS4_SERVO_FEEDBACK == PID || AXIS5_SERVO_FEEDBACK == PID || AXIS6_SERVO_FEEDBACK == PID || \ + AXIS7_SERVO_FEEDBACK == PID || AXIS8_SERVO_FEEDBACK == PID || AXIS9_SERVO_FEEDBACK == PID + +#include // https://github.com/Dlloydev/QuickPID + +// various options for QuickPID read about them here https://github.com/Dlloydev/QuickPID +#ifndef PID_SAMPLE_TIME_US + #define PID_SAMPLE_TIME_US 10000 // PID sample time in microseconds (defaults to 10 milliseconds) +#endif +#ifndef PID_PMODE + #define PID_PMODE pOnError // http://brettbeauregard.com/blog/2017/06/introducing-proportional-on-measurement/ +#endif +#ifndef PID_IMODE + #define PID_IMODE iAwCondition +#endif +#ifndef PID_DMODE + #define PID_DMODE dOnMeas +#endif + +class Pid : public Feedback { + public: + Pid(const float P, const float I, const float D); + + // initialize PID control and parameters + void init(uint8_t axisNumber, ServoControl *control); + + // reset feedback control and parameters + void reset(); + + // returns the number of axis parameters + uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // set feedback control direction + void setControlDirection(int8_t state); + + // set feedback control range + void setControlRange(float controlRange); + + // select PID param set for tracking + void selectTrackingParameters(); + + // select PID param set for slewing + void selectSlewingParameters(); + + // variable feedback, variable PID params + void variableParameters(float percent); + + inline void poll() { } + + private: + QuickPID *pid; + + char axisPrefix[26] = " Axis_ServoFeedbackPID, "; // prefix for debug messages + + bool trackingSelected = true; + + // runtime adjustable settings + AxisParameter P = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, "P"}; + AxisParameter I = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, "I"}; + AxisParameter D = {NAN, NAN, NAN, 0.0, 10000.0, AXP_FLOAT_IMMEDIATE, "D"}; + + const int numParameters = 3; + AxisParameter* parameter[4] = {&invalid, &P, &I, &D}; +}; + +#endif + +#endif diff --git a/src/lib/axis/motor/servo/filter/Filter.h b/src/lib/axis/motor/servo/filter/Filter.h new file mode 100755 index 0000000..9f36502 --- /dev/null +++ b/src/lib/axis/motor/servo/filter/Filter.h @@ -0,0 +1,7 @@ +// ----------------------------------------------------------------------------------- +// servo filter +#pragma once + +#include "Kalman/Kalman.h" +#include "Learning/Learning.h" +#include "Rolling/Rolling.h" diff --git a/src/lib/axis/motor/servo/filter/FilterBase.cpp b/src/lib/axis/motor/servo/filter/FilterBase.cpp new file mode 100755 index 0000000..08304a4 --- /dev/null +++ b/src/lib/axis/motor/servo/filter/FilterBase.cpp @@ -0,0 +1,4 @@ +// ----------------------------------------------------------------------------------- +// servo filter + +#include "FilterBase.h" diff --git a/src/lib/axis/motor/servo/filter/FilterBase.h b/src/lib/axis/motor/servo/filter/FilterBase.h new file mode 100755 index 0000000..9823c2b --- /dev/null +++ b/src/lib/axis/motor/servo/filter/FilterBase.h @@ -0,0 +1,41 @@ +// ----------------------------------------------------------------------------------- +// servo filter +#pragma once + +#include "../../../../../Common.h" + +#ifndef AXIS1_SERVO_FLTR + #define AXIS1_SERVO_FLTR OFF +#endif +#ifndef AXIS2_SERVO_FLTR + #define AXIS2_SERVO_FLTR OFF +#endif +#ifndef AXIS3_SERVO_FLTR + #define AXIS3_SERVO_FLTR OFF +#endif +#ifndef AXIS4_SERVO_FLTR + #define AXIS4_SERVO_FLTR OFF +#endif +#ifndef AXIS5_SERVO_FLTR + #define AXIS5_SERVO_FLTR OFF +#endif +#ifndef AXIS6_SERVO_FLTR + #define AXIS6_SERVO_FLTR OFF +#endif +#ifndef AXIS7_SERVO_FLTR + #define AXIS7_SERVO_FLTR OFF +#endif +#ifndef AXIS8_SERVO_FLTR + #define AXIS8_SERVO_FLTR OFF +#endif +#ifndef AXIS9_SERVO_FLTR + #define AXIS9_SERVO_FLTR OFF +#endif + +class Filter { + public: + virtual long update(long encoderCounts, long motorCounts, bool isTracking) { UNUSED(motorCounts); UNUSED(isTracking); return encoderCounts; } + + private: + bool initialized = false; +}; diff --git a/src/lib/axis/motor/servo/filter/Kalman/Kalman.cpp b/src/lib/axis/motor/servo/filter/Kalman/Kalman.cpp new file mode 100755 index 0000000..769430c --- /dev/null +++ b/src/lib/axis/motor/servo/filter/Kalman/Kalman.cpp @@ -0,0 +1,29 @@ +// ----------------------------------------------------------------------------------- +// kalman filter + +#include "Kalman.h" + +#if AXIS1_SERVO_FLTR == KALMAN || AXIS2_SERVO_FLTR == KALMAN || AXIS3_SERVO_FLTR == KALMAN || \ + AXIS4_SERVO_FLTR == KALMAN || AXIS5_SERVO_FLTR == KALMAN || AXIS6_SERVO_FLTR == KALMAN || \ + AXIS7_SERVO_FLTR == KALMAN || AXIS8_SERVO_FLTR == KALMAN || AXIS9_SERVO_FLTR == KALMAN + +KalmanFilter::KalmanFilter(float measurementUncertainty, float variance) { + kalmanFilter = new SimpleKalmanFilter(measurementUncertainty, measurementUncertainty, variance); +} + +long KalmanFilter::update(long encoderCounts, long motorCounts, bool isTracking) { + UNUSED(isTracking); + + long delta = encoderCounts - motorCounts; + + if (!initialized) { + result = encoderCounts - motorCounts; + initialized = true; + } else { + result = kalmanFilter->updateEstimate(delta); + } + + return result + motorCounts; +} + +#endif diff --git a/src/lib/axis/motor/servo/filter/Kalman/Kalman.h b/src/lib/axis/motor/servo/filter/Kalman/Kalman.h new file mode 100755 index 0000000..3cd5ad9 --- /dev/null +++ b/src/lib/axis/motor/servo/filter/Kalman/Kalman.h @@ -0,0 +1,26 @@ +// ----------------------------------------------------------------------------------- +// kalman filter +#pragma once + +#include "../FilterBase.h" + +#if AXIS1_SERVO_FLTR == KALMAN || AXIS2_SERVO_FLTR == KALMAN || AXIS3_SERVO_FLTR == KALMAN || \ + AXIS4_SERVO_FLTR == KALMAN || AXIS5_SERVO_FLTR == KALMAN || AXIS6_SERVO_FLTR == KALMAN || \ + AXIS7_SERVO_FLTR == KALMAN || AXIS8_SERVO_FLTR == KALMAN || AXIS9_SERVO_FLTR == KALMAN + +#include // https://github.com/denyssene/SimpleKalmanFilter + +class KalmanFilter : public Filter { + public: + KalmanFilter(float measurementUncertainty, float variance); + + long update(long encoderCounts, long motorCounts, bool isTracking); + + private: + bool initialized = false; + long long result; + + SimpleKalmanFilter *kalmanFilter; +}; + +#endif diff --git a/src/lib/axis/motor/servo/filter/Learning/Learning.cpp b/src/lib/axis/motor/servo/filter/Learning/Learning.cpp new file mode 100755 index 0000000..d28ee20 --- /dev/null +++ b/src/lib/axis/motor/servo/filter/Learning/Learning.cpp @@ -0,0 +1,198 @@ +// ----------------------------------------------------------------------------------- +// learning filter (experimental) +// +// This is not a conventional noise filter. +// It attempts to learn a repeatable cosine-like periodic error in the +// encoder-minus-motor delta for the RA axis and subtract part of that learned +// pattern back out. It should be treated as experimental cyclic-error +// compensation rather than a general-purpose smoothing filter. + +#include "Learning.h" + +#if AXIS1_SERVO_FLTR == LEARNING + +#include "../../../../tasks/OnTask.h" + +void learningAnalyze() { filterAxis1.analyze(); } + +LearningFilter::LearningFilter(int smoothingWindowSize, int historyLength) { + this->smoothingWindowSize = smoothingWindowSize; + this->historyLength = historyLength; + + deltaSamples = new long[this->historyLength]; + + if (deltaSamples == NULL) { active = false; return; } else active = true; + + trackingCountsPerSecond = (AXIS1_STEPS_PER_DEGREE/240.0F)*SIDEREAL_RATIO_F; + sampleSpacingCounts = trackingCountsPerSecond/4.0F; + sampleSpacingSeconds = sampleSpacingCounts/trackingCountsPerSecond; +} + +long LearningFilter::wrappedSampleIndex(long sample) const { + long wrapped = sample % historyLength; + if (wrapped < 0) wrapped += historyLength; + return wrapped; +} + +float LearningFilter::periodicCorrection(long sample) const { + if (!lock || averagePeriodSamples <= 0.0F) return 0.0F; + + float phaseOffset = fmodf((sample - averagePhaseSample), averagePeriodSamples); + if (phaseOffset < 0.0F) phaseOffset += averagePeriodSamples; + + return cosf((phaseOffset/averagePeriodSamples)*PI*2.0F) * (averageAmplitude*0.4F); +} + +long LearningFilter::update(long encoderCounts, long motorCounts, bool learningActive) { + if (!active) return encoderCounts; + + long delta = encoderCounts - motorCounts; + + if (!initialized) { + reset(); + + smoothedDelta = delta; + smoothedEncoderCounts = encoderCounts; + + VF("MSG: Filter, learning start polling task (rate 5s priority 7)... "); + if (tasks.add(2000, 0, true, 7, learningAnalyze, "Learn")) { VLF("success"); } else { VLF("FAILED!"); } + + initialized = true; + } + + smoothedDelta = (smoothedDelta*(smoothingWindowSize - 1) + delta)/smoothingWindowSize; + smoothedEncoderCounts = (smoothedEncoderCounts*(smoothingWindowSize - 1) + encoderCounts)/smoothingWindowSize; + + sampleIndex = lround(smoothedEncoderCounts/sampleSpacingCounts); + + // Take samples only while the motion is stable enough to learn from. + if (learningActive && sampleIndex != lastLearnedSampleIndex) { + lastLearnedSampleIndex = sampleIndex; + + deltaSamples[wrappedSampleIndex(sampleIndex)] = smoothedDelta; + } + + learnedCorrection = lroundf(periodicCorrection(sampleIndex)); + + return (smoothedDelta - learnedCorrection) + motorCounts; +} + +void LearningFilter::analyze() { + if (!active) return; + + // Find the strongest positive and negative error sample in the learned history. + long maxDelta = deltaSamples[wrappedSampleIndex(sampleIndex)]; + long minDelta = maxDelta; + long strongestPhaseSample = wrappedSampleIndex(sampleIndex); + for (int i = 0; i < historyLength; i++) { + int j = wrappedSampleIndex(sampleIndex - i); + if (deltaSamples[j] >= maxDelta) { maxDelta = deltaSamples[j]; strongestPhaseSample = j; } + if (deltaSamples[j] <= minDelta) { minDelta = deltaSamples[j]; } + Y; + } + phaseSample = strongestPhaseSample; + + averagePeakDelta = ((averagePeakDelta*19.0F) + maxDelta)/20.0F; + averageValleyDelta = ((averageValleyDelta*19.0F) + minDelta)/20.0F; + averageAmplitude = (averagePeakDelta - averageValleyDelta)/2.0F; + + float error; + float sampleDelta; + error = 0; + for (int i = 0; i < historyLength; i++) { + int j = wrappedSampleIndex(phaseSample - i); + sampleDelta = deltaSamples[j]; + error += sampleDelta*sampleDelta; + Y; + } + error /= historyLength; + float nativeError = error; + + float bestError = 100000; + float bestPeriod = 0; + for (int period = 200; period > 20; period -= 1) { + error = 0; + for (int i = 0; i < historyLength; i++) { + int j = wrappedSampleIndex(phaseSample - i); + sampleDelta = deltaSamples[j] - cosf(((i/(float)historyLength)*(period/20.0F))*(float)(PI)*2.0F)*averageAmplitude; + error += sampleDelta*sampleDelta; + Y; + } + error /= historyLength; + + if (error < bestError) { + bestPeriod = historyLength/(period/20.0F); + bestError = error; + } + } + + if (averagePhaseSample == 0) { + averagePhaseSample = phaseSample; + } else { + float phaseForAverage = (float)phaseSample; + float avgPhaseForAverage = (float)averagePhaseSample; + + // Smooth phase on a circular history buffer without jumping at wraparound. + if (labs(phaseSample - averagePhaseSample) > historyLength/2) { + if (phaseSample < averagePhaseSample) phaseForAverage += historyLength; else avgPhaseForAverage += historyLength; + } + + averagePhaseSample = wrappedSampleIndex(lroundf((avgPhaseForAverage*19.0F + phaseForAverage)/20.0F)); + } + if (averagePeriodSamples == 0) averagePeriodSamples = bestPeriod; + averagePeriodSamples = ((averagePeriodSamples*19.0F) + bestPeriod)/20.0F; + + long phaseDistance = labs(phaseSample - averagePhaseSample); + if (phaseDistance > historyLength/2) phaseDistance = historyLength - phaseDistance; + const long phaseTolerance = max(2L, lroundf(averagePeriodSamples*0.1F)); + + if (bestError < nativeError && + averagePeriodSamples > 0 && + bestPeriod/averagePeriodSamples > 0.8F && bestPeriod/averagePeriodSamples < 1.2F && + phaseDistance <= phaseTolerance) + { + lock = true; + } else { + lock = false; + } + + if (lock) { D("Lock: true "); } else { D("Lock: false"); } + D(", Qual: "); D((bestError > 0.0F) ? sqrtf(nativeError/bestError)*100.0F : 0.0F); D("%"); + D(", Period s: "); D(bestPeriod*sampleSpacingSeconds); D(", Period avg s: "); D(averagePeriodSamples*sampleSpacingSeconds); + D(", Phase: "); D((averagePeriodSamples > 0.0F) ? fmodf((float)phaseSample, averagePeriodSamples)/averagePeriodSamples : 0.0F); + D(", Phase avg: "); DL((averagePeriodSamples > 0.0F) ? fmodf((float)averagePhaseSample, averagePeriodSamples)/averagePeriodSamples : 0.0F); + + +// VF("MSG: Native error (sd) "); VL(sqrt(nativeError)); +// VF("MSG: Best error "); VL(sqrt(bestError)); +// VF("MSG: Default frame (samples)"); VL(historyLength); +// VF("MSG: B Period (samples) "); VL(bestPeriod); +// VF("MSG: A Period (samples) "); VL(averagePeriodSamples); +// VF("MSG: Phase (samples) "); VL(averagePhaseSample); +// VF("MSG: Phase cos() "); VL(cos((averagePhaseSample/averagePeriodSamples)*PI*2.0F)); +// VF("MSG: Amplitude (counts) "); VL(averageAmplitude); +// VL(""); + +} + +void LearningFilter::reset() { + if (!active) return; + + phaseSample = 0; + averagePhaseSample = 0; + learnedCorrection = 0; + averagePeakDelta = 0; + averageValleyDelta = 0; + averageAmplitude = 0; + averagePeriodSamples = 0; + sampleIndex = 0; + lastLearnedSampleIndex = 0; + lock = false; + for (int i = 0; i < historyLength; i++) { + deltaSamples[i] = 0; + } +} + +LearningFilter filterAxis1(AXIS1_SERVO_FLTR_WSIZE, 240); + +#endif diff --git a/src/lib/axis/motor/servo/filter/Learning/Learning.h b/src/lib/axis/motor/servo/filter/Learning/Learning.h new file mode 100755 index 0000000..f5614bb --- /dev/null +++ b/src/lib/axis/motor/servo/filter/Learning/Learning.h @@ -0,0 +1,51 @@ +// ----------------------------------------------------------------------------------- +// learning filter (experimental) +#pragma once + +#include "../FilterBase.h" + +#if AXIS1_SERVO_FLTR == LEARNING + +class LearningFilter : public Filter { + public: + // smoothingWindowSize: sample count for the rolling average + // historyLength: number of learned samples retained for periodic analysis + // learningActive: true when the motion is stable enough to train on + LearningFilter(int smoothingWindowSize, int historyLength); + + long update(long encoderCounts, long motorCounts, bool learningActive); + + void analyze(); + + void reset(); + + private: + long wrappedSampleIndex(long sample) const; + float periodicCorrection(long sample) const; + + bool active = false; + bool initialized = false; + bool lock = false; + int smoothingWindowSize; // in samples + int historyLength; // in samples + + float trackingCountsPerSecond; + float sampleSpacingCounts; + float sampleSpacingSeconds; + long phaseSample = 0; + long averagePhaseSample = 0; + long learnedCorrection = 0; + float averagePeakDelta = 0; + float averageValleyDelta = 0; + float averageAmplitude = 0; + float averagePeriodSamples = 0; + long *deltaSamples; + long sampleIndex = 0; + long long smoothedDelta; + long long smoothedEncoderCounts; + long lastLearnedSampleIndex = 0; +}; + +extern LearningFilter filterAxis1; + +#endif diff --git a/src/lib/axis/motor/servo/filter/Rolling/Rolling.cpp b/src/lib/axis/motor/servo/filter/Rolling/Rolling.cpp new file mode 100755 index 0000000..8ac0252 --- /dev/null +++ b/src/lib/axis/motor/servo/filter/Rolling/Rolling.cpp @@ -0,0 +1,28 @@ +// ----------------------------------------------------------------------------------- +// rolling average filter + +#include "Rolling.h" + +#if AXIS1_SERVO_FLTR == ROLLING || AXIS2_SERVO_FLTR == ROLLING || AXIS3_SERVO_FLTR == ROLLING || \ + AXIS4_SERVO_FLTR == ROLLING || AXIS5_SERVO_FLTR == ROLLING || AXIS6_SERVO_FLTR == ROLLING || \ + AXIS7_SERVO_FLTR == ROLLING || AXIS8_SERVO_FLTR == ROLLING || AXIS9_SERVO_FLTR == ROLLING + +RollingFilter::RollingFilter(int size) { + this->size = size; +} + +long RollingFilter::update(long encoderCounts, long motorCounts, bool isTracking) { + UNUSED(isTracking); + + long delta = encoderCounts - motorCounts; + + if (!initialized) { + result = encoderCounts - motorCounts; + initialized = true; + } + + result = (result*(size - 1) + delta)/size; + return result + motorCounts; +} + +#endif diff --git a/src/lib/axis/motor/servo/filter/Rolling/Rolling.h b/src/lib/axis/motor/servo/filter/Rolling/Rolling.h new file mode 100755 index 0000000..9d76bf9 --- /dev/null +++ b/src/lib/axis/motor/servo/filter/Rolling/Rolling.h @@ -0,0 +1,23 @@ +// ----------------------------------------------------------------------------------- +// rolling average filter +#pragma once + +#include "../FilterBase.h" + +#if AXIS1_SERVO_FLTR == ROLLING || AXIS2_SERVO_FLTR == ROLLING || AXIS3_SERVO_FLTR == ROLLING || \ + AXIS4_SERVO_FLTR == ROLLING || AXIS5_SERVO_FLTR == ROLLING || AXIS6_SERVO_FLTR == ROLLING || \ + AXIS7_SERVO_FLTR == ROLLING || AXIS8_SERVO_FLTR == ROLLING || AXIS9_SERVO_FLTR == ROLLING + +class RollingFilter : public Filter { + public: + RollingFilter(int size); + + long update(long encoderCounts, long motorCounts, bool isTracking); + + private: + bool initialized = false; + int size; + long long result; +}; + +#endif diff --git a/src/lib/axis/motor/servo/kTech/KTech.cpp b/src/lib/axis/motor/servo/kTech/KTech.cpp new file mode 100755 index 0000000..9485116 --- /dev/null +++ b/src/lib/axis/motor/servo/kTech/KTech.cpp @@ -0,0 +1,177 @@ +// ----------------------------------------------------------------------------------- +// axis servo KTech motor driver (designed for the MS4010v3 with 16bit encoder) + +#include "KTech.h" + +#ifdef SERVO_KTECH_PRESENT + +#if !defined(CAN_PLUS) || CAN_PLUS == OFF + #error "No KTECH motor CAN interface!" +#endif + +#include "../../../../tasks/OnTask.h" +#include "../../../../canPlus/CanPlus.h" + +ServoKTech *ktechServoInstance[9]; +void requestKTechServoAxis1() { ktechServoInstance[0]->requestStatus(); } +void requestKTechServoAxis2() { ktechServoInstance[1]->requestStatus(); } +void requestKTechServoAxis3() { ktechServoInstance[2]->requestStatus(); } +void requestKTechServoAxis4() { ktechServoInstance[3]->requestStatus(); } +void requestKTechServoAxis5() { ktechServoInstance[4]->requestStatus(); } +void requestKTechServoAxis6() { ktechServoInstance[5]->requestStatus(); } +void requestKTechServoAxis7() { ktechServoInstance[6]->requestStatus(); } +void requestKTechServoAxis8() { ktechServoInstance[7]->requestStatus(); } +void requestKTechServoAxis9() { ktechServoInstance[8]->requestStatus(); } + +void statusKTechServoAxis1(uint8_t data[8]) { ktechServoInstance[0]->requestStatusCallback(data); } +void statusKTechServoAxis2(uint8_t data[8]) { ktechServoInstance[1]->requestStatusCallback(data); } +void statusKTechServoAxis3(uint8_t data[8]) { ktechServoInstance[2]->requestStatusCallback(data); } +void statusKTechServoAxis4(uint8_t data[8]) { ktechServoInstance[3]->requestStatusCallback(data); } +void statusKTechServoAxis5(uint8_t data[8]) { ktechServoInstance[4]->requestStatusCallback(data); } +void statusKTechServoAxis6(uint8_t data[8]) { ktechServoInstance[5]->requestStatusCallback(data); } +void statusKTechServoAxis7(uint8_t data[8]) { ktechServoInstance[6]->requestStatusCallback(data); } +void statusKTechServoAxis8(uint8_t data[8]) { ktechServoInstance[7]->requestStatusCallback(data); } +void statusKTechServoAxis9(uint8_t data[8]) { ktechServoInstance[8]->requestStatusCallback(data); } + +ServoKTech::ServoKTech(uint8_t axisNumber, const ServoSettings *Settings, float countsToStepsRatio) + :ServoDriver(axisNumber, NULL, Settings) { + if (axisNumber < 1 || axisNumber > 9) return; + + strcpy(axisPrefix, " Axis_ServoKTech, "); + axisPrefix[5] = '0' + axisNumber; + + // the motor CAN ID is the axis number! + canID = 0x140 + axisNumber; + + this->countsToStepsRatio.valueDefault = countsToStepsRatio; + + ktechServoInstance[this->axisNumber - 1] = this; + switch (this->axisNumber) { + case 1: callback = requestKTechServoAxis1; break; + case 2: callback = requestKTechServoAxis2; break; + case 3: callback = requestKTechServoAxis3; break; + case 4: callback = requestKTechServoAxis4; break; + case 5: callback = requestKTechServoAxis5; break; + case 6: callback = requestKTechServoAxis6; break; + case 7: callback = requestKTechServoAxis7; break; + case 8: callback = requestKTechServoAxis8; break; + case 9: callback = requestKTechServoAxis9; break; + } +} + +bool ServoKTech::init(bool reverse) { + if (!ServoDriver::init(reverse)) return false; + + // automatically set fault status for known drivers + status.active = statusMode != OFF; + + // get ready for status messages + if (statusMode == ON) { + + switch (this->axisNumber) { + case 1: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis1); break; + case 2: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis2); break; + case 3: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis3); break; + case 4: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis4); break; + case 5: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis5); break; + case 6: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis6); break; + case 7: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis7); break; + case 8: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis8); break; + case 9: canPlus.callbackRegisterMessage(canID, 0x9a, statusKTechServoAxis9); break; + } + + VF("MSG:"); V(axisPrefix); VF("start callback status request task (rate "); V(KTECH_STATUS_MS); VF("ms priority 7)... "); + char name[] = "Ax_StaK"; + name[2] = axisNumber + '0'; + if (tasks.add(KTECH_STATUS_MS, 0, true, 7, callback, name)) { VLF("success"); } else { VLF("FAILED!"); } + } else { + VF("MSG:"); V(axisPrefix); VLF("no driver status"); + } + + return true; +} + +// enable or disable the driver using the enable pin or other method +void ServoKTech::enable(bool state) { + enabled = state; + + VF("MSG:"); V(axisPrefix); VF("powered "); + + static const uint8_t cmd[] = "\xa2\x00\x00\x00\x00\x00\x00\x00"; + canPlus.txWait(); + canPlus.writePacket(canID, cmd, 8); + + if (state) { + static const uint8_t cmd[] = "\x88\x00\x00\x00\x00\x00\x00\x00"; + canPlus.txWait(); + canPlus.writePacket(canID, cmd, 8); + VLF("up"); + } else { + static const uint8_t cmd[] = "\x80\x00\x00\x00\x00\x00\x00\x00"; + canPlus.txWait(); + canPlus.writePacket(canID, cmd, 8); + VLF("down"); + } + + velocityRamp = 0.0F; +} + +// velocity is in units of 0.01 deg/s +// so with an encoder of 65536 counts per turn into 360:1 reduction (65536 counts per degree) = a velocity of 36000 motor "steps"/s +float ServoKTech::setMotorVelocity(float velocity) { + velocity = ServoDriver::setMotorVelocity(velocity); + + int32_t velocityNext = lroundf(velocity*countsToStepsRatio.value); + if (reversed) velocityNext = -velocityNext; + + if (canPlus.txTryLock()) { + static const uint8_t cmd[] = "\xa2\x00\x00\x00"; + canPlus.beginPacket(canID); + canPlus.write(cmd, 4); + canPlus.write((uint8_t*)&velocityNext, 4); + canPlus.endPacket(); + velocityLast = velocityNext; + canPlus.rxBurst(1000); + } + + return velocity; +} + +// request driver status from CAN +void ServoKTech::requestStatus() { + static const uint8_t cmd[] = "\x9a\x00\x00\x00\x00\x00\x00\x00"; + if (canPlus.txTryLock()) canPlus.writePacket(canID, cmd, 8); +} + +void ServoKTech::readStatus() { + if (statusMode == OFF) return; + + if (millis() - lastStatusUpdateTime >= KTECH_STATUS_MS*4U) { + status.outputA.shortToGround = true; + status.outputA.openLoad = true; + status.outputB.shortToGround = true; + status.outputB.openLoad = true; + status.overTemperatureWarning = true; + status.overTemperature = true; + status.standstill = true; + status.fault = true; + } +} + +// update the associated driver status from CAN +void ServoKTech::requestStatusCallback(uint8_t data[8]) { + if (statusMode == OFF) return; + + uint8_t errorState = data[7]; + status.outputA.shortToGround = false; + status.outputA.openLoad = false; + status.outputB.shortToGround = false; + status.outputB.openLoad = false; + status.overTemperatureWarning = false; + status.overTemperature = bitRead(errorState, 3); + status.standstill = false; + status.fault = bitRead(errorState, 0) || bitRead(errorState, 3); + lastStatusUpdateTime = millis(); +} + +#endif diff --git a/src/lib/axis/motor/servo/kTech/KTech.h b/src/lib/axis/motor/servo/kTech/KTech.h new file mode 100755 index 0000000..a7b56f4 --- /dev/null +++ b/src/lib/axis/motor/servo/kTech/KTech.h @@ -0,0 +1,73 @@ +// ----------------------------------------------------------------------------------- +// axis servo KTech motor driver (designed for the MS4010v3 with 16bit encoder) +#pragma once + +#include +#include "../../../../../Common.h" + +#ifdef SERVO_KTECH_PRESENT + +#include "../ServoDriver.h" + +#if !defined(CAN_PLUS) || CAN_PLUS == OFF + #error "No KTECH motor CAN interface!" +#endif + +// KTECH status rate default 1Hz +#ifndef KTECH_STATUS_MS + #define KTECH_STATUS_MS 1000 +#endif + +class ServoKTech : public ServoDriver { + public: + // constructor + ServoKTech(uint8_t axisNumber, const ServoSettings *Settings, float countsToDegreesRatio); + + // decodes driver model and sets up the pin modes + bool init(bool reverse); + + // returns the number of axis parameters + uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // enable or disable the driver using the enable pin or other method + void enable(bool state); + + // set motor velocity + // \param velocity as needed to reach the target position, in encoder counts per second + // \param velocityTarget is the desired instantanous velocity at this moment, in encoder counts per second + // \returns velocity in effect, in encoder counts per second + float setMotorVelocity(float velocity); + + // request driver status from CAN + void requestStatus(); + + // read the associated driver status from CAN + void requestStatusCallback(uint8_t data[8]); + + // get the driver name + const char* name() { return "KTECH"; } + + private: + // read status info. from driver + void readStatus(); + + int canID; + unsigned long lastStatusUpdateTime = 0; + + void (*callback)() = NULL; + + // regulate velocity changes + int32_t velocityLast = 0; + + // runtime adjustable settings + AxisParameter countsToStepsRatio = {NAN, NAN, NAN, -1, 20000, AXP_FLOAT, "Count/Step ratio"}; + + const int numParameters = 3; + AxisParameter* parameter[4] = {&invalid, &acceleration, &zeroDeadband, &countsToStepsRatio}; + +}; + +#endif diff --git a/src/lib/axis/motor/servo/tmc/TmcServoDriver.cpp b/src/lib/axis/motor/servo/tmc/TmcServoDriver.cpp new file mode 100755 index 0000000..f9817f4 --- /dev/null +++ b/src/lib/axis/motor/servo/tmc/TmcServoDriver.cpp @@ -0,0 +1,133 @@ +// ----------------------------------------------------------------------------------- +// axis servo motor driver + +#include "TmcServoDriver.h" + +#ifdef SERVO_MOTOR_PRESENT + +TmcServoDriver::TmcServoDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, + int16_t microsteps, int16_t current, int8_t decay, int8_t decaySlewing) + :ServoDriver(axisNumber, Pins, Settings) { + if (axisNumber < 1 || axisNumber > 9) return; + + // get the maximum current and Rsense for this axis + float user_rSense = 0; + int16_t user_currentMax = 0; + switch (axisNumber) { + case 1: + #ifdef AXIS1_DRIVER_RSENSE + user_rSense = AXIS1_DRIVER_RSENSE; + #endif + #ifdef AXIS1_DRIVER_MAX_CURRENT + user_currentMax = AXIS1_DRIVER_MAX_CURRENT; + #endif + break; + case 2: + #ifdef AXIS2_DRIVER_RSENSE + user_rSense = AXIS2_DRIVER_RSENSE; + #endif + #ifdef AXIS2_DRIVER_MAX_CURRENT + user_currentMax = AXIS2_DRIVER_MAX_CURRENT; + #endif + break; + case 3: + #ifdef AXIS3_DRIVER_RSENSE + user_rSense = AXIS3_DRIVER_RSENSE; + #endif + #ifdef AXIS3_DRIVER_MAX_CURRENT + user_currentMax = AXIS3_DRIVER_MAX_CURRENT; + #endif + break; + case 4: + #ifdef AXIS4_DRIVER_RSENSE + user_rSense = AXIS4_DRIVER_RSENSE; + #endif + #ifdef AXIS4_DRIVER_MAX_CURRENT + user_currentMax = AXIS4_DRIVER_MAX_CURRENT; + #endif + break; + case 5: + #ifdef AXIS5_DRIVER_RSENSE + user_rSense = AXIS5_DRIVER_RSENSE; + #endif + #ifdef AXIS5_DRIVER_MAX_CURRENT + user_currentMax = AXIS5_DRIVER_MAX_CURRENT; + #endif + break; + case 6: + #ifdef AXIS6_DRIVER_RSENSE + user_rSense = AXIS6_DRIVER_RSENSE; + #endif + #ifdef AXIS6_DRIVER_MAX_CURRENT + user_currentMax = AXIS6_DRIVER_MAX_CURRENT; + #endif + break; + case 7: + #ifdef AXIS7_DRIVER_RSENSE + user_rSense = AXIS7_DRIVER_RSENSE; + #endif + #ifdef AXIS7_DRIVER_MAX_CURRENT + user_currentMax = AXIS7_DRIVER_MAX_CURRENT; + #endif + break; + case 8: + #ifdef AXIS8_DRIVER_RSENSE + user_rSense = AXIS8_DRIVER_RSENSE; + #endif + #ifdef AXIS8_DRIVER_MAX_CURRENT + user_currentMax = AXIS8_DRIVER_MAX_CURRENT; + #endif + break; + case 9: + #ifdef AXIS9_DRIVER_RSENSE + user_rSense = AXIS9_DRIVER_RSENSE; + #endif + #ifdef AXIS9_DRIVER_MAX_CURRENT + user_currentMax = AXIS9_DRIVER_MAX_CURRENT; + #endif + break; + } + + if (driverModel == SERVO_TMC5160) { currentMax = TMC5160_MAX_CURRENT; rSense = TMC5160_RSENSE; } else + if (driverModel == SERVO_TMC2209) { currentMax = TMC2209_MAX_CURRENT; rSense = TMC2209_RSENSE; } else + driverModel = 0; + + if (user_rSense != 0) rSense = user_rSense; + if (user_currentMax != 0) currentMax = user_currentMax; + + // set current defaults for TMC drivers + if (lround(current) == OFF) current = 300.0F; + + this->currentRun.valueDefault = current; + this->currentRun.max = currentMax; + + // setup decay modes + this->decay.valueDefault = decay; + if (this->decay.valueDefault == OFF) this->decay.valueDefault = STEALTHCHOP; + this->decaySlewing.valueDefault = decaySlewing; + if (this->decaySlewing.valueDefault == OFF) this->decaySlewing.valueDefault = SPREADCYCLE; +} + +bool TmcServoDriver::init(bool reverse) { + if (!ServoDriver::init(reverse)) return false; + + if (!driverModel) { + DF("WRN:"); D(axisPrefix); DLF("unknown driver model!"); + return false; + } + + VF("MSG:"); V(axisPrefix); VF("Rsense="); V(rSense); VL("Ohms"); + VF("MSG:"); V(axisPrefix); VF("max current="); V(currentMax); VL("mA"); + + iRun = this->currentRun.value; + iHold = iRun; + iHoldRatio = (float)1.0F; + + VF("MSG:"); V(axisPrefix); + VF("IHOLD="); V(lround(iHold)); VF("mA, "); + VF("IRUN="); V(lround(iRun)); VL("mA"); + + return true; +} + +#endif diff --git a/src/lib/axis/motor/servo/tmc/TmcServoDriver.h b/src/lib/axis/motor/servo/tmc/TmcServoDriver.h new file mode 100755 index 0000000..f6c9f85 --- /dev/null +++ b/src/lib/axis/motor/servo/tmc/TmcServoDriver.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------------- +// axis servo motor driver +#pragma once + +#include "../ServoDriver.h" + +#ifdef SERVO_MOTOR_PRESENT + +// default settings for any TMC drivers that may be present +#ifndef DRIVER_TMC_STEPPER_AUTOGRAD + #define DRIVER_TMC_STEPPER_AUTOGRAD true +#endif +#ifndef TMC5160_RSENSE + #define TMC5160_RSENSE 0.075F +#endif +#ifndef TMC5160_MAX_CURRENT + #define TMC5160_MAX_CURRENT 4230 // typical module rated at 3.0A RMS +#endif +#ifndef TMC2209_RSENSE + #define TMC2209_RSENSE 0.11F +#endif +#ifndef TMC2209_MAX_CURRENT + #define TMC2209_MAX_CURRENT (2820*0.8) // chip rated at 2.0A RMS, downrated to 80% due to typical step-stick form +#endif + +class TmcServoDriver : public ServoDriver { + public: + TmcServoDriver(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, int16_t microsteps, int16_t current, int8_t decay, int8_t decaySlewing); + + // decodes driver model and sets up the pin modes + virtual bool init(bool reverse); + + // returns the number of axis parameters + uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // set motor velocity + // \param velocity as needed to reach the target position, in signed encoder counts per second + // \returns velocity in effect, in signed encoder counts per second + virtual float setMotorVelocity(float velocity) { return 0; } + + protected: + // driver current control + float rSense = 0.11F; + int16_t currentMax = 0; + + float iHold, iRun, iHoldRatio; + + int16_t normalizedMicrosteps = 256; + + // runtime adjustable settings + AxisParameter countsToStepsRatio = {NAN, NAN, NAN, 0, 20000, AXP_FLOAT, AXPN_STEPS_TO_COUNTS}; + AxisParameter currentRun = {NAN, NAN, NAN, 0, 20000, AXP_INTEGER, AXPN_CURRENT_RUN}; + AxisParameter decay = {NAN, NAN, NAN, SPREADCYCLE, STEALTHCHOP, AXP_DECAY, AXPN_DECAY_MODE}; + AxisParameter decaySlewing = {NAN, NAN, NAN, SPREADCYCLE, STEALTHCHOP, AXP_DECAY, AXPN_DECAY_MODE_GOTO}; + + const int numParameters = 6; + AxisParameter* parameter[7] = {&invalid, &acceleration, &zeroDeadband, &countsToStepsRatio, ¤tRun, &decay, &decaySlewing}; +}; + +#endif diff --git a/src/lib/axis/motor/servo/tmc/tmc2209/Tmc2209.cpp b/src/lib/axis/motor/servo/tmc/tmc2209/Tmc2209.cpp new file mode 100755 index 0000000..314ef16 --- /dev/null +++ b/src/lib/axis/motor/servo/tmc/tmc2209/Tmc2209.cpp @@ -0,0 +1,180 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC2209 stepper motor driver + +#include "Tmc2209.h" + +#ifdef SERVO_TMC2209_PRESENT + +#include "../../../../../tasks/OnTask.h" +#include "../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define rx m3 +#define tx m2 + +// provide for using hardware serial +#if SERIAL_TMC == HardSerial + #include + #undef SERIAL_TMC + HardwareSerial HWSerialTMC(SERIAL_TMC_RX, SERIAL_TMC_TX); + #define SERIAL_TMC HWSerialTMC + #define SERIAL_TMC_RXTX_SET +#endif + +ServoTmc2209::ServoTmc2209(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, + float countsToStepsRatio, int16_t microsteps, int16_t current, int8_t decay, int8_t decaySlewing) + :TmcServoDriver(axisNumber, Pins, Settings, microsteps, current, decay, decaySlewing) { + if (axisNumber < 1 || axisNumber > 9) return; + + strcpy(axisPrefix, " Axis_ServoTmc2209, "); + axisPrefix[5] = '0' + axisNumber; + + this->velocityThrs = velocityThrs; + this->countsToStepsRatio.valueDefault = countsToStepsRatio; +} + +bool ServoTmc2209::init(bool reverse) { + if (!TmcServoDriver::init(reverse)) return false; + + // get TMC UART driver ready + pinModeEx(Pins->m0, OUTPUT); + pinModeEx(Pins->m1, OUTPUT); + pinModeEx(Pins->ph1, OUTPUT); // step + pinModeEx(Pins->ph2, OUTPUT); // dir + + // set S/D motion pins to known, fixed state + digitalWriteEx(Pins->ph1, LOW); // step + digitalWriteEx(Pins->ph2, LOW); // dir + + // initialize the serial port + VF("MSG:"); V(axisPrefix); VF("TMC "); + #if defined(SERIAL_TMC_HARDWARE_UART) + #if defined(DEDICATED_MODE_PINS) + // program the device address 0,1,2,3 since M0 and M1 are all unique + int deviceAddress = SERIAL_TMC_ADDRESS_MAP(axisNumber - 1); + digitalWriteEx(Pins->m0, bitRead(deviceAddress, 0)); + digitalWriteEx(Pins->m1, bitRead(deviceAddress, 1)); + #else + // help user hard code the device address 0,1,2,3 by cutting pins + digitalWriteEx(Pins->m0, HIGH); + digitalWriteEx(Pins->m1, HIGH); + #endif + + #define SerialTMC SERIAL_TMC + static bool initialized = false; + if (!initialized) { + #if defined(SERIAL_TMC_RX) && defined(SERIAL_TMC_TX) && !defined(SERIAL_TMC_RXTX_SET) + VF("HW UART driver pins rx="); V(SERIAL_TMC_RX); VF(", tx="); V(SERIAL_TMC_TX); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + SerialTMC.begin(SERIAL_TMC_BAUD, SERIAL_8N1, SERIAL_TMC_RX, SERIAL_TMC_TX); + #else + VF("HW UART driver pins on port default"); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + SerialTMC.begin(SERIAL_TMC_BAUD); + #endif + initialized = true; + } + #else + // pull MS1 and MS2 low for device address 0 + digitalWriteEx(Pins->m0, LOW); + digitalWriteEx(Pins->m1, LOW); + VF("SW UART driver pins rx="); V(Pins->rx); VF(", tx="); V(Pins->tx); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + SerialTMC = new SoftwareSerial(Pins->rx, Pins->tx); + SerialTMC->begin(SERIAL_TMC_BAUD); + #endif + + #if defined(SERIAL_TMC_HARDWARE_UART) + driver = new TMC2209Stepper(&SerialTMC, rSense, SERIAL_TMC_ADDRESS_MAP(axisNumber - 1)); + #else + driver = new TMC2209Stepper(SerialTMC, rSense, SERIAL_TMC_ADDRESS_MAP(axisNumber - 1)); + #endif + driver->begin(); + driver->intpol(true); + + if (decay.value == STEALTHCHOP && decaySlewing.value == SPREADCYCLE && velocityThrs > 0) { + VF("MSG:"); V(axisPrefix); VF("TMC decay mode velocity threshold "); V(velocityThrs); VLF(" sps"); + driver->TPWMTHRS(velocityThrs/0.715F); + } + + driver->microsteps(256); + + driver->hold_multiplier(iHoldRatio); + + VF("MSG:"); V(axisPrefix); VF("Irun="); V(lround(iRun)); VLF("mA"); + driver->rms_current(iRun*0.7071); + + driver->en_spreadCycle(true); + + // automatically set fault status for known drivers + status.active = statusMode == ON; + + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no driver detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + #endif + + return true; +} + +// enable or disable the driver using the enable pin or other method +void ServoTmc2209::enable(bool state) { + enabled = state; + + if (enablePin == SHARED) { + VF("MSG:"); V(axisPrefix); VF("powered "); if (state) { VF("up"); } else { VF("down"); } VLF(" using UART"); + if (state) { + driver->en_spreadCycle(!stealthChop()); + driver->rms_current(iRun*0.7071); + } else { + driver->en_spreadCycle(false); + driver->ihold(0); + } + } else { + VF("MSG:"); V(axisPrefix); VF("powered "); if (state) { VF("up"); } else { VF("down"); } VLF(" using enable pin"); + if (!enabled) { digitalWriteF(enablePin, !enabledState); } else { digitalWriteF(enablePin, enabledState); } + } + + velocityRamp = 0.0F; + + ServoDriver::updateStatus(); + + return; +} + +float ServoTmc2209::setMotorVelocity(float velocity) { + velocity = ServoDriver::setMotorVelocity(velocity); + + driver->VACTUAL(reversed ? -(velocity/0.715F)*countsToStepsRatio.value : (velocity/0.715F)*countsToStepsRatio.value); + + return velocity; +} + +void ServoTmc2209::readStatus() { + TMC2208_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + if (driver->CRCerror) status_result.sr = 0xFFFFFFFF; + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning = status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// calibrate the motor driver if required +void ServoTmc2209::calibrateDriver() { + if (stealthChop()) { + VF("MSG:"); V(axisPrefix); VL("TMC standstill automatic current calibration"); + driver->rms_current(iRun*0.7071); + driver->pwm_autograd(DRIVER_TMC_STEPPER_AUTOGRAD); + driver->pwm_autoscale(true); + driver->en_spreadCycle(false); + delay(1000); + } +} + +#endif diff --git a/src/lib/axis/motor/servo/tmc/tmc2209/Tmc2209.h b/src/lib/axis/motor/servo/tmc/tmc2209/Tmc2209.h new file mode 100755 index 0000000..ea26cda --- /dev/null +++ b/src/lib/axis/motor/servo/tmc/tmc2209/Tmc2209.h @@ -0,0 +1,91 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC2209 stepper motor driver +#pragma once + +#include +#include "../../../../../../Common.h" + +#ifdef SERVO_TMC2209_PRESENT + +#include "../TmcServoDriver.h" + +// default settings for any TMC UART drivers that may be present +#ifndef SERIAL_TMC + #define SERIAL_TMC SoftSerial // Use software serial w/ TX on M3 (CS) and RX on M4 (MISO) of each axis +#endif +#ifndef SERIAL_TMC_BAUD + #define SERIAL_TMC_BAUD 115200 // Baud rate +#endif +#ifndef SERIAL_TMC_ADDRESS_MAP + #define SERIAL_TMC_ADDRESS_MAP(x) (0) // driver addresses are 0 for all axes +#endif + +#if SERIAL_TMC == SoftSerial + #include // must be built into the board libraries +#endif + +#include // https://github.com/teemuatlut/TMCStepper + +typedef struct ServoTmcPins { + int16_t step; + int16_t dir; + int16_t enable; + uint8_t enabledState; + int16_t m0; + int16_t m1; + int16_t fault; +} ServoTmcPins; + +typedef struct ServoTmcSettings { + int16_t model; + int8_t status; + int32_t velocityMax; // maximum velocity in steps/second + int32_t velocityThrs; // velocity for stealthChop to spreadCycle switch in steps/second + int32_t acceleration; // acceleration steps/second/second + int16_t microsteps; + int16_t current; // peak current in mA + int8_t decay; + int8_t decaySlewing; +} ServoTmcSettings; + +class ServoTmc2209 : public TmcServoDriver { + public: + // constructor + ServoTmc2209(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float countsToStepsRatio, int16_t microsteps, int16_t current, int8_t decay, int8_t decaySlewing); + + // decodes driver model and sets up the pin modes + bool init(bool reverse); + + // enable or disable the driver using the enable pin or other method + void enable(bool state); + + // set motor velocity + // \param velocity as needed to reach the target position, in encoder counts per second + // \returns velocity in effect, in encoder counts per second + float setMotorVelocity(float velocity); + + // calibrate the motor if required + void calibrateDriver(); + + // get the driver name + const char* name() { return "TMC2209 (UART)"; } + + private: + // read status from driver + void readStatus(); + + bool stealthChop() { if (lround(decay.value) == STEALTHCHOP) return true; else return false; } + + #if SERIAL_TMC == SoftSerial + SoftwareSerial *SerialTMC; + #endif + + TMC2209Stepper *driver; + + bool sdMode = false; + + // for switching to/from spreadCycle + int16_t velocityThrs; +}; + +#endif diff --git a/src/lib/axis/motor/servo/tmc/tmc5160/Tmc5160.cpp b/src/lib/axis/motor/servo/tmc/tmc5160/Tmc5160.cpp new file mode 100755 index 0000000..377ba2d --- /dev/null +++ b/src/lib/axis/motor/servo/tmc/tmc5160/Tmc5160.cpp @@ -0,0 +1,139 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC5160 stepper motor driver + +#include "Tmc5160.h" + +#ifdef SERVO_TMC5160_PRESENT + +#include "../../../../../tasks/OnTask.h" +#include "../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +ServoTmc5160::ServoTmc5160(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *TmcSettings, + float countsToStepsRatio, int16_t microsteps, int16_t current, int8_t decay, int8_t decaySlewing) + :TmcServoDriver(axisNumber, Pins, Settings, microsteps, current, decay, decaySlewing) { + if (axisNumber < 1 || axisNumber > 9) return; + + strcpy(axisPrefix, " Axis_ServoTmc5160, "); + axisPrefix[5] = '0' + axisNumber; + + this->velocityThrs = velocityThrs; + this->countsToStepsRatio.valueDefault = countsToStepsRatio; +} + +bool ServoTmc5160::init(bool reverse) { + if (!TmcServoDriver::init(reverse)) return false; + + // get TMC SPI driver ready + pinModeEx(Pins->ph1, OUTPUT); // step + pinModeEx(Pins->ph2, OUTPUT); // dir + + // set S/D motion pins to known, fixed state + digitalWriteEx(Pins->ph1, LOW); // step + digitalWriteEx(Pins->ph2, LOW); // dir + + driver = new TMC5160Stepper(Pins->cs, rSense, Pins->mosi, Pins->miso, Pins->sck); + driver->begin(); + driver->intpol(true); + + if (decay.value == STEALTHCHOP && decaySlewing.value == SPREADCYCLE && velocityThrs > 0) { + VF("MSG:"); V(axisPrefix); VF("TMC decay mode velocity threshold "); V(velocityThrs); VLF(" sps"); + driver->TPWMTHRS(velocityThrs/0.715F); + } + + driver->microsteps(256); + + driver->hold_multiplier(iHoldRatio); + driver->rms_current(iRun*0.7071); + + unsigned long mode = driver->IOIN(); + if (mode && 0b01000000 > 0) { + DF("WRN:"); D(axisPrefix); DLF("TMC driver is in Step/Dir mode and WILL NOT WORK for TMC5160_SERVO!"); + return false; + } + + driver->en_pwm_mode(false); + driver->AMAX(65535); + driver->RAMPMODE(1); + + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + } + #endif + + return true; +} + +// enable or disable the driver using the enable pin or other method +void ServoTmc5160::enable(bool state) { + enabled = state; + + if (enablePin == SHARED) { + VF("MSG:"); V(axisPrefix); VF(", powered "); if (state) { VF("up"); } else { VF("down"); } VLF(" using SPI"); + if (state) { + driver->en_pwm_mode(stealthChop()); + driver->rms_current(iRun*0.7071); + } else { + driver->en_pwm_mode(true); + driver->ihold(0); + } + } else { + VF("MSG:"); V(axisPrefix); VF(", powered "); if (state) { VF("up"); } else { VF("down"); } VLF(" using enable pin"); + if (!enabled) { digitalWriteF(enablePin, !enabledState); } else { digitalWriteF(enablePin, enabledState); } + } + + velocityRamp = 0.0F; + + ServoDriver::updateStatus(); +} + +float ServoTmc5160::setMotorVelocity(float velocity) { + velocity = ServoDriver::setMotorVelocity(velocity); + + if (motorDirection = DIR_FORWARD) driver->shaft(reversed ? true : false); else driver->shaft(reversed ? false : true); + driver->VMAX(fabs((velocity/0.715F)*countsToStepsRatio.value)); + + return velocity; +} + +// read status info. from driver +void ServoTmc5160::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = ((TMC5160Stepper*)driver)->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning = status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// calibrate the motor driver if required +void ServoTmc5160::calibrateDriver() { + if (stealthChop()) { + VF("MSG:"); V(axisPrefix); VL("TMC standstill automatic current calibration"); + driver->rms_current(iRun*0.7071); + driver->pwm_autograd(DRIVER_TMC_STEPPER_AUTOGRAD); + driver->pwm_autoscale(true); + driver->en_pwm_mode(true); + delay(1000); + } +} + +#endif diff --git a/src/lib/axis/motor/servo/tmc/tmc5160/Tmc5160.h b/src/lib/axis/motor/servo/tmc/tmc5160/Tmc5160.h new file mode 100755 index 0000000..21baff4 --- /dev/null +++ b/src/lib/axis/motor/servo/tmc/tmc5160/Tmc5160.h @@ -0,0 +1,48 @@ +// ----------------------------------------------------------------------------------- +// axis servo TMC5160 stepper motor driver +#pragma once + +#include +#include "../../../../../../Common.h" + +#ifdef SERVO_TMC5160_PRESENT + +#include "../TmcServoDriver.h" + +#include // https://github.com/teemuatlut/TMCStepper + +class ServoTmc5160 : public TmcServoDriver { + public: + // constructor + ServoTmc5160(uint8_t axisNumber, const ServoPins *Pins, const ServoSettings *Settings, float countsToStepsRatio, int16_t microsteps, int16_t current, int8_t decay, int8_t decaySlewing); + + // decodes driver model and sets up the pin modes + bool init(bool reverse); + + // enable or disable the driver using the enable pin or other method + void enable(bool state); + + // set motor velocity + // \param velocity as needed to reach the target position, in encoder counts per second + // \returns velocity in effect, in encoder counts per second + float setMotorVelocity(float velocity); + + // calibrate the motor if required + void calibrateDriver(); + + // get the driver name + const char* name() { return "TMC5160 (SPI)"; } + + private: + // read status from driver + void readStatus(); + + bool stealthChop() { if (lround(decay.value) == STEALTHCHOP) return true; else return false; } + + TMC5160Stepper *driver; + + // for switching to/from spreadCycle + int16_t velocityThrs; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/StepDir.cpp b/src/lib/axis/motor/stepDir/StepDir.cpp new file mode 100755 index 0000000..b9182f3 --- /dev/null +++ b/src/lib/axis/motor/stepDir/StepDir.cpp @@ -0,0 +1,572 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor + +#include "StepDir.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +#include "../../../tasks/OnTask.h" + +static StepDirMotor *stepDirMotorInstance[9]; + +#ifndef AXIS1_STEP_PIN + #define AXIS1_STEP_PIN stepDirMotorInstance[0]->Pins->step +#endif +IRAM_ATTR void moveStepDirMotorAxis1() { stepDirMotorInstance[0]->move(AXIS1_STEP_PIN); } +IRAM_ATTR void moveStepDirMotorFFAxis1() { stepDirMotorInstance[0]->moveFF(AXIS1_STEP_PIN); } +IRAM_ATTR void moveStepDirMotorFRAxis1() { stepDirMotorInstance[0]->moveFR(AXIS1_STEP_PIN); } + +#ifndef AXIS2_STEP_PIN + #define AXIS2_STEP_PIN stepDirMotorInstance[1]->Pins->step +#endif +IRAM_ATTR void moveStepDirMotorAxis2() { stepDirMotorInstance[1]->move(AXIS2_STEP_PIN); } +IRAM_ATTR void moveStepDirMotorFFAxis2() { stepDirMotorInstance[1]->moveFF(AXIS2_STEP_PIN); } +IRAM_ATTR void moveStepDirMotorFRAxis2() { stepDirMotorInstance[1]->moveFR(AXIS2_STEP_PIN); } + +#ifndef AXIS3_STEP_PIN + #define AXIS3_STEP_PIN stepDirMotorInstance[2]->Pins->step +#endif +IRAM_ATTR void moveStepDirMotorAxis3() { stepDirMotorInstance[2]->move(AXIS3_STEP_PIN); } +IRAM_ATTR void moveStepDirMotorFFAxis3() { stepDirMotorInstance[2]->moveFF(AXIS3_STEP_PIN); } +IRAM_ATTR void moveStepDirMotorFRAxis3() { stepDirMotorInstance[2]->moveFR(AXIS3_STEP_PIN); } + +#ifndef AXIS4_STEP_PIN + #define AXIS4_STEP_PIN stepDirMotorInstance[3]->Pins->step +#endif +void moveStepDirMotorAxis4() { stepDirMotorInstance[3]->move(AXIS4_STEP_PIN); } +void moveStepDirMotorFFAxis4() { stepDirMotorInstance[3]->moveFF(AXIS4_STEP_PIN); } +void moveStepDirMotorFRAxis4() { stepDirMotorInstance[3]->moveFR(AXIS4_STEP_PIN); } + +#ifndef AXIS5_STEP_PIN + #define AXIS5_STEP_PIN stepDirMotorInstance[4]->Pins->step +#endif +void moveStepDirMotorAxis5() { stepDirMotorInstance[4]->move(AXIS5_STEP_PIN); } +void moveStepDirMotorFFAxis5() { stepDirMotorInstance[4]->moveFF(AXIS5_STEP_PIN); } +void moveStepDirMotorFRAxis5() { stepDirMotorInstance[4]->moveFR(AXIS5_STEP_PIN); } + +#ifndef AXIS6_STEP_PIN + #define AXIS6_STEP_PIN stepDirMotorInstance[5]->Pins->step +#endif +void moveStepDirMotorAxis6() { stepDirMotorInstance[5]->move(AXIS6_STEP_PIN); } +void moveStepDirMotorFFAxis6() { stepDirMotorInstance[5]->moveFF(AXIS6_STEP_PIN); } +void moveStepDirMotorFRAxis6() { stepDirMotorInstance[5]->moveFR(AXIS6_STEP_PIN); } + +#ifndef AXIS7_STEP_PIN + #define AXIS7_STEP_PIN stepDirMotorInstance[6]->Pins->step +#endif +void moveStepDirMotorAxis7() { stepDirMotorInstance[6]->move(AXIS7_STEP_PIN); } +void moveStepDirMotorFFAxis7() { stepDirMotorInstance[6]->moveFF(AXIS7_STEP_PIN); } +void moveStepDirMotorFRAxis7() { stepDirMotorInstance[6]->moveFR(AXIS7_STEP_PIN); } + +#ifndef AXIS8_STEP_PIN + #define AXIS8_STEP_PIN stepDirMotorInstance[7]->Pins->step +#endif +void moveStepDirMotorAxis8() { stepDirMotorInstance[7]->move(AXIS8_STEP_PIN); } +void moveStepDirMotorFFAxis8() { stepDirMotorInstance[7]->moveFF(AXIS8_STEP_PIN); } +void moveStepDirMotorFRAxis8() { stepDirMotorInstance[7]->moveFR(AXIS8_STEP_PIN); } + +#ifndef AXIS9_STEP_PIN + #define AXIS9_STEP_PIN stepDirMotorInstance[8]->Pins->step +#endif +void moveStepDirMotorAxis9() { stepDirMotorInstance[8]->move(AXIS9_STEP_PIN); } +void moveStepDirMotorFFAxis9() { stepDirMotorInstance[8]->moveFF(AXIS9_STEP_PIN); } +void moveStepDirMotorFRAxis9() { stepDirMotorInstance[8]->moveFR(AXIS9_STEP_PIN); } + +StepDirMotor::StepDirMotor(uint8_t axisNumber, int8_t reverse, + const StepDirPins *Pins, StepDirDriver *Driver, bool useFastHardwareTimers) + :Motor(axisNumber, reverse) { + strcpy(axisPrefix, " Axis_StepDir, "); + axisPrefix[5] = '0' + axisNumber; + + driverType = STEP_DIR; + + this->Pins = Pins; + + if (axisNumber > 2) useFastHardwareTimers = false; + this->useFastHardwareTimers = useFastHardwareTimers; + + driver = Driver; + pulseWidth = driver->getPulseWidth(); + + // attach the function pointers to the callbacks + stepDirMotorInstance[axisNumber - 1] = this; + switch (axisNumber) { + case 1: callback = moveStepDirMotorAxis1; callbackFF = moveStepDirMotorFFAxis1; callbackFR = moveStepDirMotorFRAxis1; break; + case 2: callback = moveStepDirMotorAxis2; callbackFF = moveStepDirMotorFFAxis2; callbackFR = moveStepDirMotorFRAxis2; break; + case 3: callback = moveStepDirMotorAxis3; callbackFF = moveStepDirMotorFFAxis3; callbackFR = moveStepDirMotorFRAxis3; break; + case 4: callback = moveStepDirMotorAxis4; callbackFF = moveStepDirMotorFFAxis4; callbackFR = moveStepDirMotorFRAxis4; break; + case 5: callback = moveStepDirMotorAxis5; callbackFF = moveStepDirMotorFFAxis5; callbackFR = moveStepDirMotorFRAxis5; break; + case 6: callback = moveStepDirMotorAxis6; callbackFF = moveStepDirMotorFFAxis6; callbackFR = moveStepDirMotorFRAxis6; break; + case 7: callback = moveStepDirMotorAxis7; callbackFF = moveStepDirMotorFFAxis7; callbackFR = moveStepDirMotorFRAxis7; break; + case 8: callback = moveStepDirMotorAxis8; callbackFF = moveStepDirMotorFFAxis8; callbackFR = moveStepDirMotorFRAxis8; break; + case 9: callback = moveStepDirMotorAxis9; callbackFF = moveStepDirMotorFFAxis9; callbackFR = moveStepDirMotorFRAxis9; break; + } +} + +bool StepDirMotor::init() { + if (ready) return true; + if (!Motor::init()) return false; + + #if DEBUG == VERBOSE + VF("MSG:"); V(axisPrefix); V("pins step="); if (Pins->step == OFF) { VF("OFF"); } else { V(Pins->step); } + VF(", dir="); if (Pins->dir == OFF) { VF("OFF"); } else { V(Pins->dir); } + VF(", en="); if (Pins->enable == OFF) { VLF("OFF"); } else if (Pins->enable == SHARED) { VLF("SHARED"); } else { VL(Pins->enable); } + #endif + + // this driver requires available pins to function + if (Pins->dir == OFF || Pins->step == OFF) { + DF("ERR:"); D(axisPrefix); DLF("step or dir pin not present!"); + return false; + } + + // init default driver direction pin for output + pinModeEx(Pins->dir, OUTPUT); + if (normalizedReverse) { dirFwd = HIGH; dirRev = LOW; } else { dirFwd = LOW; dirRev = HIGH; } + digitalWriteEx(Pins->dir, dirFwd); + direction = dirFwd; + + // init default driver step state (clear) + #ifndef DRIVER_STEP_DEFAULTS + stepSet = Pins->stepState; + stepClr = !stepSet; + #endif + pinModeEx(Pins->step, OUTPUT); + digitalWriteF(Pins->step, stepClr); + + // init default driver enable pin + if (Pins->enable != SHARED) { + pinModeEx(Pins->enable, OUTPUT); + digitalWriteEx(Pins->enable, !Pins->enabledState); + } + + // start the driver + if (!driver->init()) { DF("ERR:"); D(axisPrefix); DLF("no motor driver!"); return false; } + + // start the motor timer + VF("MSG:"); V(axisPrefix); VF("start task to move motor... "); + char timerName[] = "Motor_"; + timerName[5] = '0' + axisNumber; + taskHandle = tasks.add(0, 0, true, 0, callback, timerName); + if (taskHandle) { + V("success"); + if (useFastHardwareTimers && !tasks.requestHardwareTimer(taskHandle, 0)) { VLF(" (no hardware timer!)"); } else { VLF(""); } + } else { + VLF("FAILED!"); + return false; + } + + ready = true; + return true; +} + +void StepDirMotor::setReverse(bool state) { + if (!ready) return; + Motor::setReverse(state); + + if (reversed != normalizedReverse) { dirFwd = HIGH; dirRev = LOW; } else { dirFwd = LOW; dirRev = HIGH; } + noInterrupts(); + digitalWriteEx(Pins->dir, dirFwd); + direction = dirFwd; + interrupts(); +} + +void StepDirMotor::enable(bool state) { + if (!ready || state == enabled) return; + + if (!state) enabled = false; + + stopStepTimerAndClearPulse(); + resetToTrackingBaseline(); + + VF("MSG:"); V(axisPrefix); VF("driver powered "); VF(state ? "up" : "down"); + + bool ok = true; + + if (Pins->enable != OFF && Pins->enable != SHARED) { + VF(" using pin "); VL(Pins->enable); + digitalWriteEx(Pins->enable, state ? Pins->enabledState : !Pins->enabledState); + } else { + ok = driver->enable(state); + if (ok) { VLF(" using secondary method (if available)"); } else { VLF(" using secondary method failed"); } + } + + enabled = (state && ok); +} + +// set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) +void StepDirMotor::setFrequencySteps(float frequency) { + if (!ready) return; + + if (!enabled) { stopStepTimerAndClearPulse(); return; } + + // chart acceleration + #if DEBUG != OFF && defined(DEBUG_STEPDIR_ACCEL) + if (axisNumber == DEBUG_STEPDIR_ACCEL) { + static unsigned long t = 0; + if (millis() - t >= 100U) { + DF("Axis"); D(axisNumber); + D(" step frequency "); D(frequency); DL("Hz"); + t = millis(); + } + } + #endif + + // chart motor position + #if DEBUG != OFF && defined(DEBUG_STEPDIR_POSITION) + if (axisNumber == DEBUG_STEPDIR_POSITION) { + static unsigned long t = 0; + if (millis() - t >= 100U) { + DF("Axis"); D(axisNumber); + D(" motor step position "); D(motorSteps); + D(" motor target position "); DL(targetSteps); + t = millis(); + } + } + #endif + + if (!inBacklash) modeSwitch(); + + Y; + // negative frequency, convert to positive and reverse the direction + int dir = 0; + if (frequency > 0.0F) dir = 1; else if (frequency < 0.0F) { frequency = -frequency; dir = -1; } + + // if in backlash override the frequency OR change + // microstep mode and/or swap in fast ISRs as required + if (inBacklash) frequency = backlashFrequency; + + if (frequency != currentFrequency || microstepModeControl >= MMC_SLEWING_PAUSE) { + lastFrequency = frequency; + + // if slewing has a larger step size divide the frequency to account for it + if (microstepModeControl == MMC_SLEWING || microstepModeControl == MMC_SLEWING_READY) frequency /= stepSize; + + // frequency in steps per second to period in microsecond counts per step + // also runs the timer twice as fast if using a square wave + #if STEP_WAVE_FORM == SQUARE + float period = 500000.0F/frequency; + #else + float period = 1000000.0F/frequency; + #endif + + // range is 0 to 134 seconds/step + if (!isnan(period) && period <= 130000000.0F) { + period *= 16.0F; + lastPeriod = (unsigned long)lroundf(period); + } else { + lastPeriod = 0; + frequency = 0.0F; + dir = 0; + } + + currentFrequency = frequency; + + // change the motor rate/direction + if (step != dir) step = 0; + if (lastPeriodSet != lastPeriod) { + tasks.setPeriodSubMicros(taskHandle, lastPeriod); + lastPeriodSet = lastPeriod; + } + step = dir; + + if (microstepModeControl == MMC_TRACKING_READY) microstepModeControl = MMC_TRACKING; + if (microstepModeControl == MMC_SLEWING_READY) { + #if DEBUG == VERBOSE + VF("MSG:"); V(axisPrefix); VF("high speed swap in took "); V(millis() - switchStartTimeMs); VLF(" ms"); + #endif + microstepModeControl = MMC_SLEWING; + } + + } else { + noInterrupts(); + step = dir; + interrupts(); + } +} + +// switch microstep modes as needed +void StepDirMotor::modeSwitch() { + if (lastFrequency <= backlashFrequency*2.0F) { + if (microstepModeControl >= MMC_SLEWING) { + microstepModeControl = MMC_TRACKING_READY; + + enableMoveFast(false); + + if (driver->modeSwitchAllowed || driver->modeSwitchFastAllowed) { + VF("MSG:"); V(axisPrefix); VLF("mode switch tracking set"); + driver->modeMicrostepTracking(); + } + } + } else { + if (microstepModeControl == MMC_TRACKING) { + noInterrupts(); + if (!sync || (step == -1 && direction == dirRev) || (step == 1 && direction == dirFwd)) { + microstepModeControl = MMC_SLEWING_REQUEST; + } + interrupts(); + switchStartTimeMs = millis(); + } else + if (microstepModeControl == MMC_SLEWING_PAUSE) { + if (driver->modeSwitchAllowed || driver->modeSwitchFastAllowed) { + VF("MSG:"); V(axisPrefix); VLF("mode switch slewing set"); + stepSize = driver->modeMicrostepSlewing(); + } + enableMoveFast(true); + microstepModeControl = MMC_SLEWING_READY; + } + } +} + +float StepDirMotor::getFrequencySteps() { + if (!ready) return 0.0F; + + if (lastPeriod == 0) return 0; + #if STEP_WAVE_FORM == SQUARE + return 8000000.0F/lastPeriod; + #else + return 16000000.0F/lastPeriod; + #endif +} + +// set slewing state (hint that we are about to slew or are done slewing) +void StepDirMotor::setSlewing(bool state) { + if (!ready) return; + + if (state == true) driver->modeDecaySlewing(); else driver->modeDecayTracking(); +} + +bool StepDirMotor::isStalled() { + if (!ready || !enabled || !driver) return false; + if (!driver->hasStallDetect()) return false; +// if (microstepModeControl != MMC_SLEWING) return false; + return driver->isStalled(step >= 0 ? lastFrequency : -lastFrequency); +} + +// swaps in/out fast unidirectional ISR for slewing +bool StepDirMotor::enableMoveFast(const bool fast) { + if (fast) { + if (direction == dirRev) { + tasks.setCallback(taskHandle, callbackFR); + VF("MSG:"); V(axisPrefix); VF("high speed Rev ISR swapped in at "); V(lastFrequency); VLF(" steps/sec."); + } else { + tasks.setCallback(taskHandle, callbackFF); + VF("MSG:"); V(axisPrefix); VF("high speed Fwd ISR swapped in at "); V(lastFrequency); VLF(" steps/sec."); + } + } else { + tasks.setCallback(taskHandle, callback); + VF("MSG:"); V(axisPrefix); VF("high speed ISR swapped out at "); V(lastFrequency); VL(" steps/sec."); + } + return true; +} + +// gard local guarantee: stop the step generator and force STEP inactive. +void StepDirMotor::stopStepTimerAndClearPulse() { + + // clear any previously latched stall state now that we're "stopped" + if (driver->hasStallDetect()) driver->stallDetectReset(); + + // already stopped? + if (lastPeriod == 0 && lastPeriodSet == 0 && step == 0 && !takeStep) { + digitalWriteF(Pins->step, stepClr); + takeStep = false; + return; + } + + // stop task first + if (taskHandle) tasks.setPeriodSubMicros(taskHandle, 0); + + // clear bookkeeping + lastPeriod = 0; + lastPeriodSet = 0; + currentFrequency = 0.0F; + lastFrequency = 0.0F; + + noInterrupts(); + step = 0; + takeStep = false; + interrupts(); + + // force STEP low so we don't leave a high level latched + digitalWriteF(Pins->step, stepClr); +} + +// clears backlash/state, sets microstep tracking, etc. +void StepDirMotor::resetToTrackingBaseline() { + if (taskHandle) tasks.setCallback(taskHandle, callback); + + noInterrupts(); + // clear any pending synthetic motion state + step = 0; + takeStep = false; + + // clear backlash state so next start is “clean” + inBacklash = false; + backlashSteps = 0; + + interrupts(); + + microstepModeControl = MMC_TRACKING; + stepSize = 1; + + #if defined(GPIO_DIRECTION_PINS) + if (direction > DirNone) updateMotorDirection(); + #endif + + if (driver->modeSwitchAllowed || driver->modeSwitchFastAllowed) driver->modeMicrostepTracking(); + + // force STEP low so we don't leave a high level latched + digitalWriteF(Pins->step, stepClr); +} + +#if defined(GPIO_DIRECTION_PINS) + // change motor direction on request by polling + IRAM_ATTR void StepDirMotor::updateMotorDirection() { + if (direction == DirSetRev) { + digitalWriteEx(Pins->dir, dirRev); + direction = dirRev; + } else + if (direction == DirSetFwd) { + digitalWriteEx(Pins->dir, dirFwd); + direction = dirFwd; + } + } +#endif + +IRAM_ATTR void StepDirMotor::move(const int16_t stepPin) { + #if STEP_WAVE_FORM == PULSE + digitalWriteF(stepPin, stepClr); + #endif + + #ifdef GPIO_DIRECTION_PINS + if (direction > DirNone) return; + #endif + + if (microstepModeControl == MMC_SLEWING_REQUEST && (motorSteps + backlashSteps) % driver->getMicrostepRatio() == 0 && direction < DirNone) { + microstepModeControl = MMC_SLEWING_PAUSE; + tasks.immediate(monitorHandle); + } + if (microstepModeControl >= MMC_SLEWING_PAUSE) return; + + #if STEP_WAVE_FORM == SQUARE + if (takeStep) { + #endif + + long lastTargetSteps = targetSteps; + if (sync && !inBacklash) targetSteps += step; + + if (motorSteps > targetSteps || (inBacklash && direction == dirRev)) { + if (direction != dirRev) { + targetSteps = lastTargetSteps; + #ifdef GPIO_DIRECTION_PINS + direction = DirSetRev; + #else + direction = dirRev; + digitalWriteF(Pins->dir, dirRev); + #endif + return; + } + + if (backlashSteps > 0) { + backlashSteps--; + inBacklash = backlashSteps > 0; + } else { + motorSteps--; + inBacklash = false; + } + + #ifdef SHARED_DIRECTION_PINS + if (axisNumber > 2) { digitalWriteF(Pins->dir, direction); delayNanoseconds(pulseWidth); } + #endif + digitalWriteF(stepPin, stepSet); + } else + + if (motorSteps < targetSteps || (inBacklash && direction == dirFwd)) { + if (direction != dirFwd) { + targetSteps = lastTargetSteps; + #ifdef GPIO_DIRECTION_PINS + direction = DirSetFwd; + #else + direction = dirFwd; + digitalWriteF(Pins->dir, dirFwd); + #endif + return; + } + + if (backlashSteps < backlashAmountSteps) { + backlashSteps++; + inBacklash = backlashSteps < backlashAmountSteps; + } else { + motorSteps++; + inBacklash = false; + } + + #ifdef SHARED_DIRECTION_PINS + if (axisNumber > 2) { digitalWriteF(Pins->dir, direction); delayNanoseconds(pulseWidth); } + #endif + digitalWriteF(stepPin, stepSet); + + } else if (!inBacklash) direction = DirNone; + + #if STEP_WAVE_FORM == SQUARE + } else digitalWriteF(stepPin, stepClr); + takeStep = !takeStep; + #endif +} + +IRAM_ATTR void StepDirMotor::moveFF(const int16_t stepPin) { + #if STEP_WAVE_FORM == PULSE + digitalWriteF(stepPin, stepClr); + #endif + + if (microstepModeControl >= MMC_SLEWING_PAUSE) return; + + #if STEP_WAVE_FORM == SQUARE + if (takeStep) { + #endif + + if (sync) targetSteps += stepSize; + + if (motorSteps < targetSteps) { + motorSteps += stepSize; + + #ifdef SHARED_DIRECTION_PINS + if (axisNumber > 2) { digitalWriteF(Pins->dir, direction); delayNanoseconds(pulseWidth); } + #endif + digitalWriteF(stepPin, stepSet); + } + + #if STEP_WAVE_FORM == SQUARE + } else digitalWriteF(stepPin, stepClr); + takeStep = !takeStep; + #endif +} + +IRAM_ATTR void StepDirMotor::moveFR(const int16_t stepPin) { + #if STEP_WAVE_FORM == PULSE + digitalWriteF(stepPin, stepClr); + #endif + + if (microstepModeControl >= MMC_SLEWING_PAUSE) return; + + #if STEP_WAVE_FORM == SQUARE + if (takeStep) { + #endif + + if (sync) targetSteps -= stepSize; + + if (motorSteps > targetSteps) { + motorSteps -= stepSize; + + #ifdef SHARED_DIRECTION_PINS + if (axisNumber > 2) { digitalWriteF(Pins->dir, direction); delayNanoseconds(pulseWidth); } + #endif + digitalWriteF(stepPin, stepSet); + } + + #if STEP_WAVE_FORM == SQUARE + } else digitalWriteF(stepPin, stepClr); + takeStep = !takeStep; + #endif +} + +#endif diff --git a/src/lib/axis/motor/stepDir/StepDir.h b/src/lib/axis/motor/stepDir/StepDir.h new file mode 100755 index 0000000..5d9e3b1 --- /dev/null +++ b/src/lib/axis/motor/stepDir/StepDir.h @@ -0,0 +1,179 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor +#pragma once + +#include "../../../../Common.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +#if defined(GPIO_DIRECTION_PINS) && defined(SHARED_DIRECTION_PINS) + #error "Configuration (Config.h): Having both GPIO_DIRECTION_PINS and SHARED_DIRECTION_PINS is not allowed" +#endif + +#include "../../../gpioEx/GpioEx.h" + +#include "generic/Generic.h" +#include "tmc/legacy/tmc2130/Tmc2130.h" +#include "tmc/legacy/tmc2209/Tmc2209.h" +#include "tmc/legacy/tmc5160/Tmc5160.h" +#include "tmc/tmcStepper/tmc2130/Tmc2130.h" +#include "tmc/tmcStepper/tmc2160/Tmc2160.h" +#include "tmc/tmcStepper/tmc2208/tmc2208.h" +#include "tmc/tmcStepper/tmc2209/Tmc2209.h" +#include "tmc/tmcStepper/tmc2660/Tmc2660.h" +#include "tmc/tmcStepper/tmc5160/Tmc5160.h" +#include "tmc/tmcStepper/tmc5161/Tmc5161.h" +#include "../Motor.h" + +typedef struct StepDirPins { + int16_t step; + uint8_t stepState; + int16_t dir; + int16_t enable; + uint8_t enabledState; +} StepDirPins; + +#define DirNone 253 +#define DirSetRev 254 +#define DirSetFwd 255 + +enum MicrostepModeControl: uint8_t {MMC_TRACKING, MMC_SLEWING, MMC_SLEWING_REQUEST, MMC_SLEWING_PAUSE, MMC_SLEWING_READY, MMC_TRACKING_READY}; + +class StepDirMotor : public Motor { + public: + // constructor + StepDirMotor(uint8_t axisNumber, int8_t reverse, const StepDirPins *Pins, StepDirDriver *Driver, bool useFastHardwareTimers = true); + + // sets up the driver step/dir/enable pins + bool init(); + + // revert settings to defaults + void useDefaults() { driver->useDefaults(); } + + // returns the number of parameters from the motor/driver + uint8_t getParameterCount() { return Motor::getParameterCount() + driver->getParameterCount(); } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { + if (number > Motor::getParameterCount()) return driver->getParameter(number - Motor::getParameterCount()); else + if (number >= 1 && number <= Motor::getParameterCount()) return Motor::getParameter(number); else + return &invalid; + } + + // check if parameter is valid + bool parameterIsValid(AxisParameter* parameter, bool next = false) { + if (!Motor::parameterIsValid(parameter, next)) return false; else return driver->parameterIsValid(parameter, next); + } + + // sets reversal of axis directions + // \param state: true reverses the direction behavior specified in settings + void setReverse(bool state); + + // sets motor enable on/off (if possible) + void enable(bool value); + + // get the associated stepper driver status + DriverStatus getDriverStatus() { if (ready) { driver->updateStatus(); return driver->getStatus(); } else return errorStatus; } + + // get movement frequency in steps per second + float getFrequencySteps(); + + // set frequency (+/-) in steps per second negative frequencies move reverse in direction (0 stops motion) + void setFrequencySteps(float frequency); + + // sets overall maximum frequency + // \param frequency: rate of motion in steps (counts) per second + void setFrequencyMax(float frequency) { + driver->setFrequencyMax(frequency); + } + + // get tracking mode steps per slewing mode step + inline int getStepsPerStepSlewing() { return driver->getMicrostepRatio(); } + + // microstep sequencer steps + int getSequencerSteps() { return driver->getMicrostepMode(); }; + + // set slewing state (hint that we are about to slew or are done slewing) + void setSlewing(bool state); + + // signal that the motor load has exceeded its threshold + bool isStalled(); + + // calibrate stealthChop then return to tracking mode + void calibrateDriver() { + if (!ready) return; + digitalWriteEx(Pins->enable, Pins->enabledState); + driver->calibrateDriver(); + digitalWriteEx(Pins->enable, !Pins->enabledState); + } + + #if defined(GPIO_DIRECTION_PINS) + // monitor and respond to motor state as required + void poll() { updateMotorDirection(); } + + // change motor direction on request by polling + void updateMotorDirection(); + #endif + + // sets dir as required and moves coord toward target at setFrequencySteps() rate + void move(const int16_t stepPin); + + // fast forward axis movement, no backlash, no mode switching + void moveFF(const int16_t stepPin); + + // fast reverse axis movement, no backlash, no mode switching + void moveFR(const int16_t stepPin); + + // get the motor name + const char* name() { strcpy(nameStr, "Step/Dir, "); strcat(nameStr, driver->name()); return nameStr; } + + // a stepper motor driver, should not be used above the StepDir class + StepDirDriver *driver; + + // the stepper motor driver pins, should not be used above the StepDir class + const StepDirPins *Pins; + + private: + // switch microstep modes as needed + void modeSwitch(); + + // swaps in/out fast unidirectional ISR for slewing + bool enableMoveFast(const bool state); + + void stopStepTimerAndClearPulse(); + + void resetToTrackingBaseline(); + + uint8_t taskHandle = 0; + + #ifdef DRIVER_STEP_DEFAULTS + #define stepClr LOW // pin state to reset driver before taking a step + #define stepSet HIGH // pin state to take a step + #else + volatile uint8_t stepClr = LOW; // pin state to reset driver before taking a step + volatile uint8_t stepSet = HIGH; // pin state to take a step + #endif + volatile uint8_t dirFwd = LOW; // pin state for forward direction + volatile uint8_t dirRev = HIGH; // pin state for reverse direction + volatile uint8_t direction = LOW; // current direction in use + volatile uint32_t pulseWidth = 2000; // step/dir driver pulse width in nanoseconds + + volatile int16_t stepSize = 1; // step size during slews (for microstep mode switching) + volatile bool takeStep = false; // should we take a step + + float currentFrequency = 0.0F; // last frequency set + float lastFrequency = 0.0F; // last frequency requested in tracking mode microsteps per second + unsigned long lastPeriod = 0; // last timer period (in sub-micros) + unsigned long lastPeriodSet = 0; // last timer period actually set (in sub-micros) + unsigned long switchStartTimeMs; // log time to switch microstep mode and do ISR swap + + volatile MicrostepModeControl microstepModeControl = MMC_TRACKING; + + bool useFastHardwareTimers = true; + + void (*callback)() = NULL; + void (*callbackFF)() = NULL; + void (*callbackFR)() = NULL; +}; + +#endif \ No newline at end of file diff --git a/src/lib/axis/motor/stepDir/StepDirDriver.cpp b/src/lib/axis/motor/stepDir/StepDirDriver.cpp new file mode 100755 index 0000000..2ec800b --- /dev/null +++ b/src/lib/axis/motor/stepDir/StepDirDriver.cpp @@ -0,0 +1,310 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver + +#include "StepDirDriver.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +#include "../../../gpioEx/GpioEx.h" + +// the various microsteps for different driver models, with the bit modes for each +#define DRIVER_MODEL_COUNT 22 + +const static int8_t steps[DRIVER_MODEL_COUNT][9] = +// 1 2 4 8 16 32 64 128 256x +{ + { 0, 1, 2, 3, 7,OFF,OFF,OFF,OFF}, // A4988 + { 0, 1, 2, 3, 4, 5,OFF,OFF,OFF}, // DRV8825 + { 0, 4, 4, 4, 4, 4, 4, 4, 4}, // GENERIC + { 0, 1, 2, 3, 4, 5, 6, 7,OFF}, // LV8729 + { 4, 2, 6, 5, 3, 7,OFF,OFF,OFF}, // S109 + { 0, 1, 2, 3, 4, 5,OFF, 6, 7}, // ST820 + { 0, 1, 2, 3, 4, 5, 6, 7,OFF}, // RAPS128 + { 0, 1, 2,OFF, 3,OFF,OFF,OFF,OFF}, // TMC2100 + { 0, 1, 2,OFF, 3,OFF,OFF,OFF,OFF}, // TMC2130S + {OFF, 1, 2, 0, 3,OFF,OFF,OFF,OFF}, // TMC2208S + {OFF,OFF, 0, 1, 2, 3,OFF,OFF,OFF}, // TMC2225S + {OFF,OFF,OFF, 0, 3, 1, 2,OFF,OFF}, // TMC2209S + {OFF,OFF,OFF, 0, 3, 1, 2,OFF,OFF}, // TMC2226S + + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC2130 + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC2160 + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC2660 + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC5160 + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC5161 + + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC2208 + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC2225 + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC2209 + { 8, 7, 6, 5, 4, 3, 2, 1, 0}, // TMC2226 +}; + +const static int16_t DriverPulseWidth[DRIVER_MODEL_COUNT] = +// minimum step pulse width, in ns +{ + 1000, // A4988 + 2000, // DRV8825 + OFF, // GENERIC + 500, // LV8729 + 7000, // RAPS128 + 300, // S109 + 20, // ST820 + 103, // TMC2100 + 103, // TMC2130S + 103, // TMC2208S + 103, // TMC2225S + 103, // TMC2209S + 103, // TMC2226S + + 103, // TMC2130 + 103, // TMC2160 + 103, // TMC2660 + 103, // TMC5160 + 103, // TMC5161 + + 103, // TMC2208 + 103, // TMC2225 + 103, // TMC2209 + 103, // TMC2226 +}; + +const char* DRIVER_NAME[DRIVER_MODEL_COUNT] = { + "A4988", + "DRV8825", + "GENERIC", + "LV8729", + "RAPS128", + "S109", + "ST820", + "TMC2100", + "TMC2130", + "TMC2208", + "TMC2225", + "TMC2209", + "TMC2226", + + "TMC2130 (SPI)", + "TMC2160 (SPI)", + "TMC2660 (SPI)", + "TMC5160 (SPI)", + "TMC5161 (SPI)", + + "TMC2208 (UART)", + "TMC2225 (UART)", + "TMC2209 (UART)", + "TMC2226 (UART)", +}; + +StepDirDriver::StepDirDriver(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings) { + this->axisNumber = axisNumber; + + this->Pins = Pins; + + driverModel = Settings->model; + statusMode = Settings->status; + + microsteps.valueDefault = Settings->microsteps; + microstepsSlewing.valueDefault = Settings->microstepsSlewing; + + if (microsteps.valueDefault == OFF) microsteps.valueDefault = 1.0F; + if (microstepsSlewing.valueDefault == OFF) microstepsSlewing.valueDefault = microsteps.valueDefault; + + decay.valueDefault = Settings->decay; + decaySlewing.valueDefault = Settings->decaySlewing; +} + +bool StepDirDriver::init() { + if (driverModel >= DRIVER_MODEL_COUNT) { + DF("ERR:"); D(axisPrefix); DLF("invalid driver model"); + return false; + } + + VF("MSG:"); V(axisPrefix); VL(name()); + + // check if platform pulse width (ns) is ok for this stepper driver timing in PULSE mode + #if STEP_WAVE_FORM == PULSE + long pulseWidth = HAL_PULSE_WIDTH; + if (axisNumber > 2) pulseWidth = 2000; + + if (DriverPulseWidth[driverModel] == OFF) { + VF("MSG:"); V(axisPrefix); V(DRIVER_NAME[driverModel]); VLF(" min. pulse width unknown!"); + } + + if (DriverPulseWidth[driverModel] > pulseWidth) { + DF("WRN:"); D(axisPrefix); D(DRIVER_NAME[driverModel]); + DF(" min. pulse width "); D(DriverPulseWidth[driverModel]); DF("ns > platform at "); D(pulseWidth); DLF("ns"); + return false; + } + #endif + + normalizedMicrosteps = lround(microsteps.value); + normalizedMicrostepsSlewing = lround(microstepsSlewing.value); + VF("MSG:"); V(axisPrefix); VF("microstep mode "); V(microsteps.value); VF("X"); + VF(" (goto mode "); V(microstepsSlewing.value); VL("X)"); + + // set parmeter min/max to cover range of available microsteps for this driver + int microstep[9] = {1, 2, 4, 8, 16, 32, 64, 128, 256}; + int lowestMode = 256; + int highestMode = 1; + for (int i = 0; i < 9; i++) { + if (subdivisionsToCode(microstep[i]) != OFF) { + if (microstep[i] < lowestMode) lowestMode = microstep[i]; + if (microstep[i] > highestMode) highestMode = microstep[i]; + } + } + microsteps.min = lowestMode; + microstepsSlewing.min = lowestMode; + microsteps.max = highestMode; + microstepsSlewing.max = highestMode; + + microstepCode = subdivisionsToCode(normalizedMicrosteps); + microstepCodeSlewing = subdivisionsToCode(normalizedMicrostepsSlewing); + + if (microstepCode == OFF || microstepCodeSlewing == OFF) { + DF("MSG:"); D(axisPrefix); DLF("invalid microstep mode for this driver."); + return false; + } + + microstepRatio = normalizedMicrosteps/normalizedMicrostepsSlewing; + + normalizedDecay = lround(decay.value); + normalizedDecaySlewing = lround(decaySlewing.value); + + // automatically set fault status for known drivers + if (statusMode == ON) { + switch (driverModel) { + case DRV8825: statusMode = LOW; break; + case ST820: statusMode = LOW; break; + default: break; + } + } + + // disable certain status modes if the fault pin isn't defined + if ((statusMode == LOW || statusMode == HIGH) && Pins->fault == OFF) { + statusMode = OFF; + } + + // flag status as active or not + status.active = statusMode != OFF; + + // set fault pin mode + if (statusMode == LOW) pinModeEx(Pins->fault, INPUT_PULLUP); + #ifdef PULLDOWN + if (statusMode == HIGH) pinModeEx(Pins->fault, INPUT_PULLDOWN); + #else + if (statusMode == HIGH) pinModeEx(Pins->fault, INPUT); + #endif + + // check to see if the driver is ok if we're using a fault pin + if (statusMode == LOW || statusMode == HIGH) { + status.fault = digitalReadEx(Pins->fault) == statusMode; + if (status.fault) { + DF("ERR:"); D(axisPrefix); DLF("motor driver device fault!"); + return false; + } else { + VF("MSG:"); V(axisPrefix); VLF("motor driver device ok"); + } + } + + return true; +} + +bool StepDirDriver::parameterIsValid(AxisParameter* parameter, bool next) { + if (parameter == µsteps || parameter == µstepsSlewing) { + + int microstepsValue, microstepsSlewingValue; + if (next) { + microstepsValue = lround(microsteps.valueNv); + microstepsSlewingValue = lround(microstepsSlewing.valueNv); + } else { + microstepsValue = lround(microsteps.value); + microstepsSlewingValue = lround(microstepsSlewing.value); + } + + if (subdivisionsToCode(microstepsValue) == OFF) { + DF("WRN:"); D(axisPrefix); DLF("unsupported microsteps value"); + return false; + } + if (subdivisionsToCode(microstepsSlewingValue) == OFF) { + DF("WRN:"); D(axisPrefix); DLF("unsupported microsteps slewing value"); + return false; + } + if (microstepsValue < microstepsSlewingValue) { + DF("WRN:"); D(axisPrefix); + DF("bad microsteps "); D(microstepsValue); + DF(" must be >= microstepsGoto "); DL(microstepsSlewingValue); + return false; + } + } + return true; +} + +// get the pulse width in nanoseconds, if unknown (-1) returns 2000 nanoseconds +long StepDirDriver::getPulseWidth() { + long ns = DriverPulseWidth[driverModel]; + if (ns < 0) ns = 2000; + return ns; +} + +// different models of stepper drivers have different bit settings for microsteps +// translate the human readable microsteps in the configuration to mode bit settings +// returns bit code (0 to 7) or OFF if microsteps is not supported or unknown +int StepDirDriver::subdivisionsToCode(long microsteps) { + int allowed[9] = {1,2,4,8,16,32,64,128,256}; + if (driverModel >= DRIVER_MODEL_COUNT) return OFF; + for (int i = 0; i < 9; i++) { + if (microsteps == allowed[i]) return steps[driverModel][i]; + } + return OFF; +} + +// get the driver name +const char* StepDirDriver::name() { + return DRIVER_NAME[driverModel]; +} + +// update status info. for driver +void StepDirDriver::updateStatus() { + if (statusMode == ON) { + const unsigned long now = millis(); + + if (now - timeLastStatusUpdate > 200U) { + readStatus(); + + // open load indication is not reliable in standstill + status.fault = status.outputA.shortToGround || + status.outputB.shortToGround || + status.overTemperatureWarning || + status.overTemperature; + + timeLastStatusUpdate = now; + } + } else + if (statusMode == LOW || statusMode == HIGH) { + status.fault = digitalReadEx(Pins->fault) == statusMode; + } + + #if DEBUG == VERBOSE + if ((status.outputA.shortToGround != lastStatus.outputA.shortToGround) || +// (status.outputA.openLoad != lastStatus.outputA.openLoad) || + (status.outputB.shortToGround != lastStatus.outputB.shortToGround) || +// (status.outputB.openLoad != lastStatus.outputB.openLoad) || + (status.overTemperatureWarning != lastStatus.overTemperatureWarning) || + (status.overTemperature != lastStatus.overTemperature) || +// (status.standstill != lastStatus.standstill) || + (status.fault != lastStatus.fault)) { + VF("MSG:"); V(axisPrefix); + VF("SGA"); if (status.outputA.shortToGround) VF("< "); else VF(". "); + VF("OLA"); if (status.outputA.openLoad) VF("< "); else VF(". "); + VF("SGB"); if (status.outputB.shortToGround) VF("< "); else VF(". "); + VF("OLB"); if (status.outputB.openLoad) VF("< "); else VF(". "); + VF("OTP"); if (status.overTemperatureWarning) VF("< "); else VF(". "); + VF("OTE"); if (status.overTemperature) VF("< "); else VF(". "); + VF("SST"); if (status.standstill) VF("< "); else VF(". "); + VF("FLT"); if (status.fault) VLF("<"); else VLF("."); + } + lastStatus = status; + #endif +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/StepDirDriver.h b/src/lib/axis/motor/stepDir/StepDirDriver.h new file mode 100755 index 0000000..92a8155 --- /dev/null +++ b/src/lib/axis/motor/stepDir/StepDirDriver.h @@ -0,0 +1,146 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver +#pragma once + +#include +#include "../../../../Common.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +#include "../Drivers.h" + +typedef struct StepDirDriverPins { + int16_t m0; + int16_t m1; + int16_t m2; + uint8_t m2State; + int16_t m3; + int16_t decay; + int16_t fault; +} StepDirDriverPins; + +typedef struct StepDirDriverSettings { + int16_t model; + int8_t status; + int16_t microsteps; + int16_t microstepsSlewing; + int8_t decay; + int8_t decaySlewing; +} StepDirDriverSettings; + +class StepDirDriver { + public: + StepDirDriver(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings); + + // set up driver + virtual bool init(); + + // revert settings to defaults + virtual void useDefaults() { return; } + + // returns the number of axis parameters + virtual uint8_t getParameterCount() { return 0; } + + // returns the specified axis parameter + virtual AxisParameter* getParameter(uint8_t number) { UNUSED(number); return &invalid; } + + // check if axis parameter is valid + virtual bool parameterIsValid(AxisParameter* parameter, bool next = false); + + // set microstep mode for tracking + virtual void modeMicrostepTracking() {} + + // set microstep mode for slewing + virtual int modeMicrostepSlewing() { return 1; } + + // set decay mode for tracking + virtual void modeDecayTracking() {} + + // set decay mode for slewing + virtual void modeDecaySlewing() {} + + // get microstep ratio for slewing + inline int getMicrostepRatio() { return microstepRatio; } + + // get microstep mode used for tracking + inline int getMicrostepMode() { return normalizedMicrosteps; } + + // update status info. for driver + virtual void updateStatus(); + + // get status info. + inline DriverStatus getStatus() { return status; } + + // secondary way to power down not using the enable pin + // default assume the driver is always enabled, i.e. no control + virtual bool enable(bool state) { UNUSED(state); return true; } + + // sets frequency corrosponding to the fastest allowed slew rate + // \param frequency: rate of motion in steps per second + void setFrequencyMax(float frequency) { velocityMax = frequency; } + + // calibrate the motor driver if required + virtual void calibrateDriver() {} + + // determine if this motor can detect a stall + virtual bool hasStallDetect() const { return false; } + + // check to if the motor is stalled + virtual bool isStalled(float stepsPerSec) { (void)stepsPerSec; return false; } + + // reset any internal stall-detect state (baseline, latch, etc.) + virtual void stallDetectReset() {} + + // get the pulse width in nanoseconds, if unknown (-1) returns 2000 nanoseconds + long getPulseWidth(); + + // get the driver name + virtual const char* name(); + + // true if switching microstep modes at low speed is allowed + bool modeSwitchAllowed = false; + + // true if switching microstep modes at high speed is allowed + bool modeSwitchFastAllowed = false; + + protected: + // read status from driver + virtual void readStatus() {} + + // different models of stepper drivers have different bit settings for microsteps + // translate the human readable microsteps in the configuration to mode bit settings + // returns bit code (0 to 7) or OFF if microsteps is not supported or unknown + int subdivisionsToCode(long microsteps); + + uint8_t axisNumber; + char axisPrefix[32]; // prefix for debug messages + + DriverStatus status = {false, {false, false}, {false, false}, false, false, false, false}; + #if DEBUG != OFF + DriverStatus lastStatus = {false, {false, false}, {false, false}, false, false, false, false}; + #endif + unsigned long timeLastStatusUpdate = 0; + + const StepDirDriverPins *Pins; + + int16_t driverModel = 0; + int16_t statusMode; + int16_t normalizedMicrosteps; + int16_t normalizedMicrostepsSlewing; + int16_t normalizedDecay; + int16_t normalizedDecaySlewing; + int16_t microstepRatio = 1; + int16_t microstepCode = OFF; + int16_t microstepCodeSlewing = OFF; + + float velocityMax = 1.0F; // frequency corrosponding to the fastest allowed slew rate, in microsteps/s + + // runtime adjustable settings + AxisParameter invalid = {NAN, NAN, NAN, NAN, NAN, AXP_INVALID, ""}; + AxisParameter microsteps = {NAN, NAN, NAN, 1, 256, AXP_POW2, AXPN_MICROSTEPS}; + AxisParameter microstepsSlewing = {NAN, NAN, NAN, 1, 256, AXP_POW2, AXPN_MICROSTEPS_GOTO}; + AxisParameter decay = {NAN, NAN, NAN, DRIVER_DECAY_MODE_FIRST, DRIVER_DECAY_MODE_LAST, AXP_DECAY, AXPN_DECAY_MODE}; + AxisParameter decaySlewing = {NAN, NAN, NAN, DRIVER_DECAY_MODE_FIRST, DRIVER_DECAY_MODE_LAST, AXP_DECAY, AXPN_DECAY_MODE_GOTO}; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/generic/Generic.cpp b/src/lib/axis/motor/stepDir/generic/Generic.cpp new file mode 100755 index 0000000..9379f41 --- /dev/null +++ b/src/lib/axis/motor/stepDir/generic/Generic.cpp @@ -0,0 +1,113 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver + +#include "Generic.h" + +#ifdef STEP_DIR_LEGACY_PRESENT + +#include "../../../../gpioEx/GpioEx.h" + +// constructor +StepDirGeneric::StepDirGeneric(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings) : StepDirDriver(axisNumber, Pins, Settings) { + strcpy(axisPrefix, " Axis_StepDirGeneric, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirGeneric::init() { + if (!StepDirDriver::init()) return false; + + m0Pin = Pins->m0; + m1Pin = Pins->m1; + if (isDecayOnM2()) { decayPin = Pins->m2; m2Pin = OFF; } else { decayPin = Pins->decay; m2Pin = Pins->m2; } + pinModeEx(decayPin, OUTPUT); + digitalWriteEx(decayPin, getDecayPinState(normalizedDecay)); + + #if DEBUG == VERBOSE + VF("MSG: StepDirDriver"); V(axisNumber); + V(", pins m0="); if (Pins->m0 == OFF) V("OFF"); else V(Pins->m0); + V(", m1="); if (Pins->m1 == OFF) VF("OFF"); else V(Pins->m1); + V(", m2="); if (m2Pin == OFF) VF("OFF"); else V(m2Pin); + V(", decay="); if (decayPin == OFF) VF("OFF"); else V(decayPin); + if (statusMode == ON) { + V(", fault="); if (Pins->fault == OFF) VF("OFF"); else V(Pins->fault); + } + VL(""); + #endif + + // toggle the M2 pin states if needed, this allows extra flexibility for stand alone stepper drivers + if (Pins->m2State == LOW) { + if (bitRead(microstepCode, 2)) bitClear(microstepCode, 2); else bitSet(microstepCode, 2); + if (bitRead(microstepCodeSlewing, 2)) bitClear(microstepCodeSlewing, 2); else bitSet(microstepCodeSlewing, 2); + } + + microstepBitCode = microstepCode; + microstepBitCodeM0 = bitRead(microstepBitCode, 0); + microstepBitCodeM1 = bitRead(microstepBitCode, 1); + microstepBitCodeM2 = bitRead(microstepBitCode, 2); + + microstepBitCodeGoto = microstepCodeSlewing; + microstepBitCodeGotoM0 = bitRead(microstepBitCodeGoto, 0); + microstepBitCodeGotoM1 = bitRead(microstepBitCodeGoto, 1); + microstepBitCodeGotoM2 = bitRead(microstepBitCodeGoto, 2); + + pinModeEx(m0Pin, OUTPUT); + digitalWriteEx(m0Pin, microstepBitCodeM0); + pinModeEx(m1Pin, OUTPUT); + digitalWriteEx(m1Pin, microstepBitCodeM1); + pinModeEx(m2Pin, OUTPUT); + digitalWriteEx(m2Pin, microstepBitCodeM2); + + // set mode switching support flags + // use high speed mode + modeSwitchAllowed = false; + modeSwitchFastAllowed = microstepRatio != 1; + VF("MSG: StepDirDriver"); V(axisNumber); + if (modeSwitchAllowed) { VLF(", ModeSwitch Allowed"); }else { VLF(", ModeSwitch Disabled"); } + VF("MSG: StepDirDriver"); V(axisNumber); + if (modeSwitchFastAllowed) { VLF(", ModeSwitchFast Allowed"); } else { VLF(", ModeSwitchFast Disabled"); } + + return true; +} + +IRAM_ATTR void StepDirGeneric::modeMicrostepTracking() { + if (m0Pin != OFF) digitalWriteF(m0Pin, microstepBitCodeM0); + if (m1Pin != OFF) digitalWriteF(m1Pin, microstepBitCodeM1); + digitalWriteEx(m2Pin, microstepBitCodeM2); +} + +IRAM_ATTR int StepDirGeneric::modeMicrostepSlewing() { + if (microstepRatio > 1) { + if (m0Pin != OFF) digitalWriteF(m0Pin, microstepBitCodeGotoM0); + if (m1Pin != OFF) digitalWriteF(m1Pin, microstepBitCodeGotoM1); + digitalWriteEx(m2Pin, microstepBitCodeGotoM2); + } + return microstepRatio; +} + +void StepDirGeneric::modeDecayTracking() { + if (normalizedDecay == OFF) return; + int8_t state = getDecayPinState(normalizedDecay); + if (state != OFF) digitalWriteEx(decayPin, state); +} + +void StepDirGeneric::modeDecaySlewing() { + if (normalizedDecaySlewing == OFF) return; + int8_t state = getDecayPinState(normalizedDecaySlewing); + if (state != OFF) digitalWriteEx(decayPin, state); +} + +int8_t StepDirGeneric::getDecayPinState(int8_t decay) { + uint8_t state = OFF; + if (decay == SPREADCYCLE) state = HIGH; else + if (decay == STEALTHCHOP) state = LOW; else + if (decay == MIXED) state = LOW; else + if (decay == FAST) state = HIGH; + return state; +} + +bool StepDirGeneric::isDecayOnM2() { + if (driverModel == TMC2209S || driverModel == TMC2225S) return true; else return false; +} + +#endif diff --git a/src/lib/axis/motor/stepDir/generic/Generic.h b/src/lib/axis/motor/stepDir/generic/Generic.h new file mode 100755 index 0000000..e0694b9 --- /dev/null +++ b/src/lib/axis/motor/stepDir/generic/Generic.h @@ -0,0 +1,68 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver +#pragma once + +#include +#include "../../../../../Common.h" + +#ifdef STEP_DIR_LEGACY_PRESENT + +#include "../../Drivers.h" +#include "../StepDirDriver.h" + +class StepDirGeneric : StepDirDriver { + public: + // constructor + StepDirGeneric(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings); + + // setup driver + bool init(); + + // returns the number of axis parameters + uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // read status info. from driver + inline void readStatus() {}; + + private: + + // checks if decay pin should be HIGH/LOW for a given decay setting + int8_t getDecayPinState(int8_t decay); + + // checkes if decay control is on the M2 pin + bool isDecayOnM2(); + + uint8_t microstepBitCode = 0; + uint8_t microstepBitCodeM0 = 0; + uint8_t microstepBitCodeM1 = 0; + uint8_t microstepBitCodeM2 = 0; + uint8_t microstepBitCodeGoto = 0; + uint8_t microstepBitCodeGotoM0 = 0; + uint8_t microstepBitCodeGotoM1 = 0; + uint8_t microstepBitCodeGotoM2 = 0; + int16_t m0Pin = OFF; + int16_t m1Pin = OFF; + int16_t m2Pin = OFF; + int16_t decayPin = OFF; + + // runtime adjustable settings + const int numParameters = 2; + AxisParameter* parameter[3] = {&invalid, µsteps, µstepsSlewing}; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriver.cpp b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriver.cpp new file mode 100755 index 0000000..6e2b7ac --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriver.cpp @@ -0,0 +1,253 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver + +#include "TmcStepDirDriver.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +#ifndef TMC2130_RSENSE +#define TMC2130_RSENSE 0.11F +#endif +#ifndef TMC2130_MAX_CURRENT +#define TMC2130_MAX_CURRENT (1700*0.8) // chip typically rated at 1.2A RMS, downrated to 80% due to typical step-stick form +#endif + +#ifndef TMC2160_RSENSE +#define TMC2160_RSENSE 0.075F +#endif +#ifndef TMC2160_MAX_CURRENT +#define TMC2160_MAX_CURRENT 4230 // module typically rated at 3.0A RMS +#endif + +#ifndef TMC2208_RSENSE +#define TMC2208_RSENSE 0.11F +#endif +#ifndef TMC2208_MAX_CURRENT +#define TMC2208_MAX_CURRENT (1974*0.8) // chip rated at 1.4A RMS, downrated to 80% due to typical step-stick form +#endif + +#ifndef TMC2209_RSENSE +#define TMC2209_RSENSE 0.11F +#endif +#ifndef TMC2209_MAX_CURRENT +#define TMC2209_MAX_CURRENT (2820*0.8) // chip rated at 2.0A RMS, downrated to 80% due to typical step-stick form +#endif + +#ifndef TMC2226_RSENSE +#define TMC2226_RSENSE 0.11F +#endif +#ifndef TMC2226_MAX_CURRENT +#define TMC2226_MAX_CURRENT (2820*0.8) // chip rated at 2.0A RMS, downrated to 80% due to typical step-stick form +#endif + +#ifndef TMC2660_RSENSE +#define TMC2660_RSENSE 0.075F +#endif +#ifndef TMC2660_MAX_CURRENT +#define TMC2660_MAX_CURRENT 2820 // module/chip rated at 2.0A RMS +#endif + +#ifndef TMC5160_RSENSE +#define TMC5160_RSENSE 0.075F +#endif +#ifndef TMC5160_MAX_CURRENT +#define TMC5160_MAX_CURRENT 4230 // typical module rated at 3.0A RMS +#endif + +#ifndef TMC5161_RSENSE +#define TMC5161_RSENSE 0.075F +#endif +#ifndef TMC5161_MAX_CURRENT +#define TMC5161_MAX_CURRENT (4935*0.8) // chip rated at 3.5A RMS, downrated to 80% due to typical step-stick form +#endif + +TmcStepDirDriver::TmcStepDirDriver(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :StepDirDriver(axisNumber, Pins, Settings) { + + // get the maximum current and Rsense for this axis + float user_rSense = 0; + int16_t user_currentMax = 0; + switch (axisNumber) { + case 1: + #ifdef AXIS1_DRIVER_RSENSE + user_rSense = AXIS1_DRIVER_RSENSE; + #endif + #ifdef AXIS1_DRIVER_MAX_CURRENT + user_currentMax = AXIS1_DRIVER_MAX_CURRENT; + #endif + break; + case 2: + #ifdef AXIS2_DRIVER_RSENSE + user_rSense = AXIS2_DRIVER_RSENSE; + #endif + #ifdef AXIS2_DRIVER_MAX_CURRENT + user_currentMax = AXIS2_DRIVER_MAX_CURRENT; + #endif + break; + case 3: + #ifdef AXIS3_DRIVER_RSENSE + user_rSense = AXIS3_DRIVER_RSENSE; + #endif + #ifdef AXIS3_DRIVER_MAX_CURRENT + user_currentMax = AXIS3_DRIVER_MAX_CURRENT; + #endif + break; + case 4: + #ifdef AXIS4_DRIVER_RSENSE + user_rSense = AXIS4_DRIVER_RSENSE; + #endif + #ifdef AXIS4_DRIVER_MAX_CURRENT + user_currentMax = AXIS4_DRIVER_MAX_CURRENT; + #endif + break; + case 5: + #ifdef AXIS5_DRIVER_RSENSE + user_rSense = AXIS5_DRIVER_RSENSE; + #endif + #ifdef AXIS5_DRIVER_MAX_CURRENT + user_currentMax = AXIS5_DRIVER_MAX_CURRENT; + #endif + break; + case 6: + #ifdef AXIS6_DRIVER_RSENSE + user_rSense = AXIS6_DRIVER_RSENSE; + #endif + #ifdef AXIS6_DRIVER_MAX_CURRENT + user_currentMax = AXIS6_DRIVER_MAX_CURRENT; + #endif + break; + case 7: + #ifdef AXIS7_DRIVER_RSENSE + user_rSense = AXIS7_DRIVER_RSENSE; + #endif + #ifdef AXIS7_DRIVER_MAX_CURRENT + user_currentMax = AXIS7_DRIVER_MAX_CURRENT; + #endif + break; + case 8: + #ifdef AXIS8_DRIVER_RSENSE + user_rSense = AXIS8_DRIVER_RSENSE; + #endif + #ifdef AXIS8_DRIVER_MAX_CURRENT + user_currentMax = AXIS8_DRIVER_MAX_CURRENT; + #endif + break; + case 9: + #ifdef AXIS9_DRIVER_RSENSE + user_rSense = AXIS9_DRIVER_RSENSE; + #endif + #ifdef AXIS9_DRIVER_MAX_CURRENT + user_currentMax = AXIS9_DRIVER_MAX_CURRENT; + #endif + break; + } + + if (driverModel == TMC2130) { currentMax = TMC2130_MAX_CURRENT; rSense = TMC2130_RSENSE; } else + if (driverModel == TMC2160) { currentMax = TMC2160_MAX_CURRENT; rSense = TMC2160_RSENSE; } else + if (driverModel == TMC2660) { currentMax = TMC2660_MAX_CURRENT; rSense = TMC2660_RSENSE; } else + if (driverModel == TMC5160) { currentMax = TMC5160_MAX_CURRENT; rSense = TMC5160_RSENSE; } else + if (driverModel == TMC5161) { currentMax = TMC5161_MAX_CURRENT; rSense = TMC5161_RSENSE; } else + if (driverModel == TMC2208) { currentMax = TMC2208_MAX_CURRENT; rSense = TMC2208_RSENSE; } else + if (driverModel == TMC2209) { currentMax = TMC2209_MAX_CURRENT; rSense = TMC2209_RSENSE; } else + if (driverModel == TMC2226) { currentMax = TMC2226_MAX_CURRENT; rSense = TMC2226_RSENSE; } else + driverModel = 0; + + if (user_rSense != 0) rSense = user_rSense; + if (user_currentMax != 0) currentMax = user_currentMax; + + // set current defaults for TMC drivers + if (lround(currentRun) == OFF) { + if (driverModel == TMC2130) currentRun = 2500.0F; else currentRun = 300.0F; + } + currentHold = lround(currentHold) == OFF ? currentRun/2.0F : currentHold; + currentSlewing = lround(currentSlewing) == OFF ? currentRun : currentSlewing; + + this->currentHold.valueDefault = currentHold; + this->currentHold.max = currentMax; + this->currentRun.valueDefault = currentRun; + this->currentRun.max = currentMax; + this->currentSlewing.valueDefault = currentSlewing; + this->currentSlewing.max = currentMax; + this->intpol.valueDefault = intpol; + + // setup decay modes + if (decay.valueDefault == OFF) decay.valueDefault = STEALTHCHOP; + if (decaySlewing.valueDefault == OFF) decaySlewing.valueDefault = SPREADCYCLE; + decay.min = SPREADCYCLE; + decay.max = STEALTHCHOP; + decaySlewing.min = SPREADCYCLE; + decaySlewing.max = STEALTHCHOP; +} + +bool TmcStepDirDriver::init() { + if (!StepDirDriver::init()) return false; + + if (!driverModel) { + DF("WRN:"); D(axisPrefix); DLF("unknown driver model!"); + return false; + } + + VF("MSG:"); V(axisPrefix); VF("Rsense="); V(rSense); VL("Ohms"); + VF("MSG:"); V(axisPrefix); VF("max current="); V(currentMax); VL("mA"); + + iHold = currentHold.value; + iRun = currentRun.value; + iGoto = currentSlewing.value; + iHoldRatio = (float)iHold/iRun; + + VF("MSG:"); V(axisPrefix); + VF("IHOLD="); V(lround(iHold)); VF("mA, "); + VF("IRUN="); V(lround(iRun)); VF("mA, "); + VF("IGOTO="); V(lround(iGoto)); VL("mA"); + + // set mode switching support flags + // use low speed mode switch for TMC drivers + modeSwitchAllowed = microstepRatio != 1; + modeSwitchFastAllowed = false; + + VF("MSG:"); V(axisPrefix); + if (modeSwitchAllowed) { VLF("ModeSwitch Allowed"); }else { VLF("ModeSwitch Disabled"); } + VF("MSG:"); V(axisPrefix); + if (modeSwitchFastAllowed) { VLF("ModeSwitchFast Allowed"); } else { VLF("ModeSwitchFast Disabled"); } + + return true; +} + +bool TmcStepDirDriver::parameterIsValid(AxisParameter* parameter, bool next) { + if (!StepDirDriver::parameterIsValid(parameter, next)) return false; + + if (parameter == ¤tHold || parameter == ¤tRun || parameter == ¤tSlewing) { + int currentHoldValue; + int currentRunValue; + int currentSlewingValue; + if (next) { + currentHoldValue = lround(currentHold.valueNv); + currentRunValue = lround(currentRun.valueNv); + currentSlewingValue = lround(currentSlewing.valueNv); + } else { + currentHoldValue = lround(currentHold.value); + currentRunValue = lround(currentRun.value); + currentSlewingValue = lround(currentSlewing.value); + } + + if (currentRunValue > currentMax) { + DF("WRN:"); D(axisPrefix); DF("bad current run="); D(currentRunValue); DF("mA > current max "); D(currentMax); DLF("mA"); + return false; + } + + if (currentSlewingValue > currentMax) { + DF("WRN:"); D(axisPrefix); DF("bad current goto="); D(currentSlewingValue); DF("mA > current max "); D(currentMax); DLF("mA"); + return false; + } + + if (currentHoldValue > currentRunValue) { + DF("WRN:"); D(axisPrefix); DF("bad current hold="); D(currentHoldValue); DF("mA > current run "); D(currentRunValue); DLF("mA"); + return false; + } + } + + return true; +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriver.h b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriver.h new file mode 100755 index 0000000..a63afac --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriver.h @@ -0,0 +1,52 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver +#pragma once + +#include "../StepDirDriver.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +// default settings for any TMC drivers that may be present +#ifndef DRIVER_TMC_STEPPER_AUTOGRAD + #define DRIVER_TMC_STEPPER_AUTOGRAD true // note that AUTOGRAD was false for SPI drivers before! +#endif +#ifndef SERIAL_TMC + #define SERIAL_TMC SoftSerial // Use software serial w/ TX on M3 (CS) and RX on M4 (MISO) of each axis +#endif +#ifndef SERIAL_TMC_BAUD + #define SERIAL_TMC_BAUD 115200 // Slower and more timing disrutpions or faster and it doesn't work (platform dependant) +#endif +#ifndef SERIAL_TMC_ADDRESS_MAP + #define SERIAL_TMC_ADDRESS_MAP(x) (0) // driver addresses are 0 for all axes +#endif + +#if SERIAL_TMC == SoftSerial + #include // must be built into the board libraries +#endif + +class TmcStepDirDriver : public StepDirDriver { + public: + TmcStepDirDriver(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // set up driver + virtual bool init(); + + // check if axis parameter is valid + bool parameterIsValid(AxisParameter* parameter, bool next = false); + + protected: + // driver current control + float rSense = 0.11F; + int16_t currentMax = 0; + + float iHold, iRun, iGoto, iHoldRatio; + + // runtime adjustable settings + AxisParameter currentHold = {NAN, NAN, NAN, 0, 20000, AXP_INTEGER, AXPN_CURRENT_HOLD}; + AxisParameter currentRun = {NAN, NAN, NAN, 0, 20000, AXP_INTEGER, AXPN_CURRENT_RUN}; + AxisParameter currentSlewing = {NAN, NAN, NAN, 0, 20000, AXP_INTEGER, AXPN_CURRENT_GOTO}; + AxisParameter intpol = {NAN, NAN, NAN, -2, -1, AXP_BOOLEAN, AXPN_INTERPOLATE}; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverNSG.cpp b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverNSG.cpp new file mode 100755 index 0000000..7dbecfe --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverNSG.cpp @@ -0,0 +1,14 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver + +#include "TmcStepDirDriverNSG.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +TmcStepDirDriverNSG::TmcStepDirDriverNSG(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriver(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverNSG.h b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverNSG.h new file mode 100755 index 0000000..665e638 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverNSG.h @@ -0,0 +1,30 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver +#pragma once + +#include "TmcStepDirDriver.h" + +#ifdef STEP_DIR_MOTOR_PRESENT + +class TmcStepDirDriverNSG : public TmcStepDirDriver { + public: + TmcStepDirDriverNSG(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // returns the number of axis parameters + uint8_t getParameterCount() { return numParameters; } + + // returns the specified axis parameter + AxisParameter* getParameter(uint8_t number) { if (number > numParameters) return &invalid; else return parameter[number]; } + + protected: + + const int numParameters = 7; + AxisParameter* parameter[8] = {&invalid, µsteps, µstepsSlewing, + ¤tHold, ¤tRun, ¤tSlewing, + &decay, &decaySlewing +// ,&intpol + }; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverSG.cpp b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverSG.cpp new file mode 100755 index 0000000..adce5e7 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverSG.cpp @@ -0,0 +1,730 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver + StallGuard helper +// + +#include +#include +#include "TmcStepDirDriverSG.h" +#include "../../../../nv/Nv.h" + +#if defined(STEP_DIR_MOTOR_PRESENT) + +// Dual-rate adaptation: +// - SG Train ON => fast EMA (explicit training) +#ifndef BASE_TC_FAST + #define BASE_TC_FAST 128.0f +#endif +#ifndef DEV_TC_FAST + #define DEV_TC_FAST 256.0f +#endif +// - SG Train OFF => slow EMA drift tracking +#ifndef BASE_TC_SLOW + #define BASE_TC_SLOW 4096.0f +#endif +#ifndef DEV_TC_SLOW + #define DEV_TC_SLOW 8192.0f +#endif + +// Margin for model: +#define MARGIN_GENERAL 80 // general was 40 +#define MARGIN_NOISE 4 // noise relative was 3 + +TmcStepDirDriverSG::TmcStepDirDriverSG(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriver(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + + sgEnabled.valueDefault = (float)SG_ENABLED; + sgTrain.valueDefault = (float)SG_TRAIN; + + // Floor disabled by default (0%) + sgFloorPct.valueDefault = (float)SG_FLOOR; + sgRampAllowPct.valueDefault = (float)SG_RAMP_ALLOW; + + sgArmFps.valueDefault = (float)SG_ARM_FPS; + sgArmFaultMs.valueDefault = (float)SG_FAULT_MS; + sgSensitivity.valueDefault = (float)SG_SENS; +} + +bool TmcStepDirDriverSG::init() { + if (!TmcStepDirDriver::init()) return false; + + #ifdef DRIVER_TMC_STEPPER_STALLGUARD + + #ifdef DRIVER_TMC_STEPPER_STALLGUARD_BASIC + sgArmFps.value = (float)SG_ARM_FPS; + sgArmFaultMs.value = (float)SG_FAULT_MS; + sgSensitivity.value = (float)SG_SENS; + #endif + + sgLastFps = 0.0f; + sgRampF = 0.0f; + sgTrainPrev = sgTrain.value; + sgModelSavePending = false; + sgModelSaveIntervalMs = (uint32_t)SG_MODEL_SAVE_MS; + if (nv().device().endurance() == NvDevice::Endurance::Mid) sgModelSaveIntervalMs = (uint32_t)SG_MODEL_SAVE_MS_MID; else + if (nv().device().endurance() == NvDevice::Endurance::High) sgModelSaveIntervalMs = (uint32_t)SG_MODEL_SAVE_MS_HIGH; + sgModelLastSaveMs = millis(); + + // Default model in RAM first + sgModelInit(sgModel); + + // create the nvKey for this axis + char keyStr[24]; + sprintf(keyStr, "STALLGUARD_AXIS%u", (unsigned)axisNumber); + nvKey = nv().kv().computeKey(keyStr); + + if (sgEnabled.value == ON) { + // Read existing model (if present) + KvPartition::Status result = nv().kv().get(nvKey, sgModel); + if (result == KvPartition::Status::Ok) { + VF("MSG:"); V(axisPrefix); VLF("StallGuard model NV file read success"); + } else if (result == KvPartition::Status::NotFound) { + VF("MSG:"); V(axisPrefix); VLF("StallGuard model NV file not found (using defaults)"); + } else { + DF("WRN:"); D(axisPrefix); DLF("StallGuard model NV file read failed"); + } + + sgModelDebugDumpNv(); + + // If loaded model is stale/incompatible, reset to defaults and store back + if (!sgModelValid(sgModel)) { + DF("WRN:"); D(axisPrefix); DLF("StallGuard model invalid/discarded"); + sgModelInit(sgModel); + sgRuntimeFromModel(); + if (nv().kv().put(nvKey, sgModel) != KvPartition::Status::Ok) { DF("WRN:"); D(axisPrefix); DLF("StallGuard save failed"); } + } else { + sgRuntimeFromModel(); + } + + } else { + // Delete existing model (if present) + KvPartition::Status result = nv().kv().del(nvKey); + if (result == KvPartition::Status::Ok) { + VF("MSG:"); V(axisPrefix); VLF("StallGuard model NV file found and deleted"); + } else if (result != KvPartition::Status::NotFound) { + DF("WRN:"); D(axisPrefix); DLF("StallGuard model NV file delete error"); + } + } + + // bin cache invalid until first use + sgBinValid = false; + sgBinArmFps = 0.0f; + sgBinHi = 0.0f; + for (int i = 0; i < (int)(SG_BINS - 1); i++) sgBinEdge[i] = 0.0f; + for (int i = 0; i < (int)SG_BINS; i++) sgBinAnchor[i] = 0.0f; + #endif + + return true; +} + +// check if sg axis parameter is valid +bool TmcStepDirDriverSG::parameterIsValid(AxisParameter* parameter, bool next) { + if (!TmcStepDirDriver::parameterIsValid(parameter, next)) return false; + + if (parameter == &sgEnabled || parameter == &decaySlewing) { + int sgEnabledValue, decaySlewingValue; + if (next) { + sgEnabledValue = lround(sgEnabled.valueNv); + decaySlewingValue = lround(decaySlewing.valueNv); + } else { + sgEnabledValue = lround(sgEnabled.value); + decaySlewingValue = lround(decaySlewing.value); + } + + if (driverModel == TMC2209) { + if (sgEnabledValue == ON && (decaySlewingValue != OFF && decaySlewingValue != STEALTHCHOP)) { + DF("WRN:"); D(axisPrefix); + DLF("stallGuard SG-Enable ON requires Decay-mode-Goto StealthChop"); + return true; + } + } else { + if (sgEnabledValue == ON && (decaySlewingValue != OFF && decaySlewingValue != SPREADCYCLE)) { + DF("WRN:"); D(axisPrefix); + DLF("stallGuard SG-Enable ON requires Decay-mode-Goto SpreadCycle"); + return true; + } + } + } + + return true; +} + +bool TmcStepDirDriverSG::isStalled(float stepsPerSec) { + + #ifdef DRIVER_TMC_STEPPER_STALLGUARD + + if (sgEnabled.value == OFF) return false; + if (!stallGuardModeActive()) return false; + + sgTrainPrev = sgTrain.value; + + const uint32_t nowMs = millis(); + if (sgLastMs != 0 && (uint32_t)(nowMs - sgLastMs) < (uint32_t)SG_EVAL_MIN_MS) return sgLatch; + + // Convert signed microsteps/s -> signed fullsteps/s + const float fpsSigned = stepsPerSec / normalizedMicrosteps; + const float spd = fabsf(fpsSigned); + + // Defer model writes while slewing; flush once speed drops below SG arm threshold. + if (sgModelSavePending && spd < sgArmFps.value) { + sgModelFromRuntime(); + + if (nv().kv().put(nvKey, sgModel) == KvPartition::Status::Ok) { + DF("MSG:"); D(axisPrefix); DLF("StallGuard model NV file saved"); + } else { + DF("WRN:"); D(axisPrefix); DLF("StallGuard model NV file save failed"); + } + + sgModelSavePending = false; + sgModelLastSaveMs = nowMs; + } + + if (!(spd > 0.0f)) { + stallDetectSoftReset(); + sgLastFps = fpsSigned; + sgLastMs = nowMs; + return false; + } + + // Live tuning: if sensitivity changed, apply and reset model + const uint8_t pctNow = (uint8_t)sgSensitivity.value; + if (pctNow != sgAppliedPct) { + // First application after boot uses sentinel 255 and should not wipe + // the persisted learned model. Real runtime/user changes still hard-reset. + const bool firstApply = (sgAppliedPct == 255); + applySgSensitivityPercent(pctNow); + sgAppliedPct = pctNow; + if (firstApply) { + stallDetectSoftReset(); + } else { + stallDetectHardReset(); + } + return false; + } + + // Time delta (clamped) for integrators. millis() has 1ms resolution, so fast loops can see dtMs==0 + uint16_t dtMs = (sgLastMs == 0) ? 0 : (uint16_t)(nowMs - sgLastMs); + sgLastMs = nowMs; + if (dtMs > 50) dtMs = 50; + + // Signed accel estimate (fullsteps/sec^2) + // IMPORTANT: if dtMs==0, do not advance sgLastFps, otherwise dv gets "eaten" on fast MCUs + float a = 0.0f; // signed accel + float absA = 0.0f; // magnitude + + if (dtMs > 0) { + const float dv = fpsSigned - sgLastFps; + sgLastFps = fpsSigned; + a = dv * 1000.0f / (float)dtMs; + absA = fabsf(a); + } + + // Arm only above minimum speed (fullsteps/sec) + if (spd < sgArmFps.value) { + stallDetectSoftReset(); + sgLastFps = fpsSigned; + return false; + } + + const uint16_t armWarmMs = (uint16_t)SG_ARM_WARM_MS; + + const uint16_t sg = readSg10(); + // Treat sg==0 as "not usable for training" until warmed; 0xFFFF is invalid read. + if ((sg == 0 && sgWarmMs < armWarmMs) || sg == 0xFFFF) { + if (dtMs) { if (sgBadMs >= dtMs) sgBadMs -= dtMs; else sgBadMs = 0; } + return sgLatch; + } + + // Warm-up accumulator while armed and samples are valid. + if (dtMs && sgWarmMs < armWarmMs) { + const uint32_t w = (uint32_t)sgWarmMs + (uint32_t)dtMs; + sgWarmMs = (w > armWarmMs) ? armWarmMs : (uint16_t)w; + } + + // dir index: 0 = negative, 1 = positive. + const uint8_t dir = (fpsSigned >= 0.0f) ? 1 : 0; + + const float armFps = (float)sgArmFps.value; + + // ---------------- ramp allowance / motion-state estimate ---------------- + // Keep sgRampF as a "motion state" estimate even if rampAllowPct is 0, + // so steady-state gating works. + if (dtMs > 0) { + const float vref = (spd > armFps) ? spd : armFps; // floor avoids blow-up near zero + constexpr float Tramp = 0.8f; // seconds; detector shaping constant + float rampFactorInstant = (vref > 0.0f) ? (absA / vref) * Tramp : 0.0f; + if (rampFactorInstant < 0.0f) rampFactorInstant = 0.0f; + if (rampFactorInstant > 1.0f) rampFactorInstant = 1.0f; + + // Smooth ramp factor so sustained ramps are visible. + constexpr float TAU_MS = 80.0f; + const float alpha = (float)dtMs / (TAU_MS + (float)dtMs); + sgRampF += alpha * (rampFactorInstant - sgRampF); + } + + // Define "steady" from sgRampF (NOT from rampExtra). + // Tune range ~0.08..0.20. Lower => stricter steady requirement. + constexpr float SG_STEADY_RAMPF = 0.12f; + const bool steadyNow = (sgRampF < SG_STEADY_RAMPF); + + uint16_t rampExtra = 0; + const int rampPct = (int)sgRampAllowPct.value; + + // Optional ramp allowance (still available). This remains symmetric (absA-based). + // ------------------------------------------------------------------------- + // Cross-fade bins for trip computation (avoid discontinuities at bin edges). + uint8_t b0, b1; + float bt; + sgSpeedBlend(spd, armFps, b0, b1, bt); + const uint8_t bl = (bt < 0.5f) ? b0 : b1; // nearest bin for learning + + // Seed baseline/deviation for the nearest learning bin. + if (sgTrain.value == ON) { + if (!sgModel.init[dir][bl]) { + sgModel.base[dir][bl] = sgToFp(sg); + sgModel.dev[dir][bl] = sgToFp(10); + sgBaseF[dir][bl] = (float)sg; + sgDevF[dir][bl] = 10.0f; + sgModel.init[dir][bl] = true; + } + } + + // Blend baseline/deviation across adjacent speed bins for smoother thresholds. + const float baseF = (1.0f - bt) * sgBaseF[dir][b0] + bt * sgBaseF[dir][b1]; + const float devF = (1.0f - bt) * sgDevF [dir][b0] + bt * sgDevF [dir][b1]; + const uint16_t base = (uint16_t)lroundf(baseF); + const uint16_t devE = (uint16_t)lroundf(devF); + + uint16_t margin = (uint16_t)(MARGIN_GENERAL + (uint16_t)(MARGIN_NOISE * devE)); + + // Cap margin to 50% of baseline. + uint16_t cap = (uint16_t)(base / 2); + if (cap < MARGIN_GENERAL) cap = MARGIN_GENERAL; + if (margin > cap) margin = cap; + if (base > 0 && margin >= base) margin = base - 1; + + // ---------------- ramp allowance (optional) ---------------- + if (rampPct > 0 && base > 0) { + const uint16_t rampAllowCounts = (uint16_t)((uint32_t)base * (uint32_t)rampPct / 20U); + rampExtra = (uint16_t)lroundf((float)rampAllowCounts * sgRampF); + if (rampExtra > cap) rampExtra = cap; + } + + sgLastRampExtra = rampExtra; + // ---------------------------------------------------------- + + // "hard bad": sg==0 after warm-up. + const bool hardBad = (sg == 0) && (sgWarmMs >= armWarmMs); + + // Base trip threshold. + int trip = (int)base - (int)(margin + rampExtra); + if (trip < 0) trip = 0; + + // Rate-of-change sensitivity boost ("floor"): + { + int floorPct = (int)lroundf(sgFloorPct.value); + if (floorPct < 0) floorPct = 0; + if (floorPct > 100) floorPct = 100; + + if (floorPct > 0 && base > 0 && sgWarmMs >= SG_ZERO_GRACE_MS) { + const float vref = (spd > armFps) ? spd : armFps; + const float r = (vref > 0.0f) ? (absA / vref) : 1.0f; + + #ifndef SG_FLOOR_R0 + #define SG_FLOOR_R0 0.08f + #endif + #ifndef SG_FLOOR_R1 + #define SG_FLOOR_R1 0.25f + #endif + + float k; + if (r <= SG_FLOOR_R0) k = 1.0f; + else if (r >= SG_FLOOR_R1) k = 0.0f; + else k = (SG_FLOOR_R1 - r) / (SG_FLOOR_R1 - SG_FLOOR_R0); + + #ifndef SG_FLOOR_COUNTS_PER_PCT + #define SG_FLOOR_COUNTS_PER_PCT 2 + #endif + int floorAdd = floorPct * SG_FLOOR_COUNTS_PER_PCT; + floorAdd = (int)lroundf(k * (float)floorAdd); + + const int floorCap = (int)base/2; + if (floorAdd > floorCap) floorAdd = floorCap; + + trip += floorAdd; + if (trip > (int)base) trip = (int)base; + } + } + + const bool bad = hardBad || ((int)sg < trip); + + // debug snapshot + sgLast = sg; + sgLastMargin = margin; + sgBadLast = bad; + + const bool steadyHealthy = steadyNow && !sgLatch && !bad; + const bool adaptFast = (sgTrain.value == ON) && steadyHealthy; + const bool adaptSlow = (sgTrain.value == OFF) && steadyHealthy; + + if (adaptFast || adaptSlow) { + const float baseTc = adaptFast ? BASE_TC_FAST : BASE_TC_SLOW; + const float devTc = adaptFast ? DEV_TC_FAST : DEV_TC_SLOW; + + const float sgF = (float)sg; + + // Update only the nearest bin to avoid cross-speed drift between adjacent bins. + sgBaseF[dir][bl] += (sgF - sgBaseF[dir][bl]) / baseTc; + + // Downward-only deviation EMA update for the nearest learning bin. + const float d = sgBaseF[dir][bl] - sgF; + const float devDown = (d > 0.0f) ? d : 0.0f; + + sgDevF[dir][bl] += (devDown - sgDevF[dir][bl]) / devTc; + + // Queue periodic save; actual write is deferred until non-slew speeds. + if ((nowMs - sgModelLastSaveMs) >= sgModelSaveIntervalMs) { + sgModelSavePending = true; + } + + // decay fault accumulator toward 0 when healthy + if (dtMs) { + const uint16_t d = dtMs; + if (sgBadMs >= d) sgBadMs -= d; else sgBadMs = 0; + } + } else if (!bad) { + // Healthy sample; decay fault accumulator. + if (dtMs) { + const uint16_t d = dtMs; + if (sgBadMs >= d) sgBadMs -= d; else sgBadMs = 0; + } + } else { + // Accumulate faults once warmed. + if (sgWarmMs >= armWarmMs) { + if (dtMs) { + const uint32_t x = (uint32_t)sgBadMs + (uint32_t)dtMs; + sgBadMs = (x > 65535U) ? 65535U : (uint16_t)x; + } + } + } + + const int fpsInt = (int)lroundf(spd); + UNUSED(fpsInt); + + const uint16_t armFaultMs = (uint16_t)sgArmFaultMs.value; + if (sgTrain.value == OFF && sgBadMs >= armFaultMs && !sgLatch) { + sgLatch = true; + D("WRN:"); D(axisPrefix); + D("SG stall detected fps="); D(fpsInt); + D(", sg="); D((int)sg); + D(", trip="); DL((int)trip); + } + + #if DEBUG != OFF && defined(DEBUG_AXIS) && DEBUG_AXIS != OFF + if (axisNumber == DEBUG_AXIS) { + if (spd >= sgArmFps.value) { +// const int rampX = (int)-rampExtra; + const int fpsOut = (int)lroundf(spd); + + unsigned long tms = millis(); + static unsigned long next = 0; + if ((long)(tms - next) > 0) { + next = tms + 250; // 250ms log rate + D("sg:"); D(sg); + D(" sg-trip:"); D(trip); +// D(" sg-rampExtra:"); D(rampX); + D(" sg-binFps:"); D((int)lroundf(sgBinAnchor[bl])); +// D(" sg-bl*10:"); D((int)bl * 10); +// D(" sg-bt%:"); D((int)lroundf(bt * 100.0f)); + D(" sg-fps:"); D(fpsOut); +// D(" sg-base-trip:"); D(base); +// D(" sg-base-learn:"); D((int)lroundf(sgBaseF[dir][bl])); + D(" sg-dev:"); D(devE); + DL(""); + } + } + } + #endif + + #else + UNUSED(stepsPerSec); + #endif + + return sgLatch; +} + +void TmcStepDirDriverSG::stallDetectReset() { + #ifdef DRIVER_TMC_STEPPER_STALLGUARD + stallDetectSoftReset(); + #else + sgLatch = false; + #endif +} + +#ifdef DRIVER_TMC_STEPPER_STALLGUARD + +uint8_t TmcStepDirDriverSG::sgSpeedBin(float spd, float armFps) const { + if (armFps < 1.0f) armFps = 1.0f; + if (spd < armFps) return 0; + + float hi = (float)velocityMax / normalizedMicrosteps; + if (hi < 1.0f) hi = 1.0f; + + if (spd > hi) spd = hi; + + constexpr float EpsThreshold = 0.01f; + if (!sgBinValid || fabsf(sgBinHi - hi) > EpsThreshold) { + sgBinValid = true; + sgBinArmFps = armFps; + sgBinHi = hi; + + // Fixed anchor points by max slew speed: + // b0=low anchor, b1=0.25*Vmax, b2=0.375*Vmax, b3=0.5*Vmax, b4=0.75*Vmax, b5=Vmax + sgBinAnchor[0] = (float)SG_BIN0_FPS; + if (sgBinAnchor[0] < 0.0f) sgBinAnchor[0] = 0.0f; + sgBinAnchor[1] = hi * 0.25f; + sgBinAnchor[2] = hi * 0.33333f; + sgBinAnchor[3] = hi * 0.5f; + sgBinAnchor[4] = hi * 0.75f; + sgBinAnchor[5] = hi; + + // Keep anchors strictly increasing to avoid divide-by-zero / ordering issues. + constexpr float MinStep = 0.001f; + for (uint8_t i = 1; i < SG_BINS; i++) { + if (sgBinAnchor[i] <= sgBinAnchor[i - 1]) sgBinAnchor[i] = sgBinAnchor[i - 1] + MinStep; + } + } + + uint8_t best = 0; + float bestErr = fabsf(spd - sgBinAnchor[0]); + for (uint8_t b = 1; b < SG_BINS; b++) { + const float e = fabsf(spd - sgBinAnchor[b]); + if (e < bestErr) { + bestErr = e; + best = b; + } + } + return best; +} + +void TmcStepDirDriverSG::sgSpeedBlend(float spd, float armFps, uint8_t &b0, uint8_t &b1, float &t) const { + + if (armFps < 1.0f) armFps = 1.0f; + + if (spd <= armFps) { + b0 = 0; b1 = 0; t = 0.0f; + return; + } + + (void)sgSpeedBin(spd, armFps); + + float sp = spd; + if (sp > sgBinHi) sp = sgBinHi; + + if (sp <= sgBinAnchor[0]) { + b0 = 0; b1 = 0; t = 0.0f; + return; + } + + for (uint8_t k = 0; k < (SG_BINS - 2); k++) { + const float lo = sgBinAnchor[k]; + const float hi2 = sgBinAnchor[k + 1]; + + if (sp < hi2) { + b0 = k; + b1 = (uint8_t)(k + 1); + t = (hi2 > lo) ? (sp - lo) / (hi2 - lo) : 0.0f; + if (t < 0.0f) t = 0.0f; + if (t > 1.0f) t = 1.0f; + return; + } + } + + b0 = (uint8_t)(SG_BINS - 2); + b1 = (uint8_t)(SG_BINS - 1); + const float lo = sgBinAnchor[SG_BINS - 2]; + const float hi2 = sgBinAnchor[SG_BINS - 1]; + t = (hi2 > lo) ? (sp - lo) / (hi2 - lo) : 1.0f; + if (t < 0.0f) t = 0.0f; + if (t > 1.0f) t = 1.0f; +} + +uint16_t TmcStepDirDriverSG::readSg10() { + const uint32_t now = millis(); + + if (sgLastGoodMs != 0 && (uint32_t)(now - sgLastGoodMs) >= 500) { + sgCached = 0xFFFF; + sgLastGoodMs = 0; + sgRetryAtMs = now + 2; + } + + if ((int32_t)(now - sgRetryAtMs) < 0) return sgCached; + + const uint32_t sr = readSgRegister(); + + if (sr != 0 && sr != 0xFFFFFFFFUL) { + if (sgCached == 0xFFFF) { + sgCached = (uint16_t)(sr & 0x03FF); + } else { + sgCached = (uint16_t)((sgCached * 9U + (uint16_t)(sr & 0x03FF) + 5U) / 10U); + } + sgLastGoodMs = now; + sgRetryAtMs = now + 20; + } else { + sgRetryAtMs = now + 2; + } + + return sgCached; +} + +void TmcStepDirDriverSG::stallDetectSoftReset() { + sgLatch = false; + sgBadMs = 0; + sgWarmMs = 0; + sgRampF = 0.0f; + + sgCached = 0xFFFF; + sgLastGoodMs = 0; + sgRetryAtMs = 0; + + sgLast = 0xFFFF; + sgLastMargin = 0; + sgLastRampExtra = 0; + sgBadLast = false; +} + +void TmcStepDirDriverSG::stallDetectHardReset() { + sgModelInit(sgModel); + sgRuntimeFromModel(); + + // Persist cleared model if possible so next boot starts clean too + sgModelFromRuntime(); + if (nv().kv().put(nvKey, sgModel) == KvPartition::Status::Ok) { + VF("MSG:"); V(axisPrefix); VLF("StallGuard model reset and file saved"); + } else { + DF("WRN:"); D(axisPrefix); DLF("StallGuard model reset but file save failed"); + } + + stallDetectSoftReset(); + sgModelLastSaveMs = millis(); + + sgBinValid = false; + sgBinArmFps = 0.0f; + sgBinHi = 0.0f; + for (int i = 0; i < (int)(SG_BINS - 1); i++) sgBinEdge[i] = 0.0f; + for (int i = 0; i < (int)SG_BINS; i++) sgBinAnchor[i] = 0.0f; +} + +void TmcStepDirDriverSG::sgModelInit(SgModel& model) const { + switch (axisNumber) { + case 1: { + SgModel seed = {1, 0, 0, 0, AXIS1_SG_MODEL_INIT, AXIS1_SG_MODEL_BASE, AXIS1_SG_MODEL_DEV}; + model = seed; + break; + } + case 2: { + SgModel seed = {1, 0, 0, 0, AXIS2_SG_MODEL_INIT, AXIS2_SG_MODEL_BASE, AXIS2_SG_MODEL_DEV}; + model = seed; + break; + } + default: { + SgModel seed = {1, 0, 0, 0, SG_MODEL_INIT, SG_MODEL_BASE, SG_MODEL_DEV}; + model = seed; + break; + } + } + + // overwrite live metadata after applying compile-time seed arrays + model.version = 1; + model.normalizedMicrostepsSlewing = normalizedMicrostepsSlewing; + model.mAGoto = currentSlewing.value; + model.sgSens = sgSensitivity.value; +} + +bool TmcStepDirDriverSG::sgModelValid(const SgModel& model) const { + if (model.version != 1) return false; + if (model.normalizedMicrostepsSlewing != normalizedMicrostepsSlewing) return false; + if (lroundf(model.mAGoto) != lroundf(currentSlewing.value)) return false; + if (lroundf(model.sgSens) != lroundf(sgSensitivity.value)) return false; + return true; +} + +void TmcStepDirDriverSG::sgRuntimeFromModel() { + for (uint8_t d = 0; d < 2; d++) { + for (uint8_t b = 0; b < SG_BINS; b++) { + sgBaseF[d][b] = (float)sgModel.base[d][b] / (float)SG_FP_ONE; + sgDevF[d][b] = (float)sgModel.dev [d][b] / (float)SG_FP_ONE; + } + } +} + +void TmcStepDirDriverSG::sgModelFromRuntime() { + for (uint8_t d = 0; d < 2; d++) { + for (uint8_t b = 0; b < SG_BINS; b++) { + float base = sgBaseF[d][b]; + float dev = sgDevF[d][b]; + if (base < 0.0f) base = 0.0f; + if (dev < 0.0f) dev = 0.0f; + + const float baseFp = base * (float)SG_FP_ONE; + const float devFp = dev * (float)SG_FP_ONE; + + sgModel.base[d][b] = (baseFp > 65535.0f) ? 65535u : (uint16_t)lroundf(baseFp); + sgModel.dev [d][b] = (devFp > 65535.0f) ? 65535u : (uint16_t)lroundf(devFp); + } + } +} + +void TmcStepDirDriverSG::sgModelDebugDumpNv() const { + #if DEBUG != OFF + VF("MSG:"); V(axisPrefix); VF("Stallguard model metadata v="); + V((int)sgModel.version); + VF(" microstepsGoto="); + V((int)sgModel.normalizedMicrostepsSlewing); + VF(" mAGoto="); + V((int)lroundf(sgModel.mAGoto)); + VF(" sgSens="); + VL((int)lroundf(sgModel.sgSens)); + + #ifdef SG_DEBUG_MODEL + VLF("------------------------------------------------------------------"); + VF("#define AXIS"); V((int)axisNumber); VF("_SG_MODEL_INIT { { "); + for (int d = 0; d < 2; d++) { + for (int b = 0; b < SG_BINS; b++) { + V(sgModel.init[d][b] ? "true" : "false"); + if (b < SG_BINS - 1) { VF(", "); } + } + VF(" }"); + if (d == 0) { VF(", { "); } + } + VLF(" }"); + + VF("#define AXIS"); V((int)axisNumber); VF("_SG_MODEL_BASE { { "); + for (int d = 0; d < 2; d++) { + for (int b = 0; b < SG_BINS; b++) { + V((int)sgModel.base[d][b]); + if (b < SG_BINS - 1) { VF(", "); } + } + VF(" }"); + if (d == 0) { V(", { "); } + } + VLF(" }"); + + VF("#define AXIS"); V((int)axisNumber); VF("_SG_MODEL_DEV { { "); + for (int d = 0; d < 2; d++) { + for (int b = 0; b < SG_BINS; b++) { + V((int)sgModel.dev[d][b]); + if (b < SG_BINS - 1) { VF(", "); } + } + VF(" }"); + if (d == 0) { VF(", { "); } + } + VLF(" }"); + VLF("------------------------------------------------------------------"); + #endif + #endif +} + +#endif + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverSG.h b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverSG.h new file mode 100755 index 0000000..2b01bf1 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/TmcStepDirDriverSG.h @@ -0,0 +1,379 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver + StallGuard helper +// +// The “Arduino IDE charts” in your debug output are these runtime signals: +// +// sg = raw StallGuard result (0..1023) from the driver status. +// Lower sg => higher load / closer to stall. 0 is often “not usable” +// at low speed or before SG has warmed up. +// +// trip = computed threshold. If sg < trip => “bad sample” (potential stall). +// trip starts near the learned baseline (base) and is adjusted by +// noise margin, ramp allowance, and optional steady-state “floor”. +// +// rampExtra = extra margin during ramps (accel/decel) so normal +// transient load spikes don’t false-trip. +// Logged/charted as a negative number (rampX = -rampExtra) +// to visualize it as “headroom reduction”. +// +// sg-bin*10 = which speed bin you’re in (lower bin index * 10 for plotting). +// Bins exist so base/dev can differ by speed (SG is speed dependent). +// +// fpsOut/10 = speed in fullsteps/sec divided by 10 (cheap scaling for plotting). +// +// base = learned SG baseline for this direction + speed bin (counts). +// This is the “healthy” SG you expect at this speed when not ramping. +// +// devE = learned downward deviation (noise / dips) for this dir+bin (counts). +// Used to inflate margin: margin = M0 + M1*devE. +// +// The tunable parameters exposed in the UI (AxisParameter) are: +// +// sgEnabled (OFF/ON) +// - Master enable for StallGuard-based stall detection on this axis. +// - Hard constraint: +// * StallGuard2 devices (tmc2130/2160/5160/5161) require “decay slewing SpreadCycle” +// * StallGuard4 devices (tmc2209) require “decay slewing StealthChop” +// +// sgTrain (OFF/ON) +// - Enables learning of baseline (sgB) and deviation (sgD) while moving. +// - Training only happens when: +// * not latched, +// * sample is healthy (not “bad”), +// * and motion is “steady” (low ramp factor). +// +// sgSensitivity (0..100 percent) +// - User-facing knob mapped by applySgSensitivityPercent() into driver settings +// (typically SGTHRS / coolStep related thresholding depending on chip). +// - Changing it forces a hard reset of learned model so new sensitivity starts clean. +// +// sgArmFps (fullsteps/sec) +// - Minimum speed where SG is considered valid and the detector “arms”. +// - Below this: detector disarms, clears warm/fault accumulators, returns “no stall”. +// - This avoids the common low-speed regime where StallGuard is noisy/useless. +// +// sgArmFaultMs (milliseconds) +// - Time-integrator for “bad samples” before declaring a stall. +// - The detector accumulates sgBadMs when sg is below trip (or sg==0 after warmup), +// and decays sgBadMs back toward 0 on healthy samples. +// - When sgBadMs >= sgArmFaultMs => sgLatch=true (stall latched). +// +// sgRampAllowPct (percent-ish) +// - Optional ramp allowance. Adds extra margin during ramps proportional to: +// base * (rampAllowPct/20) * sgRampF +// - sgRampF is a normalized ramp factor derived from |accel|/speed, smoothed. +// - Purpose: reduce false stalls during accel/decel and during direction changes. +// +// sgFloorPct (0..100 percent) +// - Optional steady-state sensitivity boost. +// - When enabled, it ADDS counts to trip (making it easier to trip) *only* when +// motion is steady, and fades out as ramping increases. +// - Interpreted as fixed counts: +// floorAdd = sgFloorPct * SG_FLOOR_COUNTS_PER_PCT +// then blended by “k” based on r = |dv/dt| / vref using thresholds: +// SG_FLOOR_R0 (fully steady) .. SG_FLOOR_R1 (fully ramping) +// +// Build-time knobs that shape the plotted behavior (these affect how the above looks): +// +// SG_BINS +// - Number of speed bins used for learning base/dev. +// - More bins = more adaptation by speed, but slower learning per bin. +// +// SG_ARM_WARM_MS +// - Warm-up time after arming before sg==0 is treated as a “hard bad”. +// - During warm-up, sg==0 is treated as “not usable for training” rather than a fault. +// +// SG_ZERO_GRACE_MS +// - Additional grace period before floor logic is allowed to act (prevents early +// over-sensitivity while SG is still stabilizing). +// +// SG_FLOOR_R0 / SG_FLOOR_R1 +// - Define what “steady” vs “ramping” means for the floor fade-out. +// +// SG_FLOOR_COUNTS_PER_PCT +// - Converts floorPct into raw SG counts (default 2 counts per %). +// +// Notes on the internal state variables you see affecting plots: +// +// sgRampF +// - A smoothed [0..1] ramp factor. 0 ~= steady, 1 ~= strong ramping. +// - Used for “steadyNow” gating (training only when steady) and for rampExtra. +// +// sgModel.base[dir][bin], sgModel.dev[dir][bin] +// - Learned baseline and learned deviation per direction and speed bin. +// - Stored as U10.6 fixed-point SG counts. +// - Updated with EMA time constants BASE_TC and DEV_TC when training. +// +// sgBadMs +// - Fault integrator (ms). Accumulates “bad” time after warm-up, +// decays toward 0 on healthy samples. +// - This makes stall detection robust against single-sample noise. +// +#pragma once + +#include "TmcStepDirDriver.h" + +#if defined(STEP_DIR_MOTOR_PRESENT) + +// Warmup time (ms) after arming before SG==0 is treated as "hard bad" +// and before faults are accumulated. +#ifndef SG_ARM_WARM_MS + #define SG_ARM_WARM_MS 200 +#endif + +// Minimum period (ms) between StallGuard evaluations in isStalled(). +// This normalizes model learning and fault integration against loop-rate variance. +#ifndef SG_EVAL_MIN_MS + #define SG_EVAL_MIN_MS 20 +#endif + +#ifndef SG_MODEL_SAVE_MS + #define SG_MODEL_SAVE_MS 300000 +#endif +#ifndef SG_MODEL_SAVE_MS_MID + #define SG_MODEL_SAVE_MS_MID 150000 +#endif +#ifndef SG_MODEL_SAVE_MS_HIGH + #define SG_MODEL_SAVE_MS_HIGH 60000 +#endif + +// Default init values + +// Generic model parameters +#ifndef SG_ENABLED + #define SG_ENABLED OFF +#endif +#ifndef SG_TRAIN + #define SG_TRAIN ON +#endif +#ifndef SG_SENS + #define SG_SENS 50 +#endif +#ifndef SG_FLOOR + #define SG_FLOOR 0 +#endif +#ifndef SG_RAMP_ALLOW + #define SG_RAMP_ALLOW 0 +#endif +#ifndef SG_ARM_FPS + #define SG_ARM_FPS 150 +#endif +#ifndef SG_FAULT_MS + #define SG_FAULT_MS 200 +#endif + +// Optional low-speed anchor for SG bin0 in fullsteps/sec. +// Keep at 0 for a true low anchor, or raise to manually smooth low-speed behavior. +#ifndef SG_BIN0_FPS + #define SG_BIN0_FPS 0 +#endif + +// Generic fallback StallGuard seed model +#ifndef SG_MODEL_INIT + #define SG_MODEL_INIT { { false, false, false, false, false, false }, { false, false, false, false, false, false } } +#endif +#ifndef SG_MODEL_BASE + #define SG_MODEL_BASE { { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 } } +#endif +#ifndef SG_MODEL_DEV + #define SG_MODEL_DEV { { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 } } +#endif + +// Axis 1 override defaults to generic model if not supplied +#ifndef AXIS1_SG_MODEL_INIT + #define AXIS1_SG_MODEL_INIT SG_MODEL_INIT +#endif +#ifndef AXIS1_SG_MODEL_BASE + #define AXIS1_SG_MODEL_BASE SG_MODEL_BASE +#endif +#ifndef AXIS1_SG_MODEL_DEV + #define AXIS1_SG_MODEL_DEV SG_MODEL_DEV +#endif + +// Axis 2 override defaults to generic model if not supplied +#ifndef AXIS2_SG_MODEL_INIT + #define AXIS2_SG_MODEL_INIT SG_MODEL_INIT +#endif +#ifndef AXIS2_SG_MODEL_BASE + #define AXIS2_SG_MODEL_BASE SG_MODEL_BASE +#endif +#ifndef AXIS2_SG_MODEL_DEV + #define AXIS2_SG_MODEL_DEV SG_MODEL_DEV +#endif + +// Number of speed bins used for learning base/dev. (fixed at 6) +#define SG_BINS 6 + +typedef struct SgModel { + uint8_t version; + int16_t normalizedMicrostepsSlewing; + float mAGoto; + float sgSens; + + bool init[2][SG_BINS]; + uint16_t base[2][SG_BINS]; // U10.6 + uint16_t dev[2][SG_BINS]; // U10.6 +} SgModel; + +class TmcStepDirDriverSG : public TmcStepDirDriver { + public: + TmcStepDirDriverSG(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // set up driver + bool init(); + + uint8_t getParameterCount() { return numParameters; } + + AxisParameter* getParameter(uint8_t number) { + if (number == 0 || number > numParameters) return &invalid; + return parameter[number]; + } + + // check if axis parameter is valid + bool parameterIsValid(AxisParameter* parameter, bool next = false); + + // indicate that this driver can detect stalls + bool hasStallDetect() const { return true; } + + // Returns true if the motor is overloaded/stalled + // NOTE: stepsPerSec is signed microsteps/sec; converted internally to signed fullsteps/sec + bool isStalled(float stepsPerSec); + + // reset any internal stall-detect state (baseline, latch, etc.) + void stallDetectReset() override; + + protected: + + // read the 32bit status register + virtual uint32_t readSgRegister() { return 0xFFFFFFFFUL; } + + // should return true when in a mode where SG is meaningful + virtual bool stallGuardModeActive() const { return true; } + + // apply normalized sensitivity percent (0..100) to the concrete chip register + virtual void applySgSensitivityPercent(uint8_t pct) = 0; + + #ifdef DRIVER_TMC_STEPPER_STALLGUARD + // read SG result (0..1023), or 0xFFFF on invalid read + uint16_t readSg10(); + + // speed-bin selection using [armFps .. velocityMax] if available + uint8_t sgSpeedBin(float spd, float armFps) const; + + // cross-fade between adjacent bins to avoid abrupt trip jumps at bin edges. + void sgSpeedBlend(float spd, float armFps, uint8_t &b0, uint8_t &b1, float &t) const; + + // reset + void stallDetectSoftReset(); + void stallDetectHardReset(); + + // U10.6 fixed-point helpers for learned model storage + static constexpr uint8_t SG_FP_SHIFT = 6; + static constexpr uint16_t SG_FP_ONE = (uint16_t)(1u << SG_FP_SHIFT); + + static inline uint16_t sgToFp(uint16_t x) { + return (uint16_t)(x << SG_FP_SHIFT); + } + + static inline uint16_t sgFpToInt(uint16_t xfp) { + return (uint16_t)((xfp + (SG_FP_ONE >> 1)) >> SG_FP_SHIFT); + } + + void sgModelInit(SgModel& model) const; + bool sgModelValid(const SgModel& model) const; + void sgModelDebugDumpNv() const; + void sgRuntimeFromModel(); + void sgModelFromRuntime(); + + SgModel sgModel = {}; + float sgBaseF[2][SG_BINS] = {{0}}; + float sgDevF[2][SG_BINS] = {{0}}; + + uint16_t sgBadMs = 0; // persistence accumulator (milliseconds) + uint16_t sgWarmMs = 0; // warm-up accumulator (milliseconds) + uint32_t sgLastMs = 0; // last millis() sample time + uint8_t sgAppliedPct = 255; // last applied sensitivity percent + + // accel estimate (signed fullsteps/sec sample) + float sgLastFps = 0.0f; + + // smoothed ramp factor (0..1) + float sgRampF = 0.0f; + + // ---- speed-bin edge cache (avoid per-call powf) ---- + mutable bool sgBinValid = false; + mutable float sgBinArmFps = 0.0f; + mutable float sgBinHi = 0.0f; + mutable float sgBinAnchor[SG_BINS] = {0.0f}; + mutable float sgBinEdge[SG_BINS - 1] = {0.0f}; + // ---------------------------------------------------- + + // stallguard sample caching + uint32_t sgLastGoodMs = 0; + uint32_t sgRetryAtMs = 0; + uint16_t sgCached = 0xFFFF; + + // debug logging + uint16_t sgLast = 0xFFFF; + uint16_t sgLastMargin = 0; + uint16_t sgLastRampExtra = 0; + bool sgBadLast = false; + bool sgModelSavePending = false; + uint32_t sgModelSaveIntervalMs = (uint32_t)SG_MODEL_SAVE_MS; + uint32_t sgModelLastSaveMs = 0; + + // Used to avoid treating an immediate transient SG==0 as a fault right at arming. + // (prevents hard-bad from triggering before any SG has had a chance to settle) + static constexpr uint16_t SG_ZERO_GRACE_MS = 50; + #endif + + // debug logging + int16_t sgAppliedReg = 0; + + // indicates stalled state + bool sgLatch = false; + + // keep track of training disable events + float sgTrainPrev = OFF; + + // key for NV model storage + uint32_t nvKey = 0; + + // ---------------------- PARAMETERS ---------------------- + AxisParameter sgEnabled = {NAN, NAN, NAN, -2, -1, AXP_BOOLEAN, "SG Enable"}; + AxisParameter sgTrain = {NAN, NAN, NAN, -2, -1, AXP_BOOLEAN_IMMEDIATE, "SG Train"}; + + // Floor raises the trip threshold in steady-state only: + // higher => more aggressive, trips sooner (applied only when sgRampF is low). + AxisParameter sgFloorPct = {NAN, NAN, NAN, 0, 100, AXP_INTEGER_IMMEDIATE, "SG Floor, %"}; + AxisParameter sgRampAllowPct = {NAN, NAN, NAN, 0, 100, AXP_INTEGER_IMMEDIATE, "SG Ramp Allow, %"}; + + AxisParameter sgSensitivity = {NAN, NAN, NAN, 0, 100, AXP_INTEGER_IMMEDIATE, "SG Sens, %"}; + AxisParameter sgArmFps = {NAN, NAN, NAN, 50, 2000, AXP_INTEGER_IMMEDIATE, "SG Arm, FPS"}; + AxisParameter sgArmFaultMs = {NAN, NAN, NAN, 50, 500, AXP_INTEGER_IMMEDIATE, "SG Fault, ms"}; + + + #ifdef DRIVER_TMC_STEPPER_STALLGUARD + #ifdef DRIVER_TMC_STEPPER_STALLGUARD_BASIC + const int numParameters = 11; + #else + const int numParameters = 14; + #endif + #else + const int numParameters = 7; + #endif + + AxisParameter* parameter[15] = { + &invalid, µsteps, µstepsSlewing, + ¤tHold, ¤tRun, ¤tSlewing, + &decay, &decaySlewing, +// &intpol, + &sgEnabled, &sgTrain, + &sgFloorPct, &sgRampAllowPct, + + &sgSensitivity, &sgArmFps, &sgArmFaultMs + }; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/TmcSPI.cpp b/src/lib/axis/motor/stepDir/tmc/legacy/TmcSPI.cpp new file mode 100755 index 0000000..621a85b --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/TmcSPI.cpp @@ -0,0 +1,269 @@ +// ----------------------------------------------------------------------------------- +// Step/Dir TMC SPI motor driver + +#include "TmcSPI.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(STEP_DIR_TMC_SPI_PRESENT) + +#include "../TmcStepDirDriver.h" + +bool TmcSPI::init(int model, int16_t mosi, int16_t sck, int16_t cs, int16_t miso, int16_t axisNumber) { + active = false; + this->model = model; + UNUSED(axisNumber); + + if (model == TMC2130 || model == TMC5160) { + active = softSpi.init(mosi, sck, cs, miso); + return true; + } else + + return false; +} + +bool TmcSPI::mode(bool intpol, int decay_mode, byte micro_step_code, int irun, int ihold, float rsense) { + if (!active) return false; + + softSpi.begin(); + uint32_t data_out = 0; + + // *** My notes are limited, see the TMC2130 datasheet for more info. *** + + // IHOLDDELAY=0x00, IRUN=0x1F, IHOLD=0x1F ( 0, 31, 31 ) or 50% (0,16,16) + // 0b0000 11111 11111 + // IHOLD, default=16, range 0 to 31 (Standstill current 0=1/32... 31=32/32) + // IRUN, default=31, range 0 to 31 (Run current 0=1/32... 31=32/32) + // IHOLDDELAY, default=4, range 0 to 15 (Delay per current reduction step in x 2^18 clocks) + + float Ifs = 0.325/rsense; + if (model == TMC2130) Ifs = 0.325/(rsense + 0.02); + + long IHOLD = round(( (ihold/1000.0)/Ifs)*32.0)-1; + long IRUN = round(( (irun/1000.0)/Ifs)*32.0)-1; + if (IHOLD < 0) IHOLD = 0; + if (IHOLD > 31) IHOLD = 31; + if (IRUN < 0) IRUN = 0; + if (IRUN > 31) IRUN = 31; + + // IHOLD + IRUN + IHOLDDELAY + data_out = (IHOLD << 0) + (IRUN << 8) + (4UL << 16); + if (last_IHOLD_IRUN != data_out) { + last_IHOLD_IRUN = data_out; + uint8_t statusFlag = write(REG_IHOLD_IRUN, data_out); + softSpi.pause(); + + // if we can, check if the driver is there + #ifdef MOTOR_DRIVER_DETECT + if (!softSpi.outOnly()) { + // check for standstill status flag + bool standstill = statusFlag & 0b00001000; + // check for fault status flag + bool fault = statusFlag & 0b00000010; + if (!standstill || fault) { + softSpi.end(); + active = false; + return false; + } + } + #else + UNUSED(statusFlag); + #endif + } + + // TPOWERDOWN, default=127, range 0 to 255 (Delay after standstill for motor current power down, about 0 to 4 seconds) + data_out = (tpd_value << 0); + if (last_TPOWERDOWN != data_out) { + last_TPOWERDOWN = data_out; + write(REG_TPOWERDOWN, data_out); + softSpi.pause(); + } + + // TPWMTHRS, default=0, range 0 to 2^20 (switchover upper velocity for stealthChop voltage PWM mode) + data_out = (tpt_value << 0); + if (last_TPWMTHRS != data_out) { + last_TPWMTHRS = data_out; + write(REG_TPWMTHRS, data_out); + softSpi.pause(); + } + + // THIGH, default=0, range 0 to 2^20 (switchover rate for vhighfs/vhighchm) + data_out = (thigh_value << 0); + if (last_THIGH != data_out) { + last_THIGH = data_out; + write(REG_THIGH, data_out); + softSpi.pause(); + } + + // PWMCONF + if (model == TMC2130) { + // default=0x00050480UL + data_out = (pc_pwm_ampl<<0)+(pc_pwm_grad<<8)+(pc_pwm_freq<<16)+(pc_pwm_auto<<18)+(pc_pwm_sym<<19)+(pc_pwm_freewheel<<20); + if (last_PWMCONF != data_out) { + last_PWMCONF = data_out; + write(REG_PWMCONF, data_out); + softSpi.pause(); + } + } else + if (model == TMC5160) { + // default=0xC40C001EUL + data_out = (pc_pwm_ofs<<0)+(pc_pwm_grad<<8)+(pc_pwm_freq<<16)+(pc_pwm_auto<<18)+(pc_pwm_autograd<<19)+(pc_pwm_freewheel<<20)+(pc_pwm_reg<<24)+(pc_pwm_lim<<28); + if (last_PWMCONF != data_out) { + last_PWMCONF = data_out; + write(REG_PWMCONF, data_out); + softSpi.pause(); + } + } + + // CHOPCONF + if (intpol) cc_intpol=1; else cc_intpol=0; // set interpolation bit + // default=0x00008008UL + if (model == TMC2130) last_chop_config = (cc_toff<<0)+(cc_hstart<<4)+(cc_hend<<7)+(cc_rndtf<<13)+(cc_tbl<<15)+(cc_vsense<<17)+(cc_vhighfs<<18)+(cc_vhighchm<<19)+(cc_intpol<<28); + // default=0x10410150UL + if (model == TMC5160) last_chop_config = (cc_toff<<0)+(cc_hstart<<4)+(cc_hend<<7)+(cc_tbl<<15)+(cc_vhighfs<<18)+(cc_vhighchm<<19)+(cc_tpfd<<20)+(cc_intpol<<28); + if (micro_step_code != 255) { + data_out = last_chop_config + (((uint32_t)micro_step_code)<<24); + write(REG_CHOPCONF, data_out); + softSpi.pause(); + } + + // GCONF + // voltage on AIN is current reference + data_out = 0x00000001UL; + // set stealthChop bit + if (decay_mode == STEALTHCHOP) data_out |= 0x00000004UL; + if (last_GCONF != data_out) { + last_GCONF = data_out; + write(REG_GCONF, data_out); + } + + softSpi.end(); + return true; +} + +bool TmcSPI::error() { + if (!active) return false; + + if (model == TMC2130 || model == TMC5160) { + softSpi.begin(); + + // get global status register, look for driver error bit + uint32_t data_out = 0; + uint8_t result = read(REG_DRVSTATUS, &data_out); + + softSpi.end(); + if ((result & 2) != 0 || (result == 0 && data_out == 0)) return true; else return false; + } else + + return true; +} + +bool TmcSPI::refresh_CHOPCONF(byte micro_step_code) { + if (!active) return false; + + if (model == TMC2130 || model == TMC5160) { + softSpi.begin(); + + // default=0x00008008UL + if (model == TMC2130) last_chop_config = (cc_toff<<0)+(cc_hstart<<4)+(cc_hend<<7)+(cc_rndtf<<13)+(cc_tbl<<15)+(cc_vsense<<17)+(cc_vhighfs<<18)+(cc_vhighchm<<19)+(cc_intpol<<28); + // default=0x10410150UL + if (model == TMC5160) last_chop_config = (cc_toff<<0)+(cc_hstart<<4)+(cc_hend<<7)+(cc_tbl<<15)+(cc_vhighfs<<18)+(cc_vhighchm<<19)+(cc_tpfd<<20)+(cc_intpol<<28); + + write(REG_CHOPCONF, last_chop_config + (((uint32_t)micro_step_code)<<24)); + softSpi.end(); + return true; + } else + + return false; +} + +uint32_t TmcSPI::read_CHOPCONF() { + if (!active) return false; + + if (model == TMC2130 || model == TMC5160) { + softSpi.begin(); + + uint32_t data_out = 0; + read(REG_CHOPCONF, &data_out); + + // first write returns nothing, second the data + data_out = 0; + read(REG_DRVSTATUS, &data_out); + + softSpi.end(); + return data_out; + } else + + return 0; +} + +int TmcSPI::refresh_DRVSTATUS() { + if (!active) return false; + + if (model == TMC2130 || model == TMC5160) { + softSpi.begin(); + + // get global status register, look for driver error bit + uint32_t data_out=0; + uint8_t result = read(REG_DRVSTATUS, &data_out); + + softSpi.pause(); + + // first write returns nothing, second the status data + data_out=0; + read(REG_DRVSTATUS, &data_out); + + // get the extended status info. + if (data_out != 0 || result != 0) { + ds_stst = (bool)bitRead(data_out,31); // DRV_STATUS 31 Standstill + ds_olb = (bool)bitRead(data_out,30); // DRV_STATUS 30 Open Load B + ds_ola = (bool)bitRead(data_out,29); // DRV_STATUS 29 Open Load A + ds_s2ga = (bool)bitRead(data_out,28); // DRV_STATUS 28 Short to Ground B + ds_s2gb = (bool)bitRead(data_out,27); // DRV_STATUS 27 Short to Ground A + ds_otpw = (bool)bitRead(data_out,26); // DRV_STATUS 26 Overtemperature Pre-warning 120C + ds_ot = (bool)bitRead(data_out,25); // DRV_STATUS 25 Overtemperature Shutdown 150C + ds_stallguard = (bool)bitRead(data_out,24); // DRV_STATUS 24 stallGuard2 status + ds_cs_actual = (data_out>>16) & 0b011111; // DRV_STATUS 16 Actual current control scaling + ds_fs_active = (bool)bitRead(data_out,15); // DRV_STATUS 15 Full step active indicator + ds_result = data_out & 0b1111111111; // DRV_STATUS 0 stallGuard2 result + } else { + ds_stst = true; ds_olb = true; ds_ola = true; ds_s2ga = true; ds_s2gb = true; ds_otpw = true; + ds_ot = true; ds_stallguard = false; ds_cs_actual = 0; ds_fs_active = false; ds_result = 0; + } + + softSpi.end(); + return data_out != 0; + } else + + return 0; +} + +bool TmcSPI::refresh_COOLCONF() { + if (!active) return false; + + if (model == TMC2130 || model == TMC5160) { + softSpi.begin(); + + uint32_t data_out = (cl_semin<<0)+(cl_seup<<5)+(cl_semax<<8)+(cl_sedn<<13)+(cl_seimin<<15)+(cl_sgt<<16)+(cl_sfilt<<24); + write(REG_COOLCONF, data_out); + + softSpi.end(); + return true; + } else + + return false; +} + +uint8_t TmcSPI::write(byte Address, uint32_t data_out) { + Address = Address | 0x80; + uint8_t status_byte = softSpi.transfer(Address); + softSpi.transfer32(data_out); + return status_byte; +} + +uint8_t TmcSPI::read(byte Address, uint32_t* data_out) { + Address = Address & ~0x80; + uint8_t status_byte = softSpi.transfer(Address); + *data_out = softSpi.transfer32(*data_out); + return status_byte; +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/TmcSPI.h b/src/lib/axis/motor/stepDir/tmc/legacy/TmcSPI.h new file mode 100755 index 0000000..6ce5e08 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/TmcSPI.h @@ -0,0 +1,180 @@ +// ----------------------------------------------------------------------------------- +// Step/Dir TMC SPI motor driver +#pragma once + +#include +#include "../../../../../../Common.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(STEP_DIR_TMC_SPI_PRESENT) + +#include "../../../../../softSpi/SoftSpi.h" + +class TmcSPI { + public: + // setup SoftSpi or UART and driver model + bool init(int model, int16_t mosi, int16_t sck, int16_t cs, int16_t miso, int16_t axisNumber = 0); + + // TMC setup most common settings + // 256x interpolation: intpol + // decay mode: decay_mode (STEALTHCHOP or SPREADCYCLE) + // microstepping mode: micro_step_code (0=256x, 1=128x, 2=64x, 3=32x, 4=16x, 5=8x, 6=4x, 7=2x, 8=1x) + // irun, ihold, rsense: current in mA and sense resistor value + bool mode(bool intpol, int decay_mode, byte micro_step_code, int irun, int ihold, float rsense); + + // Check for TMC error from DRVSTATUS register + bool error(); + int refresh_DRVSTATUS(); + inline bool get_DRVSTATUS_stst() { return ds_stst; } + inline bool get_DRVSTATUS_olA() { return ds_ola; } + inline bool get_DRVSTATUS_olB() { return ds_olb; } + inline bool get_DRVSTATUS_s2gA() { return ds_s2ga; } + inline bool get_DRVSTATUS_s2gB() { return ds_s2gb; } + inline bool get_DRVSTATUS_otpw() { return ds_otpw; } + inline bool get_DRVSTATUS_ot() { return ds_ot; } + inline bool get_DRVSTATUS_stallguard() { return ds_stallguard; } + inline int get_DRVSTATUS_cs_actual() { return ds_cs_actual; } + inline bool get_DRVSTATUS_active() { return ds_fs_active; } + inline int get_DRVSTATUS_result() { return ds_result; } + + // Chopper configuration + bool refresh_CHOPCONF(byte micro_step_code); + uint32_t read_CHOPCONF(); + inline bool set_CHOPCONF_toff(int v) { if (v >= 2 && v <= 15) { cc_toff = v; return true; } return false; } + inline bool set_CHOPCONF_hstart(int v) { if (v >= 0 && v <= 7) { cc_hstart = v; return true; } return false; } + inline bool set_CHOPCONF_hend(int v) { if (v >= 0 && v <= 15) { cc_hend = v; return true; } return false; } + inline bool set_CHOPCONF_rndtf(int v) { if (v >= 0 && v <= 1) { cc_rndtf = v; return true; } return false; } + inline bool set_CHOPCONF_tbl(int v) { if (v >= 0 && v <= 3) { cc_tbl = v; return true; } return false; } + inline bool set_CHOPCONF_vsense(int v) { if (v >= 0 && v <= 1) { cc_vsense = v; return true; } return false; } + inline bool set_CHOPCONF_vhighfs(int v) { if (v >= 0 && v <= 1) { cc_vhighfs = v; return true; } return false; } + inline bool set_CHOPCONF_vhighchm(int v) { if (v >= 0 && v <= 1) { cc_vhighchm = v; return true; } return false; } + inline bool set_CHOPCONF_intpol(int v) { if (v >= 0 && v <= 1) { cc_intpol = v; return true; } return false; } + // TMC5160 specific + inline bool set_CHOPCONF_tpfd(int v) { if (v >= 0 && v <= 15) { cc_tpfd = v; return true; } return false; } + volatile uint32_t last_chop_config; + + // Misc. configuration + inline bool set_TPOWERDOWN_value(int v) { if (v >= 0 && v <= 255) { tpd_value = v; return true; } return false; } + inline bool set_TPWMTHRS_value(long v) { if (v >= 0 && v <= 1048575) { tpt_value = v; return true; } return false; } + inline bool set_THIGH_value(long v) { if (v >= 0 && v <= 1048575) { thigh_value = v; return true; } return false; } + + // PWM/stealthChop configuration + inline bool set_PWMCONF_PWM_GRAD(int v) { if (v >= 0 && v <= 255) { pc_pwm_grad = v; return true; } return false; } + inline bool set_PWMCONF_pwm_freq(int v) { if (v >= 0 && v <= 3) { pc_pwm_freq = v; return true; } return false; } + inline bool set_PWMCONF_pwm_auto(int v) { if (v >= 0 && v <= 1) { pc_pwm_auto = v; return true; } return false; } + inline bool set_PWMCONF_pwm_freewheel(int v) { if (v >= 0 && v <= 1) { pc_pwm_freewheel= v; return true; } return false; } + // TMC2130 specific + inline bool set_PWMCONF_PWM_AMPL(int v) { if (v >= 0 && v <= 255) { pc_pwm_ampl = v; return true; } return false; } + inline bool set_PWMCONF_pwm_sym(int v) { if (v >= 0 && v <= 1) { pc_pwm_sym = v; return true; } return false; } + // TMC5160/5161 specific + inline bool set_PWMCONF_PWM_OFS(int v) { if (v >= 0 && v <= 255) { pc_pwm_ofs = v; return true; } return false; } + inline bool set_PWMCONF_pwm_autograd(int v) { if (v >= 0 && v <= 1) { pc_pwm_autograd = v; return true; } return false; } + inline bool set_PWMCONF_PWM_REG(int v) { if (v >= 0 && v <= 15) { pc_pwm_reg = v; return true; } return false; } + inline bool set_PWMCONF_PWM_LIM(int v) { if (v >= 0 && v <= 15) { pc_pwm_lim = v; return true; } return false; } + + // Coolstep and stallguard configurarion + bool refresh_COOLCONF(); + inline bool set_COOLCONF_semin(int v) { if (v >= 0 && v <= 15) { cl_semin = v; return true; } return false; } + inline bool set_COOLCONF_seup(int v) { if (v >= 0 && v <= 3) { cl_seup = v; return true; } return false; } + inline bool set_COOLCONF_semax(int v) { if (v >= 0 && v <= 15) { cl_semax = v; return true; } return false; } + inline bool set_COOLCONF_sedn(int v) { if (v >= 0 && v <= 3) { cl_sedn = v; return true; } return false; } + inline bool set_COOLCONF_seimin(int v) { if (v >= 0 && v <= 1) { cl_seimin= v; return true; } return false; } + inline bool set_COOLCONF_sgt(int v) { if (v >= -64 && v <= 63) { cl_sgt= v+64; return true; } return false; } + inline bool set_COOLCONF_sfilt(int v) { if (v >= 0 && v <= 1) { cl_sfilt = v; return true; } return false; } + + private: + uint8_t write(byte Address, uint32_t data_out); + uint8_t read(byte Address, uint32_t* data_out); + + // the write flag and various TMC SPI registers + static const uint8_t WRITE = 0x80; + static const uint8_t REG_GCONF = 0x00; + static const uint8_t REG_GSTAT = 0x01; + static const uint8_t REG_IHOLD_IRUN = 0x10; + static const uint8_t REG_TPOWERDOWN = 0x11; + static const uint8_t REG_TPWMTHRS = 0x13; + static const uint8_t REG_THIGH = 0x15; + static const uint8_t REG_CHOPCONF = 0x6C; + static const uint8_t REG_COOLCONF = 0x6D; + static const uint8_t REG_DCCTRL = 0x6E; + static const uint8_t REG_DRVSTATUS = 0x6F; + static const uint8_t REG_PWMCONF = 0x70; + + // keep track of what commands need to be sent + int last_microStepCode = -1; + int last_irun = -1; + int last_ihold = -1; + + // keep track of what registers need to be updated + uint32_t last_GCONF = 0; + uint32_t last_IHOLD_IRUN = 0; + uint32_t last_TPOWERDOWN = 0; + uint32_t last_TPWMTHRS = 0; + uint32_t last_THIGH = 0; + uint32_t last_PWMCONF = 0; + + // CHOPCONF settings + uint32_t cc_toff = 4; // default=4, range 2 to 15 (Off time setting, slow decay phase) + uint32_t cc_hstart = 0; // default=0, range 0 to 7 (Hysteresis start 1, 2, ..., 8) + uint32_t cc_hend = 0; // default=0, range 0 to 15 (Hysteresis -3, -2, -1, 0, 1 ..., 12) + uint32_t cc_rndtf = 0; // default=0, range 0 to 1 (Enables small random value to be added to TOFF) + uint32_t cc_tbl = 1; // default=1, range 0 to 3 (for 6, 24, 36 or 54 clocks) + uint32_t cc_vsense = 0; // default=0, range 0 to 1 (0 for high sensitivity, 1 for low sensitivity @ 50% current setting) + uint32_t cc_vhighfs = 0; // default=0, range 0 to 1 (Enables switch to full-step when VHIGH (THIGH?) is exceeded) + uint32_t cc_vhighchm = 0; // default=0, range 0 to 1 (Enables switch to fast-decay mode VHIGH (THIGH?) is exceeded) + uint32_t cc_intpol = 1; // default=1, range 0 to 1 (Enables 256x interpolation) + + // TMC5160 specific + uint32_t cc_tpfd = 4UL; // default=4, range 0 to 15 (Passive fast decay time mid-range resonance dampening) + + // TPOWERDOWN settings + uint32_t tpd_value = 128; // default=127, range 0 to 255 (Delay after standstill for motor current power down, about 0 to 4 seconds) + + // TPWMTHRS settings + uint32_t tpt_value = 0; // default=0, range 0 to 2^20 (Switchover upper velocity for stealthChop voltage PWM mode) + + // THIGH settings + uint32_t thigh_value = 0; // default=0, range 0 to 2^20 (Switchover rate for vhighfs/vhighchm) + + // PWMCONF settings + uint32_t pc_pwm_grad = 0x04; // default=4, range 0 to 14 (PWM gradient scale using automatic current control) + uint32_t pc_pwm_freq = 0x01; // default=1, range 0 to 3 (PWM frequency 0: fpwm=2/1024 fclk, 1: fpwm=2/683 fclk, 2: fpwm=2/512 fclk, 3: fpwm=2/410 fclk) + uint32_t pc_pwm_auto = 0x01; // default=1, range 0 to 1 (PWM automatic current control 0: off, 1: on) + uint32_t pc_pwm_freewheel = 0x01; // default=1, range 0 to 3 (PWM freewheel 0: normal, 1: freewheel, 2:LS short, 3: HS short) + // TMC2130 specific + uint32_t pc_pwm_ampl = 0x80; // default=128, range 0 to 255 (PWM amplitude or switch back amplitude if pwm_auto=1) + uint32_t pc_pwm_sym = 0x00; // default=0, range 0 to 1 (PWM symmetric 0: value may change during cycle, 1: enforce; 0: disable autograd on TMC5160/5161) + // TMC2209/TMC5160 specific + uint32_t pc_pwm_ofs = 0x1e; // default=30, range 0 to 255 (PWM user defined amplitude offset related to full motor current) + uint32_t pc_pwm_autograd = 0x00; // default=0, range 0 to 1 (PWM automatic grad control 0: off, 1: on) + uint32_t pc_pwm_reg = 0x04; // default=4, range 0 to 15 (PWM maximum amplitude change per half-wave when using pwm autoscale) + uint32_t pc_pwm_lim = 0x0c; // default=12, range 0 to 15 (PWM limit for PWM_SCALE_AUTO when switching back from spreadCycle to stealthChop) + + // DRVSTATUS settings + bool ds_stst = false; + bool ds_olb = false; + bool ds_ola = false; + bool ds_s2ga = false; + bool ds_s2gb = false; + bool ds_ot = false; + bool ds_otpw = false; + bool ds_stallguard = false; + uint16_t ds_cs_actual = 0; + bool ds_fs_active = false; + uint16_t ds_result = 0; + + // COOLCONF settings + uint32_t cl_semin = 0; // default=0, range 0 to 15 (Minimum stallGuard2 value for smart current control and smart current enable) + uint32_t cl_seup = 0; // default=0, range 0 to 3 (Current up step width) + uint32_t cl_semax = 0; // default=0, range 0 to 15 (stallGuard2 hysteresis value for smart current control) + uint32_t cl_sedn = 0; // default=0, range 0 to 3 (Current down step speed) + uint32_t cl_seimin = 0; // default=0, range 0 to 1 (Minimum current for scc) + uint32_t cl_sgt = 0; // default=0, range 0 to 127 (stallGuard2 hysteresis value -64 to 63) + uint32_t cl_sfilt = 0; // default=0, range 0 to 1 (stallGuard2 filter enable) + + bool active = false; + int model; + + SoftSpi softSpi; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/tmc2130/Tmc2130.cpp b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2130/Tmc2130.cpp new file mode 100755 index 0000000..b37debb --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2130/Tmc2130.cpp @@ -0,0 +1,97 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2130Stepper + +// note: LegacySPI requires MOSI, SCK, CS but MISO is optional + +#include "Tmc2130.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(TMC2130_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +// constructor +StepDirTmc2130::StepDirTmc2130(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverNSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc2130StepDir legacy, "); + axisPrefix[5] = '0' + axisNumber; +} + +// set up driver and parameters: microsteps, microsteps goto, hold current, run current, goto current, unused +bool StepDirTmc2130::init() { + if (!TmcStepDirDriver::init()) return false; + + // get TMC SPI ready + driver.init(driverModel, Pins->m0, Pins->m1, Pins->m2, Pins->m3, axisNumber); + + // get driver ready and check to see if it's there (if possible) + if (!driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense)) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { + if (Pins->m3 != OFF) { + VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); + } + } + + return true; +} + +void StepDirTmc2130::modeMicrostepTracking() { + driver.refresh_CHOPCONF(microstepCode); +} + +int StepDirTmc2130::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver.refresh_CHOPCONF(microstepCodeSlewing); + } + return microstepRatio; +} + +void StepDirTmc2130::modeDecayTracking() { + driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense); +} + +void StepDirTmc2130::modeDecaySlewing() { + driver.mode(intpol.value == ON, decaySlewing.value, microstepCode, iGoto, iHold, rSense); +} + +void StepDirTmc2130::readStatus() { + if (driver.refresh_DRVSTATUS()) { + status.outputA.shortToGround = driver.get_DRVSTATUS_s2gA(); + status.outputA.openLoad = driver.get_DRVSTATUS_olA(); + status.outputB.shortToGround = driver.get_DRVSTATUS_s2gB(); + status.outputB.openLoad = driver.get_DRVSTATUS_olB(); + status.overTemperatureWarning = driver.get_DRVSTATUS_otpw(); + status.overTemperature = driver.get_DRVSTATUS_ot(); + status.standstill = driver.get_DRVSTATUS_stst(); + } +} + +// secondary way to power down not using the enable pin +bool StepDirTmc2130::enable(bool state) { + if (state) { + driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense); + } else { + driver.mode(intpol.value == ON, STEALTHCHOP, microstepCode, iRun, 0, rSense); + } + return true; +} + +// calibrate the motor driver if required +void StepDirTmc2130::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("TMC standstill automatic current calibration"); + driver.mode(intpol.value == ON, STEALTHCHOP, microstepCode, iRun, iRun, rSense); + delay(1000); + driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense); + } +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/tmc2130/Tmc2130.h b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2130/Tmc2130.h new file mode 100755 index 0000000..6f1e2be --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2130/Tmc2130.h @@ -0,0 +1,46 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(TMC2130_PRESENT) + +#include "../TmcSPI.h" +#include "../../TmcStepDirDriverNSG.h" + +class StepDirTmc2130 : public TmcStepDirDriverNSG { + public: + // constructor + StepDirTmc2130(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status info. from driver + void readStatus(); + + TmcSPI driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/tmc2209/Tmc2209.cpp b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2209/Tmc2209.cpp new file mode 100755 index 0000000..f1d00ff --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2209/Tmc2209.cpp @@ -0,0 +1,166 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2209Stepper + +// note: LegacyUART requires UART RX but TX is optional + +#include "Tmc2209.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(TMC2209_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define rx m3 +#define tx m2 + +#define CurrentToPercent(current) ((current/1000.0)/(0.325/(rSense + 0.02)))*100.0 + +// constructor +StepDirTmc2209::StepDirTmc2209(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverNSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc2209StepDir legacy, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirTmc2209::init() { + if (!TmcStepDirDriver::init()) return false; + + // get TMC UART driver ready + pinModeEx(Pins->m0, OUTPUT); + pinModeEx(Pins->m1, OUTPUT); + + driver = new TMC2209Stepper(); + if (driver == NULL) return false; + + int16_t rxPin = Pins->rx; + + delay(1); + #if defined(SERIAL_TMC_HARDWARE_UART) + #if defined(DEDICATED_MODE_PINS) + // program the device address 0,1,2,3 since M0 and M1 are all unique + int deviceAddress = SERIAL_TMC_ADDRESS_MAP(axisNumber - 1); + digitalWriteEx(Pins->m0, bitRead(deviceAddress, 0)); + digitalWriteEx(Pins->m1, bitRead(deviceAddress, 1)); + #else + // help user hard code the device address 0,1,2,3 by cutting pins + digitalWriteEx(Pins->m0, HIGH); + digitalWriteEx(Pins->m1, HIGH); + #endif + + VF("MSG:"); V(axisPrefix); + VF("HW UART driver pins rx="); V(SERIAL_TMC_RX); VF(", tx="); V(SERIAL_TMC_TX); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + pinModeEx(SERIAL_TMC_RX, INPUT_PULLUP); + #if SERIAL_TMC_INVERT == ON + driver->setup(SERIAL_TMC, SERIAL_TMC_BAUD, SERIAL_TMC_ADDRESS_MAP(axisNumber - 1), SERIAL_TMC_RX, SERIAL_TMC_TX, true); + #else + driver->setup(SERIAL_TMC, SERIAL_TMC_BAUD, SERIAL_TMC_ADDRESS_MAP(axisNumber - 1), SERIAL_TMC_RX, SERIAL_TMC_TX); + #endif + rxPin = SERIAL_TMC_RX; + #else + // pull MS1 and MS2 low for device address 0 + digitalWriteEx(Pins->m0, LOW); + digitalWriteEx(Pins->m1, LOW); + + #if SERIAL_TMC_RX_DISABLE == true + rxPin = OFF; + #endif + VF("MSG:"); V(axisPrefix); + VF("SW UART driver pins rx="); V(rxPin); VF(", tx="); V(Pins->tx); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + pinModeEx(rxPin, INPUT_PULLUP); + driver->setup(SERIAL_TMC_BAUD, SERIAL_TMC_ADDRESS_MAP(axisNumber - 1), rxPin, Pins->tx); + #endif + + // this driver automatically switches to one-way communications, even if a RX pin is set + // so the following only returns false if communications are "half working" + #ifdef MOTOR_DRIVER_DETECT + if (rxPin != OFF) { + if (!driver->isSetupAndCommunicating()) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + } + #endif + + driver->useExternalSenseResistors(); + driver->enableAnalogCurrentScaling(); + + driver->enable(); + driver->moveUsingStepDirInterface(); + driver->setPwmOffset(pc_pwm_ofs); + driver->setPwmGradient(pc_pwm_grad); + if (pc_pwm_auto) driver->enableAutomaticCurrentScaling(); + if (!intpol.value) { + VF("MSG:"); V(axisPrefix); VLF("driver interpolation control not supported"); + } + + modeMicrostepTracking(); + driver->setRunCurrent(CurrentToPercent(iRun)); + driver->setHoldCurrent(CurrentToPercent(iHold)); + driver->disableStealthChop(); + + return true; +} + +void StepDirTmc2209::modeMicrostepTracking() { + driver->setMicrostepsPerStep(normalizedMicrosteps); +} + +int StepDirTmc2209::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver->setMicrostepsPerStep(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void StepDirTmc2209::modeDecayTracking() { + if (normalizedDecay == SPREADCYCLE) driver->disableStealthChop(); else driver->enableStealthChop(); + driver->setRunCurrent(CurrentToPercent(iRun)); + driver->setHoldCurrent(CurrentToPercent(iHold)); +} + +void StepDirTmc2209::modeDecaySlewing() { + if (decaySlewing.value == SPREADCYCLE) driver->disableStealthChop(); else driver->enableStealthChop(); + driver->setRunCurrent(CurrentToPercent(iGoto)); + driver->setHoldCurrent(CurrentToPercent(iHold)); +} + +void StepDirTmc2209::readStatus() { + TMC2209Stepper::Status status_result = driver->getStatus(); + status.outputA.shortToGround = (bool)status_result.short_to_ground_a || (bool)status_result.low_side_short_a; + status.outputA.openLoad = (bool)status_result.open_load_a; + status.outputB.shortToGround = (bool)status_result.short_to_ground_b || (bool)status_result.low_side_short_b; + status.outputB.openLoad = (bool)status_result.open_load_b; + status.overTemperatureWarning = (bool)status_result.over_temperature_warning; + status.overTemperature = (bool)status_result.over_temperature_shutdown; + status.standstill = (bool)status_result.standstill; +} + +// secondary way to power down not using the enable pin +bool StepDirTmc2209::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + driver->enableStealthChop(); + driver->setHoldCurrent(0); + } + + return true; +} + +// calibrate the motor driver if required +void StepDirTmc2209::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG: StepDirDriver"); V(axisNumber); VL(", TMC standstill automatic current calibration"); + driver->setRunCurrent(CurrentToPercent(iRun)); + driver->setHoldCurrent(CurrentToPercent(iRun)); + driver->enableStealthChop(); + delay(1000); + driver->setRunCurrent(CurrentToPercent(iRun)); + driver->setHoldCurrent(CurrentToPercent(iHold)); + driver->disableStealthChop(); + } +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/tmc2209/Tmc2209.h b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2209/Tmc2209.h new file mode 100755 index 0000000..0566aef --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/tmc2209/Tmc2209.h @@ -0,0 +1,75 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(TMC2209_PRESENT) + +#include "../../TmcStepDirDriverNSG.h" + +// default settings for any TMC UART drivers that may be present +#ifndef SERIAL_TMC + #define SERIAL_TMC SoftSerial // Use software serial with TX on M3 (CS) of each axis +#endif +#ifndef SERIAL_TMC_RX_DISABLE + #define SERIAL_TMC_RX_DISABLE true // Recieving data often doesn't work with software serial +#endif +#ifndef SERIAL_TMC_BAUD + #define SERIAL_TMC_BAUD 115200 // Baud rate +#endif +#ifndef SERIAL_TMC_ADDRESS_MAP + #define SERIAL_TMC_ADDRESS_MAP(x) (0) // driver addresses are 0 for all axes +#endif +#ifndef SERIAL_TMC_INVERT + #define SERIAL_TMC_INVERT OFF // invert the serial interface signal logic +#endif + +#ifndef SERIAL_TMC_HARDWARE_UART + #include // source as appropriate for your platform (if not built-in) + #define TMC2209_SOFTWARE_SERIAL +#endif + +#define TMC2209_DEBUG false +#include // https://github.com/hjd1964/TMC2209 + +class StepDirTmc2209 : public TmcStepDirDriverNSG { + public: + // constructor + StepDirTmc2209(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status info. from driver + void readStatus(); + + // TMC2209/TMC5160 specific + uint32_t pc_pwm_grad = 0x04; // default=4, range 0 to 14 (PWM gradient scale using automatic current control) + uint32_t pc_pwm_auto = 0x01; // default=1, range 0 to 1 (PWM automatic current control 0: off, 1: on) + uint32_t pc_pwm_ofs = 0x1e; // default=30, range 0 to 255 (PWM user defined amplitude offset related to full motor current) + + TMC2209Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/tmc5160/Tmc5160.cpp b/src/lib/axis/motor/stepDir/tmc/legacy/tmc5160/Tmc5160.cpp new file mode 100755 index 0000000..541fee7 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/tmc5160/Tmc5160.cpp @@ -0,0 +1,97 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC5160Stepper + +// note: LegacySPI requires MOSI, SCK, CS but MISO is optional + +#include "Tmc5160.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(TMC5160_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +// constructor +StepDirTmc5160::StepDirTmc5160(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverNSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc5160StepDir legacy, "); + axisPrefix[5] = '0' + axisNumber; +} + +// set up driver and parameters: microsteps, microsteps goto, hold current, run current, goto current, unused +bool StepDirTmc5160::init() { + if (!TmcStepDirDriver::init()) return false; + + // get TMC SPI ready + driver.init(driverModel, Pins->m0, Pins->m1, Pins->m2, Pins->m3, axisNumber); + + // get driver ready and check to see if it's there (if possible) + if (!driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense)) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { + if (Pins->m3 != OFF) { + VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); + } + } + + return true; +} + +void StepDirTmc5160::modeMicrostepTracking() { + driver.refresh_CHOPCONF(microstepCode); +} + +int StepDirTmc5160::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver.refresh_CHOPCONF(microstepCodeSlewing); + } + return microstepRatio; +} + +void StepDirTmc5160::modeDecayTracking() { + driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense); +} + +void StepDirTmc5160::modeDecaySlewing() { + driver.mode(intpol.value == ON, decaySlewing.value, microstepCode, iGoto, iHold, rSense); +} + +void StepDirTmc5160::readStatus() { + if (driver.refresh_DRVSTATUS()) { + status.outputA.shortToGround = driver.get_DRVSTATUS_s2gA(); + status.outputA.openLoad = driver.get_DRVSTATUS_olA(); + status.outputB.shortToGround = driver.get_DRVSTATUS_s2gB(); + status.outputB.openLoad = driver.get_DRVSTATUS_olB(); + status.overTemperatureWarning = driver.get_DRVSTATUS_otpw(); + status.overTemperature = driver.get_DRVSTATUS_ot(); + status.standstill = driver.get_DRVSTATUS_stst(); + } +} + +// secondary way to power down not using the enable pin +bool StepDirTmc5160::enable(bool state) { + if (state) { + driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense); + } else { + driver.mode(intpol.value == ON, STEALTHCHOP, microstepCode, iRun, 0, rSense); + } + return true; +} + +// calibrate the motor driver if required +void StepDirTmc5160::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("TMC standstill automatic current calibration"); + driver.mode(intpol.value == ON, STEALTHCHOP, microstepCode, iRun, iRun, rSense); + delay(1000); + driver.mode(intpol.value == ON, decay.value, microstepCode, iRun, iHold, rSense); + } +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/legacy/tmc5160/Tmc5160.h b/src/lib/axis/motor/stepDir/tmc/legacy/tmc5160/Tmc5160.h new file mode 100755 index 0000000..dfcb2b9 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/legacy/tmc5160/Tmc5160.h @@ -0,0 +1,46 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if !defined(DRIVER_TMC_STEPPER) && defined(TMC5160_PRESENT) + +#include "../TmcSPI.h" +#include "../../TmcStepDirDriverNSG.h" + +class StepDirTmc5160 : public TmcStepDirDriverNSG { + public: + // constructor + StepDirTmc5160(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status info. from driver + void readStatus(); + + TmcSPI driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2130/Tmc2130.cpp b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2130/Tmc2130.cpp new file mode 100755 index 0000000..8817169 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2130/Tmc2130.cpp @@ -0,0 +1,130 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2130Stepper + +#include "Tmc2130.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2130_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +// constructor +StepDirTmc2130:: +StepDirTmc2130(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc2130StepDir, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirTmc2130::init() { + if (!TmcStepDirDriverSG::init()) return false; + + #ifdef DRIVER_TMC_STEPPER_HW_SPI + driver = new TMC2130Stepper(Pins->cs, rSense); + #else + driver = new TMC2130Stepper(Pins->cs, rSense, Pins->mosi, Pins->miso, Pins->sck); + #endif + driver->begin(); + driver->intpol(intpol.value == ON); + modeMicrostepTracking(); + driver->en_pwm_mode(false); + + current(iRun, iHoldRatio); + + // enable StallGuard / CoolStep speed threshold gating + // common "don't-care yet" value: always enable while moving + driver->TCOOLTHRS(0xFFFFF); // width depends on part; library handles masking + + // if you are NOT using CoolStep current scaling, keep it off + // disable CoolStep + driver->semin(0); + + // if we can, check to see if the driver is there + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + } + #endif + + return true; +} + +void StepDirTmc2130::modeMicrostepTracking() { + driver->microsteps(normalizedMicrosteps); +} + +int StepDirTmc2130::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver->microsteps(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void StepDirTmc2130::modeDecayTracking() { + setDecayMode(decay.value); + current(iRun, iHoldRatio); +} + +void StepDirTmc2130::modeDecaySlewing() { + setDecayMode(decaySlewing.value); + current(iGoto, 1.0F); +} + +void StepDirTmc2130::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning= status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// secondary way to power down not using the enable pin +bool StepDirTmc2130::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + setDecayMode(STEALTHCHOP); + driver->ihold(0); + } + return true; +} + +// calibrate the motor driver if required +void StepDirTmc2130::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("standstill automatic current calibration"); + + current(iRun, 1.0F); + driver->pwm_autoscale(true); + driver->en_pwm_mode(true); + delay(1000); + modeDecayTracking(); + } +} + +// set the decay mode STEALTHCHOP or SPREADCYCLE +void StepDirTmc2130::setDecayMode(int decayMode) { + driver->en_pwm_mode(decayMode != SPREADCYCLE); +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2130/Tmc2130.h b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2130/Tmc2130.h new file mode 100755 index 0000000..327da75 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2130/Tmc2130.h @@ -0,0 +1,69 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2130Stepper +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2130_PRESENT) + +#include // https://github.com/teemuatlut/TMCStepper + +#include "../../../../Drivers.h" +#include "../../TmcStepDirDriverSG.h" + +class StepDirTmc2130 : public TmcStepDirDriverSG { + public: + // constructor + StepDirTmc2130(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // read the 32bit stallguard register + uint32_t readSgRegister() override { return driver->DRV_STATUS(); } + + // SG only meaningful in spreadCycle (not stealthChop) + // and SG is only active while slewing so we assume this is representative of the state + bool stallGuardModeActive() const override { return decaySlewing.value == SPREADCYCLE; } + + // apply normalized sensitivity percent (0..100) to the concrete chip register + // (e.g., 2209: SGTHRS, 2130: SGT). Called only when % changes. + void applySgSensitivityPercent(uint8_t pct) override { + int sgt = 63 - (((int)pct * 127 + 50) / 100); // yields +63..-64 + driver->sgt((int8_t)sgt); + sgAppliedReg = (int16_t)sgt; + } + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status from driver + void readStatus(); + + // set the decay mode STEALTH_CHOP or SPREAD_CYCLE + void setDecayMode(int decayMode); + + // set peak current and hold current multiplier + inline void current(float mA, float mult) { driver->rms_current(mA*0.7071F, mult); } + + TMC2130Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2160/Tmc2160.cpp b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2160/Tmc2160.cpp new file mode 100755 index 0000000..1bf178c --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2160/Tmc2160.cpp @@ -0,0 +1,133 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2160Stepper + +#include "Tmc2160.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2160_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +// constructor +StepDirTmc2160:: +StepDirTmc2160(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc2160StepDir, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool Tmc2160StepDirDriver::init() { + if (!TmcStepDirDriverSG::init()) return false; + + #ifdef TMC2160_RSENSE_KRAKEN + if (axisNumber <= 4) rSense = TMC2160_RSENSE_KRAKEN; + #endif + #ifdef DRIVER_TMC_STEPPER_HW_SPI + driver = new TMC2160Stepper(Pins->cs, rSense); + #else + driver = new TMC2160Stepper(Pins->cs, rSense, Pins->mosi, Pins->miso, Pins->sck); + #endif + driver->begin(); + driver->intpol(intpol.value == ON); + modeMicrostepTracking(); + driver->en_pwm_mode(false); + + current(iRun, iHoldRatio); + + // enable StallGuard / CoolStep speed threshold gating + // common "don't-care yet" value: always enable while moving + driver->TCOOLTHRS(0xFFFFF); // width depends on part; library handles masking + + // if you are NOT using CoolStep current scaling, keep it off + // disable CoolStep + driver->semin(0); + + // if we can, check to see if the driver is there + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + } + #endif + + return true; +} + +void Tmc2160StepDirDriver::modeMicrostepTracking() { + int16_t microsteps = 0; + driver->microsteps(normalizedMicrosteps); +} + +int Tmc2160StepDirDriver::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver->microsteps(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void Tmc2160StepDirDriver::modeDecayTracking() { + setDecayMode(decay.value); + current(iRun, iHoldRatio); +} + +void Tmc2160StepDirDriver::modeDecaySlewing() { + setDecayMode(decaySlewing.value); + current(iGoto, 1.0F); +} + +void Tmc2160StepDirDriver::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning= status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// secondary way to power down not using the enable pin +bool Tmc2160StepDirDriver::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + setDecayMode(STEALTHCHOP); + driver->ihold(0); + } + return true; +} + +// calibrate the motor driver if required +void Tmc2160StepDirDriver::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("standstill automatic current calibration"); + current(iRun, 1.0F); + driver->pwm_autoscale(true); + driver->en_pwm_mode(true); + delay(1000); + modeDecayTracking(); + } +} + +// set the decay mode STEALTHCHOP or SPREADCYCLE +void Tmc2160StepDirDriver::setDecayMode(int decayMode) { + driver->en_pwm_mode(decayMode != SPREADCYCLE); +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2160/Tmc2160.h b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2160/Tmc2160.h new file mode 100755 index 0000000..b24f960 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2160/Tmc2160.h @@ -0,0 +1,69 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2160Stepper +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2160_PRESENT) + +#include // https://github.com/teemuatlut/TMCStepper + +#include "../../../../Drivers.h" +#include "../../TmcStepDirDriverSG.h" + +class StepDirTmc2160 : public TmcStepDirDriverSG { + public: + // constructor + StepDirTmc2160(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // read the 32bit stallguard register + uint32_t readSgRegister() override { return driver->DRV_STATUS(); } + + // SG only meaningful in spreadCycle (not stealthChop) + // and SG is only active while slewing so we assume this is representative of the state + bool stallGuardModeActive() const override { return decaySlewing.value == SPREADCYCLE; } + + // apply normalized sensitivity percent (0..100) to the concrete chip register + // (e.g., 2209: SGTHRS, 2130: SGT). Called only when % changes. + void applySgSensitivityPercent(uint8_t pct) override { + int sgt = 63 - (((int)pct * 127 + 50) / 100); // yields +63..-64 + driver->sgt((int8_t)sgt); + sgAppliedReg = (int16_t)sgt; + } + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status from driver + void readStatus(); + + // set the decay mode STEALTH_CHOP or SPREAD_CYCLE + void setDecayMode(int decayMode); + + // set peak current and hold current multiplier + inline void current(float mA, float mult) { driver->rms_current(mA*0.7071F, mult); } + + TMC2160Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2208/tmc2208.cpp b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2208/tmc2208.cpp new file mode 100755 index 0000000..15ac3db --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2208/tmc2208.cpp @@ -0,0 +1,152 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2208Stepper + +#include "tmc2208.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2208_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define rx m3 +#define tx m2 + +// provide for using hardware serial +#if SERIAL_TMC == HardSerial + #include + #undef SERIAL_TMC + HardwareSerial HWSerialTMC(SERIAL_TMC_RX, SERIAL_TMC_TX); + #define SERIAL_TMC HWSerialTMC + #define SERIAL_TMC_RXTX_SET +#endif + +// constructor +StepDirTmc2208:: +StepDirTmc2208(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriver(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc2208StepDir, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirTmc2208::init() { + if (!TmcStepDirDriver::init()) return false; + + // the MS1 and MS2 pins are unused + pinModeEx(Pins->m0, OUTPUT); + pinModeEx(Pins->m1, OUTPUT); + + // initialize the serial port + #if defined(SERIAL_TMC_HARDWARE_UART) + #define SerialTMC SERIAL_TMC + static bool initialized = false; + if (!initialized) { + VF("MSG:"); V(axisPrefix); + #if defined(SERIAL_TMC_RX) && defined(SERIAL_TMC_TX) && !defined(SERIAL_TMC_RXTX_SET) + VF("HW UART driver pins rx="); V(SERIAL_TMC_RX); VF(", tx="); V(SERIAL_TMC_TX); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + pinModeEx(SERIAL_TMC_RX, INPUT_PULLUP); + SerialTMC.begin(SERIAL_TMC_BAUD, SERIAL_8N1, SERIAL_TMC_RX, SERIAL_TMC_TX); + #else + VF("HW UART driver pins on port default"); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + SerialTMC.begin(SERIAL_TMC_BAUD); + #endif + initialized = true; + } + #else + VF("SW UART driver pins rx="); V(Pins->rx); VF(", tx="); V(Pins->tx); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + pinModeEx(Pins->rx, INPUT_PULLUP); + SerialTMC = new SoftwareSerial(Pins->rx, Pins->tx); + SerialTMC->begin(SERIAL_TMC_BAUD); + #endif + + // initialize the stepper driver + #if defined(SERIAL_TMC_HARDWARE_UART) + driver = new TMC2208Stepper(&SerialTMC, rSense); + #else + driver = new TMC2208Stepper(SerialTMC, rSense); + #endif + driver->begin(); + driver->intpol(intpol.value == ON); + modeMicrostepTracking(); + driver->rms_current(iRun, iHoldRatio); + driver->en_spreadCycle(true); + + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + #endif + + return true; +} + +void StepDirTmc2208::modeMicrostepTracking() { + if (normalizedMicrosteps == 1) driver->microsteps(0); else driver->microsteps(normalizedMicrosteps); +} + +int StepDirTmc2208::modeMicrostepSlewing() { + if (microstepRatio > 1) { + if (normalizedMicrostepsSlewing == 1) driver->microsteps(0); else driver->microsteps(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void StepDirTmc2208::modeDecayTracking() { + setDecayMode(decay.value); + current(iRun, iHoldRatio); +} + +void StepDirTmc2208::modeDecaySlewing() { + setDecayMode(decaySlewing.value); + current(iGoto, 1.0F); +} + +// set the decay mode STEALTHCHOP or SPREADCYCLE +void StepDirTmc2208::setDecayMode(int decayMode) { + driver->en_spreadCycle(decayMode == SPREADCYCLE); +} + +void StepDirTmc2208::readStatus() { + TMC2208_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + if (driver->CRCerror) status_result.sr = 0xFFFFFFFF; + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning = status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// secondary way to power down not using the enable pin +bool StepDirTmc2208::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + setDecayMode(STEALTHCHOP); + driver->ihold(0); + } + return true; +} + +// calibrate the motor driver if required +void StepDirTmc2208::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("standstill automatic current calibration"); + + current(iRun, 1.0F); + driver->pwm_autograd(DRIVER_TMC_STEPPER_AUTOGRAD); + driver->pwm_autoscale(true); + driver->en_spreadCycle(false); + delay(1000); + modeDecayTracking(); + } +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2208/tmc2208.h b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2208/tmc2208.h new file mode 100755 index 0000000..80e1ec9 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2208/tmc2208.h @@ -0,0 +1,62 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2208Stepper +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2208_PRESENT) + +#include // https://github.com/teemuatlut/TMCStepper + +#include "../../../../Drivers.h" +#include "../../TmcStepDirDriverNSG.h" + +#if SERIAL_TMC == SoftSerial + #include // must be built into the board libraries +#endif + +class StepDirTmc2208 : public TmcStepDirDriverNSG { + public: + // constructor + StepDirTmc2208(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // set the decay mode STEALTH_CHOP or SPREAD_CYCLE + void setDecayMode(int decayMode); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status from driver + void readStatus(); + + // set peak current and hold current multiplier + inline void current(float mA, float mult) { driver->rms_current(round(mA*0.7071F), mult); } + + #if SERIAL_TMC == SoftSerial + SoftwareSerial *SerialTMC; + #endif + + TMC2208Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2209/Tmc2209.cpp b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2209/Tmc2209.cpp new file mode 100755 index 0000000..1e6ed40 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2209/Tmc2209.cpp @@ -0,0 +1,180 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2209Stepper + +#include "Tmc2209.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2209_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define rx m3 +#define tx m2 + +// provide for using hardware serial +#if SERIAL_TMC == HardSerial + #include + #undef SERIAL_TMC + HardwareSerial HWSerialTMC(SERIAL_TMC_RX, SERIAL_TMC_TX); + #define SERIAL_TMC HWSerialTMC + #define SERIAL_TMC_RXTX_SET +#endif + +// constructor +StepDirTmc2209:: +StepDirTmc2209(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc2209StepDir, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirTmc2209::init() { + if (!TmcStepDirDriverSG::init()) return false; + + // get TMC UART driver address select pins ready + pinModeEx(Pins->m0, OUTPUT); + pinModeEx(Pins->m1, OUTPUT); + + // initialize the serial port + #if defined(SERIAL_TMC_HARDWARE_UART) + #if defined(DEDICATED_MODE_PINS) + // program the device address 0,1,2,3 since M0 and M1 are all unique + int deviceAddress = SERIAL_TMC_ADDRESS_MAP(axisNumber - 1); + digitalWriteEx(Pins->m0, bitRead(deviceAddress, 0)); + digitalWriteEx(Pins->m1, bitRead(deviceAddress, 1)); + #else + // pull MS1 and MS2 high for device address 3 + // the user can then hard code the device address 0,1,2,3 by cutting off pins as needed + digitalWriteEx(Pins->m0, HIGH); + digitalWriteEx(Pins->m1, HIGH); + #endif + + #define SerialTMC SERIAL_TMC + static bool initialized = false; + if (!initialized) { + VF("MSG:"); V(axisPrefix); + #if defined(SERIAL_TMC_RX) && defined(SERIAL_TMC_TX) && !defined(SERIAL_TMC_RXTX_SET) + VF("HW UART driver pins rx="); V(SERIAL_TMC_RX); VF(", tx="); V(SERIAL_TMC_TX); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + pinModeEx(SERIAL_TMC_RX, INPUT_PULLUP); + SerialTMC.begin(SERIAL_TMC_BAUD, SERIAL_8N1, SERIAL_TMC_RX, SERIAL_TMC_TX); + #else + VF("HW UART driver pins on port default"); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + SerialTMC.begin(SERIAL_TMC_BAUD); + #endif + initialized = true; + } + #else + // pull MS1 and MS2 low for device address 0 + digitalWriteEx(Pins->m0, LOW); + digitalWriteEx(Pins->m1, LOW); + VF("SW UART driver pins rx="); V(Pins->rx); VF(", tx="); V(Pins->tx); VF(", baud="); V(SERIAL_TMC_BAUD); VLF(" bps"); + SerialTMC = new SoftwareSerial(Pins->rx, Pins->tx); + SerialTMC->begin(SERIAL_TMC_BAUD); + #endif + + // initialize the stepper driver + #if defined(SERIAL_TMC_HARDWARE_UART) + driver = new TMC2209Stepper(&SerialTMC, rSense, SERIAL_TMC_ADDRESS_MAP(axisNumber - 1)); + #else + driver = new TMC2209Stepper(SerialTMC, rSense, SERIAL_TMC_ADDRESS_MAP(axisNumber - 1)); + #endif + driver->begin(); + + driver->pdn_disable(true); + driver->mstep_reg_select(true); + driver->toff(3); + driver->blank_time(24); + + driver->intpol(intpol.value == ON); + modeMicrostepTracking(); + current(iRun, iHoldRatio); + driver->en_spreadCycle(true); + + // enable StallGuard / CoolStep speed threshold gating + // common "don't-care yet" value: always enable while moving + driver->TCOOLTHRS(0xFFFFF); // width depends on part; library handles masking + + // if you are NOT using CoolStep current scaling, keep it off + // disable CoolStep + driver->semin(0); + + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + #endif + + return true; +} + +void StepDirTmc2209::modeMicrostepTracking() { + if (normalizedMicrosteps == 1) driver->microsteps(0); else driver->microsteps(normalizedMicrosteps); +} + +int StepDirTmc2209::modeMicrostepSlewing() { + if (microstepRatio > 1) { + if (normalizedMicrostepsSlewing == 1) driver->microsteps(0); else driver->microsteps(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void StepDirTmc2209::modeDecayTracking() { + setDecayMode(decay.value); + current(iRun, iHoldRatio); +} + +void StepDirTmc2209::modeDecaySlewing() { + setDecayMode(decaySlewing.value); + current(iGoto, 1.0F); +} + +// set the decay mode STEALTHCHOP or SPREADCYCLE +void StepDirTmc2209::setDecayMode(int decayMode) { + driver->en_spreadCycle(decayMode == SPREADCYCLE); +} + +void StepDirTmc2209::readStatus() { + TMC2208_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + if (driver->CRCerror) { status_result.sr = 0xFFFFFFFF; } + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning = status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// secondary way to power down not using the enable pin +bool StepDirTmc2209::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + setDecayMode(STEALTHCHOP); + driver->ihold(0); + } + return true; +} + +// calibrate the motor driver if required +void StepDirTmc2209::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("standstill automatic current calibration"); + + current(iRun, 1.0F); + driver->pwm_autograd(DRIVER_TMC_STEPPER_AUTOGRAD); + driver->pwm_autoscale(true); + driver->en_spreadCycle(false); + delay(1000); + modeDecayTracking(); + } +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2209/Tmc2209.h b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2209/Tmc2209.h new file mode 100755 index 0000000..7b9cfc1 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2209/Tmc2209.h @@ -0,0 +1,73 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2209Stepper +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2209_PRESENT) + +#include // https://github.com/teemuatlut/TMCStepper + +#include "../../../../Drivers.h" +#include "../../TmcStepDirDriverSG.h" + +class StepDirTmc2209 : public TmcStepDirDriverSG { + public: + // constructor + StepDirTmc2209(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // set the decay mode STEALTH_CHOP or SPREAD_CYCLE + void setDecayMode(int decayMode); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // read the 32bit stallguard register + uint32_t readSgRegister() override { return (uint32_t)(driver->SG_RESULT() & 0x03FF); } + + // SG only meaningful in stealthChop (not spreadCycle) + // and SG is only active while slewing so we assume this is representative of the state + bool stallGuardModeActive() const override { return decaySlewing.value == STEALTHCHOP; } + + // apply normalized sensitivity percent (0..100) to the concrete chip register + // (e.g., 2209: SGTHRS, 2130: SGT). Called only when % changes. + void applySgSensitivityPercent(uint8_t pct) override { + uint8_t sgthrs = (uint8_t)(((int)pct * 255 + 50) / 100); + driver->SGTHRS(sgthrs); + sgAppliedReg = (int16_t)sgthrs; + } + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status from driver + void readStatus(); + + // set peak current and hold current multiplier + inline void current(float mA, float mult) { driver->rms_current(round(mA*0.7071F), mult); } + + #if SERIAL_TMC == SoftSerial + SoftwareSerial *SerialTMC; + #endif + + TMC2209Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2660/Tmc2660.cpp b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2660/Tmc2660.cpp new file mode 100755 index 0000000..842e779 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2660/Tmc2660.cpp @@ -0,0 +1,101 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2660Stepper + +#include "Tmc2660.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2660_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +// constructor +StepDirTmc2660:: +StepDirTmc2660(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriver(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc2660StepDir, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirTmc2660::init() { + if (!TmcStepDirDriver::init()) return false; + + #ifdef DRIVER_TMC_STEPPER_HW_SPI + driver = new TMC2660Stepper(Pins->cs, rSense); + #else + driver = new TMC2660Stepper(Pins->cs, rSense, Pins->mosi, Pins->miso, Pins->sck); + #endif + driver->begin(); + driver->toff(5); + driver->intpol(intpol.value == ON); + modeMicrostepTracking(); + + current(iRun); + + // if we can, check to see if the driver is there + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + } + #endif + + return true; +} + +void StepDirTmc2660::modeMicrostepTracking() { + driver->microsteps(normalizedMicrosteps); +} + +int StepDirTmc2660::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver->microsteps(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void StepDirTmc2660::modeDecayTracking() { + current(iRun); +} + +void StepDirTmc2660::modeDecaySlewing() { + current(iGoto); +} + +void StepDirTmc2660::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning= status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// secondary way to power down not using the enable pin +bool StepDirTmc2660::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + driver->rms_current(0); + } + return true; +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2660/Tmc2660.h b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2660/Tmc2660.h new file mode 100755 index 0000000..2cc57f4 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc2660/Tmc2660.h @@ -0,0 +1,51 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC2660Stepper +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC2660_PRESENT) + +#include // https://github.com/teemuatlut/TMCStepper + +#include "../../../../Drivers.h" +#include "../../TmcStepDirDriverNSG.h" + +class StepDirTmc2660 : public TmcStepDirDriver { + public: + // constructor + StepDirTmc2660(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // calibrate the motor driver if required + void calibrateDriver() {} + + private: + // read status from driver + void readStatus(); + + // set peak current current multiplier + inline void current(float mA) { driver->rms_current(mA*0.7071F); } + + TMC2660Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5160/Tmc5160.cpp b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5160/Tmc5160.cpp new file mode 100755 index 0000000..c575592 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5160/Tmc5160.cpp @@ -0,0 +1,131 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC5160Stepper + +#include "Tmc5160.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC5160_PRESENT) + +#include "../../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +// constructor +StepDirTmc5160:: +StepDirTmc5160(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc5160StepDir, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirTmc5160::init() { + if (!TmcStepDirDriverSG::init()) return false; + + #ifdef DRIVER_TMC_STEPPER_HW_SPI + driver = new TMC5160Stepper(Pins->cs, rSense); + #else + driver = new TMC5160Stepper(Pins->cs, rSense, Pins->mosi, Pins->miso, Pins->sck); + #endif + driver->begin(); + driver->intpol(intpol.value == ON); + modeMicrostepTracking(); + driver->en_pwm_mode(false); + + current(iRun, iHoldRatio); + + // enable StallGuard / CoolStep speed threshold gating + // common "don't-care yet" value: always enable while moving + driver->TCOOLTHRS(0xFFFFF); // width depends on part; library handles masking + + // if you are NOT using CoolStep current scaling, keep it off + // disable CoolStep + driver->semin(0); + + // if we can, check to see if the driver is there + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + } + #endif + + return true; +} + +void StepDirTmc5160::modeMicrostepTracking() { + driver->microsteps(normalizedMicrosteps); +} + +int StepDirTmc5160::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver->microsteps(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void StepDirTmc5160::modeDecayTracking() { + setDecayMode(decay.value); + current(iRun, iHoldRatio); +} + +void StepDirTmc5160::modeDecaySlewing() { + setDecayMode(decaySlewing.value); + current(iGoto, 1.0F); +} + +void StepDirTmc5160::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning= status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// secondary way to power down not using the enable pin +bool StepDirTmc5160::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + setDecayMode(STEALTHCHOP); + driver->ihold(0); + } + return true; +} + +// calibrate the motor driver if required +void StepDirTmc5160::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("standstill automatic current calibration"); + + current(iRun, 1.0F); + driver->pwm_autograd(DRIVER_TMC_STEPPER_AUTOGRAD); + driver->pwm_autoscale(true); + driver->en_pwm_mode(true); + delay(1000); + modeDecayTracking(); + } +} + +// set the decay mode STEALTHCHOP or SPREADCYCLE +void StepDirTmc5160::setDecayMode(int decayMode) { + driver->en_pwm_mode(decayMode != SPREADCYCLE); +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5160/Tmc5160.h b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5160/Tmc5160.h new file mode 100755 index 0000000..1d6ad21 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5160/Tmc5160.h @@ -0,0 +1,71 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC5160Stepper +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC5160_PRESENT) + +#include // https://github.com/teemuatlut/TMCStepper + +#include "../../../../Drivers.h" + +#include "../../TmcStepDirDriverSG.h" + +class StepDirTmc5160 : public TmcStepDirDriverSG { + + public: + // constructor + StepDirTmc5160(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // read the 32bit stallguard register + uint32_t readSgRegister() override { return driver->DRV_STATUS(); } + + // SG only meaningful in spreadCycle (not stealthChop) + // and SG is only active while slewing so we assume this is representative of the state + bool stallGuardModeActive() const override { return decaySlewing.value == SPREADCYCLE; } + + // apply normalized sensitivity percent (0..100) to the concrete chip register + // (e.g., 2209: SGTHRS, 2130: SGT). Called only when % changes. + void applySgSensitivityPercent(uint8_t pct) override { + int sgt = 63 - (((int)pct * 127 + 50) / 100); // yields +63..-64 + driver->sgt((int8_t)sgt); + sgAppliedReg = (int16_t)sgt; + } + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status from driver + void readStatus(); + + // set the decay mode STEALTH_CHOP or SPREAD_CYCLE + void setDecayMode(int decayMode); + + // set peak current and hold current multiplier + inline void current(float mA, float mult) { driver->rms_current(mA*0.7071F, mult); } + + TMC5160Stepper *driver; +}; + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5161/Tmc5161.cpp b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5161/Tmc5161.cpp new file mode 100755 index 0000000..604d8d8 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5161/Tmc5161.cpp @@ -0,0 +1,131 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC5161Stepper + +#include "Tmc5161.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC5161_PRESENT) + +#include "../../../../../gpioEx/GpioEx.h" + +// help with pin names +#define mosi m0 +#define sck m1 +#define cs m2 +#define miso m3 + +// constructor +StepDirTmc5161:: +StepDirTmc5161(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, + int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol) + :TmcStepDirDriverSG(axisNumber, Pins, Settings, currentHold, currentRun, currentSlewing, intpol) { + strcpy(axisPrefix, " Axis_Tmc5161StepDir, "); + axisPrefix[5] = '0' + axisNumber; +} + +// setup driver +bool StepDirTmc5161::init() { + if (!TmcStepDirDriverSG::init()) return false; + + #ifdef DRIVER_TMC_STEPPER_HW_SPI + driver = new TMC5161Stepper(Pins->cs, rSense); + #else + driver = new TMC5161Stepper(Pins->cs, rSense, Pins->mosi, Pins->miso, Pins->sck); + #endif + driver->begin(); + driver->intpol(intpol.value == ON); + modeMicrostepTracking(); + driver->en_pwm_mode(false); + + current(iRun, iHoldRatio); + + // enable StallGuard / CoolStep speed threshold gating + // common "don't-care yet" value: always enable while moving + driver->TCOOLTHRS(0xFFFFF); // width depends on part; library handles masking + + // if you are NOT using CoolStep current scaling, keep it off + // disable CoolStep + driver->semin(0); + + // if we can, check to see if the driver is there + // check to see if the driver is there and ok + #ifdef MOTOR_DRIVER_DETECT + #ifndef DRIVER_TMC_STEPPER_HW_SPI + if (Pins->miso != OFF) + #endif + { + readStatus(); + if (!status.standstill || status.overTemperature) { + DF("ERR:"); D(axisPrefix); DLF("no motor driver device detected!"); + return false; + } else { VF("MSG:"); V(axisPrefix); VLF("motor driver device detected"); } + } + #endif + + return true; +} + +void StepDirTmc5161::modeMicrostepTracking() { + driver->microsteps(normalizedMicrosteps); +} + +int StepDirTmc5161::modeMicrostepSlewing() { + if (microstepRatio > 1) { + driver->microsteps(normalizedMicrostepsSlewing); + } + return microstepRatio; +} + +void StepDirTmc5161::modeDecayTracking() { + setDecayMode(decay.value); + current(iRun, iHoldRatio); +} + +void StepDirTmc5161::modeDecaySlewing() { + setDecayMode(decaySlewing.value); + current(iGoto, 1.0F); +} + +void StepDirTmc5161::readStatus() { + TMC2130_n::DRV_STATUS_t status_result; + status_result.sr = driver->DRV_STATUS(); + + status.outputA.shortToGround = status_result.s2ga; + status.outputA.openLoad = status_result.ola; + status.outputB.shortToGround = status_result.s2gb; + status.outputB.openLoad = status_result.olb; + status.overTemperatureWarning= status_result.otpw; + status.overTemperature = status_result.ot; + status.standstill = status_result.stst; +} + +// secondary way to power down not using the enable pin +bool StepDirTmc5161::enable(bool state) { + if (state) { + modeDecayTracking(); + } else { + setDecayMode(STEALTHCHOP); + driver->ihold(0); + } + return true; +} + +// calibrate the motor driver if required +void StepDirTmc5161::calibrateDriver() { + if (decay.value == STEALTHCHOP || decaySlewing.value == STEALTHCHOP) { + VF("MSG:"); V(axisPrefix); VL("standstill automatic current calibration"); + + current(iRun, 1.0F); + driver->pwm_autograd(DRIVER_TMC_STEPPER_AUTOGRAD); + driver->pwm_autoscale(true); + driver->en_pwm_mode(true); + delay(1000); + modeDecayTracking(); + } +} + +// set the decay mode STEALTHCHOP or SPREADCYCLE +void StepDirTmc5161::setDecayMode(int decayMode) { + driver->en_pwm_mode(decayMode != SPREADCYCLE); +} + +#endif diff --git a/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5161/Tmc5161.h b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5161/Tmc5161.h new file mode 100755 index 0000000..615fac9 --- /dev/null +++ b/src/lib/axis/motor/stepDir/tmc/tmcStepper/tmc5161/Tmc5161.h @@ -0,0 +1,69 @@ +// ----------------------------------------------------------------------------------- +// axis step/dir motor driver, TMC5161Stepper +#pragma once + +#include +#include "../../../../../../../Common.h" + +#if defined(DRIVER_TMC_STEPPER) && defined(TMC5161_PRESENT) + +#include // https://github.com/teemuatlut/TMCStepper + +#include "../../../../Drivers.h" +#include "../../TmcStepDirDriverSG.h" + +class StepDirTmc5161 : public TmcStepDirDriverSG { + public: + // constructor + StepDirTmc5161(uint8_t axisNumber, const StepDirDriverPins *Pins, const StepDirDriverSettings *Settings, int16_t currentHold, int16_t currentRun, int16_t currentSlewing, int8_t intpol); + + // setup driver + bool init(); + + // set microstep mode for tracking + void modeMicrostepTracking(); + + // set microstep mode for slewing + int modeMicrostepSlewing(); + + // set decay mode for tracking + void modeDecayTracking(); + + // set decay mode for slewing + void modeDecaySlewing(); + + // secondary way to power down not using the enable pin + bool enable(bool state); + + // read the 32bit stallguard register + uint32_t readSgRegister() override { return driver->DRV_STATUS(); } + + // SG only meaningful in spreadCycle (not stealthChop) + // and SG is only active while slewing so we assume this is representative of the state + bool stallGuardModeActive() const override { return decaySlewing.value == SPREADCYCLE; } + + // apply normalized sensitivity percent (0..100) to the concrete chip register + // (e.g., 2209: SGTHRS, 2130: SGT). Called only when % changes. + void applySgSensitivityPercent(uint8_t pct) override { + int sgt = 63 - (((int)pct * 127 + 50) / 100); // yields +63..-64 + driver->sgt((int8_t)sgt); + sgAppliedReg = (int16_t)sgt; + } + + // calibrate the motor driver if required + void calibrateDriver(); + + private: + // read status from driver + void readStatus(); + + // set the decay mode STEALTH_CHOP or SPREAD_CYCLE + void setDecayMode(int decayMode); + + // set peak current and hold current multiplier + inline void current(float mA, float mult) { driver->rms_current(mA*0.7071F, mult); } + + TMC5161Stepper *driver; +}; + +#endif diff --git a/src/lib/bluetooth/Bluetooth.defaults.h b/src/lib/bluetooth/Bluetooth.defaults.h new file mode 100755 index 0000000..acb3c34 --- /dev/null +++ b/src/lib/bluetooth/Bluetooth.defaults.h @@ -0,0 +1,91 @@ +#pragma once + +// ------------------------------------------------------------------------------------------------- +// general bt settings + +#ifndef HOST_NAME +#define HOST_NAME "Unknown" +#endif + +#ifndef SERIAL_BT_NAME +#define SERIAL_BT_NAME HOST_NAME +#endif + +// ------------------------------------------------------------------------------------------------- +// bluetooth manager settings + +#ifndef PASSWORD_DEFAULT +#define PASSWORD_DEFAULT "password" // default pwd for wifi settings +#endif + +// station 1 + +#ifndef STA1_BT_NAME +#define STA1_BT_NAME "OnStep" // the bluetooth server name this station is connecting to +#endif +#ifndef STA1_BT_ADDR +#define STA1_BT_ADDR "" // bluetooth MAC address to connect to +#endif +#ifndef STA1_BT_PASSKEY +#define STA1_BT_PASSKEY "" // bluetooth four digit passkey, leave blank to disable security +#endif + +// station 2 + +#ifndef STA2_BT_NAME +#define STA2_BT_NAME "" +#endif +#ifndef STA2_BT_ADDR +#define STA2_BT_ADDR "" +#endif +#ifndef STA2_BT_PASSKEY +#define STA2_BT_PASSKEY "" +#endif + +// station 3 + +#ifndef STA3_BT_NAME +#define STA3_BT_NAME "" +#endif +#ifndef STA3_BT_ADDR +#define STA3_BT_ADDR "" +#endif +#ifndef STA3_BT_PASSKEY +#define STA3_BT_PASSKEY "" +#endif + +// station 4 + +#ifndef STA4_BT_NAME +#define STA4_BT_NAME "" +#endif +#ifndef STA4_BT_ADDR +#define STA4_BT_ADDR "" +#endif +#ifndef STA4_BT_PASSKEY +#define STA4_BT_PASSKEY "" +#endif + +// station 5 + +#ifndef STA5_BT_NAME +#define STA5_BT_NAME "" +#endif +#ifndef STA5_BT_ADDR +#define STA5_BT_ADDR "" +#endif +#ifndef STA5_BT_PASSKEY +#define STA5_BT_PASSKEY "" +#endif + +// station 6 + +#ifndef STA6_BT_NAME +#define STA6_BT_NAME "" +#endif +#ifndef STA6_BT_ADDR +#define STA6_BT_ADDR "" +#endif +#ifndef STA6_BT_PASSKEY +#define STA6_BT_PASSKEY "" +#endif diff --git a/src/lib/bluetooth/BluetoothManager.cpp b/src/lib/bluetooth/BluetoothManager.cpp new file mode 100755 index 0000000..8d6a593 --- /dev/null +++ b/src/lib/bluetooth/BluetoothManager.cpp @@ -0,0 +1,86 @@ +// bluetooth manager +#include "BluetoothManager.h" + +#if SERIAL_BT_MODE == MASTER + +#include "../nv/Nv.h" + +bool BluetoothManager::init() { + if (!active) { + readSettings(); + + setStation(stationNumber); + + VF("MSG: Bluetooth, init device "); + VF(sta->address); DF(" "); VF(sta->host); + + int channel = 0; + BTAddress address = BTAddress(sta->address); + std::map channels = SERIAL_BT.getChannels(address); + if (channels.size() > 0) { channel = channels.begin()->first; } + VF(" on ch "); V(channel); + + if (strlen(sta->passkey) > 0) { + VF(" w/passkey "); V(bluetoothManager.sta->passkey); + SERIAL_BT.setPin(bluetoothManager.sta->passkey); + } + + VF("..."); + if (channel != 0 && SERIAL_BT.connect(address, channel, ESP_SPP_SEC_NONE, ESP_SPP_ROLE_SLAVE)) { + VLF(" success"); + active = true; + } else { + VLF(" failed"); + } + } + + return active; +} + +void BluetoothManager::setStation(int number) { + if (number >= 1 && number <= BluetoothStationCount) stationNumber = number; + sta = &settings.station[stationNumber - 1]; +} + +void BluetoothManager::disconnect() { + active = false; + VLF("MSG: Bluetooth, disconnected"); +} + +void BluetoothManager::readSettings() { + if (settingsReady) return; + + #ifdef NV_BT_SETTINGS + if (!nv().kv().getOrInit("BT_SETTINGS", settings)) { DLF("WRN: Nv, init failed for BT_SETTINGS"); } + #endif + + VF("MSG: Bluetooth, Master Pwd = "); VL(settings.masterPassword); + + VF("MSG: Bluetooth, Sta Select = "); VL(stationNumber); + + int currentStationNumber = stationNumber; + + for (int station = 1; station <= BluetoothStationCount; station++) { + setStation(station); + VF("MSG: Bluetooth, Sta"); V(station); VF(" NAME = "); VL(sta->host); + VF("MSG: Bluetooth, Sta"); V(station); VF(" MAC = "); VL(sta->address); + VF("MSG: Bluetooth, Sta"); V(station); VF(" KEY = "); VL(sta->passkey); + } + + stationNumber = currentStationNumber; + + settingsReady = true; +} + +void BluetoothManager::writeSettings() { + if (!settingsReady) return; + + #ifdef NV_BT_SETTINGS + VLF("MSG: BluetoothManager, writing settings to NV"); + nv().kv().put("BT_SETTINGS", settings); + #endif +} + +BluetoothManager bluetoothManager; + +#endif diff --git a/src/lib/bluetooth/BluetoothManager.h b/src/lib/bluetooth/BluetoothManager.h new file mode 100755 index 0000000..c82e7fa --- /dev/null +++ b/src/lib/bluetooth/BluetoothManager.h @@ -0,0 +1,75 @@ +// bluetooth manager +#pragma once + +#include "../../Common.h" + +#if SERIAL_BT_MODE == MASTER + +#include "Bluetooth.defaults.h" + +#ifndef ESP32 + #error "Configuration (Config.h): No Bluetooth support is present for this device" +#endif + +typedef struct BluetoothStationSettings { + char host[16]; + char address[18]; + char passkey[5]; +} BluetoothStationSettings; + +#ifndef BluetoothStationCount + // number of bluetooth stations supported, between 1 and 6 + #define BluetoothStationCount 3 +#endif +#define BluetoothSettingsSize (32 + BluetoothStationCount*39) +typedef struct BluetoothSettings { + char masterPassword[32]; + BluetoothStationSettings station[BluetoothStationCount]; +} BluetoothSettings; + +class BluetoothManager { + public: + bool init(); + void disconnect(); + void setStation(int number); + void readSettings(); + void writeSettings(); + + BluetoothStationSettings *sta; + + BluetoothSettings settings = { + PASSWORD_DEFAULT, + + { + #if BluetoothStationCount > 0 + {STA1_BT_NAME, STA1_BT_ADDR, STA1_BT_PASSKEY}, + #endif + #if BluetoothStationCount > 1 + {STA2_BT_NAME, STA2_BT_ADDR, STA2_BT_PASSKEY}, + #endif + #if BluetoothStationCount > 2 + {STA3_BT_NAME, STA3_BT_ADDR, STA3_BT_PASSKEY}, + #endif + #if BluetoothStationCount > 3 + {STA4_BT_NAME, STA4_BT_ADDR, STA4_BT_PASSKEY}, + #endif + #if BluetoothStationCount > 4 + {STA5_BT_NAME, STA5_BT_ADDR, STA5_BT_PASSKEY}, + #endif + #if BluetoothStationCount > 5 + {STA6_BT_NAME, STA6_BT_ADDR, STA6_BT_PASSKEY}, + #endif + } + }; + + bool active = false; + bool settingsReady = false; + int stationNumber = 1; + + private: + +}; + +extern BluetoothManager bluetoothManager; + +#endif diff --git a/src/lib/calendars/Calendars.cpp b/src/lib/calendars/Calendars.cpp new file mode 100755 index 0000000..c2ae85f --- /dev/null +++ b/src/lib/calendars/Calendars.cpp @@ -0,0 +1,73 @@ +// ----------------------------------------------------------------------------------- +// Gregorian and Julian calendars + +#include "Calendars.h" + +#ifdef CALENDARS_PRESENT + +// convert from Gregorian to Julian date/time +JulianDate Calendars::gregorianToJulian(GregorianDate date) { + JulianDate jd = gregorianToJulianDay(date); + jd.hour = date.hour; + + while (jd.hour >= 24.0) { jd.hour -= 24.0; jd.day += 1.0; } + while (jd.hour < 0.0) { jd.hour += 24.0; jd.day -= 1.0; } + + return jd; +} + +// convert from Julian to Gregorian date/time +GregorianDate Calendars::julianToGregorian(JulianDate julianDate) { + while (julianDate.hour >= 24.0) { julianDate.hour -= 24.0; julianDate.day += 1.0; } + while (julianDate.hour < 0.0) { julianDate.hour += 24.0; julianDate.day -= 1.0; } + + GregorianDate date = julianDayToGregorian(julianDate); + date.hour = julianDate.hour; + + return date; +} + +// convert from Gregorian to Julian date (does not handle hours) +JulianDate Calendars::gregorianToJulianDay(GregorianDate date) { + JulianDate julianDay; + + int y = date.year; + int m = date.month; + if (m == 1 || m == 2) { y--; m += 12; } + double B = 2.0 - floor(y/100.0) + floor(y/400.0); + julianDay.day = B + floor(365.25*y) + floor(30.6001*(m + 1.0)) + date.day + 1720994.5; + julianDay.hour = 0; + + return julianDay; +} + +// convert from Julian to Gregorian date (does not handle hours) +GregorianDate Calendars::julianDayToGregorian(JulianDate julianDate) { + double A, B, C, D, D1, E, F, G, I; + GregorianDate date; + + I = floor(julianDate.day + 0.5); + + F = 0.0; + if (I > 2299160.0) { + A = int((I - 1867216.25)/36524.25); + B = I + 1.0 + A - floor(A/4.0); + } else B = I; + + C = B + 1524.0; + D = floor((C - 122.1)/365.25); + E = floor(365.25*D); + G = floor((C - E)/30.6001); + + D1 = C - E + F - floor(30.6001*G); + date.day = floor(D1); + if (G < 13.5) date.month = floor(G - 1.0); else date.month = floor(G - 13.0); + if (date.month > 2.5) date.year = floor(D - 4716.0); else date.year = floor(D - 4715.0); + date.hour = 0.0; + + return date; +} + +Calendars calendars; + +#endif diff --git a/src/lib/calendars/Calendars.h b/src/lib/calendars/Calendars.h new file mode 100755 index 0000000..454855f --- /dev/null +++ b/src/lib/calendars/Calendars.h @@ -0,0 +1,43 @@ +// ----------------------------------------------------------------------------------- +// Gregorian and Julian calendars +#pragma once + +#include + +#define CALENDARS_PRESENT + +#ifdef CALENDARS_PRESENT + +typedef struct GregorianDate { + int16_t year; + uint8_t month; + uint8_t day; + double hour; +} GregorianDate; + +#define JulianDateSize 16 +typedef struct JulianDate { + double day; + double hour; +} JulianDate; + +class Calendars { + public: + // convert from Gregorian to Julian date/time + JulianDate gregorianToJulian(GregorianDate date); + + // convert from Julian to Gregorian date/time + GregorianDate julianToGregorian(JulianDate julianDate); + + // convert from Gregorian to Julian date (does not handle hours) + JulianDate gregorianToJulianDay(GregorianDate date); + + // convert from Julian to Gregorian date (does not handle hours) + GregorianDate julianDayToGregorian(JulianDate julianDate); + + private: +}; + +extern Calendars calendars; + +#endif diff --git a/src/lib/canPlus/CanPlus.h b/src/lib/canPlus/CanPlus.h new file mode 100755 index 0000000..d7e201f --- /dev/null +++ b/src/lib/canPlus/CanPlus.h @@ -0,0 +1,15 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + #include "san/San.h" + #include "esp32/Esp32.h" + #include "mcp2515/Mcp2515.h" + #include "teensy4/Can0.h" + #include "teensy4/Can1.h" + #include "teensy4/Can2.h" + #include "teensy4/Can3.h" +#endif diff --git a/src/lib/canPlus/CanPlusBase.cpp b/src/lib/canPlus/CanPlusBase.cpp new file mode 100755 index 0000000..354479d --- /dev/null +++ b/src/lib/canPlus/CanPlusBase.cpp @@ -0,0 +1,131 @@ +// ----------------------------------------------------------------------------------- +// CAN library + +#include "CanPlusBase.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +#include "../tasks/OnTask.h" + +int CanPlus::beginPacket(int id) { + if (!ready) return 0; + sendId = id; + sendCount = 0; + return 1; +} + +int CanPlus::endPacket() { + if (!ready) return 0; + if (sendCount < 1) return 0; + if (writePacket(sendId, sendData, sendCount)) return 1; else return 0; +} + +int CanPlus::write(uint8_t byte) { + if (!ready) return 0; + if (sendCount < 0 || sendCount > 7) return 0; + sendData[sendCount++] = byte; + return 1; +} + +int CanPlus::write(const uint8_t *buffer, size_t size) { + if (!ready) return 0; + if (sendCount < 0 || size < 1 || sendCount + size > 8) return 0; + + for (unsigned int i = 0; i < size; i++) { if (!write(buffer[i])) return 0; } + return size; +} + +// add an id only callback +int CanPlus::callbackRegisterId(int id, void (*callback)(uint8_t data[8])) { + if (!ready) return 0; + if (id < 0 || id > 0x7FF || idCallbackCount >= CAN_MAX_CALLBACKS) return 0; + idCallback[idCallbackCount] = callback; + idCallBackId[idCallbackCount] = id; + idCallbackEx[idCallbackCount] = nullptr; + idCallbackCount++; + return 1; +} + +// add an id only callback (len-aware) +int CanPlus::callbackRegisterId(int id, void (*callback)(uint8_t data[8], uint8_t len)) { + if (!ready) return 0; + if (id < 0 || id > 0x7FF || idCallbackCount >= CAN_MAX_CALLBACKS) return 0; + idCallBackId[idCallbackCount] = id; + idCallback[idCallbackCount] = nullptr; // old slot unused + idCallbackEx[idCallbackCount] = callback; // new slot used + idCallbackCount++; + return 1; +} + +// add an id + message callback +int CanPlus::callbackRegisterMessage(int id, uint8_t msg, void (*callback)(uint8_t data[8])) { + if (!ready) return 0; + if (id < 0 || id > 0x7FF || msgCallbackCount >= CAN_MAX_CALLBACKS) return 0; + msgCallback[msgCallbackCount] = callback; + msgCallBackId[msgCallbackCount] = id; + msgCallBackMsg[msgCallbackCount] = msg; + msgCallbackEx[msgCallbackCount] = nullptr; + msgCallbackCount++; + return 1; +} + +// add an id + message callback (len-aware) +int CanPlus::callbackRegisterMessage(int id, uint8_t msg, void (*callback)(uint8_t data[8], uint8_t len)) { + if (!ready) return 0; + if (id < 0 || id > 0x7FF || msgCallbackCount >= CAN_MAX_CALLBACKS) return 0; + msgCallBackId[msgCallbackCount] = id; + msgCallBackMsg[msgCallbackCount] = msg; + msgCallback[msgCallbackCount] = nullptr; // old slot unused + msgCallbackEx[msgCallbackCount] = callback; // new slot used + msgCallbackCount++; + return 1; +} + +// run id/msg based callbacks +int CanPlus::callbackProcess(int id, uint8_t *buffer, size_t size) { + uint8_t paddedBuffer[8] = {0}; + + uint8_t len = 0; + if (buffer && size) { + if (size > 8) size = 8; + len = (uint8_t)size; + memcpy(paddedBuffer, buffer, size); + } else { + len = 0; + } + + int called = 0; + + for (int i = 0; i < idCallbackCount; i++) { + if (id == idCallBackId[i]) { + if (idCallbackEx[i]) idCallbackEx[i](paddedBuffer, len); + else if (idCallback[i]) idCallback[i](paddedBuffer); + called++; + } + } + + for (int i = 0; i < msgCallbackCount; i++) { + if (len >= 1 && id == msgCallBackId[i] && paddedBuffer[0] == msgCallBackMsg[i]) { + if (msgCallbackEx[i]) msgCallbackEx[i](paddedBuffer, len); + else if (msgCallback[i]) msgCallback[i](paddedBuffer); + called++; + } + } + + return called ? 1 : 0; +} + +bool CanPlus::txTryLock(uint32_t minGapUs) { + uint32_t now = micros(); + if ((uint32_t)(now - lastTxUs) < minGapUs) return false; + lastTxUs = now; + return true; +} + +void CanPlus::txWait(uint32_t minGapUs) { + while (!txTryLock(minGapUs)) { + tasks.yield(minGapUs/1000UL); + } +} + +#endif diff --git a/src/lib/canPlus/CanPlusBase.h b/src/lib/canPlus/CanPlusBase.h new file mode 100755 index 0000000..c23eaf4 --- /dev/null +++ b/src/lib/canPlus/CanPlusBase.h @@ -0,0 +1,117 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +#ifndef DEBUG_CAN +#define DEBUG_CAN OFF +#endif + +#ifndef CAN_BAUD +#define CAN_BAUD 1000000 // 1000000 baud default +#endif +#ifndef CAN_SEND_RATE_MS +#define CAN_SEND_RATE_MS 10 // 100 Hz CAN controller send message processing rate +#endif +#ifndef CAN_RECV_RATE_MS +#define CAN_RECV_RATE_MS 2 // 500 Hz CAN controller recv. message processing rate +#endif +#ifndef CAN_RX_PIN +#define CAN_RX_PIN OFF // for ESP32 CAN interface +#endif +#ifndef CAN_TX_PIN +#define CAN_TX_PIN OFF // for ESP32 CAN interface +#endif +#ifndef CAN_CS_PIN +#define CAN_CS_PIN OFF // for MCP2515 SPI CAN controller +#endif +#ifndef CAN_INT_PIN +#define CAN_INT_PIN OFF // for MCP2515 SPI CAN controller +#endif + +#ifndef CAN_BURST_WINDOW_US +#define CAN_BURST_WINDOW_US 1000 // for polling recv. how long to wait during a burst +#endif + +#ifndef CAN_MAX_CALLBACKS + #define CAN_MAX_CALLBACKS 16 +#endif + +class CanPlus { + public: + virtual ~CanPlus() = default; + virtual int beginPacket(int id); + virtual int endPacket(); + virtual int write(uint8_t byte); + virtual int write(const uint8_t *buffer, size_t size); + virtual int writePacket(int id, const uint8_t *buffer, size_t size); + // RTR policy: supports sending RTR, but drops on receive + virtual int writePacketRtr(int id, size_t dlc) { UNUSED(id); UNUSED(dlc); return 0; } + + virtual int callbackRegisterId(int id, void (*callback)(uint8_t data[8])); + virtual int callbackRegisterMessage(int id, uint8_t msg, void (*callback)(uint8_t data[8])); + + virtual int callbackRegisterId(int id, void (*callback)(uint8_t data[8], uint8_t len)); + virtual int callbackRegisterMessage(int id, uint8_t msg, void (*callback)(uint8_t data[8], uint8_t len)); + + virtual int callbackProcess(int id, uint8_t *buffer, size_t size); + + // send pacing, non-blocking + bool txTryLock(uint32_t minGapUs = defaultPacingUs()); + + // send pacing, blocking + void txWait(uint32_t minGapUs = defaultPacingUs()); + + // allow any polling recv that may be present to happen quickly for a short period of time + // this increases the polling rate to 1ms for the specified period in microseconds + virtual void rxBurst(uint32_t periodUs = 2000) { UNUSED(periodUs); } + + static inline uint32_t defaultPacingUs() { return (uint32_t)CAN_SEND_RATE_MS*1000UL; } + + bool ready = false; + + inline void resetStats() { + noInterrupts(); + tx_ok = 0; + tx_fail = 0; + rx_ok = 0; + rx_drop_ext = 0; + rx_drop_rtr = 0; + rx_drop_len = 0; + interrupts(); + } + + volatile uint32_t rx_ok = 0; + volatile uint32_t rx_drop_ext = 0; + volatile uint32_t rx_drop_rtr = 0; + volatile uint32_t rx_drop_len = 0; + volatile uint32_t tx_ok = 0; + volatile uint32_t tx_fail = 0; + + protected: + int sendId = 0; + int sendCount = -1; + uint8_t sendData[8] = {0}; + + int idCallbackCount = 0; + int idCallBackId[CAN_MAX_CALLBACKS] = {0}; + void (*idCallback[CAN_MAX_CALLBACKS])(uint8_t data[8]) = {nullptr}; + void (*idCallbackEx[CAN_MAX_CALLBACKS])(uint8_t data[8], uint8_t len) = {nullptr}; + + int msgCallbackCount = 0; + int msgCallBackId[CAN_MAX_CALLBACKS] = {0}; + int msgCallBackMsg[CAN_MAX_CALLBACKS] = {0}; + void (*msgCallback[CAN_MAX_CALLBACKS])(uint8_t data[8]) = {nullptr}; + void (*msgCallbackEx[CAN_MAX_CALLBACKS])(uint8_t data[8], uint8_t len) = {nullptr}; + + uint32_t lastTxUs = 0; + + // for recv. path loops if present + const uint32_t budgetUs = 200; // tune ~ 100–300us + const int maxFrames = 8; // cap frames that are processed per CAN_RECV_RATE_MS, must be >= 2 +}; + +#endif \ No newline at end of file diff --git a/src/lib/canPlus/esp32/Esp32.cpp b/src/lib/canPlus/esp32/Esp32.cpp new file mode 100755 index 0000000..58cc8f5 --- /dev/null +++ b/src/lib/canPlus/esp32/Esp32.cpp @@ -0,0 +1,194 @@ +// ----------------------------------------------------------------------------------- +// CAN library (ESP32 TWAI backend) + +#include "Esp32.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN_ESP32 + +#include "../../tasks/OnTask.h" + +#include "driver/twai.h" // ESP-IDF TWAI (CAN) driver +#include "esp_err.h" + +void canEsp32Monitor() { canPlus.poll(); } + +static bool twaiReady = false; + +CanPlusESP32::CanPlusESP32() {} + +static twai_timing_config_t timingFromBaud() { + #if CAN_BAUD == 125000 + return TWAI_TIMING_CONFIG_125KBITS(); + #elif CAN_BAUD == 250000 + return TWAI_TIMING_CONFIG_250KBITS(); + #elif CAN_BAUD == 500000 + return TWAI_TIMING_CONFIG_500KBITS(); + #elif CAN_BAUD == 1000000 + return TWAI_TIMING_CONFIG_1MBITS(); + #else + return TWAI_TIMING_CONFIG_500KBITS(); + #endif +} + +void CanPlusESP32::init() { + VF("MSG: CanPlus, CAN_ESP32 (TWAI) Start... "); + + // General config: NORMAL mode means we transmit and ACK others. + twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( + (gpio_num_t)CAN_TX_PIN, + (gpio_num_t)CAN_RX_PIN, + TWAI_MODE_NORMAL + ); + + // You can tune these if you want more buffering: + g_config.tx_queue_len = 32; + g_config.rx_queue_len = 128; + + // Optional: reduce ISR workload by not receiving every single frame if you use acceptance filters. + // Here we accept everything (no filtering at hardware level). + twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + + twai_timing_config_t t_config = timingFromBaud(); + + esp_err_t err = twai_driver_install(&g_config, &t_config, &f_config); + if (err != ESP_OK) { + V("FAILED! twai_driver_install err="); VL((int)err); + ready = false; + return; + } + + err = twai_start(); + if (err != ESP_OK) { + V("FAILED! twai_start err="); VL((int)err); + twai_driver_uninstall(); + ready = false; + return; + } + + twaiReady = true; + ready = true; + + VLF("success"); + + VF("MSG: CanPlus, start callback monitor task (rate "); V(CAN_RECV_RATE_MS); + VF("ms priority 3)... "); + if (tasks.add(CAN_RECV_RATE_MS, 0, true, 3, canEsp32Monitor, "CanESP")) { + VLF("success"); + + // setup the burst task disabled (period 0) and so it only runs once + burst = tasks.add(0, 0, true, 3, canEsp32Monitor, "CanBrst"); + + } else { + VLF("FAILED!"); + } +} + +int CanPlusESP32::writePacket(int id, const uint8_t *buffer, size_t size) { + if (!ready || !twaiReady) return 0; + if (size > 8) return 0; + if (size > 0 && buffer == nullptr) return 0; + + twai_message_t msg = {}; + msg.identifier = (uint32_t)(id & 0x7FF); + msg.extd = 0; // standard 11-bit + msg.rtr = 0; + msg.data_length_code = (uint8_t)size; + if (size) memcpy(msg.data, buffer, size); + + // Zero timeout: do not block the caller; return failure if TX queue is full or bus is off. + esp_err_t err = twai_transmit(&msg, 0); + + if (err == ESP_OK) { + tx_ok++; + return 1; + } else { + tx_fail++; + // Optional 1 Hz debug: + // DL1("TWAI tx fail err="); DL1((int)err); + return 0; + } +} + +int CanPlusESP32::writePacketRtr(int id, size_t dlc) { + if (!ready || !twaiReady) return 0; + if (dlc > 8) return 0; + + twai_message_t msg = {}; + msg.identifier = (uint32_t)(id & 0x7FF); + msg.extd = 0; + msg.rtr = 1; + msg.data_length_code = (uint8_t)dlc; + + esp_err_t err = twai_transmit(&msg, 0); + + if (err == ESP_OK) { tx_ok++; return 1; } + tx_fail++; + return 0; +} + +void CanPlusESP32::rxBurst(uint32_t periodUs) { + const uint32_t now = micros(); + const uint32_t until = now + periodUs; + + if (!burstEnabled) { + burstEnabled = true; + burstUntilUs = until; + tasks.setPeriod(burst, 1); + } else { + if ((int32_t)(until - burstUntilUs) > 0) burstUntilUs = until; + } +} + +void CanPlusESP32::poll() { + if (!ready || !twaiReady) return; + + if (burstEnabled && (int32_t)(micros() - burstUntilUs) >= 0) { + tasks.setPeriod(burst, 0); + burstEnabled = false; + } + + const uint32_t start = micros(); + + // shorten workload during busts + const int frames = burstEnabled ? (maxFrames >> 1) : maxFrames; + + // drain up to N frames per poll so we don't monopolize the CPU + for (int n = 0; n < frames; n++) { + twai_message_t msg; + esp_err_t err = twai_receive(&msg, 0); // non-blocking + if (err != ESP_OK) break; + + // drop extended IDs (unless you want to support them) + if (msg.extd) { rx_drop_ext++; continue; } + // drop RTR frames (unless you want them) + if (msg.rtr) { rx_drop_rtr++; continue; } + // DLC sanity + if (msg.data_length_code < 1 || msg.data_length_code > 8) { rx_drop_len++; continue; } + + rx_ok++; + + callbackProcess((int)(msg.identifier & 0x7FF), (uint8_t*)msg.data, msg.data_length_code); + + debugStatus(); + + if ((uint32_t)(micros() - start) > budgetUs) break; + } +} + +void CanPlusESP32::debugStatus() { + #if DEBUG != OFF && DEBUG_CAN != OFF + if (!ready || !twaiReady) return; + twai_status_info_t s; + if (twai_get_status_info(&s) != ESP_OK) return; + + // 1 Hz debug suggested: + D1("MSG: TWAI, state="); D1((int)s.state); + D1(" tx_err="); D1((int)s.tx_error_counter); + D1(" rx_err="); D1((int)s.rx_error_counter); + D1(" bus_err="); DL1((int)s.bus_error_count); + #endif +} + +CanPlusESP32 canPlus; + +#endif diff --git a/src/lib/canPlus/esp32/Esp32.h b/src/lib/canPlus/esp32/Esp32.h new file mode 100755 index 0000000..3d617f0 --- /dev/null +++ b/src/lib/canPlus/esp32/Esp32.h @@ -0,0 +1,40 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN_ESP32 + +#include "../CanPlusBase.h" + +#ifndef ESP32 + #error "Configuration (Config.h): Setting CAN_PLUS CAN_ESP32 requires an ESP32 microcontroller." +#endif + +#if CAN_RX_PIN == OFF || CAN_TX_PIN == OFF + #error "CAN_ESP32 requires CAN_RX_PIN and CAN_TX_PIN to be defined." +#endif + +class CanPlusESP32 : public CanPlus { + public: + CanPlusESP32(); + void init(); + int writePacket(int id, const uint8_t *buffer, size_t size); + int writePacketRtr(int id, size_t dlc); + + void rxBurst(uint32_t periodUs = 2000); + + void debugStatus(); + + void poll(); + + private: + uint8_t burst = 0; + uint32_t burstUntilUs = 0; + bool burstEnabled = false; +}; + +extern CanPlusESP32 canPlus; + +#endif diff --git a/src/lib/canPlus/mcp2515/Mcp2515.cpp b/src/lib/canPlus/mcp2515/Mcp2515.cpp new file mode 100755 index 0000000..e068193 --- /dev/null +++ b/src/lib/canPlus/mcp2515/Mcp2515.cpp @@ -0,0 +1,152 @@ +// ----------------------------------------------------------------------------------- +// CAN library (MCP2515 via autowp/arduino-mcp2515) + +#include "Mcp2515.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN_MCP2515 + +#include "../../tasks/OnTask.h" + +#include +#include // https://github.com/autowp/arduino-mcp2515 + +// Pick the right oscillator value for your module: +// - Most shields/modules are 16MHz +// - Some are 8MHz +#ifndef CAN_MCP2515_CLOCK + #define CAN_MCP2515_CLOCK MCP_16MHZ +#endif + +IRAM_ATTR void canMcp2515Monitor() { canPlus.poll(); } + +static MCP2515 mcp2515(CAN_CS_PIN); + +CanPlusMCP2515::CanPlusMCP2515() {} + +static inline can_bitrate_t toBitrate() { + #if CAN_BAUD == 125000 + return CAN_125KBPS; + #elif CAN_BAUD == 250000 + return CAN_250KBPS; + #elif CAN_BAUD == 500000 + return CAN_500KBPS; + #elif CAN_BAUD == 1000000 + return CAN_1000KBPS; + #else + return CAN_500KBPS; + #endif +} + +void CanPlusMCP2515::init() { + VF("MSG: CanPlus, CAN_MCP2515 (autowp) Start... "); + + #if CAN_INT_PIN != OFF + pinMode(CAN_INT_PIN, INPUT_PULLUP); + #endif + + SPI.begin(); + mcp2515.reset(); + + ready = (mcp2515.setBitrate(toBitrate(), CAN_MCP2515_CLOCK) == MCP2515::ERROR_OK); + if (ready) ready = (mcp2515.setNormalMode() == MCP2515::ERROR_OK); + + if (ready) { + VLF("success"); + VF("MSG: CanPlus, start callback monitor task (rate "); V(CAN_RECV_RATE_MS); + VF("ms priority 3)... "); + if (tasks.add(CAN_RECV_RATE_MS, 0, true, 3, canMcp2515Monitor, "Can2515")) { + VLF("success"); + + // setup the burst task disabled (period 0) and so it only runs once + burst = tasks.add(0, 0, true, 3, canEsp32Monitor, "CanBrst"); + + } else VLF("FAILED!"); + } else { + VLF("FAILED!"); + } +} + +int CanPlusMCP2515::writePacket(int id, const uint8_t *buffer, size_t size) { + if (!ready) return 0; + if (size > 8) return 0; + if (size > 0 && buffer == nullptr) return 0; + + struct can_frame frame{}; + frame.can_id = (uint32_t)(id & CAN_SFF_MASK); // standard 11-bit + frame.can_dlc = (uint8_t)size; + if (size && buffer) { + memcpy(frame.data, buffer, size); + } + auto err = mcp2515.sendMessage(&frame); + if (err == MCP2515::ERROR_OK) tx_ok++; else tx_fail++; + return (err == MCP2515::ERROR_OK) ? 1 : 0; +} + +int CanPlusMCP2515::writePacketRtr(int id, size_t dlc) { + if (!ready) return 0; + if (dlc > 8) return 0; + + struct can_frame frame{}; + frame.can_id = (uint32_t)(id & CAN_SFF_MASK) | CAN_RTR_FLAG; + frame.can_dlc = (uint8_t)dlc; + + auto err = mcp2515.sendMessage(&frame); + if (err == MCP2515::ERROR_OK) tx_ok++; else tx_fail++; + return (err == MCP2515::ERROR_OK) ? 1 : 0; +} + +void CanPlusMCP2515::rxBurst(uint32_t periodUs) { + const uint32_t now = micros(); + const uint32_t until = now + periodUs; + + if (!burstEnabled) { + burstEnabled = true; + burstUntilUs = until; + tasks.setPeriod(burst, 1); + } else { + if ((int32_t)(until - burstUntilUs) > 0) burstUntilUs = until; + } +} + +void CanPlusMCP2515::poll() { + if (!ready) return; + + if (burstEnabled && (int32_t)(micros() - burstUntilUs) >= 0) { + tasks.setPeriod(burst, 0); + burstEnabled = false; + } + + const uint32_t start = micros(); + + // shorten workload during busts + const int frames = burstEnabled ? (maxFrames >> 1) : maxFrames; + + // drain up to N frames per poll so we don't monopolize the CPU + for (int n = 0; n < frames; n++) { + #if CAN_INT_PIN != OFF + if (digitalRead(CAN_INT_PIN)) break; + #endif + + struct can_frame frame{}; + auto err = mcp2515.readMessage(&frame); + + // If you're NOT using INT, this is your "no more frames" exit + if (err != MCP2515::ERROR_OK) break; + + if (frame.can_id & CAN_EFF_FLAG) { rx_drop_ext++; continue; } + if (frame.can_id & CAN_RTR_FLAG) { rx_drop_rtr++; continue; } + + const uint8_t dlc = frame.can_dlc; + if (dlc < 1 || dlc > 8) { rx_drop_len++; continue; } + + rx_ok++; + callbackProcess((int)(frame.can_id & CAN_SFF_MASK), frame.data, dlc); + + if ((uint32_t)(micros() - start) > budgetUs) break; + } +} + + +CanPlusMCP2515 canPlus; + +#endif diff --git a/src/lib/canPlus/mcp2515/Mcp2515.h b/src/lib/canPlus/mcp2515/Mcp2515.h new file mode 100755 index 0000000..b8acf4e --- /dev/null +++ b/src/lib/canPlus/mcp2515/Mcp2515.h @@ -0,0 +1,31 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN_MCP2515 + +#include "../CanPlusBase.h" + +class CanPlusMCP2515 : public CanPlus { + public: + CanPlusMCP2515(); + void init(); + + int writePacket(int id, const uint8_t *buffer, size_t size) override; + int writePacketRtr(int id, size_t dlc) override; + + void rxBurst(uint32_t periodUs = 2000); + + void poll(); + + private: + uint8_t burst = 0; + uint32_t burstUntilUs = 0; + bool burstEnabled = false; +}; + +extern CanPlusMCP2515 canPlus; + +#endif diff --git a/src/lib/canPlus/san/San.cpp b/src/lib/canPlus/san/San.cpp new file mode 100755 index 0000000..41a523a --- /dev/null +++ b/src/lib/canPlus/san/San.cpp @@ -0,0 +1,124 @@ +// ----------------------------------------------------------------------------------- +// CAN library + +#include "San.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN_SAN + +#include // https://github.com/sandeepmistry/arduino-CAN + +// Pick the right oscillator value for your module: +// - Most shields/modules are 16MHz +// - Some are 8MHz +#ifndef CAN_SAN_CLOCK + #define CAN_SAN_CLOCK 16000000UL // or 8000000UL for 8MHz modules +#endif + +IRAM_ATTR void onReceive(int packetSize) { canPlus.poll(packetSize); } + +CanPlusSan::CanPlusSan() { +} + +void CanPlusSan::init() { + CAN.setClockFrequency(CAN_SAN_CLOCK); + CAN.setPins(CAN_RX_PIN, CAN_TX_PIN); + VF("MSG: CanPlus, CAN Start... "); + ready = CAN.begin(CAN_BAUD); + if (ready) { VLF("success"); } else { VLF("FAILED!"); } + CAN.onReceive(onReceive); +} + +int CanPlusSan::beginPacket(int id) { + if (!ready) return 0; + return CAN.beginPacket(id & 0x7FF); +} + +int CanPlusSan::endPacket() { + if (!ready) return 0; + return CAN.endPacket(); +} + +int CanPlusSan::write(uint8_t byte) { + if (!ready) return 0; + return CAN.write(byte); +} + +int CanPlusSan::write(const uint8_t *buffer, size_t size) { + if (!ready) return 0; + return CAN.write(buffer, size); +} + +int CanPlusSan::writePacket(int id, const uint8_t *buffer, size_t size) { + if (!ready) return 0; + if (size > 8) return 0; + if (size > 0 && buffer == nullptr) return 0; + + const uint16_t sid = (uint16_t)id & 0x7FF; + if (!CAN.beginPacket(sid)) { tx_fail++; return 0; } + + int written = 0; + + if (size > 0) { + if (!buffer) { (void)CAN.endPacket(); tx_fail++; return 0; } + + written = CAN.write((uint8_t*)buffer, size); // only if API requires non-const + if (written != (int)size) { + (void)CAN.endPacket(); + tx_fail++; + return 0; + } + } + + bool ok = CAN.endPacket(); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +int CanPlusSan::writePacketRtr(int id, size_t dlc) { + if (!ready) return 0; + if (dlc > 8) return 0; + + // arduino-CAN supports beginPacket(id, dlc, rtr) in most versions + if (!CAN.beginPacket(id & 0x7FF, (int)dlc, true)) { tx_fail++; return 0; } + + bool ok = CAN.endPacket(); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +IRAM_ATTR void CanPlusSan::poll(int packetSize) { + if (!ready) return; + + bool ext = CAN.packetExtended(); + bool rtr = CAN.packetRtr(); + int packetId = CAN.packetId() & 0x7FF; + + if (ext) { rx_drop_ext++; while (CAN.available()) (void)CAN.read(); return; } + + // Handle RTR early; DLC may legitimately be 0 + if (rtr) { + rx_drop_rtr++; + while (CAN.available()) (void)CAN.read(); + return; + } + + if (packetSize < 1 || packetSize > 8) { + rx_drop_len++; + while (CAN.available()) (void)CAN.read(); + return; + } + + uint8_t buffer[8] = {0}; + for (int i = 0; i < packetSize; i++) { + int b = CAN.read(); + buffer[i] = (b < 0) ? 0 : (uint8_t)b; + } + while (CAN.available()) (void)CAN.read(); + + rx_ok++; + callbackProcess(packetId, buffer, (size_t)packetSize); +} + +CanPlusSan canPlus; + +#endif diff --git a/src/lib/canPlus/san/San.h b/src/lib/canPlus/san/San.h new file mode 100755 index 0000000..3775006 --- /dev/null +++ b/src/lib/canPlus/san/San.h @@ -0,0 +1,29 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN_SAN + +#include "../CanPlusBase.h" + +class CanPlusSan : public CanPlus { + public: + CanPlusSan(); + void init(); + int beginPacket(int id); + int endPacket(); + int write(uint8_t byte); + int write(const uint8_t *buffer, size_t size); + int writePacket(int id, const uint8_t *buffer, size_t size); + int writePacketRtr(int id, size_t dlc); + void poll(int packetSize); + + private: + +}; + +extern CanPlusSan canPlus; + +#endif diff --git a/src/lib/canPlus/teensy4/Can0.cpp b/src/lib/canPlus/teensy4/Can0.cpp new file mode 100755 index 0000000..338077d --- /dev/null +++ b/src/lib/canPlus/teensy4/Can0.cpp @@ -0,0 +1,92 @@ +// ----------------------------------------------------------------------------------- +// CAN library + +#include "Can0.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN0_TEENSY4 + +#include "../../tasks/OnTask.h" + +void canT4Recv0(const CAN_message_t &msg) { canPlus.poll(msg); } +void canT4Poll0() { canPlus.events(); } + +FlexCAN_T4 can0; + +CanPlus0Teesny4::CanPlus0Teesny4() { +} + +void CanPlus0Teesny4::init() { + VF("MSG: CanPlus, CAN_TEENSY4 CAN0 Start..."); + can0.begin(); + can0.setBaudRate(CAN_BAUD); + can0.setMaxMB(16); + can0.enableFIFO(); + can0.enableFIFOInterrupt(); + can0.onReceive(canT4Recv0); + ready = true; + + if (ready) { + VLF("success"); + + VF("MSG: CanPlus, start callback monitor task (rate "); V(CAN_RECV_RATE_MS); VF("ms priority 3)... "); + if (tasks.add(CAN_RECV_RATE_MS, 0, true, 3, canT4Poll0, "SysCan0")) { VLF("success"); } else { VLF("FAILED!"); } + } else { + VLF("FAILED!"); + } +} + +int CanPlus0Teesny4::writePacket(int id, const uint8_t *buffer, size_t size) { + if (!ready) return 0; + if (size > 8) return 0; + if (size > 0 && buffer == nullptr) return 0; + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)size; + msg.flags.remote = 0; + msg.flags.extended = 0; + + for (uint8_t i = 0; i < msg.len; i++) msg.buf[i] = buffer[i]; + + bool ok = can0.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +int CanPlus0Teesny4::writePacketRtr(int id, size_t dlc) { + if (!ready) return 0; + if (dlc > 8) return 0; // DLC may be 0..8 + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)dlc; + msg.flags.remote = 1; + msg.flags.extended = 0; + + bool ok = can0.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +void CanPlus0Teesny4::poll(const CAN_message_t &msg) { + if (!ready) return; + + if (msg.flags.overrun) { rx_drop_len++; return; } + if (msg.flags.extended) { rx_drop_ext++; return; } + if (msg.flags.remote) { rx_drop_rtr++; return; } + + if (msg.len < 1 || msg.len > 8) { rx_drop_len++; return; } + + rx_ok++; + callbackProcess(msg.id & 0x7FF, (uint8_t*)msg.buf, msg.len); +} + +void CanPlus0Teesny4::events() { + can0.events(); +} + +#if CAN_PLUS_DEFAULT == CAN0_TEENSY4 + CanPlus0Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canPlus/teensy4/Can0.h b/src/lib/canPlus/teensy4/Can0.h new file mode 100755 index 0000000..900be3a --- /dev/null +++ b/src/lib/canPlus/teensy4/Can0.h @@ -0,0 +1,38 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN0_TEENSY4 + +#include "../CanPlusBase.h" + +#include // https://github.com/tonton81/FlexCAN_T4 + +#if !defined(ARDUINO_TEENSY40) && !defined(ARDUINO_TEENSY41) + #error "Configuration (Config.h): Setting CAN_PLUS CAN_TEENSY4 requires an Teensy4.0 or Teensy4.1 microcontroller." +#endif + +#ifndef CAN_PLUS_DEFAULT + #define CAN_PLUS_DEFAULT CAN0_TEENSY4 +#endif + +class CanPlus0Teesny4 : public CanPlus { + public: + CanPlus0Teesny4(); + void init(); + int writePacket(int id, const uint8_t *buffer, size_t size); + int writePacketRtr(int id, size_t dlc); + void poll(const CAN_message_t &msg); + void events(); + + private: + +}; + +#if CAN_PLUS_DEFAULT == CAN0_TEENSY4 + extern CanPlus0Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canPlus/teensy4/Can1.cpp b/src/lib/canPlus/teensy4/Can1.cpp new file mode 100755 index 0000000..353d30d --- /dev/null +++ b/src/lib/canPlus/teensy4/Can1.cpp @@ -0,0 +1,92 @@ +// ----------------------------------------------------------------------------------- +// CAN library + +#include "Can1.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN1_TEENSY4 + +#include "../../tasks/OnTask.h" + +void canT4Recv1(const CAN_message_t &msg) { canPlus.poll(msg); } +void canT4Poll1() { canPlus.events(); } + +FlexCAN_T4 can1; + +CanPlus1Teesny4::CanPlus1Teesny4() { +} + +void CanPlus1Teesny4::init() { + VF("MSG: CanPlus, CAN_TEENSY4 CAN1 Start..."); + can1.begin(); + can1.setBaudRate(CAN_BAUD); + can1.setMaxMB(16); + can1.enableFIFO(); + can1.enableFIFOInterrupt(); + can1.onReceive(canT4Recv1); + ready = true; + + if (ready) { + VLF("success"); + + VF("MSG: CanPlus, start callback monitor task (rate "); V(CAN_RECV_RATE_MS); VF("ms priority 3)... "); + if (tasks.add(CAN_RECV_RATE_MS, 0, true, 3, canT4Poll1, "SysCan1")) { VLF("success"); } else { VLF("FAILED!"); } + } else { + VLF("FAILED!"); + } +} + +int CanPlus1Teesny4::writePacket(int id, const uint8_t *buffer, size_t size) { + if (!ready) return 0; + if (size > 8) return 0; + if (size > 0 && buffer == nullptr) return 0; + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)size; + msg.flags.remote = 0; + msg.flags.extended = 0; + + for (uint8_t i = 0; i < msg.len; i++) msg.buf[i] = buffer[i]; + + bool ok = can1.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +int CanPlus1Teesny4::writePacketRtr(int id, size_t dlc) { + if (!ready) return 0; + if (dlc > 8) return 0; // DLC may be 0..8 + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)dlc; + msg.flags.remote = 1; + msg.flags.extended = 0; + + bool ok = can1.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +void CanPlus1Teesny4::poll(const CAN_message_t &msg) { + if (!ready) return; + + if (msg.flags.overrun) { rx_drop_len++; return; } + if (msg.flags.extended) { rx_drop_ext++; return; } + if (msg.flags.remote) { rx_drop_rtr++; return; } + + if (msg.len < 1 || msg.len > 8) { rx_drop_len++; return; } + + rx_ok++; + callbackProcess(msg.id & 0x7FF, (uint8_t*)msg.buf, msg.len); +} + +void CanPlus1Teesny4::events() { + can1.events(); +} + +#if CAN_PLUS_DEFAULT == CAN1_TEENSY4 + CanPlus1Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canPlus/teensy4/Can1.h b/src/lib/canPlus/teensy4/Can1.h new file mode 100755 index 0000000..3052fd7 --- /dev/null +++ b/src/lib/canPlus/teensy4/Can1.h @@ -0,0 +1,38 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN1_TEENSY4 + +#include "../CanPlusBase.h" + +#include // https://github.com/tonton81/FlexCAN_T4 + +#if !defined(ARDUINO_TEENSY40) && !defined(ARDUINO_TEENSY41) + #error "Configuration (Config.h): Setting CAN_PLUS CAN_TEENSY4 requires an Teensy4.0 or Teensy4.1 microcontroller." +#endif + +#ifndef CAN_PLUS_DEFAULT + #define CAN_PLUS_DEFAULT CAN1_TEENSY4 +#endif + +class CanPlus1Teesny4 : public CanPlus { + public: + CanPlus1Teesny4(); + void init(); + int writePacket(int id, const uint8_t *buffer, size_t size); + int writePacketRtr(int id, size_t dlc); + void poll(const CAN_message_t &msg); + void events(); + + private: + +}; + +#if CAN_PLUS_DEFAULT == CAN1_TEENSY4 + extern CanPlus1Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canPlus/teensy4/Can2.cpp b/src/lib/canPlus/teensy4/Can2.cpp new file mode 100755 index 0000000..7c5b170 --- /dev/null +++ b/src/lib/canPlus/teensy4/Can2.cpp @@ -0,0 +1,92 @@ +// ----------------------------------------------------------------------------------- +// CAN library + +#include "Can2.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN2_TEENSY4 + +#include "../../tasks/OnTask.h" + +void canT4Recv2(const CAN_message_t &msg) { canPlus.poll(msg); } +void canT4Poll2() { canPlus.events(); } + +FlexCAN_T4 can2; + +CanPlus2Teesny4::CanPlus2Teesny4() { +} + +void CanPlus2Teesny4::init() { + VF("MSG: CanPlus, CAN_TEENSY4 CAN2 Start..."); + can2.begin(); + can2.setBaudRate(CAN_BAUD); + can2.setMaxMB(16); + can2.enableFIFO(); + can2.enableFIFOInterrupt(); + can2.onReceive(canT4Recv2); + ready = true; + + if (ready) { + VLF("success"); + + VF("MSG: CanPlus, start callback monitor task (rate "); V(CAN_RECV_RATE_MS); VF("ms priority 3)... "); + if (tasks.add(CAN_RECV_RATE_MS, 0, true, 3, canT4Poll2, "SysCan2")) { VLF("success"); } else { VLF("FAILED!"); } + } else { + VLF("FAILED!"); + } +} + +int CanPlus2Teesny4::writePacket(int id, const uint8_t *buffer, size_t size) { + if (!ready) return 0; + if (size > 8) return 0; + if (size > 0 && buffer == nullptr) return 0; + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)size; + msg.flags.remote = 0; + msg.flags.extended = 0; + + for (uint8_t i = 0; i < msg.len; i++) msg.buf[i] = buffer[i]; + + bool ok = can2.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +int CanPlus2Teesny4::writePacketRtr(int id, size_t dlc) { + if (!ready) return 0; + if (dlc > 8) return 0; // DLC may be 0..8 + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)dlc; + msg.flags.remote = 1; + msg.flags.extended = 0; + + bool ok = can2.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +void CanPlus2Teesny4::poll(const CAN_message_t &msg) { + if (!ready) return; + + if (msg.flags.overrun) { rx_drop_len++; return; } + if (msg.flags.extended) { rx_drop_ext++; return; } + if (msg.flags.remote) { rx_drop_rtr++; return; } + + if (msg.len < 1 || msg.len > 8) { rx_drop_len++; return; } + + rx_ok++; + callbackProcess(msg.id & 0x7FF, (uint8_t*)msg.buf, msg.len); +} + +void CanPlus2Teesny4::events() { + can2.events(); +} + +#if CAN_PLUS_DEFAULT == CAN2_TEENSY4 + CanPlus2Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canPlus/teensy4/Can2.h b/src/lib/canPlus/teensy4/Can2.h new file mode 100755 index 0000000..b91678b --- /dev/null +++ b/src/lib/canPlus/teensy4/Can2.h @@ -0,0 +1,38 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN2_TEENSY4 + +#include "../CanPlusBase.h" + +#include // https://github.com/tonton81/FlexCAN_T4 + +#if !defined(ARDUINO_TEENSY40) && !defined(ARDUINO_TEENSY41) + #error "Configuration (Config.h): Setting CAN_PLUS CAN_TEENSY4 requires an Teensy4.0 or Teensy4.1 microcontroller." +#endif + +#ifndef CAN_PLUS_DEFAULT + #define CAN_PLUS_DEFAULT CAN2_TEENSY4 +#endif + +class CanPlus2Teesny4 : public CanPlus { + public: + CanPlus2Teesny4(); + void init(); + int writePacket(int id, const uint8_t *buffer, size_t size); + int writePacketRtr(int id, size_t dlc); + void poll(const CAN_message_t &msg); + void events(); + + private: + +}; + +#if CAN_PLUS_DEFAULT == CAN2_TEENSY4 + extern CanPlus2Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canPlus/teensy4/Can3.cpp b/src/lib/canPlus/teensy4/Can3.cpp new file mode 100755 index 0000000..62999df --- /dev/null +++ b/src/lib/canPlus/teensy4/Can3.cpp @@ -0,0 +1,92 @@ +// ----------------------------------------------------------------------------------- +// CAN library + +#include "Can3.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN3_TEENSY4 + +#include "../../tasks/OnTask.h" + +void canT4Recv3(const CAN_message_t &msg) { canPlus.poll(msg); } +void canT4Poll3() { canPlus.events(); } + +FlexCAN_T4 can3; + +CanPlus3Teesny4::CanPlus3Teesny4() { +} + +void CanPlus3Teesny4::init() { + VF("MSG: CanPlus, CAN_TEENSY4 CAN3 Start..."); + can3.begin(); + can3.setBaudRate(CAN_BAUD); + can3.setMaxMB(16); + can3.enableFIFO(); + can3.enableFIFOInterrupt(); + can3.onReceive(canT4Recv3); + ready = true; + + if (ready) { + VLF("success"); + + VF("MSG: CanPlus, start callback monitor task (rate "); V(CAN_RECV_RATE_MS); VF("ms priority 3)... "); + if (tasks.add(CAN_RECV_RATE_MS, 0, true, 3, canT4Poll3, "SysCan3")) { VLF("success"); } else { VLF("FAILED!"); } + } else { + VLF("FAILED!"); + } +} + +int CanPlus3Teesny4::writePacket(int id, const uint8_t *buffer, size_t size) { + if (!ready) return 0; + if (size > 8) return 0; + if (size > 0 && buffer == nullptr) return 0; + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)size; + msg.flags.remote = 0; + msg.flags.extended = 0; + + for (uint8_t i = 0; i < msg.len; i++) msg.buf[i] = buffer[i]; + + bool ok = can3.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +int CanPlus3Teesny4::writePacketRtr(int id, size_t dlc) { + if (!ready) return 0; + if (dlc > 8) return 0; // DLC may be 0..8 + + CAN_message_t msg{}; + msg.id = id & 0x7FF; + msg.len = (uint8_t)dlc; + msg.flags.remote = 1; + msg.flags.extended = 0; + + bool ok = can3.write(msg); + if (ok) tx_ok++; else tx_fail++; + return ok ? 1 : 0; +} + +void CanPlus3Teesny4::poll(const CAN_message_t &msg) { + if (!ready) return; + + if (msg.flags.overrun) { rx_drop_len++; return; } + if (msg.flags.extended) { rx_drop_ext++; return; } + if (msg.flags.remote) { rx_drop_rtr++; return; } + + if (msg.len < 1 || msg.len > 8) { rx_drop_len++; return; } + + rx_ok++; + callbackProcess(msg.id & 0x7FF, (uint8_t*)msg.buf, msg.len); +} + +void CanPlus3Teesny4::events() { + can3.events(); +} + +#if CAN_PLUS_DEFAULT == CAN3_TEENSY4 + CanPlus3Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canPlus/teensy4/Can3.h b/src/lib/canPlus/teensy4/Can3.h new file mode 100755 index 0000000..9b8d8ae --- /dev/null +++ b/src/lib/canPlus/teensy4/Can3.h @@ -0,0 +1,38 @@ +// ----------------------------------------------------------------------------------- +// CAN library +#pragma once + +#include "../../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS == CAN3_TEENSY4 + +#include "../CanPlusBase.h" + +#include // https://github.com/tonton81/FlexCAN_T4 + +#if !defined(ARDUINO_TEENSY40) && !defined(ARDUINO_TEENSY41) + #error "Configuration (Config.h): Setting CAN_PLUS CAN_TEENSY4 requires an Teensy4.0 or Teensy4.1 microcontroller." +#endif + +#ifndef CAN_PLUS_DEFAULT + #define CAN_PLUS_DEFAULT CAN3_TEENSY4 +#endif + +class CanPlus3Teesny4 : public CanPlus { + public: + CanPlus3Teesny4(); + void init(); + int writePacket(int id, const uint8_t *buffer, size_t size); + int writePacketRtr(int id, size_t dlc); + void poll(const CAN_message_t &msg); + void events(); + + private: + +}; + +#if CAN_PLUS_DEFAULT == CAN3_TEENSY4 + extern CanPlus3Teesny4 canPlus; +#endif + +#endif diff --git a/src/lib/canTransport/CanTransportBase.cpp b/src/lib/canTransport/CanTransportBase.cpp new file mode 100755 index 0000000..f178442 --- /dev/null +++ b/src/lib/canTransport/CanTransportBase.cpp @@ -0,0 +1,227 @@ +// ----------------------------------------------------------------------------------- +// CAN transport (base) +// ----------------------------------------------------------------------------------- + +#include "CanTransportBase.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +#include "../CanPlus/CanPlus.h" + +CanTransport* CanTransport::s_instances[TRANSPORT_CAN_MAX_INSTANCES] = {nullptr}; + +CanTransport::CanTransport(uint16_t requestId, uint16_t responseId, uint8_t dualFrameOpcodeStart) + : reqId(requestId), rspId(responseId), opDualResponse(dualFrameOpcodeStart) { + tidop = 0; + opcode = 0; + + // dual-cursor state + rxLen = 0; + rxPos = 0; + txPos = 0; + + // buffers are fixed arrays; no pointer init needed + // optional: clear for determinism + memset(rxBuf, 0, sizeof(rxBuf)); + memset(txBuf, 0, sizeof(txBuf)); +} + +bool CanTransport::init() { + if (!canPlus.ready) return false; + + if (slot < 0) slot = allocSlot(this); + if (slot < 0) return false; + + return true; +} + +// ----------------------------------------------------------------------------------- +// Dual-cursor pack/unpack helpers (LE on wire) +// +// read* => rxBuf/rxPos/rxLen +// write* => txBuf/txPos/txMax +// ----------------------------------------------------------------------------------- + +bool CanTransport::writeBytes(const uint8_t *src, uint8_t n) { + if (!src) return false; + if (!allowTx(n)) return false; + memcpy(&txBuf[txPos], src, n); + txPos = (uint8_t)(txPos + n); + return true; +} + +int CanTransport::readBytes(uint8_t *dst, uint8_t n) { + if (!dst) return 0; + if (!allowRx(n)) return 0; + memcpy(dst, &rxBuf[rxPos], n); + rxPos = (uint8_t)(rxPos + n); + return (int)n; +} + +bool CanTransport::writeU8(uint8_t v) { + if (!allowTx(1)) return false; + txBuf[txPos++] = v; + return true; +} + +bool CanTransport::readU8(uint8_t &v) { + if (!allowRx(1)) return false; + v = rxBuf[rxPos++]; + return true; +} + +bool CanTransport::writeU16(uint16_t v) { + if (!allowTx(2)) return false; + txBuf[txPos + 0] = (uint8_t)(v & 0xFFu); + txBuf[txPos + 1] = (uint8_t)((v >> 8) & 0xFFu); + txPos = (uint8_t)(txPos + 2); + return true; +} + +bool CanTransport::readU16(uint16_t &v) { + if (!allowRx(2)) return false; + v = (uint16_t)rxBuf[rxPos + 0] + | (uint16_t)((uint16_t)rxBuf[rxPos + 1] << 8); + rxPos = (uint8_t)(rxPos + 2); + return true; +} + +bool CanTransport::writeU32(uint32_t v) { + if (!allowTx(4)) return false; + txBuf[txPos + 0] = (uint8_t)(v & 0xFFu); + txBuf[txPos + 1] = (uint8_t)((v >> 8) & 0xFFu); + txBuf[txPos + 2] = (uint8_t)((v >> 16) & 0xFFu); + txBuf[txPos + 3] = (uint8_t)((v >> 24) & 0xFFu); + txPos = (uint8_t)(txPos + 4); + return true; +} + +bool CanTransport::readU32(uint32_t &v) { + if (!allowRx(4)) return false; + v = (uint32_t)rxBuf[rxPos + 0] + | (uint32_t)((uint32_t)rxBuf[rxPos + 1] << 8) + | (uint32_t)((uint32_t)rxBuf[rxPos + 2] << 16) + | (uint32_t)((uint32_t)rxBuf[rxPos + 3] << 24); + rxPos = (uint8_t)(rxPos + 4); + return true; +} + +bool CanTransport::writeF32(float v) { + uint32_t u = 0; + static_assert(sizeof(float) == sizeof(uint32_t), "float must be 32-bit"); + memcpy(&u, &v, sizeof(u)); + return writeU32(u); +} + +bool CanTransport::readF32(float &v) { + uint32_t u = 0; + if (!readU32(u)) return false; + static_assert(sizeof(float) == sizeof(uint32_t), "float must be 32-bit"); + memcpy(&v, &u, sizeof(v)); + return true; +} + +bool CanTransport::writeFixedI16(float value, int32_t scale) { + if (scale == 0) return false; + if (isnan(value)) return writeI16(NAN_SENTINEL); + + const float scaled = value * (float)scale; + const float adj = (scaled >= 0.0f) ? 0.5f : -0.5f; + const int32_t v = (int32_t)(scaled + adj); + + // reserve INT16_MIN for NaN sentinel; overflow => sentinel + if (v < -32767 || v > 32767) return writeI16(NAN_SENTINEL); + + return writeI16((int16_t)v); +} + +bool CanTransport::readFixedI16(float &value, int32_t scale) { + if (scale == 0) return false; + + int16_t v = 0; + if (!readI16(v)) return false; + + if (v == NAN_SENTINEL) { value = NAN; return true; } + + value = ((float)v) / (float)scale; + return true; +} + +bool CanTransport::writeFixedI16(double value, int32_t scale) { + if (scale == 0) return false; + if (isnan(value)) return writeI16(NAN_SENTINEL); + + const double scaled = value * (double)scale; + const double adj = (scaled >= 0.0) ? 0.5 : -0.5; + const int32_t v = (int32_t)(scaled + adj); + + if (v < -32767 || v > 32767) return writeI16(NAN_SENTINEL); + + return writeI16((int16_t)v); +} + +bool CanTransport::readFixedI16(double &value, int32_t scale) { + if (scale == 0) return false; + + int16_t v = 0; + if (!readI16(v)) return false; + + if (v == NAN_SENTINEL) { value = NAN; return true; } + + value = ((double)v) / (double)scale; + return true; +} + +// -------------------- slot allocation -------------------- + +int8_t CanTransport::allocSlot(CanTransport *p) { + for (int8_t i = 0; i < (int8_t)TRANSPORT_CAN_MAX_INSTANCES; i++) { + if (s_instances[i] == nullptr) { s_instances[i] = p; return i; } + } + return -1; +} + +void CanTransport::taskThunk() { + // Pump RX once per scheduler tick/task invocation + canPlus.rxBurst(); + + // Run all instances (cheap); instances with no work return quickly + for (int i = 0; i < (int)TRANSPORT_CAN_MAX_INSTANCES; i++) { + if (s_instances[i]) s_instances[i]->process(); + } +} + +CanTransport::ThunkFn CanTransport::thunkForSlot(int8_t s) { + switch (s) { + case 0: return &CanTransport::onThunk0; + case 1: return &CanTransport::onThunk1; + case 2: return &CanTransport::onThunk2; + case 3: return &CanTransport::onThunk3; + case 4: return &CanTransport::onThunk4; + case 5: return &CanTransport::onThunk5; + case 6: return &CanTransport::onThunk6; + case 7: return &CanTransport::onThunk7; + default: return &CanTransport::onThunk0; + } +} + +// -------------------- thunks -------------------- + +#define TRANSPORT_CAN_THUNK(N) \ + void CanTransport::onThunk##N(uint8_t data[8], uint8_t len) { \ + CanTransport *p = s_instances[N]; \ + if (p) p->onFrame((const uint8_t*)data, len); \ + } + +TRANSPORT_CAN_THUNK(0) +TRANSPORT_CAN_THUNK(1) +TRANSPORT_CAN_THUNK(2) +TRANSPORT_CAN_THUNK(3) +TRANSPORT_CAN_THUNK(4) +TRANSPORT_CAN_THUNK(5) +TRANSPORT_CAN_THUNK(6) +TRANSPORT_CAN_THUNK(7) + +#undef TRANSPORT_CAN_THUNK + +#endif diff --git a/src/lib/canTransport/CanTransportBase.h b/src/lib/canTransport/CanTransportBase.h new file mode 100755 index 0000000..043a363 --- /dev/null +++ b/src/lib/canTransport/CanTransportBase.h @@ -0,0 +1,224 @@ +// ----------------------------------------------------------------------------------- +// CAN transport (base) +// +// Wire format (payload bytes): +// Request: [ tidop, args... ] (1..8 bytes) +// Response: [ tidop, status, payload... ] (2..8 bytes) +// status: [7 handled][6 numericReply][5 suppressFrame][4..0 commandError] +// +// Notes: +// - Uses global canPlus (single CAN interface typical on Arduino-class targets) +// - This base provides: +// * dual-cursor pack/unpack (RX cursor for reads, TX cursor for writes) +// * slot/thunk dispatch so callbacks can route to instances without globals +// - This base does NOT impose a global/shared tidop. tid/op correlation is a +// per-transaction concern and should be tracked by the client implementation. +// ----------------------------------------------------------------------------------- + +#pragma once + +#include "../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +#include "../commands/CommandErrors.h" + +#ifndef TRANSPORT_CAN_MAX_INSTANCES + #define TRANSPORT_CAN_MAX_INSTANCES 8 +#endif + +class CanTransport { +public: + CanTransport(uint16_t requestId, uint16_t responseId, uint8_t dualFrameOpcodeStart = 32); + + virtual bool init(); + virtual void begin() {} + + inline uint8_t opCode() const { return opcode; } + inline uint16_t requestCanId() const { return reqId; } + inline uint16_t responseCanId() const { return rspId; } + + // ------------------------- + // Raw bytes + // write* => TX cursor + // read* => RX cursor + // ------------------------- + bool writeBytes(const uint8_t *src, uint8_t n); + int readBytes(uint8_t *dst, uint8_t n); + int readRemainingBytes(uint8_t *dst) { if (rxPos > rxLen) return 0; return readBytes(dst, (uint8_t)(rxLen - rxPos)); } + + // ------------------------- + // Unsigned integers (LE on wire) + // ------------------------- + bool writeU8(uint8_t v); + bool readU8(uint8_t &v); + bool writeU16(uint16_t v); + bool readU16(uint16_t &v); + bool writeU32(uint32_t v); + bool readU32(uint32_t &v); + + // ------------------------- + // Signed integers (LE on wire) + // ------------------------- + inline bool writeI16(int16_t v) { return writeU16((uint16_t)v); } + inline bool readI16(int16_t &v) { uint16_t u = 0; if (!readU16(u)) return false; v = (int16_t)u; return true; } + inline bool writeI32(int32_t v) { return writeU32((uint32_t)v); } + inline bool readI32(int32_t &v) { uint32_t u = 0; if (!readU32(u)) return false; v = (int32_t)u; return true; } + + // ------------------------- + // Floats (IEEE-754, LE on wire) + // ------------------------- + bool writeF32(float v); + bool readF32(float &v); + + // ------------------------- + // Fixed-point helpers (int16 payload, NaN sentinel) + // ------------------------- + bool writeFixedI16(float value, int32_t scale); + bool readFixedI16(float &value, int32_t scale); + bool writeFixedI16(double value, int32_t scale); + bool readFixedI16(double &value, int32_t scale); + + // ------------------------- + // RX cursor accessors (default size/offset/remaining map to RX) + // ------------------------- + uint8_t size() const { return rxLen; } + uint8_t offset() const { return rxPos; } + uint8_t remaining() const { return (rxPos <= rxLen) ? (uint8_t)(rxLen - rxPos) : 0; } + +protected: + // Server-side handler: interpret request bytes and decide payload/status. + // Reads come from RX cursor; writes go to TX cursor. + virtual void processCommand() {} + + // Unified ingress point for CAN frames delivered via thunk. + // Default: treat the frame as a request and call processCommand() directly. + // Server implementations that need ISR-safe handling should override onFrame(). + virtual void onFrame(const uint8_t data[8], uint8_t len) { + beginNewRequest(data, len); + beginNewResponse(); + processCommand(); + } + + virtual void onResponse(const uint8_t data[8], uint8_t len) { (void)data; (void)len; } + + // Optional per-instance periodic processing hook (poll loop / task) + virtual void process() {} + + // ------------------------- + // RX/TX buffer binding + cursors + // ------------------------- + inline void beginNewRequest(const uint8_t data[8], uint8_t len) { + if (!data) { rxLen = 0; rxPos = 0; return; } + if (len > (uint8_t)sizeof(rxBuf)) len = (uint8_t)sizeof(rxBuf); + memcpy(rxBuf, data, len); + rxLen = len; + + tidop = (len >= 1) ? rxBuf[0] : 0; + opcode = unpackOp(tidop); + + // args start after tidop + rxPos = 1; + } + + inline void beginNewResponse() { + txPos = 0; + } + + inline uint8_t responsePayloadLen() const { return txPos; } + inline const uint8_t* responsePayload() const { return txBuf; } + inline uint8_t* responsePayloadMut() { return txBuf; } + + // Optional cursor control + inline void resetRx() { rxPos = 0; } + inline bool seekRx(uint8_t newOffset) { if (newOffset > rxLen) return false; rxPos = newOffset; return true; } + inline void resetTx() { txPos = 0; } + inline bool seekTx(uint8_t newOffset) { if (newOffset > txMax) return false; txPos = newOffset; return true; } + + // ------------------------- + // TidOp helpers + // ------------------------- + static inline uint8_t packTidOp(uint8_t tid, uint8_t op) { return (uint8_t)(((tid & 0x07) << 5) | (op & 0x1F)); } + static inline uint8_t unpackTid(uint8_t tidop) { return (uint8_t)((tidop >> 5) & 0x07); } + static inline uint8_t unpackOp (uint8_t tidop) { return (uint8_t)(tidop & 0x1F); } + + // ------------------------- + // Status helpers + // ------------------------- + static inline uint8_t packStatus(bool handled, bool suppressFrame, bool numericReply, CommandError commandError) { + return (uint8_t)((handled ? 0x80:0x00) | + (numericReply ? 0x40:0x00) | + (suppressFrame ? 0x20:0x00) | + ((uint8_t)commandError & 0x1F)); + } + + static inline void unpackStatus(uint8_t status, bool &handled, bool &suppressFrame, bool &numericReply, CommandError &commandError) { + handled = (status & 0x80) != 0; + numericReply = (status & 0x40) != 0; + suppressFrame = (status & 0x20) != 0; + commandError = (CommandError)(status & 0x1F); + } + + // ------------------------- + // Cursor bounds check (RX/TX) + // ------------------------- + inline bool allowRx(uint8_t n) const { + if (rxPos > rxLen) return false; + return (uint8_t)(rxLen - rxPos) >= n; + } + + inline bool allowTx(uint8_t n) const { + if (txPos > txMax) return false; + return (uint8_t)(txMax - txPos) >= n; + } + + // ------------------------- + // Slot/thunk dispatch (no global self) + // ------------------------- + int8_t slot = -1; + static CanTransport* s_instances[TRANSPORT_CAN_MAX_INSTANCES]; + + static int8_t allocSlot(CanTransport *p); + + static void taskThunk(); + + static void onThunk0(uint8_t data[8], uint8_t len); + static void onThunk1(uint8_t data[8], uint8_t len); + static void onThunk2(uint8_t data[8], uint8_t len); + static void onThunk3(uint8_t data[8], uint8_t len); + static void onThunk4(uint8_t data[8], uint8_t len); + static void onThunk5(uint8_t data[8], uint8_t len); + static void onThunk6(uint8_t data[8], uint8_t len); + static void onThunk7(uint8_t data[8], uint8_t len); + + typedef void (*ThunkFn)(uint8_t data[8], uint8_t len); + static ThunkFn thunkForSlot(int8_t s); + + // ------------------------- + // IDs + // ------------------------- + const uint16_t reqId; + const uint16_t rspId; + const uint8_t opDualResponse; + + // ------------------------- + // Dual-cursor buffers + // RX: holds last request/response frame bytes for decoding + // TX: holds payload bytes (max 12) for encoding + // ------------------------- + uint8_t rxBuf[14] = {0}; + volatile uint8_t rxLen = 0; + volatile uint8_t rxPos = 0; + + uint8_t txBuf[12] = {0}; + static constexpr uint8_t txMax = 12; + volatile uint8_t txPos = 0; + + static constexpr int16_t NAN_SENTINEL = (int16_t)0x8000; + + // Per-instance scratch + uint8_t tidop = 0; + uint8_t opcode = 0; +}; + +#endif diff --git a/src/lib/canTransport/CanTransportClient.cpp b/src/lib/canTransport/CanTransportClient.cpp new file mode 100755 index 0000000..fe5ebb5 --- /dev/null +++ b/src/lib/canTransport/CanTransportClient.cpp @@ -0,0 +1,222 @@ +// ----------------------------------------------------------------------------------- +// CAN transport - Client +// ----------------------------------------------------------------------------------- + +#include "CanTransportClient.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +#include + +bool CanTransportClient::init() { + if (!CanTransport::init()) return false; + + if (callbackRegistered) return true; + + if (canPlus.callbackRegisterId((int)responseCanId(), thunkForSlot(slot))) { + callbackRegistered = true; + } + + return callbackRegistered; +} + +// ----------------------------------------------------------------------------- +// Build request (TX cursor writes into base txBuf/txPos) +// ----------------------------------------------------------------------------- +bool CanTransportClient::beginNewRequest(uint8_t opCode) { + if (!canPlus.ready) return false; + + tid = (uint8_t)((tid + 1) & 0x07); + + opcode = (uint8_t)(opCode & 0x1F); + tidop = packTidOp(tid, opcode); + + // Use base TX buffer for request build + resetTx(); + + // First byte on wire is tidop + return writeU8(tidop); +} + +// ----------------------------------------------------------------------------- +// Unified transaction: chooses transact() vs transact2() based on opDualResponse +// On success sets RX cursor to payload start (rxPos=2). +// +// IMPORTANT: on failure, does NOT modify handled/suppressFrame/numericReply/commandError. +// ----------------------------------------------------------------------------- +bool CanTransportClient::transactRequest(bool &handled, bool &suppressFrame, bool &numericReply, CommandError &commandError) { + if (!canPlus.ready) return false; + + // Must have at least tidop written, and <= 8 total + if (txPos < 1 || txPos > 8) return false; + + const uint8_t op = unpackOp(tidop); + + // Build combined response directly into base rxBuf + uint8_t combinedLen = 0; + bool ok = false; + + if (op >= opDualResponse) { + memset(rxBuf, 0, sizeof(rxBuf)); + ok = transact2(tidop, txBuf, txPos, rxBuf, combinedLen); + } else { + memset(rxBuf, 0, 8); + ok = transact(tidop, txBuf, txPos, rxBuf, combinedLen); + } + + if (!ok) return false; + if (combinedLen < 2 || combinedLen > sizeof(rxBuf)) return false; + if (rxBuf[0] != tidop) return false; + + bool h = false; + bool sf = false; + bool nr = true; + CommandError ce = CE_NONE; + unpackStatus(rxBuf[1], h, sf, nr, ce); + + // Commit outputs only on success + handled = h; + suppressFrame = sf; + numericReply = nr; + commandError = ce; + + // Commit RX cursor for payload reads + rxLen = combinedLen; + rxPos = 2; + + return true; +} + +// ----------------------------------------------------------------------------- +// Single-frame transaction +// ----------------------------------------------------------------------------- +bool CanTransportClient::transact(uint8_t expectedTidOp, + const uint8_t *requestPayload, uint8_t requestLen, + uint8_t responsePayload[8], uint8_t &responseLen) { + responseLen = 0; + if (!canPlus.ready) return false; + if (!requestPayload || requestLen < 1 || requestLen > 8) return false; + if (!responsePayload) return false; + + rspMode = 1; + rspReady = false; + rspLenLatched = 0; + rspTidOpLatched = expectedTidOp; + + canPlus.txWait(); + if (!canPlus.writePacket((int)requestCanId(), requestPayload, requestLen)) { + rspMode = 0; + return false; + } + + const uint32_t startUs = micros(); + const uint32_t timeoutUs = (uint32_t)timeoutMs * 1000UL; + + while (!rspReady) { + canPlus.rxBurst(); + if ((uint32_t)(micros() - startUs) > timeoutUs) { + rspMode = 0; + return false; + } + tasks.yield(); + } + + responseLen = rspLenLatched; + memcpy(responsePayload, rspBuf, rspLenLatched); + + rspReady = false; + rspMode = 0; + + return (responseLen >= 2); +} + +// ----------------------------------------------------------------------------- +// Dual-frame transaction (waits for both frames) +// +// Combined buffer: +// [ tidop, status, payload0..., payload1... ] +// where payload0 = dlc0-2 bytes, payload1 = dlc1-2 bytes +// ----------------------------------------------------------------------------- +bool CanTransportClient::transact2(uint8_t expectedTidOp, + const uint8_t *requestPayload, uint8_t requestLen, + uint8_t responsePayload[14], uint8_t &responseLen) { + responseLen = 0; + if (!canPlus.ready) return false; + if (!requestPayload || requestLen < 1 || requestLen > 8) return false; + if (!responsePayload) return false; + + rspMode = 2; + + rspReady = false; + rspLenLatched = 0; + rspTidOpLatched = expectedTidOp; + + rsp1Ready = false; + rsp1LenLatched = 0; + rsp1TidOpLatched = tidopPlus1(expectedTidOp); + + canPlus.txWait(); + if (!canPlus.writePacket((int)requestCanId(), requestPayload, requestLen)) { + rspMode = 0; + return false; + } + + const uint32_t startUs = micros(); + const uint32_t timeoutUs = (uint32_t)timeoutMs * 1000UL; + + while (!(rspReady && rsp1Ready)) { + canPlus.rxBurst(); + if ((uint32_t)(micros() - startUs) > timeoutUs) { + rspMode = 0; + return false; + } + tasks.yield(); + } + + if (!(rspLenLatched >= 2 && rspLenLatched <= 8)) { rspMode = 0; return false; } + if (!(rsp1LenLatched >= 2 && rsp1LenLatched <= 8)) { rspMode = 0; return false; } + if (rspBuf[0] != expectedTidOp) { rspMode = 0; return false; } + if (rsp1Buf[0] != rsp1TidOpLatched) { rspMode = 0; return false; } + + const uint8_t d0 = (uint8_t)(rspLenLatched - 2); // 0..6 + const uint8_t d1 = (uint8_t)(rsp1LenLatched - 2); // 0..6 + + // Build combined: tidop/status from frame0, then payload0 + payload1 + responsePayload[0] = rspBuf[0]; + responsePayload[1] = rspBuf[1]; + + if (d0 > 0) memcpy(&responsePayload[2], &rspBuf[2], d0); + if (d1 > 0) memcpy(&responsePayload[2 + d0], &rsp1Buf[2], d1); + + responseLen = (uint8_t)(2 + d0 + d1); + + rspReady = false; + rsp1Ready = false; + rspMode = 0; + + return (responseLen >= 2); +} + +// ----------------------------------------------------------------------------- +// Response callback (invoked via onFrame()) +// ----------------------------------------------------------------------------- +void CanTransportClient::onResponse(const uint8_t data[8], uint8_t len) { + if (len > 8) len = 8; + if (len < 2) return; + + if (!rspReady && rspMode > 0 && data[0] == rspTidOpLatched) { + memcpy(rspBuf, data, len); + rspLenLatched = len; + rspReady = true; + return; + } + + if (!rsp1Ready && rspMode > 1 && data[0] == rsp1TidOpLatched) { + memcpy(rsp1Buf, data, len); + rsp1LenLatched = len; + rsp1Ready = true; + return; + } +} + +#endif diff --git a/src/lib/canTransport/CanTransportClient.h b/src/lib/canTransport/CanTransportClient.h new file mode 100755 index 0000000..69c3fca --- /dev/null +++ b/src/lib/canTransport/CanTransportClient.h @@ -0,0 +1,84 @@ +// ----------------------------------------------------------------------------------- +// CAN transport - Client (single/dual-frame request/response, 1 in-flight per instance) +// ----------------------------------------------------------------------------------- + +#pragma once + +#include "../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +#include "../tasks/OnTask.h" +#include "../canPlus/CanPlus.h" +#include "CanTransportBase.h" + +class CanTransportClient : public CanTransport { +public: + CanTransportClient(uint16_t requestId, uint16_t responseId, uint8_t dualFrameOpcodeStart = 32) + : CanTransport(requestId, responseId, dualFrameOpcodeStart) {} + + bool init(); + + // Changes timeout, default is 10ms + void setTimeoutMs(uint16_t ms) { timeoutMs = ms; } + +protected: + // Clients do NOT treat inbound frames as requests; they treat them as responses. + void onFrame(const uint8_t data[8], uint8_t len) override { onResponse(data, len); } + + // Start building a request frame using opCode (0..31). + // Resets TX cursor and writes tidop as the first byte. + bool beginNewRequest(uint8_t opCode); + + // Sends the request and waits for the reply. + // Auto-selects single/dual based on opcode >= opDualResponse. + // On success: + // rxLen/rxPos updated; rxPos=2 (payload start after tidop/status) + // readXXX() helpers read from rxBuf via rxPos + bool transactRequest(bool &handled, bool &suppressFrame, bool &numericReply, CommandError &commandError); + +private: + // Response callback invoked via onFrame(). + void onResponse(const uint8_t data[8], uint8_t len) override; + + // Send request and wait for correlated response (tidop match). + bool transact(uint8_t expectedTidOp, + const uint8_t *requestPayload, uint8_t requestLen, + uint8_t responsePayload[8], uint8_t &responseLen); + + // Send request and wait for two correlated responses (tidop and tidop+1 match). + bool transact2(uint8_t expectedTidOp, + const uint8_t *requestPayload, uint8_t requestLen, + uint8_t responsePayload[14], uint8_t &responseLen); + + static inline uint8_t tidopPlus1(uint8_t tidop) { + const uint8_t op = (uint8_t)(tidop & 0x1F); + const uint8_t tid = (uint8_t)((tidop >> 5) & 0x07); + const uint8_t tid1 = (uint8_t)((tid + 1) & 0x07); + return (uint8_t)((tid1 << 5) | op); + } + + uint16_t timeoutMs = 50; + + // Per-instance TID (3 bits). No global coupling. + uint8_t tid = 0; + + bool callbackRegistered = false; + + // in-flight request correlation mode: 0=idle, 1=single, 2=dual + volatile uint8_t rspMode = 0; + + // first in-flight response correlation + volatile bool rspReady = false; + volatile uint8_t rspLenLatched = 0; + volatile uint8_t rspTidOpLatched = 0; + uint8_t rspBuf[8] = {0}; + + // second in-flight response correlation + volatile bool rsp1Ready = false; + volatile uint8_t rsp1LenLatched = 0; + volatile uint8_t rsp1TidOpLatched = 0; + uint8_t rsp1Buf[8] = {0}; +}; + +#endif diff --git a/src/lib/canTransport/CanTransportServer.cpp b/src/lib/canTransport/CanTransportServer.cpp new file mode 100755 index 0000000..35285ca --- /dev/null +++ b/src/lib/canTransport/CanTransportServer.cpp @@ -0,0 +1,108 @@ +// ----------------------------------------------------------------------------------- +// CAN transport - Server +// ----------------------------------------------------------------------------------- + +#include "CanTransportServer.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +bool CanTransportServer::init(bool startTask, uint16_t processPeriodMs) { + if (!CanTransport::init()) return false; + + // Register request callback for this CAN ID; thunk routes to onFrame() + if (!canPlus.callbackRegisterId((int)requestCanId(), thunkForSlot(slot))) return false; + + if (startTask) { + if (!tasks.add(processPeriodMs, 0, true, 5, &CanTransport::taskThunk, "CanTrS")) return false; + } + + return true; +} + +void CanTransportServer::onFrame(const uint8_t data[8], uint8_t len) { + enqueueFromCallback(data, len); +} + +void CanTransportServer::process() { + Frame f; + while (dequeue(f)) { + if (f.len < 1) continue; + + // Bind RX (copies into base rxBuf, sets tidop/opcode, sets rxPos=1) + beginNewRequest(f.data, f.len); + + // Fresh TX response (txPos=0) + beginNewResponse(); + + // Execute command in non-ISR context + processCommand(); + } +} + +void CanTransportServer::sendResponse(bool handled, bool suppressFrame, bool numericReply, CommandError commandError) { + const uint8_t status = packStatus(handled, suppressFrame, numericReply, commandError); + + // Payload bytes written so far (cap at txMax == 12) + uint8_t plen = txPos; + if (plen > txMax) plen = txMax; + + const bool dual = (opcode >= opDualResponse); + + // Single-frame mode: clamp payload to 6 + if (!dual && plen > 6) plen = 6; + + // ---------------- frame 0 ---------------- + uint8_t out0[8] = {0}; + out0[0] = tidop; + out0[1] = status; + + const uint8_t n0 = (plen > 6) ? 6 : plen; // 0..6 + for (uint8_t i = 0; i < n0; ++i) out0[2 + i] = txBuf[i]; + + const uint8_t out0Len = (uint8_t)(2 + n0); // 2..8 + canPlus.txWait(); + (void)canPlus.writePacket((int)responseCanId(), out0, out0Len); + + // ---------------- optional frame 1 ---------------- + if (dual) { + const uint8_t tidop1 = tidopPlus1(tidop); + + uint8_t out1[8] = {0}; + out1[0] = tidop1; + out1[1] = status; + + const uint8_t rem = (plen > 6) ? (uint8_t)(plen - 6) : 0; // 0..6 + const uint8_t n1 = (rem > 6) ? 6 : rem; // 0..6 + + for (uint8_t i = 0; i < n1; ++i) out1[2 + i] = txBuf[6 + i]; + + const uint8_t out1Len = (uint8_t)(2 + n1); // 2..8 (may be 2) + canPlus.txWait(); + (void)canPlus.writePacket((int)responseCanId(), out1, out1Len); + } +} + +void CanTransportServer::enqueueFromCallback(const uint8_t data[8], uint8_t len) { + if (len > 8) len = 8; + if (len < 1) return; + + const uint8_t head = qHead; + const uint8_t next = (uint8_t)((head + 1u) % (uint8_t)TRANSPORT_CAN_SERVER_RXQ); + if (next == qTail) return; // full, drop + + q[head].len = len; + for (uint8_t i = 0; i < len; i++) q[head].data[i] = data[i]; + + qHead = next; +} + +bool CanTransportServer::dequeue(Frame &out) { + const uint8_t tail = qTail; + if (tail == qHead) return false; + + out = q[tail]; + qTail = (uint8_t)((tail + 1u) % (uint8_t)TRANSPORT_CAN_SERVER_RXQ); + return true; +} + +#endif diff --git a/src/lib/canTransport/CanTransportServer.h b/src/lib/canTransport/CanTransportServer.h new file mode 100755 index 0000000..e1e68b6 --- /dev/null +++ b/src/lib/canTransport/CanTransportServer.h @@ -0,0 +1,67 @@ +// ----------------------------------------------------------------------------------- +// CAN transport - Server (ISR-safe enqueue; process() executes commands) +// +// Request: [ tidop, args... ] (1..8 bytes) +// Response0: [ tidop, status, payload0... ] (2..8 bytes) +// Response1: [ tidop+1, status, payload1... ] (2..8 bytes) // dual ops only +// +// Notes: +// - Uses global canPlus +// - ISR-safe: callback only enqueues; process() drains and calls processCommand() +// - process() binds RX via beginNewRequest() (args at rxPos=1) and starts a new TX response via beginNewResponse() +// - Dual-frame responses enabled when: opcode >= opDualResponse +// In dual mode, frame0 and frame1 are always sent, with DLC matching payload length +// ----------------------------------------------------------------------------------- + +#pragma once + +#include "../../Common.h" + +#if defined(CAN_PLUS) && CAN_PLUS != OFF + +#include "../tasks/OnTask.h" +#include "../canPlus/CanPlus.h" +#include "CanTransportBase.h" + +#ifndef TRANSPORT_CAN_SERVER_RXQ + #define TRANSPORT_CAN_SERVER_RXQ 16 +#endif + +class CanTransportServer : public CanTransport { +public: + CanTransportServer(uint16_t requestId, uint16_t responseId, uint8_t dualFrameOpcodeStart = 32) + : CanTransport(requestId, responseId, dualFrameOpcodeStart) {} + + bool init(bool startTask = true, uint16_t processPeriodMs = 2); + +protected: + // ISR ingress: enqueue only (do not execute command processing here) + void onFrame(const uint8_t data[8], uint8_t len) override; + + // Send response using current tidop (captured from the request frame). + // Uses TX payload bytes written so far (0..12) via txBuf/txPos in base. + void sendResponse(bool handled, bool suppressFrame, bool numericReply, CommandError commandError); + + void process() override; + +private: + struct Frame { + uint8_t len; + uint8_t data[8]; + }; + + void enqueueFromCallback(const uint8_t data[8], uint8_t len); + bool dequeue(Frame &out); + + static inline uint8_t tidopPlus1(uint8_t t) { + // increment TID (bits 7..5) modulo 8, opcode unchanged + return (uint8_t)(((t & 0xE0u) + 0x20u) | (t & 0x1Fu)); + } + + // RX queue (written from callback, drained in process()) + volatile uint8_t qHead = 0; + volatile uint8_t qTail = 0; + Frame q[TRANSPORT_CAN_SERVER_RXQ]; +}; + +#endif diff --git a/src/lib/commands/BufferCmds.cpp b/src/lib/commands/BufferCmds.cpp new file mode 100755 index 0000000..7311057 --- /dev/null +++ b/src/lib/commands/BufferCmds.cpp @@ -0,0 +1,100 @@ +// ----------------------------------------------------------------------------------- +// Command processing + +#include +#include "../Constants.h" +#include "BufferCmds.h" + +void Buffer::init(int mountType) { + this->mountType = mountType; +} + +bool Buffer::add(char c) { + // (chr)6 is a special status command for the LX200 protocol + if (c == (char)6 && cbp == 0) { + cb[0] = ':'; cb[1] = (char)6; + if (mountType == 3) cb[2] = 'A'; else cb[2] = 'P'; + cb[3] = 0; cbp = 3; c = '#'; + } + + // ignore spaces/lf/cr + if (c != (char)32 && c != (char)10 && c != (char)13 && c != (char)6) { + if (cbp > bufferSize-2) cbp = bufferSize - 2; + cb[cbp] = c; cbp++; cb[cbp] = (char)0; + } + + if (c == '#') { + // validate the command frame, normal command + if (!(cbp > 1) && ((cb[0] == ':') || (cb[0] == ';')) && (cb[cbp-1] == '#')) { flush(); return false; } + if (((cb[0] == ':') || (cb[0] == ';')) && (cb[1] == '#') && (cb[2] == 0)) { flush(); return false; } + + checksum = (cb[0] == ';'); + if (checksum) { + byte len = strlen(cb) - 1; + + // Minimum length for a valid command is 5 ';CCS#' + if (len < 5) { + flush(); + cb[0] = ':'; cb[1] = (char)6; cb[2] = '0'; cb[3] = '#'; cb[4] = 0; cbp = 4; + return true; + } + + // checksum the data, for example ";111111CCS#". I don't include the command frame in the checksum. The error response is a checksumed string "CK_FAILS#" to request re-transmit. + byte cks = 0; + for (int cksCount0 = 1; cksCount0 < len - 3; cksCount0++) { cks += cb[cksCount0]; } + + char chkSum[12]; + snprintf(chkSum, sizeof(chkSum), "%02X", cks); + + seq = cb[len - 1]; + if (!((chkSum[0] == cb[len - 3]) && (chkSum[1] == cb[len - 2]))) { + flush(); + cb[0] = ':'; cb[1] = (char)6; cb[2] = '0'; cb[3] = '#'; cb[4] = 0; + cbp = 4; + return true; + } + + // remove the sequence char and checksum from string + --len; --len; cb[--len] = 0; + } + + return true; + } else { + return false; + } +} + +char* Buffer::getCmd() { + // the command is either one or two chars in length + cmd[0] = 0; + memmove(cmd, (char *)&cb[1], 2); + cmd[2] = 0; + if (cmd[1] == '#' && cmd[2] == 0) cmd[1] = 0; + return cmd; +} + +char* Buffer::getParameter() { + // the remaining parameter + pb[0] = 0; + if (cbp > 4) memmove(pb, (char *)&cb[3], cbp-4); + pb[cbp-4] = 0; + return pb; +} + +char* Buffer::getSeq() { + static char s[2] = " "; + s[0] = seq; + return s; +} + +bool Buffer::ready() { + if (!cbp) return false; + if (cb[cbp-1] == '#' && cbp == 1) flush(); + return (cb[cbp-1] == '#'); +} + +bool Buffer::flush() { + cbp = 0; + cb[0] = (char)0; + return true; +} diff --git a/src/lib/commands/BufferCmds.h b/src/lib/commands/BufferCmds.h new file mode 100755 index 0000000..b2e4bd5 --- /dev/null +++ b/src/lib/commands/BufferCmds.h @@ -0,0 +1,30 @@ +// ----------------------------------------------------------------------------------- +// Command processing +#pragma once + +class Buffer { + public: + bool checksum = false; + + void init(int mountType); + + bool add(char c); + char* getCmd(); + char* getParameter(); + char* getSeq(); + bool ready(); + bool flush(); + + private: + int mountType = 0; + char channel; + char (*reader)(); + char (*writer)(char c); + + const static int bufferSize = 80; + char cmd[4] = ""; + char pb[bufferSize] = ""; + char cb[bufferSize] = ""; + int cbp = 0; + char seq = 0; +}; diff --git a/src/lib/commands/CommandErrors.h b/src/lib/commands/CommandErrors.h new file mode 100755 index 0000000..3f40289 --- /dev/null +++ b/src/lib/commands/CommandErrors.h @@ -0,0 +1,16 @@ +// ----------------------------------------------------------------------------------- +// Command processing errors +#pragma once + +typedef enum CommandError { + CE_NONE, CE_0, CE_CMD_UNKNOWN, CE_REPLY_UNKNOWN, CE_PARAM_RANGE, CE_PARAM_FORM, + CE_ALIGN_FAIL, CE_ALIGN_NOT_ACTIVE, CE_NOT_PARKED_OR_AT_HOME, CE_PARKED, + CE_PARK_FAILED, CE_NOT_PARKED, CE_NO_PARK_POSITION_SET, CE_GOTO_FAIL, CE_LIBRARY_FULL, + + // goto/guide errors + CE_SLEW_ERR_BELOW_HORIZON, CE_SLEW_ERR_ABOVE_OVERHEAD, CE_SLEW_ERR_IN_STANDBY, + CE_SLEW_ERR_IN_PARK, CE_SLEW_IN_SLEW, CE_SLEW_ERR_OUTSIDE_LIMITS, CE_SLEW_ERR_HARDWARE_FAULT, + CE_SLEW_IN_MOTION, CE_SLEW_ERR_UNSPECIFIED, + + CE_NULL, CE_1 +} CommandError; diff --git a/src/lib/commands/SerialWrapper.cpp b/src/lib/commands/SerialWrapper.cpp new file mode 100755 index 0000000..4eacede --- /dev/null +++ b/src/lib/commands/SerialWrapper.cpp @@ -0,0 +1,413 @@ +// ----------------------------------------------------------------------------------- +// SerialWrapper a single class to allow uniform access to other serial port classes + +#include "SerialWrapper.h" + +#if SERIAL_A == HardSerial + #undef SERIAL_A + HardwareSerial HWSerialA(SERIAL_A_RX, SERIAL_A_TX); + #define SERIAL_A HWSerialA + #define SERIAL_A_RXTX_SET +#endif + +#if SERIAL_B == HardSerial + #undef SERIAL_B + HardwareSerial HWSerialB(SERIAL_B_RX, SERIAL_B_TX); + #define SERIAL_B HWSerialB + #define SERIAL_B_RXTX_SET +#endif + +#if SERIAL_C == HardSerial + #undef SERIAL_C + HardwareSerial HWSerialC(SERIAL_C_RX, SERIAL_C_TX); + #define SERIAL_C HWSerialC + #define SERIAL_C_RXTX_SET +#endif + +#ifdef SERIAL_BT + BluetoothSerial bluetoothSerial; +#endif + +SerialWrapper::SerialWrapper() { + static uint8_t channel = 0; + #ifdef SERIAL_A + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_B + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_C + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_D + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_ST4 + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_BT + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_PIP1 + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_PIP2 + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_PIP3 + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_SIP + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + #ifdef SERIAL_LOCAL + if (!hasChannel(channel)) { thisChannel = channel; setChannel(channel); return; } + channel++; + #endif + UNUSED(channel); +} + +void SerialWrapper::begin() { begin(9600); } + +void SerialWrapper::begin(long baud) { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) + #if defined(SERIAL_A_RX) && defined(SERIAL_A_TX) && !defined(SERIAL_A_RXTX_SET) + SERIAL_A.begin(baud, SERIAL_8N1, SERIAL_A_RX, SERIAL_A_TX); + #else + SERIAL_A.begin(baud); + #endif + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) + #if defined(SERIAL_B_RX) && defined(SERIAL_B_TX) && !defined(SERIAL_B_RXTX_SET) + SERIAL_B.begin(baud, SERIAL_8N1, SERIAL_B_RX, SERIAL_B_TX); + #else + SERIAL_B.begin(baud); + #endif + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) + #if defined(SERIAL_C_RX) && defined(SERIAL_C_TX) && !defined(SERIAL_C_RXTX_SET) + SERIAL_C.begin(baud, SERIAL_8N1, SERIAL_C_RX, SERIAL_C_TX); + #else + SERIAL_C.begin(baud); + #endif + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) + #if defined(SERIAL_D_RX) && defined(SERIAL_D_TX) && !defined(SERIAL_D_RXTX_SET) + SERIAL_D.begin(baud, SERIAL_8N1, SERIAL_D_RX, SERIAL_D_TX); + #else + SERIAL_D.begin(baud); + #endif + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) SERIAL_ST4.begin(baud); + #endif + #ifdef SERIAL_BT + //if (isChannel(channel++)) SERIAL_BT.begin(SERIAL_BT_NAME); // started early in .ino file + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) SERIAL_PIP1.begin(9996, 10L*1000L, true); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) SERIAL_PIP2.begin(9997, 10L*1000L, true); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) SERIAL_PIP3.begin(9998, 10L*1000L, true); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) SERIAL_SIP.begin(9999, 10L*1000L, true); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) SERIAL_LOCAL.begin(baud); + #endif + UNUSED(baud); + UNUSED(channel); +} + +void SerialWrapper::end() { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) SERIAL_A.end(); + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) SERIAL_B.end(); + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) SERIAL_C.end(); + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) SERIAL_D.end(); + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) SERIAL_ST4.end(); + #endif + #ifdef SERIAL_BT + if (isChannel(channel++)) SERIAL_BT.end(); + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) SERIAL_PIP1.end(); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) SERIAL_PIP2.end(); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) SERIAL_PIP3.end(); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) SERIAL_SIP.end(); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) SERIAL_LOCAL.end(); + #endif + UNUSED(channel); +} + +size_t SerialWrapper::write(uint8_t data) { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) return SERIAL_A.write(data); + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) return SERIAL_B.write(data); + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) return SERIAL_C.write(data); + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) return SERIAL_D.write(data); + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) return SERIAL_ST4.write(data); + #endif + #ifdef SERIAL_BT + if (isChannel(channel++)) return SERIAL_BT.write(data); + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) return SERIAL_PIP1.write(data); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) return SERIAL_PIP2.write(data); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) return SERIAL_PIP3.write(data); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) return SERIAL_SIP.write(data); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) return SERIAL_LOCAL.write(data); + #endif + UNUSED(data); + UNUSED(channel); + return -1; +} + +size_t SerialWrapper::write(const uint8_t *data, size_t quantity) { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) return SERIAL_A.write(data, quantity); + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) return SERIAL_B.write(data, quantity); + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) return SERIAL_C.write(data, quantity); + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) return SERIAL_D.write(data, quantity); + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) return SERIAL_ST4.write(data, quantity); + #endif + #ifdef SERIAL_BT + if (isChannel(channel++)) return SERIAL_BT.write(data, quantity); + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) return SERIAL_PIP1.write(data, quantity); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) return SERIAL_PIP2.write(data, quantity); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) return SERIAL_PIP3.write(data, quantity); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) return SERIAL_SIP.write(data, quantity); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) return SERIAL_LOCAL.write(data, quantity); + #endif + UNUSED(data); + UNUSED(quantity); + UNUSED(channel); + return -1; +} + +int SerialWrapper::available() { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) return SERIAL_A.available(); + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) return SERIAL_B.available(); + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) return SERIAL_C.available(); + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) return SERIAL_D.available(); + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) return SERIAL_ST4.available(); + #endif + #ifdef SERIAL_BT + if (isChannel(channel++)) return SERIAL_BT.available(); + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) return SERIAL_PIP1.available(); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) return SERIAL_PIP2.available(); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) return SERIAL_PIP3.available(); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) return SERIAL_SIP.available(); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) return SERIAL_LOCAL.available(); + #endif + UNUSED(channel); + return 0; +} + +int SerialWrapper::read() { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) return SERIAL_A.read(); + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) return SERIAL_B.read(); + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) return SERIAL_C.read(); + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) return SERIAL_D.read(); + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) return SERIAL_ST4.read(); + #endif + #ifdef SERIAL_BT + if (isChannel(channel++)) return SERIAL_BT.read(); + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) return SERIAL_PIP1.read(); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) return SERIAL_PIP2.read(); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) return SERIAL_PIP3.read(); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) return SERIAL_SIP.read(); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) return SERIAL_LOCAL.read(); + #endif + UNUSED(channel); + return -1; +} + +int SerialWrapper::peek() { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) return SERIAL_A.peek(); + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) return SERIAL_B.peek(); + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) return SERIAL_C.peek(); + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) return SERIAL_D.peek(); + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) return SERIAL_ST4.peek(); + #endif + #ifdef SERIAL_BT + if (isChannel(channel++)) return SERIAL_BT.peek(); + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) return SERIAL_PIP1.peek(); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) return SERIAL_PIP2.peek(); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) return SERIAL_PIP3.peek(); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) return SERIAL_SIP.peek(); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) return SERIAL_LOCAL.peek(); + #endif + UNUSED(channel); + return -1; +} + +void SerialWrapper::flush() { + uint8_t channel = 0; + #ifdef SERIAL_A + if (isChannel(channel++)) SERIAL_A.flush(); + #endif + #ifdef SERIAL_B + if (isChannel(channel++)) SERIAL_B.flush(); + #endif + #ifdef SERIAL_C + if (isChannel(channel++)) SERIAL_C.flush(); + #endif + #ifdef SERIAL_D + if (isChannel(channel++)) SERIAL_D.flush(); + #endif + #ifdef SERIAL_ST4 + if (isChannel(channel++)) SERIAL_ST4.flush(); + #endif + #ifdef SERIAL_BT + if (isChannel(channel++)) SERIAL_BT.flush(); + #endif + #ifdef SERIAL_PIP1 + if (isChannel(channel++)) SERIAL_PIP1.flush(); + #endif + #ifdef SERIAL_PIP2 + if (isChannel(channel++)) SERIAL_PIP2.flush(); + #endif + #ifdef SERIAL_PIP3 + if (isChannel(channel++)) SERIAL_PIP3.flush(); + #endif + #ifdef SERIAL_SIP + if (isChannel(channel++)) SERIAL_SIP.flush(); + #endif + #ifdef SERIAL_LOCAL + if (isChannel(channel++)) SERIAL_LOCAL.flush(); + #endif + UNUSED(channel); +} diff --git a/src/lib/commands/SerialWrapper.h b/src/lib/commands/SerialWrapper.h new file mode 100755 index 0000000..8a802da --- /dev/null +++ b/src/lib/commands/SerialWrapper.h @@ -0,0 +1,50 @@ +// ----------------------------------------------------------------------------------- +// SerialWrapper a single class to allow uniform access to other serial port classes +#pragma once + +#include "../../Common.h" + +#include "../serial/Serial_IP_Wifi.h" +#include "../serial/Serial_IP_Ethernet.h" + +#ifdef MOUNT_PRESENT + #if ST4_INTERFACE == ON && ST4_HAND_CONTROL == ON + #include "../serial/Serial_ST4_Master.h" + #endif +#endif +#include "../serial/Serial_Local.h" + +static uint8_t _wrapper_channels = 0; + +#define isChannel(x) (x == thisChannel) + +class SerialWrapper : public Stream { + public: + SerialWrapper(); + + void begin(); + void begin(long baud); + + void end(); + + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *, size_t); + virtual int available(void); + virtual int read(void); + virtual int peek(void); + virtual void flush(void); + + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + + inline bool hasChannel(uint8_t channel) { return bitRead(_wrapper_channels, channel); } + inline void setChannel(uint8_t channel) { bitSet(_wrapper_channels, channel); } + inline void clrChannel(uint8_t channel) { bitClear(_wrapper_channels, channel); } + + using Print::write; + + private: + uint8_t thisChannel = 0; +}; diff --git a/src/lib/commands/commands.ino b/src/lib/commands/commands.ino new file mode 100755 index 0000000..0f91199 --- /dev/null +++ b/src/lib/commands/commands.ino @@ -0,0 +1,4 @@ +// Placeholder file +// Nothing to see here ... +// +// This file is only present so the Arduino IDE can edit the .h file(s) diff --git a/src/lib/convert/Convert.cpp b/src/lib/convert/Convert.cpp new file mode 100755 index 0000000..1f04773 --- /dev/null +++ b/src/lib/convert/Convert.cpp @@ -0,0 +1,339 @@ +//-------------------------------------------------------------------------------------------------- +// convert to/from C strings + +#include "../../Common.h" +#include "Convert.h" + +#if defined(ARDUINO_ARDUINO_NANO33BLE) || \ + defined(__TEENSYDUINO__) || \ + defined(ARDUINO_ARCH_SAMD) || \ + defined(ARDUINO_ARCH_MBED_RP2040) || \ + defined(ARDUINO_ARCH_RP2040) + void sprintF(char *result, const char *source, double f) { + sprintf(result, source, f); + } +#else + void sprintF(char *result, const char *source, double f) { + bool ok = false; + uint8_t mas = 0, frac = 0, len = 0; + char* a; + do { + a = (char*)strchr(source, '%'); + if (a == NULL) return; + // "%f" form + if (a[1] == 'f') { len = 2; ok = true; break; } + // "%.2f" form + if (a[1] == '.' && a[2] >= '0' && a[2] <= '9' && a[3] == 'f') { frac = a[2] - '0'; len = 4; ok = true; break; } + // "%1.2f" form + if (a[1] >= '0' && a[1] <= '9' && a[2] == '.' && a[3] >= '0' && a[3] <= '9' && a[4] == 'f') { + mas = a[1] - '0'; + frac = a[3] - '0'; + len = 5; + ok = true; + break; + } + } while (!ok); + + char s[20]; + dtostrf(f, mas, frac, s); + + uint8_t j = 0; + for (uint8_t i = 0; i < a - source; i++) result[j++] = source[i]; + for (uint8_t i = 0; i < strlen(s); i++) result[j++] = s[i]; + for (uint8_t i = len; i < strlen(a); i++) result[j++] = a[i]; + result[j++] = 0; + } +#endif + +bool Convert::tzToDouble(double *value, char *hm) { + int16_t sign = 1; + int16_t hour, minute = 0; + + if (strlen(hm) < 1 || strlen(hm) > 6) return false; + + // if there's a decimal part just convert it and return + if (strchr(hm, '.') != NULL) { return atof2(hm, value); } + + // determine if the sign was used, skip any '+' + if (hm[0] == '-') { sign = -1; hm++; } else if (hm[0] == '+') hm++; + + // if there's a minute part convert it and mark the end of the hours string + char* m = strchr(hm, ':'); + if (m != NULL) { + m[0] = 0; + m++; + if (strlen(m) != 1 && strlen(m) != 2) return false; + if (!atoi2(m, &minute, false)) return false; + // only these exact minutes are allowed for time zones + if (minute != 45 && minute != 30 && minute != 0) return false; + } + + if (!atoi2(hm, &hour, false)) return false; + *value = sign*(hour + minute/60.0); + + return true; +} + +bool Convert::hmsToDouble(double *value, char *hms, PrecisionMode p) { + char h[3], m[5]; + int16_t hour, minute, decimal = 0; + double second = 0; + + while (*hms == ' ') hms++; + + if (strlen(hms) > 13) hms[13] = 0; + int16_t length = strlen(hms); + + if (p == PM_HIGHEST || p == PM_HIGH) { if (length != 8 && length < 10) return false; } else + if (p == PM_LOW) { if (length != 7) return false; } + + // convert the hours part + h[0] = *hms++; h[1] = *hms++; h[2] = 0; + if (!atoi2(h, &hour, false)) return false; + + // make sure the seperator is an allowed character, then convert the minutes part + if (*hms++ != ':') return false; + m[0] = *hms++; m[1] = *hms++; m[2] = 0; + if (!atoi2(m, &minute, false)) return false; + + if (p == PM_HIGHEST || p == PM_HIGH) { + // make sure the seperator is an allowed character, then convert the seconds part + if (*hms++ != ':') return false; + if (!atof2(hms, &second, false)) return false; + } else + if (p == PM_LOW) { + // make sure the seperator is an allowed character, then convert the decimal minutes part + if (*hms++ != '.') return false; + decimal = (*hms++) - '0'; + } + + if (hour < 0 || hour > 23 || minute < 0 || minute > 59 || decimal < 0 || decimal > 9 || second < 0 || second > 59.9999) return false; + + *value = hour + minute/60.0 + (double)decimal/600.0 + second/3600.0; + return true; +} + +bool Convert::hmsToDouble(double *value, char *hms) { + if (!hmsToDouble(value, hms, PM_HIGHEST)) + if (!hmsToDouble(value, hms, PM_HIGH)) + if (!hmsToDouble(value, hms, PM_LOW)) return false; + return true; +} + +// convert string in format as follows to double (in degrees): +// sDD:MM PM_LOW +// DDD:MM PM_LOW +// sDD*MM PM_LOW +// DDD*MM PM_LOW +// sDD:MM:SS PM_HIGH +// DDD:MM:SS PM_HIGH +// sDD:MM:SS.SSS PM_HIGHEST +bool Convert::dmsToDouble(double *value, char *dms, bool signPresent, PrecisionMode p) { + char d[4], m[5]; + int16_t length; + int16_t sign = 1, deg, minute, lowLimit = 0, highLimit = 360; + double second = 0; + bool secondsOff = false; + + while (*dms == ' ') dms++; + if (strlen(dms) > 13) dms[13] = 0; + length = strlen(dms); + + if (p == PM_HIGHEST || p == PM_HIGH) { if (length != 9 && length < 11) return false; } else + if (p == PM_LOW) { + if (length == 6) secondsOff = true; else + if (length == 9) secondsOff = false; else return false; + } + + // determine if the sign was used and accept it if so, then convert the degrees part + if (signPresent) { + if (*dms == '-') sign = -1; else if (*dms != '+') return false; + dms++; d[0] = *dms++; d[1] = *dms++; d[2] = 0; + if (!atoi2(d, °, false)) return false; + } else { + d[0] = *dms++; d[1] = *dms++; d[2] = *dms++; d[3] = 0; + if (!atoi2(d, °, false)) return false; + } + + // make sure the seperator is an allowed character, then convert the minutes part + if (*dms != ':' && *dms != '*' && *dms != char(223)) return false; else dms++; + m[0] = *dms++; m[1] = *dms++; m[2] = 0; + if (!atoi2(m, &minute, false)) return false; + + if ((p == PM_HIGHEST || p == PM_HIGH) && !secondsOff) { + // make sure the seperator is an allowed character, then convert the seconds part + if (*dms++ != ':' && *dms++ != '\'') return false; + if (!atof2(dms, &second, false)) return false; + } + + if (signPresent) { lowLimit = -90; highLimit = 90; } + if (deg < lowLimit || deg > highLimit || minute < 0 || minute > 59 || second < 0 || second > 59.999) return false; + + *value = sign*(deg + minute/60.0 + second/3600.0); + return true; +} + +// convert string in format as follows to double (in degrees): +// sDD:MM PM_LOW +// DDD:MM PM_LOW +// sDD*MM PM_LOW +// DDD*MM PM_LOW +// sDD:MM:SS PM_HIGH +// DDD:MM:SS PM_HIGH +// sDD:MM:SS.SSS PM_HIGHEST +// automatically detects PrecisionMode +bool Convert::dmsToDouble(double *value, char *dms, bool signPresent) { + if (!dmsToDouble(value, dms, signPresent, PM_HIGHEST)) + if (!dmsToDouble(value, dms, signPresent, PM_HIGH)) + if (!dmsToDouble(value, dms, signPresent, PM_LOW)) return false; + return true; +} + +void Convert::doubleToHms(char *reply, double value, bool signPresent, PrecisionMode p) { + char sign[2] = ""; + double hour, minute, second, decimal = 0; + + // setup formatting, handle adding the sign + if (signPresent) { + if (value < 0) { value = -value; strcpy(sign, "-"); } else strcpy(sign, "+"); + } + + // round to 0.00005 second or 0.5 second, depending on precision mode + if (p == PM_HIGHEST) value += 0.0000000139; else value += 0.000139; + + hour = floor(value); + minute = (value - hour)*60.0; + second = (minute - floor(minute))*60.0; + + // finish off calculations for hms and form string template + char form[]="%s%02d:%02d:%02d.%04d"; + if (p == PM_HIGHEST) { decimal = (second - floor(second))*10000.0; } else + if (p == PM_HIGH) { form[16] = 0; } else + if (p == PM_LOW) { form[16] = 0; form[11] = '.'; form[14] = '1'; second = second/6.0; } else + if (p == PM_LOWEST) { form[11] = 0; second = 0; } + + // set sign and return result string + if (p == PM_HIGHEST) sprintf(reply, form, sign, (int)hour, (int)minute, (int)second, (int)decimal); else + if (p == PM_LOWEST) sprintf(reply, form, sign, (int)hour, (int)minute); else + sprintf(reply, form, sign, (int)hour, (int)minute, (int)second); +} + +// convert double (in degrees) to string in format as follows: +// sDD:MM PM_LOW +// DDD:MM PM_LOW +// sDD*MM PM_LOW +// DDD*MM PM_LOW +// DDD:MM:SS PM_HIGH +// sDD:MM:SS.SSS PM_HIGHEST +void Convert::doubleToDms(char *reply, double value, bool fullRange, bool signPresent, PrecisionMode p) { + char sign[2] = ""; + double deg, minute, second = 0, decimal = 0; + + // setup formatting, handle adding the sign + if (signPresent) { + if (value < 0) { value = -value; strcpy(sign, "-"); } else strcpy(sign, "+"); + } + + // round to 0.0005 arc-second or 0.5 arc-second, depending on precision mode + if (p == PM_HIGHEST) value += 0.000000139; else value += 0.000139; + + deg = floor(value); + minute = (value - deg)*60.0; + second = (minute - floor(minute))*60.0; + + // finish off calculations for dms and form string template + char form[]="%s%02d*%02d:%02d.%03d"; + if (p != PM_HIGHEST) form[16] = 0; else decimal = (second - floor(second))*1000.0; + if (p == PM_LOW) form[11] = 0; + if (fullRange) form[4] = '3'; + + // return result string + if (p == PM_HIGHEST) sprintf(reply, form, sign, (int)deg, (int)minute, (int)second, (int)decimal); else + if (p == PM_HIGH) sprintf(reply, form, sign, (int)deg, (int)minute, (int)second); else + if (p == PM_LOW) sprintf(reply, form, sign, (int)deg, (int)minute); +} + +bool Convert::atoi2(char *a, int16_t *i, bool sign) { + int16_t len = strlen(a); + if (len == 0 || len > 6) return false; + for (int l = 0; l < len; l++) { + if (l == 0 && (a[l] == '+' || a[l] == '-') && sign) continue; + if (a[l] < '0' || a[l] > '9') return false; + } + long l = atol(a); + if (l < -32767 || l > 32768) return false; + *i = l; + return true; +} + +bool Convert::atoi2(char *a, uint8_t *u, bool sign) { + int16_t i = *u; + bool result = atoi2(a, &i, sign); + if (i >= 0 && i <= 255) { *u = i; return result; } else return false; +} + +bool Convert::atof2(char *a, double *d, bool sign) { + int16_t dc = 0; + int16_t len = strlen(a); + for (int l = 0; l < len; l++) { + if (l == 0 && (a[l] == '+' || a[l] == '-') && sign) continue; + if (a[l] == '.') { if (dc == 0) { dc++; continue; } else return false; } + if (a[l] < '0' || a[l] > '9') return false; + } + *d = atof(a); + return true; +} + +void Convert::stripNumericStr(char* s, bool trailingDecimal) { + int pp = -1; + for (unsigned int p = 0; p < strlen(s); p++) if (s[p] == '.') { pp = p; break; } + if (pp != -1) { + int p; + for (p = strlen(s) - 1; p >= pp; p--) { + if (trailingDecimal && p - 1 > 0 && s[p] == '0' && s[p - 1] == '.') break; + if (s[p] != '0') break; + s[p] = 0; + } + if (s[p] == '.') s[p] = 0; + } + if (s[0] == '-' || s[0] == '+') { + while (s[1] == '0' && s[2] != '.' && strlen(s) > 2) memmove(&s[1], &s[2], strlen(s) - 1); + } else { + while (s[0] == '0' && s[1] != '.' && strlen(s) > 1) memmove(&s[0], &s[1], strlen(s)); + } +} + +uint8_t Convert::packSeconds(float t) { + float f = 10.0F; // default is 1 second + if (t <= 0.0162F) f = 0.0F; else // 0.0156 (1/64 second) (0) + if (t <= 0.0313F) f = 1.0F; else // 0.0313 (1/32 second) (1) + if (t <= 0.0625F) f = 2.0F; else // 0.0625 (1/16 second) (2) + if (t <= 1.0F) f = 2.0F + t*8.0F; else // 0.125 seconds to 1 seconds (2 to 10) + if (t <= 10.0F) f = 6.0F + t*4.0F; else // 0.25 seconds to 10 seconds (10 to 46) + if (t <= 30.0F) f = 26.0F + t*2.0F; else // 0.5 seconds to 30 seconds (46 to 86) + if (t <= 120.0F) f = 56.0F + t; else // 1 second to 120 seconds (86 to 176) + if (t <= 600.0F) f = 168.0F + t/15.0F; else // 15 seconds to 300 seconds (176 to 208) + if (t <= 3360.0F) f = 198.0F + t/60.0F; else // 1 minute to 56 minutes (208 to 254) + if (t <= 3600.0F) f = 255.0F; // 1 hour (255) + if (f < 0.0F) f = 0.0F; + if (f > 255.0F) f = 255.0F; + return lroundf(f); +} + +float Convert::unpackSeconds(uint8_t b) { + float f = 1.0; // default is 1 second + if (b == 0) f = 0.016125F; else // 0.0156 (1/64 second) (0) + if (b == 1) f = 0.03125F; else // 0.0313 (1/32 second) (1) + if (b == 2) f = 0.0625F; else // 0.0625 (1/16 second) (2) + if (b <= 10) f = (b - 2.0F)/8.0F; else // 0.125 seconds to 1 seconds (2 to 10) + if (b <= 46) f = (b - 6.0F)/4.0F; else // 0.25 seconds to 10 seconds (10 to 46) + if (b <= 86) f = (b - 26.0F)/2.0F; else // 0.5 seconds to 30 seconds (46 to 86) + if (b <= 176) f = (b - 56.0F); else // 1 second to 120 seconds (86 to 176) + if (b <= 208) f = (b - 168.0F)*15.0F; else // 15 seconds to 300 seconds (176 to 208) + if (b <= 254) f = (b - 198.0F)*60.0F; else // 1 minute to 56 minutes (208 to 254) + if (b == 255) f = 3600.0F; // 1 hour (255) + return f; +} + + +Convert convert; \ No newline at end of file diff --git a/src/lib/convert/Convert.h b/src/lib/convert/Convert.h new file mode 100755 index 0000000..9a5b85c --- /dev/null +++ b/src/lib/convert/Convert.h @@ -0,0 +1,94 @@ +//-------------------------------------------------------------------------------------------------- +// convert to/from C strings +#pragma once + +#include + +enum PrecisionMode {PM_LOWEST, PM_LOW, PM_HIGH, PM_HIGHEST, PM_UNKNOWN}; + +// sprintf like function for float type, limited to one parameter +extern void sprintF(char *result, const char *source, double f); + +class Convert { + public: + // convert timezone string sHH:MM to double (in hours): + // (also handles) sHH + bool tzToDouble(double *value, char *hm); + + // convert string in format as follows to double (in hours): + // HH:MM PM_LOWEST + // HH:MM.M PM_LOW + // HH:MM:SS PM_HIGH (DEFAULT) + // HH:MM:SS.SSSS PM_HIGHEST + bool hmsToDouble(double *value, char *hms, PrecisionMode p); + + // convert string in format as follows to double (in hours): + // HH:MM PM_LOWEST + // HH:MM.M PM_LOW + // HH:MM:SS PM_HIGH (DEFAULT) + // HH:MM:SS.SSSS PM_HIGHEST + // automatically detects PrecisionMode + bool hmsToDouble(double *value, char *hms); + + // convert string in format as follows to double (in degrees): + // sDD:MM PM_LOW + // DDD:MM PM_LOW + // sDD*MM PM_LOW + // DDD*MM PM_LOW + // sDD:MM:SS PM_HIGH + // DDD:MM:SS PM_HIGH + // sDD:MM:SS.SSS PM_HIGHEST + bool dmsToDouble(double *value, char *dms, bool signPresent, PrecisionMode p); + + // convert string in format as follows to double (in degrees): + // sDD:MM PM_LOW + // DDD:MM PM_LOW + // sDD*MM PM_LOW + // DDD*MM PM_LOW + // sDD:MM:SS PM_HIGH + // DDD:MM:SS PM_HIGH + // sDD:MM:SS.SSS PM_HIGHEST + // automatically detects PrecisionMode + bool dmsToDouble(double *value, char *dms, bool signPresent); + + // convert double (in hours) to string in format as follows: + // HH:MM PM_LOWEST + // HH:MM.M PM_LOW + // HH:MM:SS PM_HIGH, + // HH:MM:SS.SSSS PM_HIGHEST + void doubleToHms(char *reply, double value, bool signPresent, PrecisionMode p); + + // convert double (in degrees) to string in format as follows: + // sDD:MM PM_LOW + // DDD:MM PM_LOW + // sDD*MM PM_LOW + // DDD*MM PM_LOW + // DDD:MM:SS PM_HIGH + // sDD:MM:SS.SSS PM_HIGHEST + void doubleToDms(char *reply, double value, bool fullRange, bool signPresent, PrecisionMode p); + + // convert seconds with range from 1/16s to an hour into a packed byte + uint8_t packSeconds(float t); + + // convert a packed byte into seconds with range from 1/16s to an hour + float unpackSeconds(uint8_t b); + + // string to int with error checking + bool atoi2(char *a, int16_t *i, bool sign = true); + + // string to byte with error checking + bool atoi2(char *a, uint8_t *u, bool sign = true); + + // string to double with error checking + bool atof2(char *a, double *d, bool sign = true); + + // removes unnecessary digits from floating point numeric string + void stripNumericStr(char* s, bool trailingDecimal = false); + + PrecisionMode precision = PM_HIGH; + + private: + +}; + +extern Convert convert; \ No newline at end of file diff --git a/src/lib/debug/Debug.cpp b/src/lib/debug/Debug.cpp new file mode 100755 index 0000000..1078840 --- /dev/null +++ b/src/lib/debug/Debug.cpp @@ -0,0 +1,114 @@ +// Helper macros for debugging, with less typing + +#include "Debug.h" + +#if defined(DEBUG) && DEBUG != OFF + #if defined(REMOTE) && DEBUG == REMOTE + bool debugRemoteConnected = false; + void debugPrint(const char* s) { + char s1[255]; + sstrcpy(s1, s); + int length = strlen(s1); + for (unsigned int i = 0; i < length; i++) if (s1[i] == ' ') s1[i] = '_'; + SERIAL_ONSTEP.print(s1); + } + #endif + + #ifdef TASKS_PROFILER_ENABLE + #include "../tasks/OnTask.h" + + #warning "Warning: the OnTask PROFILER is for debugging only, there is siginficant overhead associated with enabling this feature." + #define PROFILER_VT100 ON + + char scale_unit(double *d) { + long v = labs(lround(*d)); + if (v > 999999) { *d /= 1000000.0; return 's'; } // s + if (v > 999) { *d /= 1000.0; return 'm'; } // ms + return 'u'; // us + } + + void profiler() { + char s[120]; + char aau = 'u'; char axu = 'u'; char rtu = 'u'; char rau = 'u'; char rxu = 'u'; + static int count; + static double AAA,AXA,RTT,RAA,RXA; + + static int handle = tasks.getFirstHandle(); + if (!handle) { + AAA /= count; AXA /= count; RAA /= count; RXA /= count; + aau = scale_unit(&AAA); + axu = scale_unit(&AXA); + rtu = scale_unit(&RTT); + rau = scale_unit(&RAA); + rxu = scale_unit(&RXA); + + snprintf(s, sizeof(s), " ----------- ------------ ---------- -------- ----------"); + SERIAL_DEBUG.print(s); Y; + SERIAL_DEBUG.println(); Y; + SERIAL_DEBUG.print("\x1b[K"); + + snprintf(s, sizeof(s), " avgd %5ld%cs avgd %4ld%cs totalled %4ld%cs avgd %4ld%cs avgd %4ld%cs", + lround(AAA), aau, lround(AXA), axu, lround(RTT), rtu, lround(RAA), rau, lround(RXA), rxu); Y; + + SERIAL_DEBUG.print(s); Y; + SERIAL_DEBUG.println(); Y; + SERIAL_DEBUG.print("\x1b[K"); + + count = 0; + handle = tasks.getFirstHandle(); + #if PROFILER_VT100 == ON + SERIAL_DEBUG.print("\x1b[J"); // clear to end of screen + SERIAL_DEBUG.print("\x1b[H"); // cursor to upper left + SERIAL_DEBUG.print("\x1b[K"); // clear to end of line + #endif + SERIAL_DEBUG.println(); Y; + SERIAL_DEBUG.print("\x1b[K"); + snprintf(s, sizeof(s), "Profiler %2d.%02d%s Task Profiler", 1, 0, "b"); + SERIAL_DEBUG.println(s); Y; + SERIAL_DEBUG.print("\x1b[K"); + SERIAL_DEBUG.println(); + SERIAL_DEBUG.print("\x1b[K"); + + AAA = 0; AXA = 0; RTT = 0; RAA = 0; RXA = 0; + } + + char *name = tasks.getNameStr(handle); + + if (!strstr(name, "Profilr")) { + char priority = tasks.getPriority(handle) + '0'; + + double AA = tasks.getArrivalAvg(handle); Y; + AAA += AA; + aau = scale_unit(&AA); + + double AX = tasks.getArrivalMax(handle); Y; + AXA += AX; + axu = scale_unit(&AX); + + double RT = tasks.getRuntimeTotal(handle); Y; + RTT += RT; + rtu = scale_unit(&RT); + + double RTcount = tasks.getRuntimeTotalCount(handle); Y; + double RA; + if (RTcount == 0) RA = 0; else RA = RT/RTcount; + RAA += RA; + rau = scale_unit(&RA); + + double RX = tasks.getRuntimeMax(handle); Y; + RXA += RX; + rxu = scale_unit(&RX); + + count++; + + snprintf(s, sizeof(s), "[%-9sP%c] arrive avg %5ld%cs, max ±%4ld%cs; run total %4ld%cs, avg %4ld%cs, max %4ld%cs", + name, priority, lround(AA), aau, lround(AX), axu, lround(RT), rtu, lround(RA), rau, lround(RX), rxu); Y; + + SERIAL_DEBUG.print(s); Y; + SERIAL_DEBUG.println(); + } + + handle = tasks.getNextHandle(handle); + } + #endif +#endif diff --git a/src/lib/debug/Debug.h b/src/lib/debug/Debug.h new file mode 100755 index 0000000..947fc25 --- /dev/null +++ b/src/lib/debug/Debug.h @@ -0,0 +1,51 @@ +// ----------------------------------------------------------------------------------- +// Helper macros for debugging, with less typing +#pragma once + +#include "../../Common.h" + +#if DEBUG == PROFILER + #define TASKS_PROFILER_ENABLE +#endif + +#if defined(DEBUG) && DEBUG != OFF && DEBUG != PROFILER + #if defined(REMOTE) && DEBUG == REMOTE + // echo strings to OnStep debug interface (supports embedded spaces and cr/lf) + extern void debugPrint(const char* s); + extern bool debugRemoteConnected; + + #define D(x) { if (debugRemoteConnected) { SERIAL_ONSTEP.print(":EC"); SERIAL_ONSTEP.print(x); SERIAL_ONSTEP.print("#"); delay(50); } } + #define DF(x) { if (debugRemoteConnected) { SERIAL_ONSTEP.print(":EC"); debugPrint(x); SERIAL_ONSTEP.print("#"); delay(50); } } + #define DL(x) { if (debugRemoteConnected) { SERIAL_ONSTEP.print(":EC"); SERIAL_ONSTEP.print(x); SERIAL_ONSTEP.print("&#"); delay(50); } } + #define DLF(x) { if (debugRemoteConnected) { SERIAL_ONSTEP.print(":EC"); debugPrint(x); SERIAL_ONSTEP.print("&#"); delay(50); } } + #else + #define D(x) SERIAL_DEBUG.print(x) + #define DF(x) SERIAL_DEBUG.print(F(x)) + #define DL(x) SERIAL_DEBUG.println(x) + #define DLF(x) SERIAL_DEBUG.println(F(x)) + #endif + #define D1(x) { static uint32_t t = 0; if (millis() > t + 1000UL) { D(x); t = millis(); } } + #define DL1(x) { static uint32_t t = 0; if (millis() > t + 1000UL) { DL(x); t = millis(); } } +#else + #define D(x) + #define DF(x) + #define DL(x) + #define D1(x) + #define DL1(x) + #define DLF(x) +#endif + +#if (defined(VERBOSE) && DEBUG == VERBOSE) || (defined(REMOTE) && DEBUG == REMOTE) + #define V(x) D(x) + #define V1(x) D1(x) + #define VF(x) DF(x) + #define VL(x) DL(x) + #define VL1(x) DL1(x) + #define VLF(x) DLF(x) +#else + #define V(x) + #define VF(x) + #define VL(x) + #define VL1(x) + #define VLF(x) +#endif diff --git a/src/lib/encoder/Encoder.h b/src/lib/encoder/Encoder.h new file mode 100755 index 0000000..2c16495 --- /dev/null +++ b/src/lib/encoder/Encoder.h @@ -0,0 +1,15 @@ +// general purpose encoder class +#pragma once + +#include "bissc/As37h39bb.h" +#include "bissc/Jtw24.h" +#include "bissc/Jtw26.h" +#include "bissc/Asc85.h" +#include "ktech/KTech.h" +#include "cwCcw/CwCcw.h" +#include "pulseDir/PulseDir.h" +#include "pulseOnly/PulseOnly.h" +#include "quadrature/Quadrature.h" +#include "quadratureEsp32/QuadratureEsp32.h" +#include "serialBridge/SerialBridge.h" +#include "virtualEnc/VirtualEnc.h" diff --git a/src/lib/encoder/EncoderBase.cpp b/src/lib/encoder/EncoderBase.cpp new file mode 100755 index 0000000..e974c68 --- /dev/null +++ b/src/lib/encoder/EncoderBase.cpp @@ -0,0 +1,228 @@ +// general purpose encoder class + +#include "EncoderBase.h" + +#if AXIS1_ENCODER != OFF || AXIS2_ENCODER != OFF || AXIS3_ENCODER != OFF || \ + AXIS4_ENCODER != OFF || AXIS5_ENCODER != OFF || AXIS6_ENCODER != OFF || \ + AXIS7_ENCODER != OFF || AXIS8_ENCODER != OFF || AXIS9_ENCODER != OFF + +#include "Arduino.h" +#include "../tasks/OnTask.h" + +Encoder *encoder[9]; + +void encoderCallback1() { encoder[0]->poll(); } +void encoderCallback2() { encoder[1]->poll(); } +void encoderCallback3() { encoder[2]->poll(); } +void encoderCallback4() { encoder[3]->poll(); } +void encoderCallback5() { encoder[4]->poll(); } +void encoderCallback6() { encoder[5]->poll(); } +void encoderCallback7() { encoder[6]->poll(); } +void encoderCallback8() { encoder[7]->poll(); } +void encoderCallback9() { encoder[8]->poll(); } + +// get device ready for use +bool Encoder::init() { + if (ready) return true; + if (axis < 1 || axis > 9) return false; + + encoder[axis - 1] = this; + + uint8_t taskHandle = 0; + VF("MSG: Encoder"); V(axis); VF(", start status monitor task... "); + + switch (axis) { + case 1: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback1, "encSta1"); + break; + case 2: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback2, "encSta2"); + break; + case 3: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback3, "encSta3"); + break; + case 4: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback4, "encSta4"); + break; + case 5: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback5, "encSta5"); + break; + case 6: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback6, "encSta6"); + break; + case 7: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback7, "encSta7"); + break; + case 8: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback8, "encSta8"); + break; + case 9: + taskHandle = tasks.add(1000, 0, true, 6, encoderCallback9, "encSta9"); + break; + } + + if (taskHandle) { VL("success"); } else { VLF("FAILED!"); return false; } + + #if ENCODER_FILTER > 0 + ticksToWait = halNsToTicks(ENCODER_FILTER); + if (ticksToWait > 0x7FFFFFFFUL) ticksToWait = 0x7FFFFFFFUL; + #endif + + #if ENCODER_VELOCITY == ON + velTicksPerSec = HAL_TICKS_PER_SECOND(); + + // Cache stop timeout (us) converted to ticks (avoids 64-bit math in readVelocityCps()) + uint64_t stopTicks64 = (uint64_t)ENCODER_VEL_STOP_US * (uint64_t)velTicksPerSec; + stopTicks64 = (stopTicks64 + 500000ULL) / 1000000ULL; // rounded divide by 1e6 + if (stopTicks64 > 0x7FFFFFFFULL) stopTicks64 = 0x7FFFFFFFULL; + velStopTicks = (uint32_t)stopTicks64; + + velLastEdgeTicks = 0; + velLastPeriodTicks = 0; + velLastDir = 0; + + velWinLastCount = (int32_t)count; + velWinLastUs = 0; + velBlendCps = NAN; + velHasEstimate = false; + #endif + + return true; +} + +float Encoder::readVelocityCps() { + #if ENCODER_VELOCITY != ON + return NAN; + #else + + // snapshot state (safe if velLast* can be updated from ISR) + uint32_t lastEdgeTicks, lastPeriodTicks; + int8_t dirSnap; + int32_t countSnap; + + noInterrupts(); + lastEdgeTicks = velLastEdgeTicks; + lastPeriodTicks = velLastPeriodTicks; + dirSnap = velLastDir; + countSnap = count; + interrupts(); + + // cached stop timeout (ticks) computed once in init() + const uint32_t nowTicks = HAL_FAST_TICKS(); + const uint32_t stopTicks = velStopTicks; + + const uint32_t ageTicks = nowTicks - lastEdgeTicks; + const bool stale = (lastEdgeTicks == 0) || (ageTicks > stopTicks); + + // (A) period/reciprocal estimate (best at low speed) + float v_per = NAN; + if (!stale && lastPeriodTicks != 0 && dirSnap != 0) { + v_per = (dirSnap > 0 ? 1.0F : -1.0F) * ((float)velTicksPerSec / (float)lastPeriodTicks); + } + + // (B) windowed count estimate (best at higher speed) + const uint32_t nowUs = micros(); + if (velWinLastUs == 0) { + velWinLastUs = nowUs; + velWinLastCount = countSnap; + + // if we already have a period estimate, expose it immediately (low-speed friendly) + if (!isnan(v_per)) { + velBlendCps = v_per; + velHasEstimate = true; + return velBlendCps; + } + + // otherwise: no usable estimate yet. + return NAN; + } + + const uint32_t dtUs = nowUs - velWinLastUs; + const int32_t dc = countSnap - velWinLastCount; + + if (dtUs >= (uint32_t)ENCODER_VEL_WINDOW_US) { + const float v_cnt = (dtUs != 0) ? (dc*(1000000.0F/(float)dtUs)) : 0.0F; + + // if period estimate is unavailable, do NOT bias toward 0; use v_cnt directly + if (isnan(v_per)) { + velBlendCps = v_cnt; + } else { + float a = fabsf((float)dc) / (float)ENCODER_VEL_BLEND_COUNTS; + if (a < 0.0F) a = 0.0F; + if (a > 1.0F) a = 1.0F; + velBlendCps = a * v_cnt + (1.0F - a) * v_per; + } + + velHasEstimate = true; + + // reset window + velWinLastUs = nowUs; + velWinLastCount = countSnap; + } else { + // before we've ever produced an estimate, allow v_per to seed velBlendCps + if (!velHasEstimate && !isnan(v_per)) { + velBlendCps = v_per; + velHasEstimate = true; + } + + // stop timeout means stop timeout (once we have any estimate) + if (velHasEstimate && stale) { + velBlendCps = 0.0F; + } + } + + return velBlendCps; + +#endif +} + +// update encoder status +void Encoder::poll() { + #if ENCODER_FILTER > 0 + noInterrupts(); + msNow += 1000; + interrupts(); + #endif + + // run once every 5 seconds + if (++tick % 5 == 0) { + + noInterrupts(); + uint32_t w = warn; + warn = 0; + uint32_t e = error; + error = 0; + interrupts(); + + totalWarningCount += w; + totalErrorCount += e; + + uint32_t errors = e; + #ifdef ENCODER_WARN_AS_ERROR + errors += w; + #endif + + // look at how often errors and warnings occur on a given axis + // VF("MSG: Encoder"); V(axis); VF(" status, errors="); V(totalErrorCount); VF(" warnings="); VL(totalWarningCount); + + if (errors > UINT16_MAX) errors = UINT16_MAX; + errorCount[errorCountIndex++ % 12] = errors; + + // sum the errors and set the error flag as required + errors = 0; + for (int i = 0; i < 12; i++) { errors += errorCount[i]; } + + errorState = errors > ENCODER_ERROR_COUNT_THRESHOLD; + + if (errorState != lastErrorState) { + if (errorState) { + DF("WRN: Encoder"); D(axis); DF(" errors, exceeded threshold/minute at "); DL(errors); + } else { + DF("MSG: Encoder"); D(axis); DF(" errors, within threshold/minute at "); DL(errors); + } + lastErrorState = errorState; + } + } +} + +#endif diff --git a/src/lib/encoder/EncoderBase.h b/src/lib/encoder/EncoderBase.h new file mode 100755 index 0000000..50b1826 --- /dev/null +++ b/src/lib/encoder/EncoderBase.h @@ -0,0 +1,292 @@ +// general purpose encoder class +#pragma once + +#include "../../Common.h" + +#include + +// -------------------------------------------------------------------------------------------------------- + +#ifndef ENCODER_VELOCITY + #define ENCODER_VELOCITY OFF +#endif + +#if ENCODER_VELOCITY == ON + + // ENCODER_VEL_WINDOW_US: + // Window length for dc/dt estimate (microseconds). Smaller = more responsive/noisier + // larger = smoother/more lag. Typical: 5000–20000 us for 100–1000 Hz control loops + #ifndef ENCODER_VEL_WINDOW_US + #define ENCODER_VEL_WINDOW_US 10000UL // default 10ms (10000UL) + #endif + + // ENCODER_VEL_STOP_US: + // If no new edges/deltas occur for this long, velocity is forced to 0 + // If velocity drops to 0 at very slow tracking/creep speeds, increase this + // Typical: 300000 us (0.3s) up to 2000000 us (2s) for very low counts/sec systems + #ifndef ENCODER_VEL_STOP_US + #define ENCODER_VEL_STOP_US 300000UL // 0.3s (300000UL) + #endif + + // ENCODER_VEL_BLEND_COUNTS: + // Blend point between period (t/d) and windowed (d/t) estimates + // Larger values favor period estimate longer (better at low counts/sec) + // Typical: 4–12 counts + #ifndef ENCODER_VEL_BLEND_COUNTS + #define ENCODER_VEL_BLEND_COUNTS 4.0F // 4 counts (4.0F) + #endif + + // -------------------------------------------------------------------------- + // An example for very low count systems (copy/paste into your Config.h to override): + // + // #define ENCODER_VEL_WINDOW_US 20000UL // 20ms smoother + // #define ENCODER_VEL_STOP_US 1000000UL // 1.0s avoid false “stops” + // #define ENCODER_VEL_BLEND_COUNTS 8.0F // rely on period estimate longer + // -------------------------------------------------------------------------- + +#endif + +// ENCODER_ERROR_COUNT_THRESHOLD: +// allow up to 20 errors per minute before throwing a fault +#ifndef ENCODER_ERROR_COUNT_THRESHOLD + #define ENCODER_ERROR_COUNT_THRESHOLD 20 // 20 errors (20) +#endif + +// OFF is disabled, ON disregards unexpected quadrature encoder signals, or +// a value > 0 (nanoseconds) disregards repeat signal events for that timer period +#ifndef ENCODER_FILTER + #define ENCODER_FILTER OFF +#endif + +// these should allow time for an encoder signal to stabalize +#if ENCODER_FILTER > 0 + // once a signal state changes don't allow the ISR to run again for ENCODER_FILTER nanoseconds + #define ENCODER_FILTER_UNTIL() do { \ + const uint32_t _msNow = (uint32_t)msNow; \ + if ((int32_t)(_msNow - ticksInvalidMillis) >= 0) { ticksNext = 0; } \ + const uint32_t _ticksNow = HAL_FAST_TICKS(); \ + if (ticksNext && (int32_t)(_ticksNow - ticksNext) < 0) return; \ + ticksNext = _ticksNow + ticksToWait; \ + ticksInvalidMillis = _msNow + 2000U; \ + } while (0) +#endif + +// signal mode for pulse and cw/ccw encoders, default RISING or CHANGE or FALLING +// quadrature encoders ignore this and use CHANGE +#ifndef ENCODER_SIGNAL_MODE + #define ENCODER_SIGNAL_MODE RISING +#endif + +// -------------------------------------------------------------------------------------------------------- + +#ifndef AXIS1_ENCODER + #define AXIS1_ENCODER OFF +#endif +#ifndef AXIS2_ENCODER + #define AXIS2_ENCODER OFF +#endif +#ifndef AXIS3_ENCODER + #define AXIS3_ENCODER OFF +#endif +#ifndef AXIS4_ENCODER + #define AXIS4_ENCODER OFF +#endif +#ifndef AXIS5_ENCODER + #define AXIS5_ENCODER OFF +#endif +#ifndef AXIS6_ENCODER + #define AXIS6_ENCODER OFF +#endif +#ifndef AXIS7_ENCODER + #define AXIS7_ENCODER OFF +#endif +#ifndef AXIS8_ENCODER + #define AXIS8_ENCODER OFF +#endif +#ifndef AXIS9_ENCODER + #define AXIS9_ENCODER OFF +#endif + +#if AXIS1_ENCODER == AS37_H39B_B || AXIS2_ENCODER == AS37_H39B_B + #define HAS_AS37_H39B_B +#endif + +#if AXIS1_ENCODER == LIKA_ASC85 || AXIS2_ENCODER == LIKA_ASC85 + #define HAS_LIKA_ASC85 +#endif + +#if AXIS1_ENCODER == JTW_24BIT || AXIS2_ENCODER == JTW_24BIT + #define HAS_JTW_24BIT +#endif + +#if AXIS1_ENCODER == JTW_26BIT || AXIS2_ENCODER == JTW_26BIT + #define HAS_JTW_26BIT +#endif + +#if defined(HAS_AS37_H39B_B) || defined(HAS_LIKA_ASC85) || defined(HAS_JTW_24BIT) || defined(HAS_JTW_26BIT) + #define HAS_BISS_C +#endif + +#if AXIS1_ENCODER != OFF || AXIS2_ENCODER != OFF || AXIS3_ENCODER != OFF || \ + AXIS4_ENCODER != OFF || AXIS5_ENCODER != OFF || AXIS6_ENCODER != OFF || \ + AXIS7_ENCODER != OFF || AXIS8_ENCODER != OFF || AXIS9_ENCODER != OFF + +class Encoder { + public: + // get device ready for use + virtual bool init(); + + // true when this encoder can report absolute position after boot + virtual bool isAbsolute() const { return false; } + + // set encoder origin + virtual void setOrigin(int32_t counts) { origin = counts; } + + // get current position + virtual int32_t read() { return 0; } + + // set current position + virtual void write(int32_t position) { UNUSED(position); } + + // set the virtual encoder velocity in counts per second + virtual void setVelocity(float countsPerSec) { UNUSED(countsPerSec); } + + // fast, control-grade velocity + virtual float readVelocityCps(); + + // set the virtual encoder direction (-1 is reverse, 1 is forward) + virtual void setDirection(volatile int8_t *direction) { UNUSED(direction); } + + // check error state + virtual bool errorThresholdExceeded() { return errorState; } + + // update encoder status + void poll(); + + // total number of errors + uint32_t totalErrorCount = 0; + + // total number of warnings + uint32_t totalWarningCount = 0; + + // true if encoder count is ready + bool ready = false; + + // true if this is a virtual encoder + bool isVirtual = false; + + // raw count as last read (includes origin for absolute encoders) + volatile int32_t count = 0; + + // raw index as last set + volatile int32_t index = 0; + + // raw origin as last set (for absolute encoders) + uint32_t origin = 0; + + // for encoders with slow readout, available before init()/ready()/hardware detection + virtual bool supportsTimeAlignedMotorSteps() const { return false; } + inline bool hasMotorStepsAtLastRead() const { return hasMotorStepsAtLastReadValue; } + inline long motorStepsAtLastRead() const { return motorStepsAtLastReadValue; } + inline void setMotorStepsPtr(volatile long* p) { motorStepsPtr = p; } + + protected: + #if ENCODER_VELOCITY == ON + inline void IRAM_ATTR velNoteEdge(int8_t dir) { + const uint32_t now = HAL_FAST_TICKS(); + const uint32_t prev = velLastEdgeTicks; + velLastEdgeTicks = now; + velLastDir = dir; + if (prev != 0) velLastPeriodTicks = now - prev; + } + + inline void velNoteDelta(int32_t dc, uint32_t dtUs) { + if (dc == 0 || dtUs == 0) return; + + const uint32_t nowTicks = HAL_FAST_TICKS(); + const uint32_t tps = velTicksPerSec; + const uint32_t absdc = (dc > 0) ? (uint32_t)dc : (uint32_t)(-dc); + + uint64_t dtTicks = (uint64_t)dtUs * (uint64_t)tps; + dtTicks = (dtTicks + 500000ULL) / 1000000ULL; + + uint32_t perTicks = (uint32_t)((dtTicks + (absdc/2)) / absdc); + if (perTicks == 0) perTicks = 1; + + noInterrupts(); + velLastEdgeTicks = nowTicks; + velLastPeriodTicks = perTicks; + velLastDir = (dc > 0) ? 1 : -1; + interrupts(); + } + + // Feed velocity estimator from sampled count reads (for devices without edge ISR info) + // Note: pass raw encoder count (pre-index) to avoid velocity spikes on write()/index changes + inline void velNoteSampledCount(int32_t rawCount) { + const uint32_t nowUs = micros(); + + if (!velSampleInit) { + velSampleInit = true; + velSampleLastCount = rawCount; + velSampleLastUs = nowUs; + return; + } + + const uint32_t dtUs = nowUs - velSampleLastUs; + const int32_t dc = rawCount - velSampleLastCount; + + velSampleLastCount = rawCount; + velSampleLastUs = nowUs; + + if (dc != 0 && dtUs != 0) velNoteDelta(dc, dtUs); + } + + bool velHasEstimate = false; + bool velSampleInit = false; + int32_t velSampleLastCount = 0; + uint32_t velSampleLastUs = 0; + uint32_t velStopTicks = 0; + + volatile uint32_t velLastEdgeTicks = 0; + volatile uint32_t velLastPeriodTicks = 0; + volatile int8_t velLastDir = 0; + uint32_t velTicksPerSec = 1000000UL; + + int32_t velWinLastCount = 0; + uint32_t velWinLastUs = 0; + float velBlendCps = 0.0F; + #endif + + // axis number from 1 to 9 + int16_t axis = 0; + + // accumulator for warning detection + volatile uint32_t warn = 0; + + // accumulator for error detection + volatile uint32_t error = 0; + + // number of errors over the last minute + uint16_t errorCount[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + // keep track of the error state + bool errorState = false; + bool lastErrorState = false; + uint16_t tick = UINT16_MAX; + uint16_t errorCountIndex = UINT16_MAX; + + #if ENCODER_FILTER > 0 + // approximate time keeping for filtering + volatile uint32_t msNow = 0; + volatile uint32_t ticksNext = 0; + volatile uint32_t ticksToWait = 0; + volatile uint32_t ticksInvalidMillis = 0; + #endif + + // for encoders with slow readout + volatile bool hasMotorStepsAtLastReadValue = false; + volatile long motorStepsAtLastReadValue = 0; + volatile long * volatile motorStepsPtr = nullptr; +}; + +#endif diff --git a/src/lib/encoder/bissc/As37h39bb.cpp b/src/lib/encoder/bissc/As37h39bb.cpp new file mode 100755 index 0000000..2bab5d5 --- /dev/null +++ b/src/lib/encoder/bissc/As37h39bb.cpp @@ -0,0 +1,48 @@ +// Broadcom AS37-H39B-B BISS-C encoders + +#include "As37h39bb.h" + +#ifdef HAS_AS37_H39B_B + +#include + +// initialize BiSS-C encoder +As37h39bb::As37h39bb(int16_t axis, int16_t maPin, int16_t sloPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + + this->maPin = maPin; + this->sloPin = sloPin; + + snprintf(encoderName, sizeof(encoderName), "AS37_H39B_B Ax%d", axis); + encoderBits = 23; + encoderMultiTurnBits = 16; + encoderErrInvert = false; +} + +// Designed according protocol description found in as38-H39e-b-an100.pdf and +// Renishaw application note E201D02_02 + +// BiSS-C 6-bit CRC of 41 bit data (16 multi-turn + 23 position + 2 err/wrn) +uint8_t As37h39bb::crc6(uint64_t data) { + uint8_t crc; + uint64_t idx; + idx = ((data >> 36) & 0b011111); + crc = ((data >> 30) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 24) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 18) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 12) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 6) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = (data & 0b111111); + idx = crc ^ CRC6[idx]; + crc = CRC6[idx]; + return (~crc & 0b111111); +} + +#endif diff --git a/src/lib/encoder/bissc/As37h39bb.h b/src/lib/encoder/bissc/As37h39bb.h new file mode 100755 index 0000000..a188368 --- /dev/null +++ b/src/lib/encoder/bissc/As37h39bb.h @@ -0,0 +1,30 @@ +// Broadcom AS37-H39B-B BISS-C encoders +#pragma once + +#include "Bissc.h" + +#ifdef HAS_AS37_H39B_B + + class As37h39bb : public Bissc { + public: + // initialize BiSS-C encoder + As37h39bb(int16_t axis, int16_t maPin, int16_t sloPin); + + private: + + // BiSS-C 6-bit CRC of 41 bit data (16 multi-turn + 23 position + 2 err/wrn) + uint8_t crc6(uint64_t data); + + // BiSS-C 6-bit CRC table (x^6 + x^1 + 1) + uint8_t CRC6[64] = { + 0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09, + 0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11, + 0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39, + 0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21, + 0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A, + 0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32, + 0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A, + 0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02}; + }; + +#endif diff --git a/src/lib/encoder/bissc/Asc85.cpp b/src/lib/encoder/bissc/Asc85.cpp new file mode 100755 index 0000000..a029e6b --- /dev/null +++ b/src/lib/encoder/bissc/Asc85.cpp @@ -0,0 +1,43 @@ +// Lika ASC85 25 BIT BISS-C encoder + +#include "Asc85.h" + +#ifdef HAS_LIKA_ASC85 + +#include + +// initialize BiSS-C encoder +LikaAsc85::LikaAsc85(int16_t axis, int16_t maPin, int16_t sloPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + + this->maPin = maPin; + this->sloPin = sloPin; + + snprintf(encoderName, sizeof(encoderName), "LIKA_ASC85 Ax%d", axis); + encoderBits = 25; + encoderMultiTurnBits = 0; +} + +// Designed according protocol description found in as38-H39e-b-an100.pdf and +// Renishaw application note E201D02_02 + +// BiSS-C 6-bit CRC of 27 bit data (25 position + 2 err/wrn) +uint8_t LikaAsc85::crc6(uint64_t data) { + uint8_t crc; + uint64_t idx; + idx = ((data >> 24) & 0b000111); + crc = ((data >> 18) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 12) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 6) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = (data & 0b111111); + idx = crc ^ CRC6[idx]; + crc = CRC6[idx]; + return (~crc & 0b111111); +} + +#endif diff --git a/src/lib/encoder/bissc/Asc85.h b/src/lib/encoder/bissc/Asc85.h new file mode 100755 index 0000000..70f7df8 --- /dev/null +++ b/src/lib/encoder/bissc/Asc85.h @@ -0,0 +1,30 @@ +// Lika ASC85 25 BIT BISS-C encoder +#pragma once + +#include "Bissc.h" + +#ifdef HAS_LIKA_ASC85 + + class LikaAsc85 : public Bissc { + public: + // initialize BiSS-C encoder + LikaAsc85(int16_t axis, int16_t maPin, int16_t sloPin); + + private: + + // BiSS-C 6-bit CRC of 27 bit data (25 position + 2 err/wrn) + uint8_t crc6(uint64_t data); + + // BiSS-C 6-bit CRC table (x^6 + x^1 + 1) + uint8_t CRC6[64] = { + 0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09, + 0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11, + 0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39, + 0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21, + 0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A, + 0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32, + 0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A, + 0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02}; + }; + +#endif diff --git a/src/lib/encoder/bissc/Bissc.cpp b/src/lib/encoder/bissc/Bissc.cpp new file mode 100755 index 0000000..8a6eb9d --- /dev/null +++ b/src/lib/encoder/bissc/Bissc.cpp @@ -0,0 +1,284 @@ +// BISS-C encoders + +#include "Bissc.h" + +#ifdef HAS_BISS_C + +#if BISSC_SAMPLE_IN_LOW_PHASE == ON + #define BISSC_SAMPLE_LOW_AND_READ(sampleAction) \ + delayNanoseconds(tSample); \ + sampleAction; \ + delayNanoseconds(rate - tSample); \ + digitalWriteF(maPin, HIGH_MA); \ + delayNanoseconds(rate) +#else + #define BISSC_SAMPLE_LOW_AND_READ(sampleAction) \ + delayNanoseconds(rate - tSample); \ + digitalWriteF(maPin, HIGH_MA); \ + delayNanoseconds(tSample); \ + sampleAction; \ + delayNanoseconds(rate - tSample) +#endif + +// get device ready for use +bool Bissc::init() { + if (ready) return true; + if (!Encoder::init()) return false; + if (encoderBits < 8 || encoderBits > 31) return false; + + pinMode(maPin, OUTPUT); + digitalWriteF(maPin, LOW); + pinMode(sloPin, INPUT_PULLUP); + + // get the counts/mask ready + encoderCounts = (uint32_t)(1UL << encoderBits); + encoderHalfCounts = (int32_t)(encoderCounts >> 1); + + const uint8_t lowTurnBits = (encoderBits >= 32) ? 0 : (uint8_t)(32U - encoderBits); + encoderMultiTurnMask = (lowTurnBits >= 32) ? 0xFFFFFFFFUL : ((1UL << lowTurnBits) - 1UL); + + VF("MSG: Encoder "); V(encoderName); VLF(", confirming attempt 1..."); + + // see if the encoder is there + uint32_t temp; + if (!getCount(temp)) { + VF("MSG: Encoder "); V(encoderName); VLF(", confirming attempt 2..."); + if (!getCount(temp)) { + VF("MSG: Encoder "); V(encoderName); VLF(", confirming attempt 3..."); + if (!getCount(temp)) return false; + } + } + + VF("MSG: Encoder "); V(encoderName); VLF(", confirmation success"); + + ready = true; + return true; +} + +// set encoder origin +void Bissc::setOrigin(int32_t counts) { + if (!ready) { DF("WRN: Encoder "); V(encoderName); DLF(", setOrigin(); failed not ready"); return; } + + // origin must always remains in the of range +/- encoderCounts/2 which is the + // normalized absolute encoder range (prior to applying the origin and any index) + if (counts < -encoderHalfCounts || counts >= encoderHalfCounts) { + DF("WRN: Encoder "); V(encoderName); DLF(", setOrigin(); failed counts out of range"); return; + } + + const int32_t temp = index; + index = 0; + origin = 0; + + VLF("----------------------------------------------------------------------------------------"); + VF("MSG: Encoder "); V(encoderName); VLF(", >>> with the mount in the home position <<<"); + VF("MSG: Encoder "); V(encoderName); VF(", if used AXIS"); V(axis); VF("_ENCODER_OFFSET in counts should be set to "); VL(-read()); + origin = counts; + + VF("MSG: Encoder "); V(encoderName); + uint32_t currentCounts; + if (getCount(currentCounts)) { + VF(", counts at home should be 0 and currently are "); VL((int32_t)currentCounts); + } else { + VLF(", getCount read FAILED!"); + } + VLF("----------------------------------------------------------------------------------------"); + + index = temp; +} + +// read encoder position +int32_t Bissc::read() { + if (!ready) return 0; + + int32_t newCount = getCountWithErrorRecovery(); + + if (newCount != INT32_MAX) return newCount + index; else return INT32_MAX; +} + +// write encoder position +void Bissc::write(int32_t position) { + if (!ready) return; + + if (position != INT32_MAX) { + int32_t newCount; + newCount = getCountWithErrorRecovery(true); + if (newCount == INT32_MAX) { + error = ENCODER_ERROR_COUNT_THRESHOLD + 1; + return; + } + + index = position - newCount; + } +} + +// read encoder count with 1 second error recovery +// returns encoder count or INT32_MAX on error +int32_t Bissc::getCountWithErrorRecovery(bool immediate) { + const unsigned long now = millis(); + if (immediate || now - lastValidTime > 2) { + uint32_t newCount = 0; + if (getCount(newCount)) { + lastValidTime = now; + lastValidCount = newCount; + + #if ENCODER_VELOCITY == ON + velNoteSampledCount((int32_t)newCount); + #endif + + } else { + if (now - lastValidTime > 1000U) lastValidCount = INT32_MAX; + } + } + + return lastValidCount; +} + +// get encoder count relative to origin +bool Bissc::getCount(uint32_t &count) { + + // prepare for a reading + foundAck = false; + foundStart = false; + foundCds = false; + + nErr = 1; + nWrn = 1; + frameCrc = 0; + + turns = 0; + count = 0; + + #ifdef ESP32 + static portMUX_TYPE bisscMutex = portMUX_INITIALIZER_UNLOCKED; + taskENTER_CRITICAL(&bisscMutex); + #else + noInterrupts(); + #endif + + getCountBitBang(count); + + #ifdef ESP32 + taskEXIT_CRITICAL(&bisscMutex); + #else + interrupts(); + #endif + + // trap errors + int16_t errors = 0; + + uint64_t encData = (uint64_t)count | ((uint64_t)turns << encoderBits); + encData = (encData << 1) | nErr; + encData = (encData << 1) | nWrn; + if (foundAck && foundStart && foundCds) { + if (crc6(encData) == frameCrc) { + if (encoderWrnInvert ? !nWrn : nWrn) { + D1("WRN: Encoder "); D1(encoderName); DL1(", Warn bit set"); + warn++; + } + if (encoderErrInvert ? !nErr : nErr) { + D1("WRN: Encoder "); D1(encoderName); DL1(", Error bit set"); + errors++; + } else + good++; + } else { + bad++; + #if DEBUG != OFF + float goodBadRatio = (float)bad/(good + bad); + D1("WRN: Encoder "); D1(encoderName); D1(", Crc invalid (overall "); D1(goodBadRatio*100.0F); D1("%"); DL1(")"); + #endif + errors++; + } + } else { + D1("WRN: Encoder "); D1(encoderName); + if (!foundAck) { D1(", Ack"); } else + if (!foundStart) { D1(", Start"); } else + if (!foundCds) { D1(", Cds"); } else + { DL1(" bit invalid"); } + errors++; + } + + if (errors > 0) { + error++; + return false; + } + + #if BISSC_SINGLE_TURN != ON + // combine absolute and low order bits of multi-turn count for a 32 bit count + count = count | ((turns & encoderMultiTurnMask) << encoderBits); + #endif + + // apply origin + count += origin; + + // center count about the +/- half counts range (a signed value stored unsigned) + // note: instead shifts an multiTurn count but for historical reasons thats preferred + count = count - (encoderCounts >> 1); + + #ifdef BISSC_RESOLUTION_DIVISOR + count = (int32_t)count/(int32_t)(BISSC_RESOLUTION_DIVISOR); + #endif + + return true; +} + +IRAM_ATTR void Bissc::getCountBitBang(uint32_t &count) { + + // sync phase + for (int i = 0; i < BISSC_SYNC_PHASE; i++) { + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(if (digitalReadF(sloPin) == LOW_SLO) foundAck = true); + if (foundAck) break; + } + + // if we have an Ack + if (foundAck) { + for (int i = 0; i < BISSC_ACK_PHASE; i++) { + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(if (digitalReadF(sloPin) == HIGH_SLO) foundStart = true); + if (foundStart) break; + } + + // if we have an Start + if (foundStart) { + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(if (digitalReadF(sloPin) == LOW_SLO) foundCds = true); + + // if we have an Cds, read the data + if (foundCds) { + + // the first n bits are the multi-turn count + for (int i = 1; i <= encoderMultiTurnBits; i++) { + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(if (digitalReadF(sloPin) == HIGH_SLO) bitSet(turns, encoderMultiTurnBits - i)); + } + + // the next n bits are the encoder absolute count + for (int i = 1; i <= encoderBits; i++) { + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(if (digitalReadF(sloPin) == HIGH_SLO) bitSet(count, encoderBits - i)); + } + + // the Err bit + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(nErr = (digitalReadF(sloPin) == HIGH_SLO)); + + // the Wrn bit + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(nWrn = (digitalReadF(sloPin) == HIGH_SLO)); + + // the last 6 bits are the CRC + for (int i = 0; i < 6; i++) { + digitalWriteF(maPin, LOW_MA); + BISSC_SAMPLE_LOW_AND_READ(if (digitalReadF(sloPin) == HIGH_SLO) bitSet(frameCrc, 5 - i)); + } + } + } + } + + // send a CDM (invert) + digitalWriteF(maPin, LOW_MA); + delayNanoseconds(rate*4); + digitalWriteF(maPin, HIGH_MA); +} + +#endif diff --git a/src/lib/encoder/bissc/Bissc.h b/src/lib/encoder/bissc/Bissc.h new file mode 100755 index 0000000..0991d64 --- /dev/null +++ b/src/lib/encoder/bissc/Bissc.h @@ -0,0 +1,137 @@ +// BISS-C encoders +#pragma once + +#include "../EncoderBase.h" + +#ifdef HAS_BISS_C + + // designed according protocol description found in as38-H39e-b-an100.pdf + + // similar broadcom devices have range of 10MHz to 80 KHz + #ifndef BISSC_CLOCK_RATE_KHZ + #define BISSC_CLOCK_RATE_KHZ 4000 + #endif + + // default to single turn mode + #ifndef BISSC_SINGLE_TURN + #define BISSC_SINGLE_TURN ON + #endif + + // default sync phase + #ifndef BISSC_SYNC_PHASE + #define BISSC_SYNC_PHASE 20 + #endif + + // default ack phase + #ifndef BISSC_ACK_PHASE + #define BISSC_ACK_PHASE 20 + #endif + + // sample during the low phase after LOW_MA + #ifndef BISSC_SAMPLE_IN_LOW_PHASE + #define BISSC_SAMPLE_IN_LOW_PHASE ON + #endif + + // sample during the high phase after HIGH_MA + #ifndef BISSC_SAMPLE_IN_HIGH_PHASE + #define BISSC_SAMPLE_IN_HIGH_PHASE OFF + #endif + + // sample point within the selected half-clock phase, in quarter-cycle steps (2 is mid-cycle and default) + #ifndef BISSC_TSAMPLE_QUARTERS + #define BISSC_TSAMPLE_QUARTERS 2 + #endif + + #if BISSC_SAMPLE_IN_LOW_PHASE == BISSC_SAMPLE_IN_HIGH_PHASE + #error "Configuration (Config.h): Set exactly one of BISSC_SAMPLE_IN_LOW_PHASE or BISSC_SAMPLE_IN_HIGH_PHASE to ON" + #endif + #if BISSC_TSAMPLE_QUARTERS < 1 + #error "Configuration (Config.h): BISSC_TSAMPLE_QUARTERS must be >= 1" + #endif + #if BISSC_TSAMPLE_QUARTERS > 3 + #error "Configuration (Config.h): BISSC_TSAMPLE_QUARTERS must be between 1 and 3" + #endif + + // resolution adjustment use 2, 4, or 8 (example:) + //#define BISSC_RESOLUTION_DIVISOR 4 + + // allow for inverting the signal state to fix incorrect wiring if needed + #ifndef HIGH_SLO + #define HIGH_SLO HIGH + #endif + #ifndef LOW_SLO + #define LOW_SLO LOW + #endif + #ifndef HIGH_MA + #define HIGH_MA HIGH + #endif + #ifndef LOW_MA + #define LOW_MA LOW + #endif + + class Bissc : public Encoder { + public: + // get device ready for use + bool init(); + + bool isAbsolute() const override { return true; } + + // set encoder origin + void setOrigin(int32_t counts); + + // read encoder position + int32_t read(); + + // write encoder position + void write(int32_t position); + + protected: + // read encoder count with 1 second error recovery + // returns encoder count or INT32_MAX on error + int32_t getCountWithErrorRecovery(bool immediate = false); + + // read encoder count + bool getCount(uint32_t &count); + + // low level bitbang read encoder count + IRAM_ATTR void getCountBitBang(uint32_t &count); + + // the custom crc routine + virtual uint8_t crc6(uint64_t data) = 0; + + bool foundAck = false; + bool foundStart = false; + bool foundCds = false; + + uint8_t nErr = 1; + uint8_t nWrn = 1; + uint8_t frameCrc = 0; + + uint32_t turns = 0; + + // this design allows for 8 to 31 bit encoders + char encoderName[16] = {0}; + uint8_t encoderBits = 0; + uint32_t encoderCounts = 0; + int32_t encoderHalfCounts = 0; + uint8_t encoderMultiTurnBits = 0; + uint32_t encoderMultiTurnMask = 0; + bool encoderWrnInvert = true; + bool encoderErrInvert = true; + + uint32_t good = 0; + uint32_t bad = 0; + uint16_t nvAddress = 0; + + // bit delay in nanoseconds + const uint32_t rate = 500000/BISSC_CLOCK_RATE_KHZ; + const uint32_t tSample = (rate*BISSC_TSAMPLE_QUARTERS)/4; + + int16_t maPin; + int16_t sloPin; + + uint32_t lastValidTime = 0; + int32_t lastValidCount = 0; + }; + +#endif diff --git a/src/lib/encoder/bissc/Jtw24.cpp b/src/lib/encoder/bissc/Jtw24.cpp new file mode 100755 index 0000000..1948082 --- /dev/null +++ b/src/lib/encoder/bissc/Jtw24.cpp @@ -0,0 +1,45 @@ +// JTW 24 BIT BISS-C encoders + +#include "Jtw24.h" + +#ifdef HAS_JTW_24BIT + +#define ENC_BISSC_NAME "JTW24" + +#include + +// initialize BiSS-C encoder +Jtw24::Jtw24(int16_t axis, int16_t maPin, int16_t sloPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + + this->maPin = maPin; + this->sloPin = sloPin; + + snprintf(encoderName, sizeof(encoderName), "JTW_24BIT Ax%d", axis); + encoderBits = 24; + encoderMultiTurnBits = 0; +} + +// Designed according protocol description found in as38-H39e-b-an100.pdf and +// Renishaw application note E201D02_02 + +// BiSS-C 6-bit CRC of 26 bit data (24 position + 2 err/wrn) +uint8_t Jtw24::crc6(uint64_t data) { + uint8_t crc; + uint64_t idx; + idx = ((data >> 24) & 0b000011); + crc = ((data >> 18) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 12) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 6) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = (data & 0b111111); + idx = crc ^ CRC6[idx]; + crc = CRC6[idx]; + return (~crc & 0b111111); +} + +#endif diff --git a/src/lib/encoder/bissc/Jtw24.h b/src/lib/encoder/bissc/Jtw24.h new file mode 100755 index 0000000..7d35817 --- /dev/null +++ b/src/lib/encoder/bissc/Jtw24.h @@ -0,0 +1,30 @@ +// JTW 24 BIT BISS-C encoders +#pragma once + +#include "Bissc.h" + +#ifdef HAS_JTW_24BIT + + class Jtw24 : public Bissc { + public: + // initialize BiSS-C encoder + Jtw24(int16_t axis, int16_t maPin, int16_t sloPin); + + private: + + // BiSS-C 6-bit CRC of 26 bit data (24 position + 2 err/wrn) + uint8_t crc6(uint64_t data); + + // BiSS-C 6-bit CRC table (x^6 + x^1 + 1) + uint8_t CRC6[64] = { + 0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09, + 0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11, + 0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39, + 0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21, + 0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A, + 0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32, + 0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A, + 0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02}; + }; + +#endif diff --git a/src/lib/encoder/bissc/Jtw26.cpp b/src/lib/encoder/bissc/Jtw26.cpp new file mode 100755 index 0000000..ee80da5 --- /dev/null +++ b/src/lib/encoder/bissc/Jtw26.cpp @@ -0,0 +1,45 @@ +// JTW 26 BIT BISS-C encoders + +#include "Jtw26.h" + +#ifdef HAS_JTW_26BIT + +#define ENC_BISSC_NAME "JTW26" + +#include + +// initialize BiSS-C encoder +Jtw26::Jtw26(int16_t axis, int16_t maPin, int16_t sloPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + + this->maPin = maPin; + this->sloPin = sloPin; + + snprintf(encoderName, sizeof(encoderName), "JTW_26BIT Ax%d", axis); + encoderBits = 26; + encoderMultiTurnBits = 0; +} + +// Designed according protocol description found in as38-H39e-b-an100.pdf and +// Renishaw application note E201D02_02 + +// BiSS-C 6-bit CRC of 28 bit data (26 position + 2 err/wrn) +uint8_t Jtw26::crc6(uint64_t data) { + uint8_t crc; + uint64_t idx; + idx = ((data >> 24) & 0b001111); + crc = ((data >> 18) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 12) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = ((data >> 6) & 0b111111); + idx = crc ^ CRC6[idx]; + crc = (data & 0b111111); + idx = crc ^ CRC6[idx]; + crc = CRC6[idx]; + return (~crc & 0b111111); +} + +#endif diff --git a/src/lib/encoder/bissc/Jtw26.h b/src/lib/encoder/bissc/Jtw26.h new file mode 100755 index 0000000..513918a --- /dev/null +++ b/src/lib/encoder/bissc/Jtw26.h @@ -0,0 +1,30 @@ +// JTW 26 BIT BISS-C encoders +#pragma once + +#include "Bissc.h" + +#ifdef HAS_JTW_26BIT + + class Jtw26 : public Bissc { + public: + // initialize BiSS-C encoder + Jtw26(int16_t axis, int16_t maPin, int16_t sloPin); + + private: + + // BiSS-C 6-bit CRC of 50 bit data (24 multi-turn + 24 position + 2 err/wrn) + uint8_t crc6(uint64_t data); + + // BiSS-C 6-bit CRC table (x^6 + x^1 + 1) + uint8_t CRC6[64] = { + 0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09, + 0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11, + 0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39, + 0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21, + 0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A, + 0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32, + 0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A, + 0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02}; + }; + +#endif diff --git a/src/lib/encoder/cwCcw/CwCcw.cpp b/src/lib/encoder/cwCcw/CwCcw.cpp new file mode 100755 index 0000000..6182caa --- /dev/null +++ b/src/lib/encoder/cwCcw/CwCcw.cpp @@ -0,0 +1,180 @@ +// CW/CCW encoders + +#include "CwCcw.h" + +#if AXIS1_ENCODER == CW_CCW || AXIS2_ENCODER == CW_CCW || AXIS3_ENCODER == CW_CCW || \ + AXIS4_ENCODER == CW_CCW || AXIS5_ENCODER == CW_CCW || AXIS6_ENCODER == CW_CCW || \ + AXIS7_ENCODER == CW_CCW || AXIS8_ENCODER == CW_CCW || AXIS9_ENCODER == CW_CCW + +CwCcw *cwCcwInstance[9]; + +#if AXIS1_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis1() { cwCcwInstance[0]->cw(); } + IRAM_ATTR void cwCcw_B_Axis1() { cwCcwInstance[0]->ccw(); } +#endif + +#if AXIS2_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis2() { cwCcwInstance[1]->cw();} + IRAM_ATTR void cwCcw_B_Axis2() { cwCcwInstance[1]->ccw(); } +#endif + +#if AXIS3_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis3() { cwCcwInstance[2]->cw(); } + IRAM_ATTR void cwCcw_B_Axis3() { cwCcwInstance[2]->ccw(); } +#endif + +#if AXIS4_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis4() { cwCcwInstance[3]->cw(); } + IRAM_ATTR void cwCcw_B_Axis4() { cwCcwInstance[3]->ccw(); } +#endif + +#if AXIS5_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis5() { cwCcwInstance[4]->cw(); } + IRAM_ATTR void cwCcw_B_Axis5() { cwCcwInstance[4]->ccw(); } +#endif + +#if AXIS6_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis6() { cwCcwInstance[5]->cw(); } + IRAM_ATTR void cwCcw_B_Axis6() { cwCcwInstance[5]->ccw(); } +#endif + +#if AXIS7_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis7() { cwCcwInstance[6]->cw(); } + IRAM_ATTR void cwCcw_B_Axis7() { cwCcwInstance[6]->ccw(); } +#endif + +#if AXIS8_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis8() { cwCcwInstance[7]->cw(); } + IRAM_ATTR void cwCcw_B_Axis8() { cwCcwInstance[7]->ccw(); } +#endif + +#if AXIS9_ENCODER == CW_CCW + IRAM_ATTR void cwCcw_A_Axis9() { cwCcwInstance[8]->cw(); } + IRAM_ATTR void cwCcw_B_Axis9() { cwCcwInstance[8]->ccw(); } +#endif + +CwCcw::CwCcw(int16_t axis, int16_t cwPin, int16_t ccwPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + axis_index = axis - 1; + + CwPin = cwPin; + CcwPin = ccwPin; + + cwCcwInstance[axis_index] = this; +} + +bool CwCcw::init() { + if (ready) return true; + if (!Encoder::init()) return false; + + pinMode(CwPin, INPUT_PULLUP); + pinMode(CcwPin, INPUT_PULLUP); + + int cwPin = digitalPinToInterrupt(CwPin); + int ccwPin = digitalPinToInterrupt(CcwPin); + + if (cwPin < 0 || ccwPin < 0) { + DF("ERR: Encoder CwCcw"); D(axis); DLF(" init(), couldn't attach interrupt!"); + return false; + } + + switch (axis) { + #if AXIS1_ENCODER == CW_CCW + case 1: + attachInterrupt(cwPin, cwCcw_A_Axis1, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis1, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS2_ENCODER == CW_CCW + case 2: + attachInterrupt(cwPin, cwCcw_A_Axis2, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis2, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS3_ENCODER == CW_CCW + case 3: + attachInterrupt(cwPin, cwCcw_A_Axis3, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis3, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS4_ENCODER == CW_CCW + case 4: + attachInterrupt(cwPin, cwCcw_A_Axis4, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis4, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS5_ENCODER == CW_CCW + case 5: + attachInterrupt(cwPin, cwCcw_A_Axis5, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis5, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS6_ENCODER == CW_CCW + case 6: + attachInterrupt(cwPin, cwCcw_A_Axis6, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis6, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS7_ENCODER == CW_CCW + case 7: + attachInterrupt(cwPin, cwCcw_A_Axis7, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis7, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS8_ENCODER == CW_CCW + case 8: + attachInterrupt(cwPin, cwCcw_A_Axis8, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis8, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS9_ENCODER == CW_CCW + case 9: + attachInterrupt(cwPin, cwCcw_A_Axis9, ENCODER_SIGNAL_MODE); + attachInterrupt(ccwPin, cwCcw_B_Axis9, ENCODER_SIGNAL_MODE); + break; + #endif + } + + ready = true; + return true; +} + +int32_t CwCcw::read() { + if (!ready) return 0; + + return ATOMIC_LOAD(count) + index; +} + +void CwCcw::write(int32_t position) { + if (!ready) return; + + index = position - ATOMIC_LOAD(count); +} + +IRAM_ATTR void CwCcw::cw() { + #if ENCODER_FILTER > 0 + ENCODER_FILTER_UNTIL(); + #endif + + count++; + + #if ENCODER_VELOCITY == ON + velNoteEdge(+1); + #endif +} + +IRAM_ATTR void CwCcw::ccw() { + #if ENCODER_FILTER > 0 + ENCODER_FILTER_UNTIL(); + #endif + + count--; + + #if ENCODER_VELOCITY == ON + velNoteEdge(-1); + #endif +} + +#endif diff --git a/src/lib/encoder/cwCcw/CwCcw.h b/src/lib/encoder/cwCcw/CwCcw.h new file mode 100755 index 0000000..f5432c3 --- /dev/null +++ b/src/lib/encoder/cwCcw/CwCcw.h @@ -0,0 +1,31 @@ +// CW/CCW encoders +#pragma once + +#include "../EncoderBase.h" + +#if AXIS1_ENCODER == CW_CCW || AXIS2_ENCODER == CW_CCW || AXIS3_ENCODER == CW_CCW || \ + AXIS4_ENCODER == CW_CCW || AXIS5_ENCODER == CW_CCW || AXIS6_ENCODER == CW_CCW || \ + AXIS7_ENCODER == CW_CCW || AXIS8_ENCODER == CW_CCW || AXIS9_ENCODER == CW_CCW + +// for example: +// CwCcw encoder1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + +class CwCcw : public Encoder { + public: + CwCcw(int16_t axis, int16_t cwPin, int16_t ccwPin); + bool init(); + + int32_t read(); + void write(int32_t position); + + void cw(); + void ccw(); + + private: + int16_t axis_index; + + int16_t CwPin; + int16_t CcwPin; +}; + +#endif diff --git a/src/lib/encoder/ktech/KTech.cpp b/src/lib/encoder/ktech/KTech.cpp new file mode 100755 index 0000000..2042742 --- /dev/null +++ b/src/lib/encoder/ktech/KTech.cpp @@ -0,0 +1,207 @@ +// KTech in motor encoder (designed for the MS4010v3 with 16bit encoder) + +#include "KTech.h" + +#if AXIS1_ENCODER == KTECH_IME || AXIS2_ENCODER == KTECH_IME || AXIS3_ENCODER == KTECH_IME || \ + AXIS4_ENCODER == KTECH_IME || AXIS5_ENCODER == KTECH_IME || AXIS6_ENCODER == KTECH_IME || \ + AXIS7_ENCODER == KTECH_IME || AXIS8_ENCODER == KTECH_IME || AXIS9_ENCODER == KTECH_IME + +#if !defined(CAN_PLUS) || CAN_PLUS == OFF + #error "No KTECH motor CAN interface!" +#endif + +#include "../../tasks/OnTask.h" +#include "../../canPlus/CanPlus.h" + +KTechIME *ktechEncoderInstance[9]; +void requestKTechEncoderAxis1() { ktechEncoderInstance[0]->requestPosition(); } +void requestKTechEncoderAxis2() { ktechEncoderInstance[1]->requestPosition(); } +void requestKTechEncoderAxis3() { ktechEncoderInstance[2]->requestPosition(); } +void requestKTechEncoderAxis4() { ktechEncoderInstance[3]->requestPosition(); } +void requestKTechEncoderAxis5() { ktechEncoderInstance[4]->requestPosition(); } +void requestKTechEncoderAxis6() { ktechEncoderInstance[5]->requestPosition(); } +void requestKTechEncoderAxis7() { ktechEncoderInstance[6]->requestPosition(); } +void requestKTechEncoderAxis8() { ktechEncoderInstance[7]->requestPosition(); } +void requestKTechEncoderAxis9() { ktechEncoderInstance[8]->requestPosition(); } + +void positionKTechEncoderAxis1(uint8_t data[8]) { ktechEncoderInstance[0]->updatePositionCallback(data); } +void positionKTechEncoderAxis2(uint8_t data[8]) { ktechEncoderInstance[1]->updatePositionCallback(data); } +void positionKTechEncoderAxis3(uint8_t data[8]) { ktechEncoderInstance[2]->updatePositionCallback(data); } +void positionKTechEncoderAxis4(uint8_t data[8]) { ktechEncoderInstance[3]->updatePositionCallback(data); } +void positionKTechEncoderAxis5(uint8_t data[8]) { ktechEncoderInstance[4]->updatePositionCallback(data); } +void positionKTechEncoderAxis6(uint8_t data[8]) { ktechEncoderInstance[5]->updatePositionCallback(data); } +void positionKTechEncoderAxis7(uint8_t data[8]) { ktechEncoderInstance[6]->updatePositionCallback(data); } +void positionKTechEncoderAxis8(uint8_t data[8]) { ktechEncoderInstance[7]->updatePositionCallback(data); } +void positionKTechEncoderAxis9(uint8_t data[8]) { ktechEncoderInstance[8]->updatePositionCallback(data); } + +void velocityKTechEncoderAxis1(uint8_t data[8]) { ktechEncoderInstance[0]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis2(uint8_t data[8]) { ktechEncoderInstance[1]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis3(uint8_t data[8]) { ktechEncoderInstance[2]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis4(uint8_t data[8]) { ktechEncoderInstance[3]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis5(uint8_t data[8]) { ktechEncoderInstance[4]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis6(uint8_t data[8]) { ktechEncoderInstance[5]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis7(uint8_t data[8]) { ktechEncoderInstance[6]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis8(uint8_t data[8]) { ktechEncoderInstance[7]->updateVelocityCallback(data); } +void velocityKTechEncoderAxis9(uint8_t data[8]) { ktechEncoderInstance[8]->updateVelocityCallback(data); } + +KTechIME::KTechIME(int16_t axis) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + axis_index = axis - 1; + + // the motor CAN ID is the axis number! + canID = 0x140 + axis; + + ktechEncoderInstance[axis_index] = this; + + switch (this->axis) { + case 1: callback = requestKTechEncoderAxis1; break; + case 2: callback = requestKTechEncoderAxis2; break; + case 3: callback = requestKTechEncoderAxis3; break; + case 4: callback = requestKTechEncoderAxis4; break; + case 5: callback = requestKTechEncoderAxis5; break; + case 6: callback = requestKTechEncoderAxis6; break; + case 7: callback = requestKTechEncoderAxis7; break; + case 8: callback = requestKTechEncoderAxis8; break; + case 9: callback = requestKTechEncoderAxis9; break; + } +} + +bool KTechIME::init() { + if (ready) return true; + if (!Encoder::init()) return false; + + if (!canPlus.ready) { + DLF("WRN: Encoder KTech_IME"); D(axis); DLF(", no CAN interface!"); + ready = false; + return false; + } + + switch (this->axis) { + case 1: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis1); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis1); + break; + case 2: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis2); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis2); + break; + case 3: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis3); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis3); + break; + case 4: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis4); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis4); + break; + case 5: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis5); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis5); + break; + case 6: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis6); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis6); + break; + case 7: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis7); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis7); + break; + case 8: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis8); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis8); + break; + case 9: + canPlus.callbackRegisterMessage(canID, 0x90, positionKTechEncoderAxis9); + canPlus.callbackRegisterMessage(canID, 0xA2, velocityKTechEncoderAxis9); + break; + } + + VF("MSG: Encoder KTech_IME"); V(axis); VF(", start request position task (rate 500ms priority 6)... "); + char name[] = "SysEnK_"; + name[6] = axis + '0'; + if (tasks.add(500, 0, true, 6, callback, name)) { VLF("success"); } else { VLF("FAILED!"); } + + ready = true; + return true; +} + +int32_t KTechIME::read() { + if (!ready) return 0; + + int32_t result = ATOMIC_LOAD(count) + index; + motorStepsAtLastReadValue = motorStepsAtLastUpdate; + + #if ENCODER_VELOCITY == ON + velNoteSampledCount(ATOMIC_LOAD(count)); + #endif + + return result; +} + +void KTechIME::write(int32_t position) { + if (!ready) return; + + index = position - ATOMIC_LOAD(count); +} + +void KTechIME::requestPosition() { + uint32_t now = millis(); + + if ((uint32_t)(now - lastUpdateByPositionCommand) > 1000UL && + (uint32_t)(now - lastUpdateByVelocityCommand) > 1000UL) error++; + + if ((uint32_t)(now - lastUpdateByVelocityCommand) < 500UL) return; + + static const uint8_t cmd[] = "\x90\x00\x00\x00\x00\x00\x00\x00"; + if (canPlus.txTryLock()) canPlus.writePacket(canID, cmd, 8); +} + +IRAM_ATTR void KTechIME::updatePositionCallback(uint8_t data[8]) { + lastUpdateByPositionCommand = millis(); + + if (motorStepsPtr) { + motorStepsAtLastUpdate = *motorStepsPtr; + hasMotorStepsAtLastReadValue = true; + } + + uint16_t countSingleTurn = (uint16_t)data[2] | ((uint16_t)data[3] << 8); + + static constexpr uint16_t high = (KTECH_SINGLE_TURN*3)/4; + static constexpr uint16_t low = (KTECH_SINGLE_TURN*1)/4; + + // count wraps forward and reverse + if (!firstCall) { + if (lastCountSingleTurn > high && countSingleTurn < low) countTurns++; + if (lastCountSingleTurn < low && countSingleTurn > high) countTurns--; + } + firstCall = false; + + lastCountSingleTurn = countSingleTurn; + count = countSingleTurn + countTurns*KTECH_SINGLE_TURN; +} + +IRAM_ATTR void KTechIME::updateVelocityCallback(uint8_t data[8]) { + lastUpdateByVelocityCommand = millis(); + + if (motorStepsPtr) { + motorStepsAtLastUpdate = *motorStepsPtr; + hasMotorStepsAtLastReadValue = true; + } + + uint16_t countSingleTurn = (uint16_t)data[6] | ((uint16_t)data[7] << 8); + + static constexpr uint16_t high = (KTECH_SINGLE_TURN*3)/4; + static constexpr uint16_t low = (KTECH_SINGLE_TURN*1)/4; + + if (!firstCall) { + if (lastCountSingleTurn > high && countSingleTurn < low) countTurns++; + if (lastCountSingleTurn < low && countSingleTurn > high) countTurns--; + } + firstCall = false; + + lastCountSingleTurn = countSingleTurn; + count = countSingleTurn + countTurns*KTECH_SINGLE_TURN; +} + +#endif diff --git a/src/lib/encoder/ktech/KTech.h b/src/lib/encoder/ktech/KTech.h new file mode 100755 index 0000000..c333aa8 --- /dev/null +++ b/src/lib/encoder/ktech/KTech.h @@ -0,0 +1,48 @@ +// KTech in motor encoder (designed for the MS4010v3 with 16bit encoder) +#pragma once + +#include "../EncoderBase.h" + +#ifndef KTECH_SINGLE_TURN + #define KTECH_SINGLE_TURN 65536 +#endif + +// for example: +// KTechIME encoder1(1); + +#if AXIS1_ENCODER == KTECH_IME || AXIS2_ENCODER == KTECH_IME || AXIS3_ENCODER == KTECH_IME || \ + AXIS4_ENCODER == KTECH_IME || AXIS5_ENCODER == KTECH_IME || AXIS6_ENCODER == KTECH_IME || \ + AXIS7_ENCODER == KTECH_IME || AXIS8_ENCODER == KTECH_IME || AXIS9_ENCODER == KTECH_IME + +class KTechIME : public Encoder { + public: + KTechIME(int16_t axis); + bool init(); + + int32_t read(); + void write(int32_t position); + + bool supportsTimeAlignedMotorSteps() const { return true; } + void updatePositionCallback(uint8_t data[8]); + void updateVelocityCallback(uint8_t data[8]); + + void requestPosition(); + + private: + int16_t axis_index = 0; + + bool firstCall = true; + + int canID = 0; + void (*callback)() = NULL; + + volatile uint16_t lastCountSingleTurn = 0; + volatile long countTurns = 0; + + volatile long motorStepsAtLastUpdate = 0; + + uint32_t lastUpdateByPositionCommand = 0; + uint32_t lastUpdateByVelocityCommand = 0; +}; + +#endif diff --git a/src/lib/encoder/pulseDir/PulseDir.cpp b/src/lib/encoder/pulseDir/PulseDir.cpp new file mode 100755 index 0000000..adb4697 --- /dev/null +++ b/src/lib/encoder/pulseDir/PulseDir.cpp @@ -0,0 +1,159 @@ +// Pulse/Dir encoders + +#include "PulseDir.h" + +#if AXIS1_ENCODER == PULSE_DIR || AXIS2_ENCODER == PULSE_DIR || AXIS3_ENCODER == PULSE_DIR || \ + AXIS4_ENCODER == PULSE_DIR || AXIS5_ENCODER == PULSE_DIR || AXIS6_ENCODER == PULSE_DIR || \ + AXIS7_ENCODER == PULSE_DIR || AXIS8_ENCODER == PULSE_DIR || AXIS9_ENCODER == PULSE_DIR + +PulseDir *pulseDirInstance[9]; + +#if AXIS1_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis1() { pulseDirInstance[0]->pulse(AXIS1_ENCODER_B_PIN); } +#endif + +#if AXIS2_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis2() { pulseDirInstance[1]->pulse(AXIS2_ENCODER_B_PIN); } +#endif + +#if AXIS3_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis3() { pulseDirInstance[2]->pulse(AXIS3_ENCODER_B_PIN); } +#endif + +#if AXIS4_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis4() { pulseDirInstance[3]->pulse(AXIS4_ENCODER_B_PIN); } +#endif + +#if AXIS5_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis5() { pulseDirInstance[4]->pulse(AXIS5_ENCODER_B_PIN); } +#endif + +#if AXIS6_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis6() { pulseDirInstance[5]->pulse(AXIS6_ENCODER_B_PIN); } +#endif + +#if AXIS7_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis7() { pulseDirInstance[6]->pulse(AXIS7_ENCODER_B_PIN); } +#endif + +#if AXIS8_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis8() { pulseDirInstance[7]->pulse(AXIS8_ENCODER_B_PIN); } +#endif + +#if AXIS9_ENCODER == PULSE_DIR + IRAM_ATTR void pulseDir_A_Axis9() { pulseDirInstance[8]->pulse(AXIS9_ENCODER_B_PIN); } +#endif + +PulseDir::PulseDir(int16_t axis, int16_t pulsePin, int16_t dirPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + axis_index = axis - 1; + + PulsePin = pulsePin; + DirPin = dirPin; + + pulseDirInstance[axis_index] = this; +} + +bool PulseDir::init() { + if (ready) return true; + if (!Encoder::init()) return false; + + pinMode(PulsePin, INPUT_PULLUP); + pinMode(DirPin, INPUT_PULLUP); + + int pulsePin = digitalPinToInterrupt(PulsePin); + + if (pulsePin < 0) { + DF("ERR: Encoder PulseDir"); D(axis); DLF(" init(), couldn't attach interrupt!"); + return false; + } + + switch (axis) { + #if AXIS1_ENCODER == PULSE_DIR + case 1: + attachInterrupt(pulsePin, pulseDir_A_Axis1, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS2_ENCODER == PULSE_DIR + case 2: + attachInterrupt(pulsePin, pulseDir_A_Axis2, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS3_ENCODER == PULSE_DIR + case 3: + attachInterrupt(pulsePin, pulseDir_A_Axis3, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS4_ENCODER == PULSE_DIR + case 4: + attachInterrupt(pulsePin, pulseDir_A_Axis4, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS5_ENCODER == PULSE_DIR + case 5: + attachInterrupt(pulsePin, pulseDir_A_Axis5, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS6_ENCODER == PULSE_DIR + case 6: + attachInterrupt(pulsePin, pulseDir_A_Axis6, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS7_ENCODER == PULSE_DIR + case 7: + attachInterrupt(pulsePin, pulseDir_A_Axis7, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS8_ENCODER == PULSE_DIR + case 8: + attachInterrupt(pulsePin, pulseDir_A_Axis8, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS9_ENCODER == PULSE_DIR + case 9: + attachInterrupt(pulsePin, pulseDir_A_Axis9, ENCODER_SIGNAL_MODE); + break; + #endif + } + + ready = true; + return true; +} + +int32_t PulseDir::read() { + if (!ready) return 0; + + return ATOMIC_LOAD(count) + index; +} + +void PulseDir::write(int32_t position) { + if (!ready) return; + + index = position - ATOMIC_LOAD(count); +} + +void IRAM_ATTR PulseDir::pulse(const int16_t dirPin) { + #if ENCODER_FILTER > 0 + ENCODER_FILTER_UNTIL(); + #endif + + if (digitalReadF(dirPin)) { + count--; + + #if ENCODER_VELOCITY == ON + velNoteEdge(-1); + #endif + + } else { + count++; + + #if ENCODER_VELOCITY == ON + velNoteEdge(+1); + #endif + + } +} + +#endif diff --git a/src/lib/encoder/pulseDir/PulseDir.h b/src/lib/encoder/pulseDir/PulseDir.h new file mode 100755 index 0000000..89208bc --- /dev/null +++ b/src/lib/encoder/pulseDir/PulseDir.h @@ -0,0 +1,30 @@ +// Pulse/Dir encoders +#pragma once + +#include "../EncoderBase.h" + +#if AXIS1_ENCODER == PULSE_DIR || AXIS2_ENCODER == PULSE_DIR || AXIS3_ENCODER == PULSE_DIR || \ + AXIS4_ENCODER == PULSE_DIR || AXIS5_ENCODER == PULSE_DIR || AXIS6_ENCODER == PULSE_DIR || \ + AXIS7_ENCODER == PULSE_DIR || AXIS8_ENCODER == PULSE_DIR || AXIS9_ENCODER == PULSE_DIR + +// for example: +// PulseDir encoder1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + +class PulseDir : public Encoder { + public: + PulseDir(int16_t axis, int16_t pulsePin, int16_t dirPin); + bool init(); + + int32_t read(); + void write(int32_t position); + + void pulse(const int16_t dirPin); + + private: + int16_t axis_index; + + int16_t PulsePin + volatile int16_t DirPin; +}; + +#endif diff --git a/src/lib/encoder/pulseOnly/PulseOnly.cpp b/src/lib/encoder/pulseOnly/PulseOnly.cpp new file mode 100755 index 0000000..d057c1a --- /dev/null +++ b/src/lib/encoder/pulseOnly/PulseOnly.cpp @@ -0,0 +1,158 @@ +// Pulse only encoders + +#include "PulseOnly.h" + +#if AXIS1_ENCODER == PULSE_ONLY || AXIS2_ENCODER == PULSE_ONLY || AXIS3_ENCODER == PULSE_ONLY || \ + AXIS4_ENCODER == PULSE_ONLY || AXIS5_ENCODER == PULSE_ONLY || AXIS6_ENCODER == PULSE_ONLY || \ + AXIS7_ENCODER == PULSE_ONLY || AXIS8_ENCODER == PULSE_ONLY || AXIS9_ENCODER == PULSE_ONLY + +PulseOnly *pulseOnlyInstance[9]; + +#if AXIS1_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis1() { pulseOnlyInstance[0]->pulse(); } +#endif + +#if AXIS2_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis2() { pulseOnlyInstance[1]->pulse(); } +#endif + +#if AXIS3_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis3() { pulseOnlyInstance[2]->pulse(); } +#endif + +#if AXIS4_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis4() { pulseOnlyInstance[3]->pulse(); } +#endif + +#if AXIS5_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis5() { pulseOnlyInstance[4]->pulse(); } +#endif + +#if AXIS6_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis6() { pulseOnlyInstance[5]->pulse(); } +#endif + +#if AXIS7_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis7() { pulseOnlyInstance[6]->pulse(); } +#endif + +#if AXIS8_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis8() { pulseOnlyInstance[7]->pulse(); } +#endif + +#if AXIS9_ENCODER == PULSE_ONLY + IRAM_ATTR void pulse_A_Axis9() { pulseOnlyInstance[8]->pulse(); } +#endif + +PulseOnly::PulseOnly(int16_t axis, int16_t pulsePin, volatile int8_t *direction) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + axis_index = axis - 1; + + PulsePin = pulsePin; + pulseDirection = direction; + + pulseOnlyInstance[axis_index] = this; +} + +bool PulseOnly::init() { + if (ready) return true; + if (!Encoder::init()) return false; + + pinMode(pulsePin, INPUT_PULLUP); + + int pulsePin = digitalPinToInterrupt(PulsePin); + + if (pulsePin < 0) { + DF("ERR: Encoder PulseOnly"); D(axis); DLF(" init(), couldn't attach interrupt!"); + return false; + } + + switch (axis) { + #if AXIS1_ENCODER == PULSE_ONLY + case 1: + attachInterrupt(pulsePin, pulse_A_Axis1, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS2_ENCODER == PULSE_ONLY + case 2: + attachInterrupt(pulsePin, pulse_A_Axis2, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS3_ENCODER == PULSE_ONLY + case 3: + attachInterrupt(pulsePin, pulse_A_Axis3, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS4_ENCODER == PULSE_ONLY + case 4: + attachInterrupt(pulsePin, pulse_A_Axis4, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS5_ENCODER == PULSE_ONLY + case 5: + attachInterrupt(pulsePin, pulse_A_Axis5, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS6_ENCODER == PULSE_ONLY + case 6: + attachInterrupt(pulsePin, pulse_A_Axis6, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS7_ENCODER == PULSE_ONLY + case 7: + attachInterrupt(pulsePin, pulse_A_Axis7, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS8_ENCODER == PULSE_ONLY + case 8: + attachInterrupt(pulsePin, pulse_A_Axis8, ENCODER_SIGNAL_MODE); + break; + #endif + #if AXIS9_ENCODER == PULSE_ONLY + case 9: + attachInterrupt(pulsePin, pulse_A_Axis9, ENCODER_SIGNAL_MODE); + break; + #endif + } + + ready = true; +} + +int32_t PulseOnly::read() { + if (!ready) return 0; + + noInterrupts(); + count = count; + interrupts(); + + return count + index; +} + +void PulseOnly::write(int32_t position) { + if (!ready) return; + + noInterrupts(); + index = position - count; + interrupts(); +} + +void PulseOnly::setDirection(volatile int8_t *direction) { + if (!ready) return; + + pulseDirection = direction; +} + +IRAM_ATTR void PulseOnly::pulse() { + #if ENCODER_FILTER > 0 + ENCODER_FILTER_UNTIL(); + #endif + count += *pulseDirection; + + #if ENCODER_VELOCITY == ON + velNoteEdge(*pulseDirection); + #endif +} + +#endif diff --git a/src/lib/encoder/pulseOnly/PulseOnly.h b/src/lib/encoder/pulseOnly/PulseOnly.h new file mode 100755 index 0000000..268d528 --- /dev/null +++ b/src/lib/encoder/pulseOnly/PulseOnly.h @@ -0,0 +1,32 @@ +// Pulse only encoders +#pragma once + +#include "../EncoderBase.h" + +#if AXIS1_ENCODER == PULSE_ONLY || AXIS2_ENCODER == PULSE_ONLY || AXIS3_ENCODER == PULSE_ONLY || \ + AXIS4_ENCODER == PULSE_ONLY || AXIS5_ENCODER == PULSE_ONLY || AXIS6_ENCODER == PULSE_ONLY || \ + AXIS7_ENCODER == PULSE_ONLY || AXIS8_ENCODER == PULSE_ONLY || AXIS9_ENCODER == PULSE_ONLY + +// for example: +// PulseOnly encoder1(1, int16_t axis, AXIS1_ENCODER_A_PIN, &direction); + +class PulseOnly : public Encoder { + public: + PulseOnly(int16_t axis, int16_t pulsePin, volatile int8_t *direction); + bool init(); + + int32_t read(); + void write(int32_t position); + + void setDirection(volatile int8_t *direction); + + void pulse(); + + private: + int16_t axis_index; + + int16_t PulsePin; + volatile int8_t *pulseDirection; +}; + +#endif diff --git a/src/lib/encoder/quadrature/Quadrature.cpp b/src/lib/encoder/quadrature/Quadrature.cpp new file mode 100755 index 0000000..9c27327 --- /dev/null +++ b/src/lib/encoder/quadrature/Quadrature.cpp @@ -0,0 +1,235 @@ +// A/B Quadrature encoders + +#include "Quadrature.h" + +#if AXIS1_ENCODER == AB || AXIS2_ENCODER == AB || AXIS3_ENCODER == AB || \ + AXIS4_ENCODER == AB || AXIS5_ENCODER == AB || AXIS6_ENCODER == AB || \ + AXIS7_ENCODER == AB || AXIS8_ENCODER == AB || AXIS9_ENCODER == AB + +Quadrature *quadratureInstance[9]; + +#if AXIS1_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis1() { quadratureInstance[0]->A(AXIS1_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis1() { quadratureInstance[0]->B(AXIS1_ENCODER_B_PIN); } +#endif + +#if AXIS2_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis2() { quadratureInstance[1]->A(AXIS2_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis2() { quadratureInstance[1]->B(AXIS2_ENCODER_B_PIN); } +#endif + +#if AXIS3_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis3() { quadratureInstance[2]->A(AXIS3_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis3() { quadratureInstance[2]->B(AXIS3_ENCODER_B_PIN); } +#endif + +#if AXIS4_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis4() { quadratureInstance[3]->A(AXIS4_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis4() { quadratureInstance[3]->B(AXIS4_ENCODER_B_PIN); } +#endif + +#if AXIS5_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis5() { quadratureInstance[4]->A(AXIS5_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis5() { quadratureInstance[4]->B(AXIS5_ENCODER_B_PIN); } +#endif + +#if AXIS6_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis6() { quadratureInstance[5]->A(AXIS6_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis6() { quadratureInstance[5]->B(AXIS6_ENCODER_B_PIN); } +#endif + +#if AXIS7_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis7() { quadratureInstance[6]->A(AXIS7_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis7() { quadratureInstance[6]->B(AXIS7_ENCODER_B_PIN); } +#endif + +#if AXIS8_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis8() { quadratureInstance[7]->A(AXIS8_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis8() { quadratureInstance[7]->B(AXIS8_ENCODER_B_PIN); } +#endif + +#if AXIS9_ENCODER == AB + IRAM_ATTR void quadrature_A_Axis9() { quadratureInstance[8]->A(AXIS9_ENCODER_A_PIN); } + IRAM_ATTR void quadrature_B_Axis9() { quadratureInstance[8]->B(AXIS9_ENCODER_B_PIN); } +#endif + +Quadrature::Quadrature(int16_t axis, int16_t APin, int16_t BPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + axis_index = axis - 1; + + this->APin = APin; + this->BPin = BPin; + quadratureInstance[axis_index] = this; +} + +bool Quadrature::init() { + if (ready) return true; + if (!Encoder::init()) return false; + + pinMode(APin, INPUT_PULLUP); + pinMode(BPin, INPUT_PULLUP); + + stateA = digitalRead(APin); + lastA = stateA; + stateB = digitalRead(BPin); + lastB = stateB; + + int aPin = digitalPinToInterrupt(APin); + int bPin = digitalPinToInterrupt(BPin); + + if (aPin < 0 || bPin < 0) { + DF("MSG: Encoder Quadrature"); D(axis); DLF(" init(), couldn't attach interrupt!"); + return false; + } + + VF("MSG: Encoder Quadrature"); V(axis); VF(" init(), attaching interrupts to "); + VF("APin="); V(APin); VF(" and BPin="); VL(BPin); + + switch (axis) { + #if AXIS1_ENCODER == AB + case 1: + attachInterrupt(aPin, quadrature_A_Axis1, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis1, CHANGE); + break; + #endif + #if AXIS2_ENCODER == AB + case 2: + attachInterrupt(aPin, quadrature_A_Axis2, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis2, CHANGE); + break; + #endif + #if AXIS3_ENCODER == AB + case 3: + attachInterrupt(aPin, quadrature_A_Axis3, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis3, CHANGE); + break; + #endif + #if AXIS4_ENCODER == AB + case 4: + attachInterrupt(aPin, quadrature_A_Axis4, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis4, CHANGE); + break; + #endif + #if AXIS5_ENCODER == AB + case 5: + attachInterrupt(aPin, quadrature_A_Axis5, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis5, CHANGE); + break; + #endif + #if AXIS6_ENCODER == AB + case 6: + attachInterrupt(aPin, quadrature_A_Axis6, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis6, CHANGE); + break; + #endif + #if AXIS7_ENCODER == AB + case 7: + attachInterrupt(aPin, quadrature_A_Axis7, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis7, CHANGE); + break; + #endif + #if AXIS8_ENCODER == AB + case 8: + attachInterrupt(aPin, quadrature_A_Axis8, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis8, CHANGE); + break; + #endif + #if AXIS9_ENCODER == AB + case 9: + attachInterrupt(aPin, quadrature_A_Axis9, CHANGE); + attachInterrupt(bPin, quadrature_B_Axis9, CHANGE); + break; + #endif + } + + ready = true; + return true; +} + +int32_t Quadrature::read() { + if (!ready) return 0; + + return ATOMIC_LOAD(count) + index; +} + +void Quadrature::write(int32_t position) { + if (!ready) return; + + index = position - ATOMIC_LOAD(count); +} + +// Phase 1: LLHH LLHH +// Phase 2: LHHL LHHL +// ...00 01 11 10 00 01 11 10 00 01 11 10... + +ICACHE_RAM_ATTR void Quadrature::A(const int16_t pin) { + #if ENCODER_FILTER > 0 + ENCODER_FILTER_UNTIL(); + #endif + stateA = digitalReadF(pin); + + uint8_t v = (stateA<<3) + (stateB<<2) + (lastA<<1) + lastB; + switch (v) { + case 0b0000: QUAD_F2; break; // skipped pulse (invalid A state, valid B state) + case 0b0001: dir = -1; break; + case 0b0010: dir = 1; break; + case 0b0011: QUAD_F1; break; // skipped pulse (valid A state, impossible B state) + case 0b0100: dir = 1; break; + case 0b0101: QUAD_F2; break; // skipped pulse (invalid A state, valid B state) + case 0b0110: QUAD_F1; break; // skipped pulse (valid A state, impossible B state) + case 0b0111: dir = -1; break; + case 0b1000: dir = -1; break; + case 0b1001: QUAD_F1; break; // skipped pulse (valid A state, impossible B state) + case 0b1010: QUAD_F2; break; // skipped pulse (invalid A state, valid B state) + case 0b1011: dir = 1; break; + case 0b1100: QUAD_F1; break; // skipped pulse (valid A state, impossible B state) + case 0b1101: dir = 1; break; + case 0b1110: dir = -1; break; + case 0b1111: QUAD_F2; break; // skipped pulse (invalid A state, valid B state) + } + count += dir; + + #if ENCODER_VELOCITY == ON + if (dir) { velNoteEdge(dir); } + #endif + + lastA = stateA; +} + +ICACHE_RAM_ATTR void Quadrature::B(const int16_t pin) { + #if ENCODER_FILTER > 0 + ENCODER_FILTER_UNTIL(); + #endif + stateB = digitalReadF(pin); + + uint8_t v = (stateA<<3) + (stateB<<2) + (lastA<<1) + lastB; + switch (v) { + case 0b0000: QUAD_F2; break; // skipped pulse (valid A state, invalid B state) + case 0b0001: dir = -1; break; + case 0b0010: dir = 1; break; + case 0b0011: QUAD_F1; break; // skipped pulse (impossible A state, valid B state) + case 0b0100: dir = 1; break; + case 0b0101: QUAD_F2; break; // skipped pulse (valid A state, invalid B state) + case 0b0110: QUAD_F1; break; // skipped pulse (impossible A state, valid B state) + case 0b0111: dir = -1; break; + case 0b1000: dir = -1; break; + case 0b1001: QUAD_F1; break; // skipped pulse (impossible A state, valid B state) + case 0b1010: QUAD_F2; break; // skipped pulse (valid A state, invalid B state) + case 0b1011: dir = 1; break; + case 0b1100: QUAD_F1; break; // skipped pulse (impossible A state, valid B state) + case 0b1101: dir = 1; break; + case 0b1110: dir = -1; break; + case 0b1111: QUAD_F2; break; // skipped pulse (valid A state, invalid B state) + } + count += dir; + + #if ENCODER_VELOCITY == ON + if (dir) { velNoteEdge(dir); } + #endif + + lastB = stateB; +} + +#endif diff --git a/src/lib/encoder/quadrature/Quadrature.h b/src/lib/encoder/quadrature/Quadrature.h new file mode 100755 index 0000000..de1c79d --- /dev/null +++ b/src/lib/encoder/quadrature/Quadrature.h @@ -0,0 +1,59 @@ +// A/B Quadrature encoders +#pragma once + +#include "../EncoderBase.h" + +#if AXIS1_ENCODER == AB || AXIS2_ENCODER == AB || AXIS3_ENCODER == AB || \ + AXIS4_ENCODER == AB || AXIS5_ENCODER == AB || AXIS6_ENCODER == AB || \ + AXIS7_ENCODER == AB || AXIS8_ENCODER == AB || AXIS9_ENCODER == AB + +// for example: +// Quadrature encoder1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + +// Phase 1: LLHH LLHH +// Phase 2: LHHL LHHL +// ...00 01 11 10 00 01 11 10 00 01 11 10... +// + +// quadrature encoder behavior for invalid signal state + +#ifndef ENCODER_AB_STRICT + #define ENCODER_AB_STRICT OFF +#endif + +#if ENCODER_AB_STRICT == OFF + // current A and/or B signal didn't toggle, skip counting and flag warning + #define QUAD_F1 dir = 0; warn++ + #define QUAD_F2 dir = 0; warn++ +#else + // current A and/or B signal didn't toggle, skip counting and flag error + #define QUAD_F1 dir = 0; error++ + #define QUAD_F2 dir = 0; error++ +#endif + +class Quadrature : public Encoder { + public: + Quadrature(int16_t axis, int16_t APin, int16_t BPin); + bool init(); + + int32_t read(); + void write(int32_t position); + + void A(const int16_t pin); + void B(const int16_t pin); + + private: + int16_t axis_index; + + int16_t APin = OFF; + int16_t BPin = OFF; + + volatile int16_t stateA; + volatile int16_t stateB; + volatile int16_t lastA; + volatile int16_t lastB; + volatile int16_t dir; +}; + + +#endif diff --git a/src/lib/encoder/quadratureEsp32/QuadratureEsp32.cpp b/src/lib/encoder/quadratureEsp32/QuadratureEsp32.cpp new file mode 100755 index 0000000..9ed1818 --- /dev/null +++ b/src/lib/encoder/quadratureEsp32/QuadratureEsp32.cpp @@ -0,0 +1,63 @@ +// A/B Quadrature encoders (fast ESP32 hardware decode) + +#include "QuadratureEsp32.h" + +#if AXIS1_ENCODER == AB_ESP32 || AXIS2_ENCODER == AB_ESP32 || AXIS3_ENCODER == AB_ESP32 || \ + AXIS4_ENCODER == AB_ESP32 || AXIS5_ENCODER == AB_ESP32 || AXIS6_ENCODER == AB_ESP32 || \ + AXIS7_ENCODER == AB_ESP32 || AXIS8_ENCODER == AB_ESP32 || AXIS9_ENCODER == AB_ESP32 + +// for example: +// QuadratureEsp32 encoder1(AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN, 1); + +QuadratureEsp32::QuadratureEsp32(int16_t axis, int16_t APin, int16_t BPin) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + + this->APin = APin; + this->BPin = BPin; +} + +bool QuadratureEsp32::init() { + if (ready) return true; + if (!Encoder::init()) return false; + + ab = new ESP32Encoder; + if (ab == NULL) { + DF("ERR: Encoder QuadratureEsp32"); D(axis); DLF(" init(), didn't get instance!"); + return false; + } + + ab->attachFullQuad(APin, BPin); + if (!ab->isAttached()) { + DF("ERR: Encoder QuadratureEsp32"); D(axis); DLF(" init(), couldn't attach interrupts!"); + delete ab; + ab = nullptr; + return false; + } + + ab->setCount(0); + + ready = true; + return true; +} + +int32_t QuadratureEsp32::read() { + if (!ready) return 0; + + count = (int32_t)ab->getCount(); + + #if ENCODER_VELOCITY == ON + velNoteSampledCount(count); + #endif + + return count + index; +} + +void QuadratureEsp32::write(int32_t position) { + if (!ready) return; + + index = position - (int32_t)ab->getCount(); +} + +#endif diff --git a/src/lib/encoder/quadratureEsp32/QuadratureEsp32.h b/src/lib/encoder/quadratureEsp32/QuadratureEsp32.h new file mode 100755 index 0000000..f40db62 --- /dev/null +++ b/src/lib/encoder/quadratureEsp32/QuadratureEsp32.h @@ -0,0 +1,30 @@ +// A/B Quadrature encoders (fast ESP32 hardware decode) +#pragma once + +#include "../EncoderBase.h" + +#if AXIS1_ENCODER == AB_ESP32 || AXIS2_ENCODER == AB_ESP32 || AXIS3_ENCODER == AB_ESP32 || \ + AXIS4_ENCODER == AB_ESP32 || AXIS5_ENCODER == AB_ESP32 || AXIS6_ENCODER == AB_ESP32 || \ + AXIS7_ENCODER == AB_ESP32 || AXIS8_ENCODER == AB_ESP32 || AXIS9_ENCODER == AB_ESP32 + +#include // https://github.com/madhephaestus/ESP32Encoder/tree/master + +// for example: +// QuadratureEsp32 encoder1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + +class QuadratureEsp32 : public Encoder { + public: + QuadratureEsp32(int16_t axis, int16_t APin, int16_t BPin); + bool init(); + + int32_t read(); + void write(int32_t position); + + ESP32Encoder *ab = nullptr; + + private: + int16_t APin = OFF; + int16_t BPin = OFF; +}; + +#endif diff --git a/src/lib/encoder/serialBridge/SerialBridge.cpp b/src/lib/encoder/serialBridge/SerialBridge.cpp new file mode 100755 index 0000000..ec67515 --- /dev/null +++ b/src/lib/encoder/serialBridge/SerialBridge.cpp @@ -0,0 +1,87 @@ +// Serial bridge encoder + +#include "SerialBridge.h" + +#if (AXIS1_ENCODER == SERIAL_BRIDGE || AXIS2_ENCODER == SERIAL_BRIDGE || AXIS3_ENCODER == SERIAL_BRIDGE || \ + AXIS4_ENCODER == SERIAL_BRIDGE || AXIS5_ENCODER == SERIAL_BRIDGE || AXIS6_ENCODER == SERIAL_BRIDGE || \ + AXIS7_ENCODER == SERIAL_BRIDGE || AXIS8_ENCODER == SERIAL_BRIDGE || AXIS9_ENCODER == SERIAL_BRIDGE) && defined(SERIAL_ENCODER) + +bool _serial_bridge_initialized = false; + +#if SERIAL_ENCODER == HardSerial + #undef SERIAL_ENCODER + HardwareSerial HWSerialEncoder(SERIAL_ENCODER_RX, SERIAL_ENCODER_TX); + #define SERIAL_ENCODER HWSerialEncoder + #define SERIAL_ENCODER_RXTX_SET +#endif + +SerialBridge::SerialBridge(int16_t axis) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + + this->channel[0] = '0' + axis; + + ready = true; +} + +int32_t SerialBridge::read() { + if (!ready) return 0; + + const unsigned long now = millis(); + if (now - lastReadMillis > 10U) { + count = getCount(); + + #if ENCODER_VELOCITY == ON + velNoteSampledCount(count); + #endif + + lastReadMillis = now; + } + + return count + index; +} + +void SerialBridge::write(int32_t position) { + if (!ready) return; + + index = position - getCount(); +} + +int32_t SerialBridge::getCount() { + if (!_serial_bridge_initialized) { + #if defined(SERIAL_ENCODER_RX) && defined(SERIAL_ENCODER_TX) && !defined(SERIAL_ENCODER_RXTX_SET) + SERIAL_ENCODER.begin(SERIAL_ENCODER_BAUD, SERIAL_8N1, SERIAL_ENCODER_RX, SERIAL_ENCODER_TX); + #else + SERIAL_ENCODER.begin(SERIAL_ENCODER_BAUD); + #endif + delay(100); + _serial_bridge_initialized = true; + } + + SERIAL_ENCODER.print(channel); + + char result[32] = ""; + char c; + int i = 0; + errorDetected = false; + unsigned long start = millis(); + do { + if (SERIAL_ENCODER.available()) c = SERIAL_ENCODER.read(); else c = 'x'; + if ((c >= '0' && c <= '9') || c == '-') { + result[i++] = c; + result[i] = 0; + } + if (c == 'E') errorDetected = true; + } while (c != 13 && i < 16 && (millis() - start < 4U); + + if (strlen(result) > 0) { + return atoi(result); + } else { + DLF("WRN: SerialBridge getCount(), timed out!"); + error++; + return 0; + } +} + +#endif diff --git a/src/lib/encoder/serialBridge/SerialBridge.h b/src/lib/encoder/serialBridge/SerialBridge.h new file mode 100755 index 0000000..1183d27 --- /dev/null +++ b/src/lib/encoder/serialBridge/SerialBridge.h @@ -0,0 +1,34 @@ +// Serial bridge encoder +#pragma once + +#include "../EncoderBase.h" + +#if (AXIS1_ENCODER == SERIAL_BRIDGE || AXIS2_ENCODER == SERIAL_BRIDGE || AXIS3_ENCODER == SERIAL_BRIDGE || \ + AXIS4_ENCODER == SERIAL_BRIDGE || AXIS5_ENCODER == SERIAL_BRIDGE || AXIS6_ENCODER == SERIAL_BRIDGE || \ + AXIS7_ENCODER == SERIAL_BRIDGE || AXIS8_ENCODER == SERIAL_BRIDGE || AXIS9_ENCODER == SERIAL_BRIDGE) && defined(SERIAL_ENCODER) + +#ifndef SERIAL_ENCODER_BAUD + #define SERIAL_ENCODER_BAUD 460800 +#endif + +#ifndef SERIAL_ENCODER_ABSOLUTE + #define SERIAL_ENCODER_ABSOLUTE OFF +#endif + +class SerialBridge : public Encoder { + public: + SerialBridge(int16_t axis); + bool isAbsolute() const override { return SERIAL_ENCODER_ABSOLUTE == ON; } + int32_t read(); + void write(int32_t count); + bool errorThresholdExceeded() { return errorDetected; } + + private: + int32_t getCount(); + + char channel[2] = "0"; + unsigned long lastReadMillis = 0; + bool errorDetected = false; +}; + +#endif diff --git a/src/lib/encoder/virtualEnc/VirtualEnc.cpp b/src/lib/encoder/virtualEnc/VirtualEnc.cpp new file mode 100755 index 0000000..6ec04f3 --- /dev/null +++ b/src/lib/encoder/virtualEnc/VirtualEnc.cpp @@ -0,0 +1,155 @@ +// Pulse only encoders + +#include "VirtualEnc.h" + +#if AXIS1_ENCODER == VIRTUAL || AXIS2_ENCODER == VIRTUAL || AXIS3_ENCODER == VIRTUAL || \ + AXIS4_ENCODER == VIRTUAL || AXIS5_ENCODER == VIRTUAL || AXIS6_ENCODER == VIRTUAL || \ + AXIS7_ENCODER == VIRTUAL || AXIS8_ENCODER == VIRTUAL || AXIS9_ENCODER == VIRTUAL + +#include "../../tasks/OnTask.h" + +float _virtual_count[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; +float _virtual_direction[9]; +float _virtual_increment[9]; + +#if AXIS1_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis1() { _virtual_count[0] += _virtual_direction[0] * _virtual_increment[0]; } +#endif + +#if AXIS2_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis2() { _virtual_count[1] += _virtual_direction[1] * _virtual_increment[1]; } +#endif + +#if AXIS3_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis3() { _virtual_count[2] += _virtual_direction[2] * _virtual_increment[2]; } +#endif + +#if AXIS4_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis4() { _virtual_count[3] += _virtual_direction[3] * _virtual_increment[3]; } +#endif + +#if AXIS5_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis5() { _virtual_count[4] += _virtual_direction[4] * _virtual_increment[4]; } +#endif + +#if AXIS6_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis6() { _virtual_count[5] += _virtual_direction[5] * _virtual_increment[5]; } +#endif + +#if AXIS7_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis7() { _virtual_count[6] += _virtual_direction[6] * _virtual_increment[6]; } +#endif + +#if AXIS8_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis8() { _virtual_count[7] += _virtual_direction[7] * _virtual_increment[7]; } +#endif + +#if AXIS9_ENCODER == VIRTUAL + IRAM_ATTR void pulse_A_Axis9() { _virtual_count[8] += _virtual_direction[8] * _virtual_increment[8]; } +#endif + +VirtualEnc::VirtualEnc(int16_t axis) { + if (axis < 1 || axis > 9) return; + + this->axis = axis; + axis_index = axis - 1; + + this->countsPerSec = countsPerSec; + + _virtual_direction[axis_index] = 1; + _virtual_increment[axis_index] = 0.0F; + + isVirtual = true; +} + +bool VirtualEnc::init() { + if (ready) return true; + Encoder::init(); + + virtualEncoderHandle = 0; + + switch (axis) { + #if AXIS1_ENCODER == VIRTUAL + case 1: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis1, "venc1"); + break; + #endif + #if AXIS2_ENCODER == VIRTUAL + case 2: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis2, "venc2"); + break; + #endif + #if AXIS3_ENCODER == VIRTUAL + case 3: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis3, "venc3"); + break; + #endif + #if AXIS4_ENCODER == VIRTUAL + case 4: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis4, "venc4"); + break; + #endif + #if AXIS5_ENCODER == VIRTUAL + case 5: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis5, "venc5"); + break; + #endif + #if AXIS6_ENCODER == VIRTUAL + case 6: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis6, "venc6"); + break; + #endif + #if AXIS7_ENCODER == VIRTUAL + case 7: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis7, "venc7"); + break; + #endif + #if AXIS8_ENCODER == VIRTUAL + case 8: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis8, "venc8"); + break; + #endif + #if AXIS9_ENCODER == VIRTUAL + case 9: + virtualEncoderHandle = tasks.add(timerRateMs, 0, true, 3, pulse_A_Axis9, "venc9"); + break; + #endif + } + + if (!virtualEncoderHandle) { + DF("ERR: Encoder Virtual"); D(axis); DLF(" init(), couldn't get task!"); + return false; + } + + ready = true; + return true; +} + +int32_t VirtualEnc::read() { + if (!ready) return 0; + + noInterrupts(); + count = _virtual_count[axis_index]; + interrupts(); + + return count + index; +} + +void VirtualEnc::write(int32_t position) { + if (!ready) return; + + noInterrupts(); + index = position - _virtual_count[axis_index]; + interrupts(); +} + +void VirtualEnc::setVelocity(float countsPerSec) { + if (!ready) { DF("WRN: Encoder Virtual"); D(axis); DLF(" setVelocity(), not ready!"); return; } + _virtual_increment[axis_index] = (countsPerSec)*(timerRateMs/1000.0F); +} + +void VirtualEnc::setDirection(volatile int8_t *direction) { + _virtual_direction[axis_index] = *direction; +} + +#endif diff --git a/src/lib/encoder/virtualEnc/VirtualEnc.h b/src/lib/encoder/virtualEnc/VirtualEnc.h new file mode 100755 index 0000000..3dc2253 --- /dev/null +++ b/src/lib/encoder/virtualEnc/VirtualEnc.h @@ -0,0 +1,29 @@ +// Pulse only encoders +#pragma once + +#include "../EncoderBase.h" + +#if AXIS1_ENCODER == VIRTUAL || AXIS2_ENCODER == VIRTUAL || AXIS3_ENCODER == VIRTUAL || \ + AXIS4_ENCODER == VIRTUAL || AXIS5_ENCODER == VIRTUAL || AXIS6_ENCODER == VIRTUAL || \ + AXIS7_ENCODER == VIRTUAL || AXIS8_ENCODER == VIRTUAL || AXIS9_ENCODER == VIRTUAL + +class VirtualEnc : public Encoder { + public: + VirtualEnc(int16_t axis); + bool init(); + + int32_t read(); + void write(int32_t count); + + void setVelocity(float countsPerSec); + void setDirection(volatile int8_t *direction); + + private: + int16_t axis_index; + + uint8_t virtualEncoderHandle = 0; + long countsPerSec = 0; + long timerRateMs = 3000/HAL_FRACTIONAL_SEC; // (1.0/HAL_FRACTIONAL_SEC)*3*1000 +}; + +#endif diff --git a/src/lib/ethernet/EthernetManager.cpp b/src/lib/ethernet/EthernetManager.cpp new file mode 100755 index 0000000..e51cdef --- /dev/null +++ b/src/lib/ethernet/EthernetManager.cpp @@ -0,0 +1,174 @@ +// ethernet manager, used by the webserver and ethernet serial IP +#include "EthernetManager.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST + +#include "../tasks/OnTask.h" +#include "../nv/Nv.h" + +#if MDNS_SERVER == ON + enum MdnsReady {MD_WAIT, MD_READY, MD_FAIL}; + + #if OPERATIONAL_MODE != ETHERNET_TEENSY41 + EthernetUDP udp; + MDNS mdns(udp); + #endif + + void mdnsPoll() { + static MdnsReady mdnsReady = MD_WAIT; + if (mdnsReady == MD_WAIT && millis() > 5000) { + char name[] = MDNS_NAME; + strtohostname2(name); + #if OPERATIONAL_MODE == ETHERNET_TEENSY41 + MDNS.begin(name, 1); + VF("MSG: Ethernet, mDNS started for "); VL(name); + // MDNS.addService("_http._tcp", 80); // adding a webserver service would look like this + mdnsReady = MD_READY; + #else + if (mdns.begin(Ethernet.localIP(), name)) { + VF("MSG: Ethernet, mDNS started for "); VL(name); + mdnsReady = MD_READY; + } else { + DF("WRN: Ethernet, mDNS start FAILED for "); DL(name); + mdnsReady = MD_FAIL; + } + #endif + } + + if (mdnsReady == MD_READY) { + #if OPERATIONAL_MODE != ETHERNET_TEENSY41 + mdns.run(); + #endif + } + } +#endif + +bool EthernetManager::init() { + if (!active) { + readSettings(); + + #if defined(ETHERNET_CS_PIN) && ETHERNET_CS_PIN != OFF + VF("MSG: Ethernet, device ETHERNET_CS_PIN ("); V(ETHERNET_CS_PIN); VL(")"); + Ethernet.init(ETHERNET_CS_PIN); + #endif + + if (ETHERNET_RESET_PIN != OFF) { + VF("MSG: Ethernet, device ETHERNET_RESET_PIN ("); V(ETHERNET_RESET_PIN); VL(")"); + pinMode(ETHERNET_RESET_PIN, OUTPUT); + digitalWrite(ETHERNET_RESET_PIN, LOW); + delay(1000); + digitalWrite(ETHERNET_RESET_PIN, HIGH); + delay(1000); + } + + if (settings.dhcpEnabled) { + Ethernet.begin(settings.mac); + } else { + Ethernet.begin(settings.mac, settings.ip, settings.dns, settings.gw, settings.sn); + } + + if (Ethernet.hardwareStatus() == EthernetNoHardware) { + DLF("WRN: Ethernet, no hardware"); + return false; + } + + if (Ethernet.linkStatus() == LinkOFF) { + DLF("WRN: Ethernet, no cable"); + return false; + } + + active = true; + + VLF("MSG: Ethernet, initialized"); + + #if MDNS_SERVER == ON + VF("MSG: Ethernet, starting mDNS polling"); + VF(" task (rate 5ms priority 7)... "); + if (tasks.add(5, 0, true, 7, mdnsPoll, "mdPoll")) { VL("success"); } else { VL("FAILED!"); } + #endif + } + + return active; +} + +void EthernetManager::restart() { + if (ETHERNET_RESET_PIN != OFF) { + VF("MSG: Ethernet, device ETHERNET_RESET_PIN ("); V(ETHERNET_RESET_PIN); VL(")"); + pinMode(ETHERNET_RESET_PIN, OUTPUT); + digitalWrite(ETHERNET_RESET_PIN, LOW); + delay(1000); + digitalWrite(ETHERNET_RESET_PIN, HIGH); + delay(1000); + } + + VLF("MSG: Ethernet, restart"); + if (settings.dhcpEnabled) { + active = Ethernet.begin(settings.mac); + } else { + Ethernet.begin(settings.mac, settings.ip, settings.dns, settings.gw, settings.sn); + active = true; + } +} + +void EthernetManager::disconnect() { + VLF("MSG: Ethernet, disconnect ignored!"); +} + +void EthernetManager::setStation(int number) { + if (number >= 1 && number <= EthernetStationCount) stationNumber = number; + sta = &station[stationNumber - 1]; +} + +void EthernetManager::readSettings() { + if (settingsReady) return; + + #ifdef NV_ETHERNET_SETTINGS + VLF("MSG: Ethernet, reading settings from NV"); + if (!nv().kv().getOrInit("ETHERNET_SETTINGS", settings)) { DLF("WRN: Nv, init failed for ETHERNET_SETTINGS"); } + + for (int i = 1; i <= EthernetStationCount; i++) { + char keyStr[24]; + snprintf(keyStr, sizeof(keyStr), "ETHERNET_STATION%u", i); + if (!nv().kv().getOrInit(keyStr, station[i - 1])) { DF("WRN: Nv, init failed for "); DL(keyStr); } + nv().kv().put(keyStr, station[i - 1]); + } + #endif + + #if DEBUG != OFF + int currentStationNumber = stationNumber; + + VF("MSG: Ethernet, DHCP En = "); VL(settings.dhcpEnabled); + VF("MSG: Ethernet, IP = "); V(settings.ip[0]); V("."); V(settings.ip[1]); V("."); V(settings.ip[2]); V("."); VL(settings.ip[3]); + VF("MSG: Ethernet, GW = "); V(settings.gw[0]); V("."); V(settings.gw[1]); V("."); V(settings.gw[2]); V("."); VL(settings.gw[3]); + VF("MSG: Ethernet, SN = "); V(settings.sn[0]); V("."); V(settings.sn[1]); V("."); V(settings.sn[2]); V("."); VL(settings.sn[3]); + + for (int i = 1; i <= EthernetStationCount; i++) { + setStation(i); + + VF("MSG: Ethernet, Sta"); V(stationNumber); VF(" NAME = "); VL(sta->host); + VF("MSG: Ethernet, Sta"); V(stationNumber); VF(" TARGET = "); V(sta->target[0]); V("."); V(sta->target[1]); V("."); V(sta->target[2]); V("."); VL(sta->target[3]); + } + stationNumber = currentStationNumber; + #endif + + settingsReady = true; +} + +void EthernetManager::writeSettings() { + if (!settingsReady) return; + + #ifdef NV_ETHERNET_SETTINGS + VLF("MSG: Ethernet, writing settings to NV"); + nv().kv().put("ETHERNET_SETTINGS", settings); + + for (int i = 1; i <= EthernetStationCount; i++) { + char keyStr[24]; + snprintf(keyStr, sizeof(keyStr), "ETHERNET_STATION%u", i); + nv().kv().put(keyStr, station[i - 1]); + } + #endif +} + +EthernetManager ethernetManager; + +#endif diff --git a/src/lib/ethernet/EthernetManager.defaults.h b/src/lib/ethernet/EthernetManager.defaults.h new file mode 100755 index 0000000..81fac55 --- /dev/null +++ b/src/lib/ethernet/EthernetManager.defaults.h @@ -0,0 +1,142 @@ +#pragma once + +// ------------------------------------------------------------------------------------------------- +// general ip settings + +#ifndef HOST_NAME +#define HOST_NAME "Unknown" +#endif + +// default ETHERNET_XXX disabled +#ifndef OPERATIONAL_MODE +#define OPERATIONAL_MODE OFF +#endif + +// optional web-server +#ifndef WEB_SERVER +#define WEB_SERVER OFF // ON for website at port 80 +#endif + +// optional LX200 command server IP channels 9996 to 9999 (listens to clients) used by SWS +// OFF or STANDARD (port 9999), or PERSISTENT (ports 9996 to 9998), or BOTH +#ifndef COMMAND_SERVER +#define COMMAND_SERVER OFF +#endif + +// optional Arduino Serial class work-alike IP channels 9996 to 9999 as a server (listens to clients) +// OFF or STANDARD (port 9999), or PERSISTENT (ports 9996 to 9998), or BOTH +#ifndef SERIAL_SERVER +#define SERIAL_SERVER OFF // SERIAL_SIP, SERIAL_PIP1, etc. +#endif + +// optional Arduino Serial class work-alike IP channel to port 9998 as a client (connects to a server) +#ifndef SERIAL_CLIENT +#define SERIAL_CLIENT OFF // ON for SERIAL_IP at port 9998 +#endif + +// mDNS defaults +#ifndef MDNS_SERVER +#define MDNS_SERVER OFF +#endif + +#ifndef MDNS_NAME +#define MDNS_NAME HOST_NAME +#endif + +// ------------------------------------------------------------------------------------------------- +// ethernet manager settings + +#ifndef MAC +#define MAC {0xDE,0xAD,0xBE,0xEF,0xFE,0xED} // unique MAC address +#endif + +#ifndef PASSWORD_DEFAULT +#define PASSWORD_DEFAULT "password" // default password for settings +#endif + +#ifndef DHCP_ENABLED +#define DHCP_ENABLED true // true to use LAN DHCP addresses +#endif + +#ifndef STA_IP_ADDR +#define STA_IP_ADDR {192,168,0,2} // IP Address +#endif + +#ifndef STA_GW_ADDR +#define STA_GW_ADDR {192,168,0,1} // GATEWAY Address +#endif + +#ifndef STA_SN_MASK +#define STA_SN_MASK {255,255,255,0} // SUBNET Mask +#endif + +#ifndef STA_HOST_NAME +#define STA_HOST_NAME "Home" // Host name to connect to (OnStep for example) +#endif + +#ifndef STA_TARGET_IP_ADDR +#define STA_TARGET_IP_ADDR {192,168,0,1} // IP Address to connect to +#endif +#ifndef STA_TARGET +#define STA_TARGET STA_TARGET_IP_ADDR +#endif + +// station 1 + +#ifndef STA1_HOST_NAME +#define STA1_HOST_NAME STA_HOST_NAME +#endif + +#ifndef STA1_TARGET_IP_ADDR +#define STA1_TARGET_IP_ADDR STA_TARGET +#endif + +// station 2 + +#ifndef STA2_HOST_NAME +#define STA2_HOST_NAME "" +#endif + +#ifndef STA2_TARGET_IP_ADDR +#define STA2_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 3 + +#ifndef STA3_HOST_NAME +#define STA3_HOST_NAME "" +#endif + +#ifndef STA3_TARGET_IP_ADDR +#define STA3_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 4 + +#ifndef STA4_HOST_NAME +#define STA4_HOST_NAME "" +#endif + +#ifndef STA4_TARGET_IP_ADDR +#define STA4_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 5 + +#ifndef STA5_HOST_NAME +#define STA5_HOST_NAME "" +#endif + +#ifndef STA5_TARGET_IP_ADDR +#define STA5_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 6 + +#ifndef STA6_HOST_NAME +#define STA6_HOST_NAME "" +#endif + +#ifndef STA6_TARGET_IP_ADDR +#define STA6_TARGET_IP_ADDR {192,168,0,1} +#endif diff --git a/src/lib/ethernet/EthernetManager.h b/src/lib/ethernet/EthernetManager.h new file mode 100755 index 0000000..efe4346 --- /dev/null +++ b/src/lib/ethernet/EthernetManager.h @@ -0,0 +1,114 @@ +// ethernet manager, used by the webserver and ethernet serial IP +#pragma once + +#include "../../Common.h" + +#ifndef OPERATIONAL_MODE +#define OPERATIONAL_MODE OFF +#endif + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST + +#include "EthernetManager.defaults.h" + +#ifdef ESP8266 + #ifndef ETHERNET_W5500 + #error "The ESP8266 Ethernet option supports the W5500 only" + #endif + #include // https://github.com/adafruit/Ethernet2 +#else + #if OPERATIONAL_MODE == ETHERNET_TEENSY41 + #include // built-in library or my https://github.com/hjd1964/Ethernet for ESP32 and ASCOM Alpaca support + #else + #include // built-in library or my https://github.com/hjd1964/Ethernet for ESP32 and ASCOM Alpaca support + #if MDNS_SERVER == ON + #include // built-in library + #include // https://www.arduino.cc/reference/en/libraries/arduinomdns/ + #endif + #endif +#endif + +#ifndef EthernetStationCount + // number of ethernet stations supported, between 1 and 6 + #define EthernetStationCount 1 +#endif + +#define EthernetStationSize 36 +typedef struct EthernetStationSettings { + char host[32]; + uint8_t target[4]; +} EthernetStationSettings; + +#define EthernetSettingsSize 72 +typedef struct EthernetSettings { + char masterPassword[32]; + + unsigned char mac[6]; + bool dhcpEnabled; + IPAddress ip, dns, gw, sn; + +} EthernetSettings; + +class EthernetManager { + public: + // starts Ethernet + bool init(); + + // resets Ethernet + void restart(); + + // Ethernet disconnect is ignored! + void disconnect(); + + // set the currently active station + // \param number from 1 to EthernetStationCount + void setStation(int number); + + void readSettings(); + void writeSettings(); + + // currently selected station + EthernetStationSettings *sta; + + // currently selected station number + int stationNumber = 1; + + // optional station host name lookup overrides target IP (not implemented!) + bool staNameLookup = false; + + EthernetSettings settings = { + PASSWORD_DEFAULT, + MAC, + STA_DHCP_ENABLED, + STA_IP_ADDR, STA_GW_ADDR, STA_GW_ADDR, STA_SN_MASK + }; + + bool active = false; + bool settingsReady = false; + private: + + EthernetStationSettings station[EthernetStationCount] = { + #if EthernetStationCount > 0 + {STA1_HOST_NAME, STA1_TARGET_IP_ADDR}, + #endif + #if EthernetStationCount > 1 + {STA2_HOST_NAME, STA2_TARGET_IP_ADDR}, + #endif + #if EthernetStationCount > 2 + {STA3_HOST_NAME, STA3_TARGET_IP_ADDR}, + #endif + #if EthernetStationCount > 3 + {STA4_HOST_NAME, STA4_TARGET_IP_ADDR}, + #endif + #if EthernetStationCount > 4 + {STA5_HOST_NAME, STA5_TARGET_IP_ADDR}, + #endif + #if EthernetStationCount > 5 + {STA6_HOST_NAME, STA6_TARGET_IP_ADDR}, + #endif + }; +}; + +extern EthernetManager ethernetManager; + +#endif diff --git a/src/lib/ethernet/cmdServer/CmdServer.cpp b/src/lib/ethernet/cmdServer/CmdServer.cpp new file mode 100755 index 0000000..d7db8c4 --- /dev/null +++ b/src/lib/ethernet/cmdServer/CmdServer.cpp @@ -0,0 +1,78 @@ +// ethernet IP command server +#include "CmdServer.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && COMMAND_SERVER != OFF + + #include "../../tasks/OnTask.h" + #include "../../../libApp/cmd/Cmd.h" + + CmdServer::CmdServer(uint32_t port, long clientTimeoutMs, bool persist) { + cmdBuffer[0] = 0; + cmdBufferPos = 0; + clientEndTimeMs = 0; + this->clientTimeoutMs = clientTimeoutMs; + this->persist = persist; + cmdSvr = new EthernetServer(port); + } + + void CmdServer::begin() { + cmdSvr->begin(); + } + + void CmdServer::handleClient() { + const unsigned long now = millis(); + + // drop stale client socket (disconnected or timed out/hung) + if (cmdSvrClient && + (!cmdSvrClient.connected() || (long)(clientEndTimeMs - now) < 0)) { + cmdSvrClient.stop(); + cmdBuffer[0] = 0; + cmdBufferPos = 0; + } + + // new client + if (!cmdSvrClient) { + cmdSvrClient = cmdSvr->available(); + if (cmdSvrClient) { + clientEndTimeMs = now + clientTimeoutMs; + cmdBuffer[0] = 0; + cmdBufferPos = 0; + } + } + + // check clients for data, if found get the command, pass to OnStep and pickup the response, then return the response to client + while (cmdSvrClient && cmdSvrClient.connected() && cmdSvrClient.available() > 0) { + // still active? push back disconnect + if (persist) clientEndTimeMs = now + clientTimeoutMs; + + // get the data + int c = cmdSvrClient.read(); + if (c < 0 || c > 255) break; + + // insert into the command buffer + cmdBuffer[cmdBufferPos] = (char)c; + cmdBufferPos++; + if (cmdBufferPos > 39) cmdBufferPos = 39; + cmdBuffer[cmdBufferPos] = 0; + + // send cmd and pickup the response + if (c == '#' || (strlen(cmdBuffer) == 1 && c == (char)6)) { + char result[40] = ""; + + onStep.processCommand(cmdBuffer, result, cmdTimeout); + + // pickup response + if (strlen(result) > 0) { + if (cmdSvrClient && cmdSvrClient.connected()) { + cmdSvrClient.print(result); // there was a delay(2); after this + } + } + + // reset command buffer + cmdBuffer[0] = 0; + cmdBufferPos = 0; + } else tasks.yield(); + } + } + +#endif diff --git a/src/lib/ethernet/cmdServer/CmdServer.h b/src/lib/ethernet/cmdServer/CmdServer.h new file mode 100755 index 0000000..9921bdc --- /dev/null +++ b/src/lib/ethernet/cmdServer/CmdServer.h @@ -0,0 +1,27 @@ +// ethernet IP command server +#pragma once + +#include "../../../Common.h" +#include "../EthernetManager.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && COMMAND_SERVER != OFF + + class CmdServer { + public: + CmdServer(uint32_t port, long clientTimeoutMs, bool persist = false); + void begin(); + void handleClient(); + + private: + EthernetServer *cmdSvr; + EthernetClient cmdSvrClient; + + char cmdBuffer[40]; + int cmdBufferPos; + + unsigned long clientTimeoutMs; + unsigned long clientEndTimeMs; + bool persist; + }; + +#endif diff --git a/src/lib/ethernet/webServer/WebServer.cpp b/src/lib/ethernet/webServer/WebServer.cpp new file mode 100755 index 0000000..25f5f6c --- /dev/null +++ b/src/lib/ethernet/webServer/WebServer.cpp @@ -0,0 +1,335 @@ +// ----------------------------------------------------------------------------------- +// Web server + +#include "WebServer.h" +#include "../../tasks/OnTask.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && WEB_SERVER != OFF + + void WebServer::begin(long port, long timeToClose, bool autoReset) { + if (webServer == NULL) { + ethernetManager.init(); + + this->port = port; + this->timeToClose = timeToClose; + this->autoReset = autoReset; + + webServer = new EthernetServer(port); + webServer->begin(); + Ethernet.setRetransmissionCount(8); + Ethernet.setRetransmissionTimeout(25); + VF("MSG: Ethernet, started web server on port "); VL(port); + } + } + + void WebServer::handleClient() { + client = webServer->available(); + + if (client) { + WLF("--------------------------------------------------"); + WF("MSG: Webserver, new client socket "); WL(client.getSocketNumber()); + + parameter_count = 0; + String line = ""; + int currentSection = 1; + int handler_number = -1; + lastMethod = HTTP_UNKNOWN; + + unsigned long to = millis() + WEB_SOCKET_TIMEOUT; + while (client.connected() && (long)(millis() - to) < 0 && currentSection <= 2) { + if (client.available()) { + // read in a char + char c = client.read(); if (c == '\r') continue; + + // build up ea. line + if (line.length() <= 1024) line += c; + + // loop until an entire line is present + if (c != '\n' && client.available()) continue; + + // look for end of sections + if (line.equals("\n")) { line = ""; currentSection++; continue; } + + // scan the header + if (currentSection == 1) { + if (!modifiedSinceFound && line.indexOf("If-Modified-Since:") >= 0) modifiedSinceFound = true; + if (lastMethod == HTTP_UNKNOWN) { + int index = line.indexOf("GET "); + if (index >= 0) { + lastMethod = HTTP_GET; + line = line.substring(index + 4); + handler_number = getHandler(&line); + if (handler_number >= 0) processGet(&line); + break; + } else { + index = line.indexOf("POST "); + if (index >= 0) { + lastMethod = HTTP_POST; + line = line.substring(index + 5); + handler_number = getHandler(&line); + } else { + index = line.indexOf("PUT "); + if (index >= 0) { + lastMethod = HTTP_PUT; + line = line.substring(index + 4); + handler_number = getHandler(&line); + if (handler_number >= 0) processPut(&line); + } + } + } + } + } else + + // scan the request + if (currentSection == 2 && handler_number >= 0) { + if (lastMethod == HTTP_PUT) processPut(&line); else + if (lastMethod == HTTP_POST) processPost(&line); + } + + line = ""; + } + Y; + } + + // process get or post request + bool handlerFound = false; + if (handler_number >= 0) { + if (handlers[handler_number] != NULL) { + WF("MSG: Webserver, running handler# "); WL(handler_number); + (*handlers[handler_number])(); + handlerFound = true; + } + } + + // handle page not found + if (!handlerFound && notFoundHandler != NULL) { + WLF("MSG: Webserver, didn't find handler"); + (*notFoundHandler)(); + } + + // port timeout to close + tasks.yield(timeToClose); + + // make sure everything is sent + client.flush(); + + // close the connection: + client.stop(); + + // reset the webserver if requested + if (autoReset) webServer->begin(); + + modifiedSinceFound = false; + + WL("MSG: Webserver, client disconnected"); + } + } + + int WebServer::getHandler(String* line) { + int url_end = line->indexOf("HTTP/"); + if (url_end <= 0) return -1; + + WF("MSG: Webserver, checking handler for "); + + // isolate the content + *line = line->substring(0, url_end); + int url_start = 0; + int url_end2 = line->indexOf('?'); + if (url_end2 > 0) url_end = url_end2; + requestedHandler = line->substring(url_start, url_end); + *line = line->substring(url_end); + requestedHandler.trim(); + if (requestedHandler.length() == 0) requestedHandler = "/"; + WF("["); W(requestedHandler); WL("]"); + + for (int i = 0; i < handler_count; i++) { + if (requestedHandler.equals(handlers_fn[i])) { + WF("MSG: Webserver, found handler# "); WL(i); + return i; + } + } + return -1; + } + + void WebServer::processGet(String* line) { + WLF("MSG: Webserver, checking header GET parameters"); + + if (line->equals(EmptyStr)) { WLF("MSG: Webserver, GET empty"); return; } + + // isolate any parameters, get their values + // look for form "?a=1&" or "&a=1" + while ((*line)[0] == '?' || (*line)[0] == '&') { + *line = line->substring(1); + int j = line->indexOf('='); + if (j == -1) j = line->length(); + if (j == -1) break; // invalid formatting + int j1 = line->indexOf('&'); + if (j1 == -1) j1 = line->length() + 1; + String thisArg = line->substring(0, j); + String thisVal = line->substring(j + 1, j1); + if (thisArg != "") { + if (++parameter_count > PARAMETER_COUNT_MAX) parameter_count = PARAMETER_COUNT_MAX; + parameters[parameter_count - 1] = thisArg; + thisVal.trim(); + values[parameter_count - 1] = thisVal; + } + if ((int)line->length() > j1) *line = line->substring(j1); else *line = ""; + + WF("MSG: Webserver, found "); W(thisArg); W(" = "); WL(thisVal); + } + } + + void WebServer::processPut(String* line) { + WLF("MSG: Webserver, checking header PUT parameters"); + + // make all tokens start with '&' + *line = "&" + *line; + + if (line->equals("&")) { WLF("MSG: Webserver, PUT empty"); return; } + + // isolate any parameters, get their values + // look for form "&a=1" + while ((*line)[0] == '?' || (*line)[0] == '&') { + *line = line->substring(1); + int j = line->indexOf('='); + if (j == -1) j = line->length(); + if (j == -1) break; // invalid formatting + int j1 = line->indexOf('&'); + if (j1 == -1) j1 = line->length() + 1; + String thisArg = line->substring(0, j); + if (thisArg.startsWith("?")) thisArg = thisArg.substring(1); + String thisVal = line->substring(j + 1, j1); + if (thisArg != "") { + if (++parameter_count > PARAMETER_COUNT_MAX) parameter_count = PARAMETER_COUNT_MAX; + parameters[parameter_count - 1] = thisArg; + thisVal.trim(); + values[parameter_count - 1] = thisVal; + } + if ((int)line->length() > j1) *line = line->substring(j1); else *line = ""; + + WF("MSG: Webserver found "); W(thisArg); W(" = "); WL(thisVal); + } + } + + void WebServer::processPost(String* line) { + WLF("MSG: Webserver, checking header POST parameters"); + + // make all tokens start with '&' + *line = "&" + *line; + + if (line->equals(EmptyStr)) { WLF("MSG: Webserver, POST empty"); return; } + + // isolate any parameters, get their values + // look for form "&a=1" + while ((*line)[0] == '&') { + *line = line->substring(1); + int j = line->indexOf('='); + if (j == -1) j = line->length(); + if (j == -1) break; // invalid formatting + int j1 = line->indexOf('&'); + if (j1 == -1) j1 = line->length() + 1; + String thisArg = line->substring(0, j); + String thisVal = line->substring(j + 1, j1); + if (thisArg != "") { + if (++parameter_count > PARAMETER_COUNT_MAX) parameter_count = PARAMETER_COUNT_MAX; + parameters[parameter_count - 1] = thisArg; + values[parameter_count - 1] = thisVal; + } + if ((int)line->length() > j1) *line = line->substring(j1); else *line = ""; + + WF("MSG: Webserver, found "); W(thisArg); W(" = "); WL(thisVal); + } + } + + void WebServer::on(String fn, webFunction handler) { + handler_count++; if (handler_count > WEB_HANDLER_COUNT_MAX) { handler_count = WEB_HANDLER_COUNT_MAX; return; } + handlers[handler_count - 1] = handler; + handlers_fn[handler_count - 1] = fn; + } + + void WebServer::onNotFound(webFunction handler) { + notFoundHandler = handler; + } + + // get argument value by identifier + String WebServer::arg(String id) { + for (int i = 0; i < parameter_count; i++) { + if (id == parameters[i]) return values[i]; + } + return EmptyStr; + } + + // get argument value by index + String WebServer::arg(int i) { + if (i >= 0 && i < parameter_count) { + return values[i]; + } else return ""; + } + + // get argument identifier by index + String WebServer::argName(int i) { + if (i >= 0 && i < parameter_count) { + return parameters[i]; + } else return EmptyStr; + } + + // get arguments count + int WebServer::args() { + return parameter_count; + } + + // check if argument exists + bool WebServer::hasArg(String id) { + for (int i = 0; i < parameter_count; i++) { + if (id == parameters[i]) return true; + } + return false; + } + + void WebServer::setContentLength(long length) { + this->length = length; + } + + void WebServer::setResponseHeader(const char* str) { + if (!str) return; + strcpy_P(responseHeader, str); + } + + void WebServer::sendHeader(const char* key, const char* val, bool first) { + String line = key; + line.concat(": "); + line.concat(val); + line.concat("\r\n"); + if (first) header = line + header; else header.concat(line); + } + + void WebServer::send(int code, const char* content_type, const String& content) { + header = "HTTP/1.1 " + String(code) + " OK\r\n" + header; + header += "Content-Type: "; header += content_type; header += "\r\n"; + if (length == CONTENT_LENGTH_UNKNOWN) { + header += "Connection: close\r\n"; + } else { + if (length == CONTENT_LENGTH_NOT_SET) length = content.length(); + header += "content-length: " + String(length) + "\r\n"; + } + header += "\r\n"; + + client.print(header); + + if (length) client.print(content); + + length = CONTENT_LENGTH_NOT_SET; + header = ""; + } + + void WebServer::sendContent(String s) { + client.print(s); + } + + void WebServer::sendContent(const char * s) { + client.print(s); + } + + WebServer www; + +#endif diff --git a/src/lib/ethernet/webServer/WebServer.h b/src/lib/ethernet/webServer/WebServer.h new file mode 100755 index 0000000..16ec6ee --- /dev/null +++ b/src/lib/ethernet/webServer/WebServer.h @@ -0,0 +1,115 @@ +// ----------------------------------------------------------------------------------- +// Web server +#pragma once + +#include "../../../Common.h" + +#include "../EthernetManager.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && WEB_SERVER != OFF + + // Turn ON to allow webserver debug messages + #ifndef DEBUG_WEBSERVER + #define DEBUG_WEBSERVER OFF + #endif + + #if DEBUG_WEBSERVER == ON + #define W(x) V(x) + #define WF(x) VF(x) + #define WL(x) VL(x) + #define WLF(x) VLF(x) + #else + #define W(x) + #define WF(x) + #define WL(x) + #define WLF(x) + #endif + + // misc. + #ifndef WEB_SOCKET_TIMEOUT + #define WEB_SOCKET_TIMEOUT 250 + #endif + #ifndef WEB_HANDLER_COUNT_MAX + #define WEB_HANDLER_COUNT_MAX 24 + #endif + #define PARAMETER_COUNT_MAX 12 + #define CONTENT_LENGTH_UNKNOWN -1 + #define CONTENT_LENGTH_NOT_SET -2 + #define HTTP_UNKNOWN 0 + #define HTTP_GET 1 + #define HTTP_PUT 2 + #define HTTP_POST 3 + + #define sendContentAndClear(x) sendContent(x); x = ""; + + typedef void (* webFunction) (); + + class WebServer { + public: + void begin(long port = 80, long timeToClose = 50, bool autoReset = false); + + void handleClient(); + + void on(String fn, webFunction handler); + void onNotFound(webFunction handler); + + // get argument value by identifier + String arg(String id); + // get argument value by index + String arg(int i); + // get argument name by index + String argName(int i); + // get arguments count + int args(); + // check if argument exists + bool hasArg(String id); + + // check http last method used, returns HTTP_UNKNOWN, HTTP_GET, HTTP_PUT, or HTTP_POST + inline int method() { return lastMethod; } + + // return uniform resource identifier + String uri() { return requestedHandler; } + + // return modified since state + bool modifiedSince() { return modifiedSinceFound; } + + void setContentLength(long length); + void setResponseHeader(const char *str); + void sendHeader(const char* key, const char* val, bool first = false); + void send(int code, const char* content_type = "text/html", const String& content = ""); + void sendContent(String s); + void sendContent(const char * s); + + EthernetServer *webServer = NULL; + EthernetClient client; + private: + int getHandler(String* line); + void processGet(String* line); + void processPut(String* line); + void processPost(String* line); + + char responseHeader[200] = ""; + bool modifiedSinceFound = false; + + webFunction notFoundHandler = NULL; + webFunction handlers[WEB_HANDLER_COUNT_MAX]; + String handlers_fn[WEB_HANDLER_COUNT_MAX]; + int handler_count = 0; + + String requestedHandler; + String parameters[PARAMETER_COUNT_MAX]; + String values[PARAMETER_COUNT_MAX]; + int parameter_count; + int port = -1; + bool autoReset = false; + long timeToClose = 100; + + int lastMethod = HTTP_UNKNOWN; + + long length; + String header; + }; + + extern WebServer www; + +#endif diff --git a/src/lib/gpioEx/GpioBase.cpp b/src/lib/gpioEx/GpioBase.cpp new file mode 100755 index 0000000..7eb8360 --- /dev/null +++ b/src/lib/gpioEx/GpioBase.cpp @@ -0,0 +1,18 @@ +// ----------------------------------------------------------------------------------- +// standard external GPIO library + +#include "GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE != OFF + +bool Gpio::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + UNUSED(reply); + UNUSED(command); + UNUSED(parameter); + UNUSED(suppressFrame); + UNUSED(numericReply); + UNUSED(commandError); + return false; +} + +#endif diff --git a/src/lib/gpioEx/GpioBase.h b/src/lib/gpioEx/GpioBase.h new file mode 100755 index 0000000..295463d --- /dev/null +++ b/src/lib/gpioEx/GpioBase.h @@ -0,0 +1,68 @@ +// ----------------------------------------------------------------------------------- +// standard external GPIO library +#pragma once + +#include "../../Common.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE != OFF + + #include "../commands/CommandErrors.h" + + class Gpio { + public: + virtual bool init(); + virtual bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + virtual void pinMode(int pin, int mode); + virtual int digitalRead(int pin); + virtual void digitalWrite(int pin, int value); + + bool lateInitError = false; + private: + }; + + #if defined(HAL_DAC_AS_DIGITAL) + // external GPIO and DAC as digital + #define pinModeEx(pin,mode) { if (pin >= 0x200) gpio.pinMode(pin-0x200,mode); else if (pin >= 0x100) pinMode(pin-0x100,mode); else if (pin >= 0) pinMode(CLEAN_PIN(pin),mode); } + #define digitalWriteEx(pin,value) { if (pin >= 0x200) gpio.digitalWrite(pin-0x200,value); else if (pin >= 0x100) analogWrite(pin-0x100,value); else if (pin >= 0) digitalWriteF(CLEAN_PIN(pin),value); } + // special case(s) allowing digitalWriteF() to work with GPIOs + #if GPIO_DEVICE == SSR74HC595 + #define digitalWriteF(pin,value) { if (pin >= 0x200) gpio.digitalWrite(pin-0x200,value); else if (pin >= 0x100) analogWrite(pin-0x100,value); else if (pin >= 0) digitalWrite(CLEAN_PIN(pin),value); } + #endif + #else + // external GPIO but no DAC as digital + #define pinModeEx(pin,mode) { if (pin >= 0x200) gpio.pinMode(pin-0x200,mode); else if (pin >= 0) pinMode(CLEAN_PIN(pin),mode); } + #define digitalWriteEx(pin,value) { if (pin >= 0x200) gpio.digitalWrite(pin-0x200,value); else if (pin >= 0) digitalWriteF(CLEAN_PIN(pin),value); } + // special case(s) allowing digitalWriteF() to work with GPIOs + #if GPIO_DEVICE == SSR74HC595 + #define digitalWriteF(pin,value) { if (pin >= 0x200) gpio.digitalWrite(pin-0x200,value); else if (pin >= 0) digitalWrite(CLEAN_PIN(pin),value); } + #endif + #endif + // no support for DAC input + #define digitalReadEx(pin) ( (pin >= 0)?((pin < 0x100)?digitalReadF(CLEAN_PIN(pin)):gpio.digitalRead(pin-0x200)):0 ) + + // external GPIO analogWrite + #if GPIO_DEVICE == SWS + #define analogWriteEx(pin,value) { if (pin >= 0x200) gpio.analogWrite(pin-0x200,value); else if (pin >= 0) analogWrite(CLEAN_PIN(pin),value); } + #endif +#else + #if defined(HAL_DAC_AS_DIGITAL) + // DAC but no external GPIO + #define pinModeEx(pin,mode) { if (pin > 0x100) pinMode(CLEAN_PIN(pin-0x100),mode); else if (pin >= 0) pinMode(CLEAN_PIN(pin),mode); } + #define digitalWriteEx(pin,value) { if (pin > 0x100) analogWrite(CLEAN_PIN(pin-0x100),value); else if (pin >= 0) { digitalWriteF(CLEAN_PIN(pin),value); } } + #else + // neither DAC as digital or external GPIO + #define pinModeEx(pin,mode) { if (pin >= 0) pinMode(pin,mode); } + #define digitalWriteEx(pin,value) { if (pin >= 0) { digitalWriteF(pin,value); } } + #endif + // no support for DAC input and no external GPIO + #define digitalReadEx(pin) ( (pin >= 0)?digitalReadF(pin):0 ) +#endif + +// automatically use analogWrite +#ifndef analogWriteEx + #define analogWriteEx(pin,value) { analogWrite(pin,value); } +#endif + +// use "Ex" functions to exclude pins that are OFF or SHARED +// pins in range 0x00 to 0xFF are normal, 0x100 to 0x1FF are DAC as digital outputs, 0x200 to 0x2FF are external GPIO pins +#define CLEAN_PIN(pin) ((uint8_t)((pin) & 0xff)) diff --git a/src/lib/gpioEx/GpioEx.h b/src/lib/gpioEx/GpioEx.h new file mode 100755 index 0000000..333a684 --- /dev/null +++ b/src/lib/gpioEx/GpioEx.h @@ -0,0 +1,12 @@ +// ----------------------------------------------------------------------------------- +// standard external GPIO library +#pragma once + +#include "ds2413/Ds2413.h" +#include "mcp23008/Mcp23008.h" +#include "mcp23017/Mcp23017.h" +#include "tca9555/Tca9555.h" +#include "pcf8575/Pcf8575.h" +#include "pcf8574/Pcf8574.h" +#include "sws/Sws.h" +#include "ssr74HC595/Ssr74HC595.h" diff --git a/src/lib/gpioEx/ds2413/Ds2413.cpp b/src/lib/gpioEx/ds2413/Ds2413.cpp new file mode 100755 index 0000000..0148561 --- /dev/null +++ b/src/lib/gpioEx/ds2413/Ds2413.cpp @@ -0,0 +1,136 @@ +// ----------------------------------------------------------------------------------- +// Dallas/Maxim 1-Wire DS2413 device support + +#include "Ds2413.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == DS2413 + +#include "../../1wire/1Wire.h" + +#include // my DallasGPIO library https://github.com/hjd1964/Arduino-DS2413GPIO-Control-Library +DallasGPIO DS2413GPIO(&oneWire); + +#include "../../tasks/OnTask.h" + +void ds2413Wrapper() { gpio.poll(); } + +// scan for a DS2413 device on the 1-wire bus +bool GpioDs2413::init() { + static bool initialized = false; + if (initialized) return found; + + // scan the 1-wire bus and record the devices found + oneWire.reset_search(); + + #if DEBUG == VERBOSE + bool detected = false; + #endif + + VLF("*********************************************"); + VLF("MSG: Dallas/Maxim 1-wire DS2413 device s/n's:"); + + uint8_t addressFound[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + while (oneWire.search(addressFound)) { + if (oneWire.crc8(addressFound, 7) == addressFound[7]) { + if (addressFound[0] == 0x3A || addressFound[0] == 0xBA) { + #if DEBUG == VERBOSE + VF("DS2413: 0x"); + for (int i = 0; i < 8; i++) { + if (addressFound[i] < 16) { V("0"); } + if (DEBUG != OFF) SERIAL_DEBUG.print(addressFound[i], HEX); + } + if (deviceCount < DS2413_MAX_DEVICES) { + VF(" allocated to GPIO("); V(deviceCount*2); VF(") and GPIO("); V(deviceCount*2 + 1); VLF(")"); + } else { VLF(" not assigned"); } + + detected = true; + #endif + + if (deviceCount < DS2413_MAX_DEVICES) { + for (int i = 0; i < 8; i++) address[deviceCount][i] = addressFound[i]; + deviceCount++; + } + + } + } + } + + #if DEBUG == VERBOSE + if (!detected) { VLF("No DS2413 devices found"); } + #endif + + VLF("*********************************************"); + + if (deviceCount > 0) { + found = true; + VF("MSG: GPIO, start DS2413 monitor task (rate 100ms priority 7)... "); + if (tasks.add(100, 0, true, 7, ds2413Wrapper, "ds2413")) { VLF("success"); } else { VLF("FAILED!"); } + } else found = false; + + lastValidPin = deviceCount*2 - 1; + + initialized = true; + return found; +} + +// set GPIO pin mode for INPUT or OUTPUT (both pins of any device must be in the same mode) +void GpioDs2413::pinMode(int pin, int mode) { + if (found && pin >= 0 && pin <= lastValidPin) { + if (mode == INPUT_PULLUP) mode = INPUT; + this->mode[pin/2] = mode; + } +} + +// get GPIO pin state +int GpioDs2413::digitalRead(int pin) { + if (found && (long)(millis() - goodUntil[pin/2]) < 0 && pin >= 0 && pin <= lastValidPin) { + return state[pin]; + } else return -1; +} + +// set GPIO pin state +void GpioDs2413::digitalWrite(int pin, int value) { + if (found && pin >= 0 && pin <= lastValidPin) state[pin] = value; +} + +// update the DS2413 +void GpioDs2413::poll() { + if (found) { + // loop to get/set the GPIO + // tasks.yield() during the 1-wire command sequence is ok since: + // 1. only higher priority level tasks are allowed to run during a yield + // 2. all 1-wire task polling is run at the lowest priority level + for (int index = 0; index < deviceCount; index++) { + if (mode[index] == INPUT) { + for (int i = 0; i < 22; i++) { + if (DS2413GPIO.getStateByAddress(address[0 + index], &state[0 + index*2], &state[1 + index*2], true)) break; else tasks.yield(2); + } + } else + if (mode[index] == OUTPUT) { + if (lastState[0 + index*2] != state[0 + index*2] || lastState[1 + index*2] != state[1 + index*2]) { + for (int i = 0; i < 22; i++) { + if (DS2413GPIO.setStateByAddress(address[0 + index], state[0 + index*2], state[1 + index*2], true)) break; else tasks.yield(2); + } + } + } + lastState[0 + index*2] = state[0 + index*2]; + lastState[1 + index*2] = state[1 + index*2]; + + if (DS2413GPIO.success()) { + goodUntil[index] = millis() + 5000; + } else { + state[0 + index*2] = state[1 + index*2] = INVALID; + DF("ERR: DS2413 0x"); + for (int i = 0; i < 8; i++) { + if (address[0 + index][i] < 16) { V("0"); } + if (DEBUG != OFF) SERIAL_DEBUG.print(address[0 + index][i], HEX); + } + DLF(" GPIO comms failure"); + } + } + } +} + +GpioDs2413 gpio; + +#endif diff --git a/src/lib/gpioEx/ds2413/Ds2413.h b/src/lib/gpioEx/ds2413/Ds2413.h new file mode 100755 index 0000000..44e8d92 --- /dev/null +++ b/src/lib/gpioEx/ds2413/Ds2413.h @@ -0,0 +1,44 @@ +// ----------------------------------------------------------------------------------- +// Dallas/Maxim 1-Wire DS2413 device support +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == DS2413 + +#define DS2413_MAX_DEVICES 2 + +class GpioDs2413 : public Gpio { + public: + // scan for DS2413 devices on the 1-wire bus + bool init(); + + // set GPIO pin mode for INPUT or OUTPUT (both pins of any device must be in the same mode) + void pinMode(int pin, int mode); + + // get GPIO pin state + int digitalRead(int pin); + + // set GPIO pin state + void digitalWrite(int pin, int value); + + // update the DS2413 + void poll(); + + private: + bool found = false; + uint8_t deviceCount = 0; + int lastValidPin = -1; + + uint8_t address[DS2413_MAX_DEVICES][8]; + + int8_t mode[DS2413_MAX_DEVICES] = {INPUT, INPUT}; + uint8_t state[DS2413_MAX_DEVICES*2] = {false, false, false, false}; + int16_t lastState[DS2413_MAX_DEVICES*2] = {INVALID, INVALID, INVALID, INVALID}; + + unsigned long goodUntil[DS2413_MAX_DEVICES] = {0, 0}; +}; + +extern GpioDs2413 gpio; + +#endif diff --git a/src/lib/gpioEx/mcp23008/Mcp23008.cpp b/src/lib/gpioEx/mcp23008/Mcp23008.cpp new file mode 100755 index 0000000..aad30c4 --- /dev/null +++ b/src/lib/gpioEx/mcp23008/Mcp23008.cpp @@ -0,0 +1,64 @@ +// ----------------------------------------------------------------------------------- +// I2C MCP23008 GPIO support + +#include "Mcp23008.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == MCP23008 + +#ifndef GPIO_MCP23008_I2C_ADDRESS + #define GPIO_MCP23008_I2C_ADDRESS 0x20 +#endif + +// needs: https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library and https://github.com/adafruit/Adafruit_BusIO +#include "Adafruit_MCP23X08.h" +Adafruit_MCP23X08 mcp; + +// check for MCP23008 device on the I2C bus +bool GpioMcp23008::init() { + static bool initialized = false; + if (initialized) return found; + + if (mcp.begin_I2C(GPIO_MCP23008_I2C_ADDRESS, &HAL_WIRE)) { + found = true; + for (int i = 0; i < 8; i++) { mcp.pinMode(i, INPUT); } + } else { found = false; DF("WRN: Gpio.init(), MCP23008 (I2C 0x"); if (DEBUG != OFF) SERIAL_DEBUG.print(GPIO_MCP23008_I2C_ADDRESS, HEX); DLF(") not found"); } + HAL_WIRE_SET_CLOCK(); + + return found; +} + +// set GPIO pin (0 to 7) mode for INPUT, INPUT_PULLUP, or OUTPUT +void GpioMcp23008::pinMode(int pin, int mode) { + if (found && pin >= 0 && pin <= 7) { + #ifdef INPUT_PULLDOWN + if (mode == INPUT_PULLDOWN) mode = INPUT; + #endif + mcp.pinMode(pin, mode); + this->mode[pin] = mode; + } +} + +// one eight channel MCP23008 GPIO is supported, this gets the last set value +int GpioMcp23008::digitalRead(int pin) { + if (found && pin >= 0 && pin <= 7) { + if (mode[pin] == INPUT || mode[pin] == INPUT_PULLUP) { + return mcp.digitalRead(pin); + } else return state[pin]; + } else return 0; +} + +// one eight channel MCP23008 GPIO is supported, this sets each output on or off +void GpioMcp23008::digitalWrite(int pin, int value) { + if (found && pin >= 0 && pin <= 7) { + state[pin] = value; + if (mode[pin] == OUTPUT) { + mcp.digitalWrite(pin, value); + } else { + if (value == HIGH) pinMode(pin, INPUT_PULLUP); else pinMode(pin, INPUT); + } + } else return; +} + +GpioMcp23008 gpio; + +#endif diff --git a/src/lib/gpioEx/mcp23008/Mcp23008.h b/src/lib/gpioEx/mcp23008/Mcp23008.h new file mode 100755 index 0000000..afe557f --- /dev/null +++ b/src/lib/gpioEx/mcp23008/Mcp23008.h @@ -0,0 +1,31 @@ +// ----------------------------------------------------------------------------------- +// I2C MCP23008 GPIO support +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == MCP23008 + +class GpioMcp23008 : public Gpio { + public: + // scan for MCP23008 device + bool init(); + + void pinMode(int pin, int mode); + + // one eight channel MCP23008 GPIO is supported, this gets the last set value + int digitalRead(int pin); + + // one eight channel MCP23008 GPIO is supported, this sets each output on or off + void digitalWrite(int pin, int value); + + private: + bool found = false; + + int8_t mode[8] = { INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT }; + bool state[8] = { false, false, false, false, false, false, false, false }; +}; + +extern GpioMcp23008 gpio; + +#endif diff --git a/src/lib/gpioEx/mcp23017/Mcp23017.cpp b/src/lib/gpioEx/mcp23017/Mcp23017.cpp new file mode 100755 index 0000000..6552a4f --- /dev/null +++ b/src/lib/gpioEx/mcp23017/Mcp23017.cpp @@ -0,0 +1,67 @@ +// ----------------------------------------------------------------------------------- +// I2C MCP23017 GPIO support + +#include "Mcp23017.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == MCP23017 + +#ifndef GPIO_MCP23017_I2C_ADDRESS + #define GPIO_MCP23017_I2C_ADDRESS 0x20 +#endif + +// needs: https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library and https://github.com/adafruit/Adafruit_BusIO +#include "Adafruit_MCP23X17.h" +Adafruit_MCP23X17 mcp; + +// check for MCP23017 device on the I2C bus +bool GpioMcp23017::init() { + static bool initialized = false; + if (initialized) return found; + + if (mcp.begin_I2C(GPIO_MCP23017_I2C_ADDRESS, &HAL_WIRE)) { + found = true; + for (int i = 0; i < 16; i++) { mcp.pinMode(i, INPUT); } + } else { + found = false; + DF("WRN: Gpio.init(), MCP23017 (I2C 0x"); if (DEBUG != OFF) SERIAL_DEBUG.print(GPIO_MCP23017_I2C_ADDRESS, HEX); DLF(") not found"); + } + HAL_WIRE_SET_CLOCK(); + + return found; +} + +// set GPIO pin (0 to 15) mode for INPUT, INPUT_PULLUP, or OUTPUT +void GpioMcp23017::pinMode(int pin, int mode) { + if (found && pin >= 0 && pin <= 15) { + #ifdef INPUT_PULLDOWN + if (mode == INPUT_PULLDOWN) mode = INPUT; + #endif + mcp.pinMode(pin, mode); + this->mode[pin] = mode; + } +} + +// one sixteen channel MCP23017 GPIO is supported, this gets the last set value +int GpioMcp23017::digitalRead(int pin) { + if (found && pin >= 0 && pin <= 15) { + if (mode[pin] == INPUT || mode[pin] == INPUT_PULLUP) { + return mcp.digitalRead(pin); + } else return state[pin]; + } else return 0; +} + +// one sixteen channel MCP23017 GPIO is supported, this sets each output on or off +void GpioMcp23017::digitalWrite(int pin, int value) { + if (found && pin >= 0 && pin <= 15) { + state[pin] = value; + if (mode[pin] == OUTPUT) { + mcp.digitalWrite(pin, value); + } else { + if (value == HIGH) pinMode(pin, INPUT_PULLUP); else pinMode(pin, INPUT); + } + } else return; +} + +GpioMcp23017 gpio; + +#endif diff --git a/src/lib/gpioEx/mcp23017/Mcp23017.h b/src/lib/gpioEx/mcp23017/Mcp23017.h new file mode 100755 index 0000000..c7f7237 --- /dev/null +++ b/src/lib/gpioEx/mcp23017/Mcp23017.h @@ -0,0 +1,31 @@ +// ----------------------------------------------------------------------------------- +// I2C MCP23017 GPIO support +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == MCP23017 + +class GpioMcp23017 : public Gpio { + public: + // scan for MCP23017 device + bool init(); + + void pinMode(int pin, int mode); + + // one sixteen channel MCP23017 GPIO is supported, this gets the last set value + int digitalRead(int pin); + + // one sixteen channel MCP23017 GPIO is supported, this sets each output on or off + void digitalWrite(int pin, int value); + + private: + bool found = false; + + int8_t mode[16] = { INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT }; + bool state[16] = { false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false }; +}; + +extern GpioMcp23017 gpio; + +#endif diff --git a/src/lib/gpioEx/pcf8574/Pcf8574.cpp b/src/lib/gpioEx/pcf8574/Pcf8574.cpp new file mode 100755 index 0000000..a5aa510 --- /dev/null +++ b/src/lib/gpioEx/pcf8574/Pcf8574.cpp @@ -0,0 +1,102 @@ +// ----------------------------------------------------------------------------------- +// I2C PCF8574 GPIO support + +#include "Pcf8574.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == X8574 + +// check for up to four PCF8574 devices on the I2C bus +bool GpioPcf8574::init() { + static bool initialized = false; + if (initialized) return found; + + for (int i = 0; i < 32; i++) mode[i] = -1; + + HAL_WIRE.begin(); + HAL_WIRE_SET_CLOCK(); + + // check to be sure we find all devices + for (int i = 0; i < GPIO_PCF8574_I2C_NUM_DEVICES; i++) { + HAL_WIRE.beginTransmission(iicAddress[i]); + uint8_t error = HAL_WIRE.endTransmission(); + if (error) { + // try again + HAL_WIRE.beginTransmission(iicAddress[i]); + uint8_t error = HAL_WIRE.endTransmission(); + if (error) { + if (DEBUG != OFF) { DF("WRN: Gpio.init(), PCF8574 (I2C 0x"); SERIAL_DEBUG.print(iicAddress[i], HEX); DLF(") not found"); } + found = false; + return false; + } + } + } + + found = true; + return found; +} + +// set GPIO pin (0 to 31) mode for INPUT or OUTPUT +void GpioPcf8574::pinMode(int pin, int mode) { + if (found && pin >= 0 && pin <= GPIO_PCF8574_I2C_NUM_DEVICES*8 - 1) { + uint8_t device = pin >> 3; + uint8_t devicePin = pin & 0b0111; + + #ifdef INPUT_PULLDOWN + if (mode == INPUT_PULLDOWN) mode = INPUT; + #endif + if (mode == INPUT_PULLUP) mode = INPUT; + if (mode != INPUT && mode != OUTPUT) return; + + if (this->mode[pin] != mode) { + bitWrite(state[device], devicePin, (mode == INPUT) ? 1 : 0); + + HAL_WIRE.beginTransmission(iicAddress[device]); + HAL_WIRE.write(state[device]); + HAL_WIRE.endTransmission(); + + this->mode[pin] = mode; + } + } +} + +// get GPIO pin (0 to 31) state +int GpioPcf8574::digitalRead(int pin) { + if (found && pin >= 0 && pin <= GPIO_PCF8574_I2C_NUM_DEVICES*8 - 1 && mode[pin] > 0) { + uint8_t device = pin >> 3; + uint8_t devicePin = pin & 0b0111; + + if (mode[pin] == INPUT) { + + HAL_WIRE.requestFrom(iicAddress[device], (uint8_t)1); + + unsigned long timeout = millis() + 1000UL; + do { if ((long)(timeout - millis()) < 0) return 0; } while (Wire.available() < 1); + + uint8_t data = HAL_WIRE.read(); + return bitRead(data, devicePin); + } else + return bitRead(state[device], devicePin); + } + + return 0; +} + +// set GPIO pin (0 to 31) state +void GpioPcf8574::digitalWrite(int pin, int value) { + if (found && pin >= 0 && pin <= GPIO_PCF8574_I2C_NUM_DEVICES*8 - 1 && mode[pin] > 0) { + if (mode[pin] == OUTPUT) { + uint8_t device = pin >> 3; + uint8_t devicePin = pin & 0b0111; + + bitWrite(state[device], devicePin, (value == 0) ? 0 : 1); + + HAL_WIRE.beginTransmission(iicAddress[device]); + HAL_WIRE.write(state[device]); + HAL_WIRE.endTransmission(); + } + } +} + +GpioPcf8574 gpio; + +#endif diff --git a/src/lib/gpioEx/pcf8574/Pcf8574.h b/src/lib/gpioEx/pcf8574/Pcf8574.h new file mode 100755 index 0000000..ff283c3 --- /dev/null +++ b/src/lib/gpioEx/pcf8574/Pcf8574.h @@ -0,0 +1,48 @@ +// ----------------------------------------------------------------------------------- +// I2C PCF8574 GPIO support +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == X8574 + +#ifndef GPIO_PCF8574_I2C_NUM_DEVICES + #define GPIO_PCF8574_I2C_NUM_DEVICES 1 +#endif + +#ifndef GPIO_PCF8574_I2C_ADDRESS1 + #define GPIO_PCF8574_I2C_ADDRESS1 0x39 +#endif +#ifndef GPIO_PCF8574_I2C_ADDRESS2 + #define GPIO_PCF8574_I2C_ADDRESS2 0x3A +#endif +#ifndef GPIO_PCF8574_I2C_ADDRESS3 + #define GPIO_PCF8574_I2C_ADDRESS3 0x3B +#endif +#ifndef GPIO_PCF8574_I2C_ADDRESS4 + #define GPIO_PCF8574_I2C_ADDRESS4 0x3C +#endif + +class GpioPcf8574 : public Gpio { + public: + // scan for PCF8574 device + bool init(); + + void pinMode(int pin, int mode); + + int digitalRead(int pin); + + void digitalWrite(int pin, int value); + + private: + bool found = false; + + uint8_t iicAddress[4] = {GPIO_PCF8574_I2C_ADDRESS1, GPIO_PCF8574_I2C_ADDRESS2, GPIO_PCF8574_I2C_ADDRESS3, GPIO_PCF8574_I2C_ADDRESS4}; + + int8_t mode[32]; + uint8_t state[4] = {0, 0, 0, 0}; +}; + +extern GpioPcf8574 gpio; + +#endif diff --git a/src/lib/gpioEx/pcf8575/Pcf8575.cpp b/src/lib/gpioEx/pcf8575/Pcf8575.cpp new file mode 100755 index 0000000..55bf8d4 --- /dev/null +++ b/src/lib/gpioEx/pcf8575/Pcf8575.cpp @@ -0,0 +1,105 @@ +// ----------------------------------------------------------------------------------- +// I2C PCF8575 GPIO support + +#include "Pcf8575.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == X8575 + +// check for up to two PCF8575 devices on the I2C bus +bool GpioPcf8575::init() { + static bool initialized = false; + if (initialized) return found; + + for (int i = 0; i < 32; i++) mode[i] = -1; + + HAL_WIRE.begin(); + HAL_WIRE_SET_CLOCK(); + + // check to be sure we find all devices + for (int i = 0; i < GPIO_PCF8575_I2C_NUM_DEVICES; i++) { + HAL_WIRE.beginTransmission(iicAddress[i]); + uint8_t error = HAL_WIRE.endTransmission(); + if (error) { + // try again + HAL_WIRE.beginTransmission(iicAddress[i]); + uint8_t error = HAL_WIRE.endTransmission(); + if (error) { + if (DEBUG != OFF) { DF("WRN: Gpio.init(), PCF8575 (I2C 0x"); SERIAL_DEBUG.print(iicAddress[i], HEX); DLF(") not found"); } + found = false; + return false; + } + } + } + + found = true; + return found; +} + +// set GPIO pin (0 to 31) mode for INPUT or OUTPUT +void GpioPcf8575::pinMode(int pin, int mode) { + if (found && pin >= 0 && pin <= GPIO_PCF8575_I2C_NUM_DEVICES*16 - 1) { + uint8_t device = pin >> 4; + uint8_t devicePin = pin & 0b01111; + + #ifdef INPUT_PULLDOWN + if (mode == INPUT_PULLDOWN) mode = INPUT; + #endif + if (mode == INPUT_PULLUP) mode = INPUT; + if (mode != INPUT && mode != OUTPUT) return; + + if (this->mode[pin] != mode) { + bitWrite(state[device], devicePin, (mode == INPUT) ? 1 : 0); + + HAL_WIRE.beginTransmission(iicAddress[device]); + HAL_WIRE.write(state[device]); + HAL_WIRE.endTransmission(); + + this->mode[pin] = mode; + } + } +} + +// get GPIO pin (0 to 31) state +int GpioPcf8575::digitalRead(int pin) { + if (found && pin >= 0 && pin <= GPIO_PCF8575_I2C_NUM_DEVICES*16 - 1 && mode[pin] > 0) { + uint8_t device = pin >> 4; + uint8_t devicePin = pin & 0b01111; + + if (mode[pin] == INPUT) { + + HAL_WIRE.requestFrom(iicAddress[device], (uint8_t)2); + + unsigned long timeout = millis() + 1000UL; + do { if ((long)(timeout - millis()) < 0) return 0; } while (Wire.available() < 2); + + uint16_t data = HAL_WIRE.read(); + data |= HAL_WIRE.read() << 8; + + return bitRead(data, devicePin); + } else + return bitRead(state[device], devicePin); + } + + return 0; +} + +// set GPIO pin (0 to 31) state +void GpioPcf8575::digitalWrite(int pin, int value) { + if (found && pin >= 0 && pin <= GPIO_PCF8575_I2C_NUM_DEVICES*16 - 1 && mode[pin] > 0) { + if (mode[pin] == OUTPUT) { + uint8_t device = pin >> 4; + uint8_t devicePin = pin & 0b01111; + + bitWrite(state[device], devicePin, (value == 0) ? 0 : 1); + + HAL_WIRE.beginTransmission(iicAddress[device]); + HAL_WIRE.write(state[device] & 0x000FF); + HAL_WIRE.write((state[device] & 0x0FF00) >> 8); + HAL_WIRE.endTransmission(); + } + } +} + +GpioPcf8575 gpio; + +#endif diff --git a/src/lib/gpioEx/pcf8575/Pcf8575.h b/src/lib/gpioEx/pcf8575/Pcf8575.h new file mode 100755 index 0000000..6725ac2 --- /dev/null +++ b/src/lib/gpioEx/pcf8575/Pcf8575.h @@ -0,0 +1,42 @@ +// ----------------------------------------------------------------------------------- +// I2C PCF8575 GPIO support +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == X8575 + +#ifndef GPIO_PCF8575_I2C_NUM_DEVICES + #define GPIO_PCF8575_I2C_NUM_DEVICES 1 +#endif + +#ifndef GPIO_PCF8575_I2C_ADDRESS1 + #define GPIO_PCF8575_I2C_ADDRESS1 0x20 +#endif +#ifndef GPIO_PCF8575_I2C_ADDRESS2 + #define GPIO_PCF8575_I2C_ADDRESS2 0x21 +#endif + +class GpioPcf8575 : public Gpio { + public: + // scan for PCF8575 device + bool init(); + + void pinMode(int pin, int mode); + + int digitalRead(int pin); + + void digitalWrite(int pin, int value); + + private: + bool found = false; + + uint8_t iicAddress[2] = {GPIO_PCF8575_I2C_ADDRESS1, GPIO_PCF8575_I2C_ADDRESS2}; + + int8_t mode[32]; + uint16_t state[2] = {0, 0}; +}; + +extern GpioPcf8575 gpio; + +#endif diff --git a/src/lib/gpioEx/ssr74HC595/Ssr74HC595.cpp b/src/lib/gpioEx/ssr74HC595/Ssr74HC595.cpp new file mode 100755 index 0000000..81f88b2 --- /dev/null +++ b/src/lib/gpioEx/ssr74HC595/Ssr74HC595.cpp @@ -0,0 +1,127 @@ +// ----------------------------------------------------------------------------------- +// 74HC595 GPIO support + +#include "Ssr74HC595.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == SSR74HC595 + +#if !defined(GPIO_SSR74HC595_LATCH_PIN) || GPIO_SSR74HC595_LATCH_PIN == OFF + #error "GPIO device SSR74HC595 GPIO_SSR74HC595_LATCH_PIN must be present and not OFF." +#endif +#if !defined(GPIO_SSR74HC595_CLOCK_PIN) || GPIO_SSR74HC595_CLOCK_PIN == OFF + #error "GPIO device SSR74HC595 GPIO_SSR74HC595_CLOCK_PIN must be present and not OFF." +#endif +#if !defined(GPIO_SSR74HC595_DATA_PIN) || GPIO_SSR74HC595_DATA_PIN == OFF + #error "GPIO device SSR74HC595 GPIO_SSR74HC595_DATA_PIN must be present and not OFF." +#endif +#ifndef GPIO_SSR74HC595_COUNT + #error "GPIO device SSR74HC595 GPIO_SSR74HC595_COUNT must be present." +#endif +#if GPIO_SSR74HC595_COUNT != 8 && GPIO_SSR74HC595_COUNT != 16 && GPIO_SSR74HC595_COUNT != 24 && GPIO_SSR74HC595_COUNT != 32 + #error "GPIO device SSR74HC595 supports GPIO_SSR74HC595_COUNT of 8, 16, 24, or 32 only." +#endif + +#ifdef ESP32 + // ESP32 GPIO SSR74HC595 macros (if used, code below only works for pins 0 to 31) + #define GPIO_SSR74HC595_LATCH_LOW() { GPIO.out_w1tc = ((uint32_t)1 << GPIO_SSR74HC595_LATCH_PIN); } + #define GPIO_SSR74HC595_LATCH_HIGH() { GPIO.out_w1ts = ((uint32_t)1 << GPIO_SSR74HC595_LATCH_PIN); } + #define GPIO_SSR74HC595_CLOCK_LOW() { GPIO.out_w1tc = ((uint32_t)1 << GPIO_SSR74HC595_CLOCK_PIN); } + #define GPIO_SSR74HC595_CLOCK_HIGH() { GPIO.out_w1ts = ((uint32_t)1 << GPIO_SSR74HC595_CLOCK_PIN); } + #define GPIO_SSR74HC595_DATA_LOW() { GPIO.out_w1tc = ((uint32_t)1 << GPIO_SSR74HC595_DATA_PIN); } + #define GPIO_SSR74HC595_DATA_HIGH() { GPIO.out_w1ts = ((uint32_t)1 << GPIO_SSR74HC595_DATA_PIN); } +#else + // generic GPIO SSR74HC595 macros + #define GPIO_SSR74HC595_LATCH_LOW() digitalWrite(GPIO_SSR74HC595_LATCH_PIN, LOW) + #define GPIO_SSR74HC595_LATCH_HIGH() digitalWrite(GPIO_SSR74HC595_LATCH_PIN, HIGH) + #define GPIO_SSR74HC595_CLOCK_LOW() digitalWrite(GPIO_SSR74HC595_CLOCK_PIN, LOW) + #define GPIO_SSR74HC595_CLOCK_HIGH() digitalWrite(GPIO_SSR74HC595_CLOCK_PIN, HIGH) + #define GPIO_SSR74HC595_DATA_LOW() digitalWrite(GPIO_SSR74HC595_DATA_PIN, LOW) + #define GPIO_SSR74HC595_DATA_HIGH() digitalWrite(GPIO_SSR74HC595_DATA_PIN, HIGH) +#endif + +// designed for a 20MHz max bit rate +IRAM_ATTR void shiftOut20MHz(uint8_t val) { + if ((val & 0b10000000) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); + if ((val & 0b01000000) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); + if ((val & 0b00100000) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); + if ((val & 0b00010000) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); + if ((val & 0b00001000) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); + if ((val & 0b00000100) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); + if ((val & 0b00000010) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); + if ((val & 0b00000001) == 0) { GPIO_SSR74HC595_DATA_LOW(); } else { GPIO_SSR74HC595_DATA_HIGH(); } + GPIO_SSR74HC595_CLOCK_HIGH(); + GPIO_SSR74HC595_CLOCK_LOW(); +} + +// get device ready +bool GpioSsr74HC595::init() { + static bool initialized = false; + if (initialized) return found; + + ::pinMode(GPIO_SSR74HC595_LATCH_PIN, OUTPUT); + ::pinMode(GPIO_SSR74HC595_CLOCK_PIN, OUTPUT); + ::pinMode(GPIO_SSR74HC595_DATA_PIN, OUTPUT); + + for (int i = 0; i < GPIO_SSR74HC595_COUNT; i++) { + mode[i] = OUTPUT; + state[i] = false; + } + + VLF("MSG: GPIO, SSR74HC595 Initialized"); + + found = true; + return found; +} + +// set GPIO pin mode for INPUT, INPUT_PULLUP, or OUTPUT (input does nothing always, false) +void GpioSsr74HC595::pinMode(int pin, int mode) { + if (found && pin >= 0 && pin < GPIO_SSR74HC595_COUNT) { + #ifdef INPUT_PULLDOWN + if (mode == INPUT_PULLDOWN) mode = INPUT; + #endif + this->mode[pin] = mode; + } +} + +// up to four eight channel 74HC595 GPIOs are supported, this gets the last set value (output only) +int GpioSsr74HC595::digitalRead(int pin) { + if (found && pin >= 0 && pin < GPIO_SSR74HC595_COUNT) { + return state[pin]; + } else return 0; +} + +// up to four eight channel 74HC595 GPIOs are supported, this sets each output on or off +void GpioSsr74HC595::digitalWrite(int pin, int value) { + if (found && pin >= 0 && pin < GPIO_SSR74HC595_COUNT) { + cli(); + state[pin] = value; + if (mode[pin] == OUTPUT) { + if (value) bitSet(register_value, pin); else bitClear(register_value, pin); + GPIO_SSR74HC595_LATCH_LOW(); + if (GPIO_SSR74HC595_COUNT >= 32) shiftOut20MHz((register_value>>24) & 0xff); + if (GPIO_SSR74HC595_COUNT >= 24) shiftOut20MHz((register_value>>16) & 0xff); + if (GPIO_SSR74HC595_COUNT >= 16) shiftOut20MHz((register_value>>8) & 0xff); + shiftOut20MHz((register_value) & 0xff); + GPIO_SSR74HC595_LATCH_HIGH(); + } + sei(); + } else return; +} + +GpioSsr74HC595 gpio; + +#endif diff --git a/src/lib/gpioEx/ssr74HC595/Ssr74HC595.h b/src/lib/gpioEx/ssr74HC595/Ssr74HC595.h new file mode 100755 index 0000000..2b94e9f --- /dev/null +++ b/src/lib/gpioEx/ssr74HC595/Ssr74HC595.h @@ -0,0 +1,33 @@ +// ----------------------------------------------------------------------------------- +// 74HC595 GPIO support +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == SSR74HC595 + +class GpioSsr74HC595 : public Gpio { + public: + // init for SSR74HC595 device + bool init(); + + // set GPIO pin mode for INPUT, INPUT_PULLUP, or OUTPUT (input does nothing always, false) + void pinMode(int pin, int mode); + + // up to four eight channel 74HC595 GPIOs are supported, this gets the last set value (output only) + int digitalRead(int pin); + + // up to four eight channel 74HC595 GPIOs are supported, this sets each output on or off + void digitalWrite(int pin, int value); + + private: + uint32_t register_value = 0; + bool found = false; + + int8_t mode[32]; + bool state[32]; +}; + +extern GpioSsr74HC595 gpio; + +#endif diff --git a/src/lib/gpioEx/sws/Sws.cpp b/src/lib/gpioEx/sws/Sws.cpp new file mode 100755 index 0000000..51a8e19 --- /dev/null +++ b/src/lib/gpioEx/sws/Sws.cpp @@ -0,0 +1,146 @@ +// ----------------------------------------------------------------------------------- +// Smart Web Server GPIO support (optional when SWS encoders are not used) + +#include "Sws.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == SWS + +#include "../../tasks/OnTask.h" +#include "../../../libApp/commands/ProcessCmds.h" + +void swsGpioWrapper() { gpio.poll(); } + +// check for SWS device +bool GpioSws::init() { + if (found) return true; + + VF("MSG: GPIO, start SwsGpio monitor task (rate 1000ms priority 7)... "); + if (tasks.add(1000, 0, true, 7, swsGpioWrapper, "SwsGpio")) { VLF("success"); } else { VLF("FAILED!"); return false; } + + found = true; + return true; +} + +// no command processing +bool GpioSws::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + if (command[0] == 'G') { + if (command[1] == 'X' && parameter[2] == 0) { + // :GXGA# Get Gpio presence + // Returns: 8# on success or 0 (error) if not present + if (parameter[0] == 'G' && parameter[1] == 'A') { + strcpy(reply, "8"); + + active = true; + lastActiveTimeMs = millis(); + *numericReply = false; + } else + + // :GXGO# Get Gpio Output mode or State + // Returns: Value + if (parameter[0] == 'G' && parameter[1] == 'O') { + for (int i = 0; i < 8; i++) { + char newMode = 'X'; + if (mode[i] == INPUT) { if (virtualRead[i] == LOW) newMode = 'i'; else newMode = 'I'; } else + if (mode[i] == INPUT_PULLUP) { if (virtualRead[i] == LOW) newMode = 'u'; else newMode = 'U'; } else + if (mode[i] == OUTPUT) newMode = 'O'; + + if (virtualMode[i] != newMode) { + reply[i] = newMode; + virtualMode[i] = newMode; + } else { + if (virtualMode[i] == 'O') { + if (virtualWrite[i] == 0) reply[i] = '0'; else + if (virtualWrite[i] == 1) reply[i] = '1'; else + if (virtualWrite[i] >= 2) reply[i] = (char)((virtualWrite[i] - 2) + 128); + } else reply[i] = newMode; + } + + reply[i + 1] = 0; + } + + active = true; + lastActiveTimeMs = millis(); + *numericReply = false; + } else return false; + } else return false; + } else + + if (command[0] == 'S') { + if (command[1] == 'X') { + if (parameter[2] != ',') { *commandError = CE_PARAM_FORM; return true; } + + // :SXG[n],[v]# Set Gpio input [n]umber to [v]alue + // Return: 0 on failure + // 1 on success + if (parameter[0] == 'G' && parameter[1] >= '0' && parameter[1] <= '7') { + int i = parameter[1] - '0'; + int v = atol(¶meter[3]); + if (v == 0 || v == 1) { + virtualRead[i] = v; + } else *commandError = CE_PARAM_RANGE; + + active = true; + lastActiveTimeMs = millis(); + } else return false; + } else return false; + } else return false; + + return true; +} + +// set GPIO pin (0 to 7) mode for INPUT, INPUT_PULLUP, or OUTPUT +void GpioSws::pinMode(int pin, int mode) { + if (pin >= 0 && pin <= 7) { + #ifdef INPUT_PULLDOWN + if (mode == INPUT_PULLDOWN) mode = INPUT; + #endif + this->mode[pin] = mode; + } +} + +// one eight channel SWS GPIO is supported, this gets the last set value +int GpioSws::digitalRead(int pin) { + if (readyStage2 && pin >= 0 && pin <= 7) { + if (mode[pin] == INPUT || mode[pin] == INPUT_PULLUP) { + return virtualRead[pin]; + } else return state[pin]; + } else return 0; +} + +// one eight channel SWS GPIO is supported, this sets each output on or off +void GpioSws::digitalWrite(int pin, int value) { + if (pin >= 0 && pin <= 7) { + state[pin] = value; + virtualWrite[pin] = value; + } else return; +} + +// one eight channel SWS GPIO is supported +void GpioSws::analogWrite(int pin, int value) { + value = (value*127)/ANALOG_WRITE_RANGE + 2; + if (pin >= 0 && pin <= 7 && value >= 2 && value <= 129) { + state[pin] = value; + virtualWrite[pin] = value; + } else return; +} + +// monitor SWS presence +void GpioSws::poll() { + if (lateInitError) return; + + // init wait for indication of SWS presence + if (active && !readyStage1) { startTimeMs = millis(); readyStage1 = true; VLF("MSG: GpioSws, active"); } + + // init delay for synchronization + if (readyStage1 && !readyStage2 && millis() - startTimeMs > 2000U) { readyStage2 = true; VLF("MSG: GpioSws, ready"); } + + // init timeout + if (!readyStage1 && !readyStage2 && millis() - startTimeMs > 60000U) { lateInitError = true; DLF("ERR: GpioSws, connection failed"); } + + // running timeout + if (readyStage2 && millis() - lastActiveTimeMs > 5000U) { startTimeMs = millis(); lastActiveTimeMs = millis(); active = false; readyStage1 = false; readyStage2 = false; DLF("WRN: GpioSws, connection restart"); } +} + +GpioSws gpio; + +#endif diff --git a/src/lib/gpioEx/sws/Sws.h b/src/lib/gpioEx/sws/Sws.h new file mode 100755 index 0000000..cedc39b --- /dev/null +++ b/src/lib/gpioEx/sws/Sws.h @@ -0,0 +1,52 @@ +// ----------------------------------------------------------------------------------- +// Smart Web Server GPIO support (optional when SWS Encoders are not used) +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == SWS + +#include "../../commands/CommandErrors.h" + +class GpioSws : public Gpio { + public: + // scan for SWS device + bool init(); + + // process any gpio commands + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + void pinMode(int pin, int mode); + + // one eight channel SWS GPIO is supported, this gets the last set value + int digitalRead(int pin); + + // one eight channel SWS GPIO is supported, this sets each output on or off + void digitalWrite(int pin, int value); + + // one eight channel SWS GPIO is supported + void analogWrite(int pin, int value); + + // monitor SWS presence + void poll(); + + private: + + bool found = false; + bool active = false; + bool readyStage1 = false; + bool readyStage2 = false; + unsigned long startTimeMs = 0; + unsigned long lastActiveTimeMs = 0; + + bool virtualRead[8] = {false, false, false, false, false, false, false, false}; + uint8_t virtualWrite[8] = {false, false, false, false, false, false, false, false}; + char virtualMode[8] = {'X', 'X', 'X', 'X', 'X', 'X', 'X', 'X' }; + + int8_t mode[8] = {OFF, OFF, OFF, OFF, OFF, OFF, OFF, OFF}; + bool state[8] = {false, false, false, false, false, false, false, false}; +}; + +extern GpioSws gpio; + +#endif diff --git a/src/lib/gpioEx/tca9555/Tca9555.cpp b/src/lib/gpioEx/tca9555/Tca9555.cpp new file mode 100755 index 0000000..9e1bb35 --- /dev/null +++ b/src/lib/gpioEx/tca9555/Tca9555.cpp @@ -0,0 +1,65 @@ +// ----------------------------------------------------------------------------------- +// I2C TCA9555 GPIO support + +#include "Tca9555.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == X9555 + +#ifndef GPIO_TCA9555_I2C_ADDRESS + #define GPIO_TCA9555_I2C_ADDRESS 0x27 +#endif + +#include // https://www.arduino.cc/reference/en/libraries/tca9555/ + +TCA9555 tca(GPIO_TCA9555_I2C_ADDRESS, &HAL_WIRE); // might need to change this I2C Address? + +// check for TCA9555 device on the I2C bus +bool GpioTca9555::init() { + static bool initialized = false; + if (initialized) return found; + + if (tca.begin()) { + found = true; + for (int i = 0; i < 16; i++) { tca.pinMode1(i, INPUT); } + } else { found = false; DLF("WRN: Gpio.init(), TCA9555 (I2C 0x"); if (DEBUG != OFF) SERIAL_DEBUG.print(GPIO_TCA9555_I2C_ADDRESS, HEX); DLF(") not found"); } + HAL_WIRE_SET_CLOCK(); + + return found; +} + +// set GPIO pin (0 to 15) mode for INPUT, INPUT_PULLUP, or OUTPUT +void GpioTca9555::pinMode(int pin, int mode) { + if (found && pin >= 0 && pin <= 15) { + #ifdef INPUT_PULLDOWN + if (mode == INPUT_PULLDOWN) mode = INPUT; + #endif + if (mode == INPUT_PULLUP) mode = INPUT; + tca.pinMode1(pin, mode); + this->mode[pin] = mode; + } +} + +// one sixteen channel Tca9555 GPIO is supported, this gets the last set value +int GpioTca9555::digitalRead(int pin) { + if (found && pin >= 0 && pin <= 15) { + if (mode[pin] == INPUT || mode[pin] == INPUT_PULLUP) { + return tca.read1(pin); + } else return state[pin]; + } else return 0; +} + +// one sixteen channel Tca9555 GPIO is supported, this sets each output on or off +void GpioTca9555::digitalWrite(int pin, int value) { + if (found && pin >= 0 && pin <= 15) { + state[pin] = value; + if (mode[pin] == OUTPUT) { + tca.write1(pin, value); + } else { + if (value == HIGH) tca.pinMode1(pin, INPUT_PULLUP); else tca.pinMode1(pin, INPUT); + } + } else return; +} + +GpioTca9555 gpio; + +#endif diff --git a/src/lib/gpioEx/tca9555/Tca9555.h b/src/lib/gpioEx/tca9555/Tca9555.h new file mode 100755 index 0000000..b73eaa5 --- /dev/null +++ b/src/lib/gpioEx/tca9555/Tca9555.h @@ -0,0 +1,31 @@ +// ----------------------------------------------------------------------------------- +// I2C TCA9555 GPIO support +#pragma once + +#include "../GpioBase.h" + +#if defined(GPIO_DEVICE) && GPIO_DEVICE == X9555 + +class GpioTca9555 : public Gpio { + public: + // scan for TCA9555 device + bool init(); + + void pinMode(int pin, int mode); + + // one sixteen channel TCA9555 GPIO is supported, this gets the last set value + int digitalRead(int pin); + + // one sixteen channel TCA9555 GPIO is supported, this sets each output on or off + void digitalWrite(int pin, int value); + + private: + bool found = false; + + int8_t mode[16] = { INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT, INPUT }; + bool state[16] = { false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false }; +}; + +extern GpioTca9555 gpio; + +#endif diff --git a/src/lib/math/Crc.cpp b/src/lib/math/Crc.cpp new file mode 100755 index 0000000..dbd75bd --- /dev/null +++ b/src/lib/math/Crc.cpp @@ -0,0 +1,39 @@ +// Crc.cpp +#include "Crc.h" + +// CRC-24/OPENPGP table (poly=0x864CFB), MSB-first, no reflect. +// This out-of-class definition avoids linker/ODR issues on Arduino/ESP32 toolchains. +const uint32_t Crc24::table_[256] = { + 0x000000u, 0x864CFBu, 0x8AD50Du, 0x0C99F6u, 0x93E6E1u, 0x15AA1Au, 0x1933ECu, 0x9F7F17u, + 0xA18139u, 0x27CDC2u, 0x2B5434u, 0xAD18CFu, 0x3267D8u, 0xB42B23u, 0xB8B2D5u, 0x3EFE2Eu, + 0xC54E89u, 0x430272u, 0x4F9B84u, 0xC9D77Fu, 0x56A868u, 0xD0E493u, 0xDC7D65u, 0x5A319Eu, + 0x64CFB0u, 0xE2834Bu, 0xEE1ABDu, 0x685646u, 0xF72951u, 0x7165AAu, 0x7DFC5Cu, 0xFBB0A7u, + 0x0CD1E9u, 0x8A9D12u, 0x8604E4u, 0x00481Fu, 0x9F3708u, 0x197BF3u, 0x15E205u, 0x93AEFEu, + 0xAD50D0u, 0x2B1C2Bu, 0x2785DDu, 0xA1C926u, 0x3EB631u, 0xB8FACAu, 0xB4633Cu, 0x322FC7u, + 0xC99F60u, 0x4FD39Bu, 0x434A6Du, 0xC50696u, 0x5A7981u, 0xDC357Au, 0xD0AC8Cu, 0x56E077u, + 0x681E59u, 0xEE52A2u, 0xE2CB54u, 0x6487AFu, 0xFBF8B8u, 0x7DB443u, 0x712DB5u, 0xF7614Eu, + 0x19A3D2u, 0x9FEF29u, 0x9376DFu, 0x153A24u, 0x8A4533u, 0x0C09C8u, 0x00903Eu, 0x86DCC5u, + 0xB822EBu, 0x3E6E10u, 0x32F7E6u, 0xB4BB1Du, 0x2BC40Au, 0xAD88F1u, 0xA11107u, 0x275DFCu, + 0xDCED5Bu, 0x5AA1A0u, 0x563856u, 0xD074ADu, 0x4F0BBAu, 0xC94741u, 0xC5DEB7u, 0x43924Cu, + 0x7D6C62u, 0xFB2099u, 0xF7B96Fu, 0x71F594u, 0xEE8A83u, 0x68C678u, 0x645F8Eu, 0xE21375u, + 0x15723Bu, 0x933EC0u, 0x9FA736u, 0x19EBCDu, 0x8694DAu, 0x00D821u, 0x0C41D7u, 0x8A0D2Cu, + 0xB4F302u, 0x32BFF9u, 0x3E260Fu, 0xB86AF4u, 0x2715E3u, 0xA15918u, 0xADC0EEu, 0x2B8C15u, + 0xD03CB2u, 0x567049u, 0x5AE9BFu, 0xDCA544u, 0x43DA53u, 0xC596A8u, 0xC90F5Eu, 0x4F43A5u, + 0x71BD8Bu, 0xF7F170u, 0xFB6886u, 0x7D247Du, 0xE25B6Au, 0x641791u, 0x688E67u, 0xEEC29Cu, + 0x3347A4u, 0xB50B5Fu, 0xB992A9u, 0x3FDE52u, 0xA0A145u, 0x26EDBEu, 0x2A7448u, 0xAC38B3u, + 0x92C69Du, 0x148A66u, 0x181390u, 0x9E5F6Bu, 0x01207Cu, 0x876C87u, 0x8BF571u, 0x0DB98Au, + 0xF6092Du, 0x7045D6u, 0x7CDC20u, 0xFA90DBu, 0x65EFCCu, 0xE3A337u, 0xEF3AC1u, 0x69763Au, + 0x578814u, 0xD1C4EFu, 0xDD5D19u, 0x5B11E2u, 0xC46EF5u, 0x42220Eu, 0x4EBBF8u, 0xC8F703u, + 0x3F964Du, 0xB9DAB6u, 0xB54340u, 0x330FBBu, 0xAC70ACu, 0x2A3C57u, 0x26A5A1u, 0xA0E95Au, + 0x9E1774u, 0x185B8Fu, 0x14C279u, 0x928E82u, 0x0DF195u, 0x8BBD6Eu, 0x872498u, 0x016863u, + 0xFAD8C4u, 0x7C943Fu, 0x700DC9u, 0xF64132u, 0x693E25u, 0xEF72DEu, 0xE3EB28u, 0x65A7D3u, + 0x5B59FDu, 0xDD1506u, 0xD18CF0u, 0x57C00Bu, 0xC8BF1Cu, 0x4EF3E7u, 0x426A11u, 0xC426EAu, + 0x2AE476u, 0xACA88Du, 0xA0317Bu, 0x267D80u, 0xB90297u, 0x3F4E6Cu, 0x33D79Au, 0xB59B61u, + 0x8B654Fu, 0x0D29B4u, 0x01B042u, 0x87FCB9u, 0x1883AEu, 0x9ECF55u, 0x9256A3u, 0x141A58u, + 0xEFAAFFu, 0x69E604u, 0x657FF2u, 0xE33309u, 0x7C4C1Eu, 0xFA00E5u, 0xF69913u, 0x70D5E8u, + 0x4E2BC6u, 0xC8673Du, 0xC4FECBu, 0x42B230u, 0xDDCD27u, 0x5B81DCu, 0x57182Au, 0xD154D1u, + 0x26359Fu, 0xA07964u, 0xACE092u, 0x2AAC69u, 0xB5D37Eu, 0x339F85u, 0x3F0673u, 0xB94A88u, + 0x87B4A6u, 0x01F85Du, 0x0D61ABu, 0x8B2D50u, 0x145247u, 0x921EBCu, 0x9E874Au, 0x18CBB1u, + 0xE37B16u, 0x6537EDu, 0x69AE1Bu, 0xEFE2E0u, 0x709DF7u, 0xF6D10Cu, 0xFA48FAu, 0x7C0401u, + 0x42FA2Fu, 0xC4B6D4u, 0xC82F22u, 0x4E63D9u, 0xD11CCEu, 0x575035u, 0x5BC9C3u, 0xDD8538u +}; diff --git a/src/lib/math/Crc.h b/src/lib/math/Crc.h new file mode 100755 index 0000000..38731e7 --- /dev/null +++ b/src/lib/math/Crc.h @@ -0,0 +1,205 @@ +// ----------------------------------------------------------------------------------- +// Crc, crc8/crc16/crc32 helpers with optional cooperative yielding + +#pragma once +#include +#include + +#include "../tasks/OnTask.h" + +#ifndef CRC_YIELD_BYTES + #define CRC_YIELD_BYTES 64 +#endif + +class Crc8 { +public: + // crc8/atm (aka crc-8) + // poly=0x07 init=0x00 refin=false refout=false xorout=0x00 + + static inline uint8_t init() { return 0x00u; } + + static inline uint8_t updateByte(uint8_t crc, uint8_t data) { + crc ^= data; + for (uint8_t i = 0; i < 8; i++) { + crc = (crc & 0x80u) ? (uint8_t)((crc << 1) ^ 0x07u) : (uint8_t)(crc << 1); + } + return crc; + } + + static inline uint8_t update(uint8_t crc, const uint8_t* data, uint16_t len) { + uint16_t sinceYield = 0; + for (uint16_t i = 0; i < len; i++) { + crc = updateByte(crc, data[i]); + #if CRC_YIELD_BYTES > 0 + if (++sinceYield >= CRC_YIELD_BYTES) { sinceYield = 0; tasks.yield(); } + #endif + } + return crc; + } + + static inline uint8_t compute(const uint8_t* data, uint16_t len) { + return update(init(), data, len); + } + + // Salted ASCII helper (optional convenience) + static inline uint8_t saltedAscii(const char* saltAscii, const char* textAscii) { + if (!saltAscii) saltAscii = ""; + if (!textAscii) textAscii = ""; + uint8_t crc = init(); + crc = update(crc, (const uint8_t*)saltAscii, (uint16_t)strlen(saltAscii)); + crc = update(crc, (const uint8_t*)textAscii, (uint16_t)strlen(textAscii)); + return crc; + } +}; + +class Crc16 { +public: + // crc16/ccitt-false + // poly=0x1021 init=0xffff refin=false refout=false xorout=0x0000 + + static inline uint16_t init() { return 0xFFFFu; } + + static inline uint16_t updateByte(uint16_t crc, uint8_t data) { + crc ^= (uint16_t)data << 8; + for (uint8_t i = 0; i < 8; i++) { + crc = (crc & 0x8000u) ? (uint16_t)((crc << 1) ^ 0x1021u) : (uint16_t)(crc << 1); + } + return crc; + } + + static inline uint16_t update(uint16_t crc, const uint8_t* data, uint16_t len) { + uint16_t sinceYield = 0; + for (uint16_t i = 0; i < len; i++) { + crc = updateByte(crc, data[i]); + #if CRC_YIELD_BYTES > 0 + if (++sinceYield >= CRC_YIELD_BYTES) { sinceYield = 0; tasks.yield(); } + #endif + } + return crc; + } + + static inline uint16_t compute(const uint8_t* data, uint16_t len) { + return update(init(), data, len); + } + + // Salted ASCII helpers (for volume/partition IDs, tags, etc.) + static inline uint16_t saltedAscii(const char* saltAscii, const char* textAscii) { + if (!saltAscii) saltAscii = ""; + if (!textAscii) textAscii = ""; + uint16_t crc = init(); + crc = update(crc, (const uint8_t*)saltAscii, (uint16_t)strlen(saltAscii)); + crc = update(crc, (const uint8_t*)textAscii, (uint16_t)strlen(textAscii)); + return crc; + } + + static inline uint16_t saltedAscii2(const char* saltAscii, const char* aAscii, const char* bAscii) { + if (!saltAscii) saltAscii = ""; + if (!aAscii) aAscii = ""; + if (!bAscii) bAscii = ""; + uint16_t crc = init(); + crc = update(crc, (const uint8_t*)saltAscii, (uint16_t)strlen(saltAscii)); + crc = update(crc, (const uint8_t*)aAscii, (uint16_t)strlen(aAscii)); + crc = update(crc, (const uint8_t*)bAscii, (uint16_t)strlen(bAscii)); + return crc; + } +}; + +/*class Crc24 { +public: + // CRC-24/OPENPGP + // poly=0x864CFB init=0xB704CE refin=false refout=false xorout=0x000000 + + static inline uint32_t init() { return 0x00B704CEu; } // 24-bit + + static inline uint32_t updateByte(uint32_t crc, uint8_t data) { + crc ^= (uint32_t)data << 16; // align byte to top of 24-bit register + for (uint8_t i = 0; i < 8; i++) { + crc = (crc & 0x800000u) ? ((crc << 1) ^ 0x864CFBu) : (crc << 1); + crc &= 0xFFFFFFu; + } + return crc; + } + + static inline uint32_t update(uint32_t crc, const uint8_t* data, uint16_t len) { + uint16_t sinceYield = 0; + for (uint16_t i = 0; i < len; i++) { + crc = updateByte(crc, data[i]); + #if CRC_YIELD_BYTES > 0 + if (++sinceYield >= CRC_YIELD_BYTES) { sinceYield = 0; tasks.yield(); } + #endif + } + return crc & 0xFFFFFFu; + } + + static inline uint32_t compute(const uint8_t* data, uint16_t len) { + return update(init(), data, len) & 0xFFFFFFu; + } +}; +*/ + +class Crc24 { +public: + static inline uint32_t init() { return 0x00B704CEu; } + + static inline uint32_t updateByte(uint32_t crc, uint8_t data) { + const uint8_t idx = (uint8_t)(((crc >> 16) ^ (uint32_t)data) & 0xFFu); + crc = ((crc << 8) & 0xFFFFFFu) ^ table_[idx]; + return crc & 0xFFFFFFu; + } + + static inline uint32_t update(uint32_t crc, const uint8_t* data, uint16_t len) { + for (uint16_t i = 0; i < len; i++) crc = updateByte(crc, data[i]); + return crc & 0xFFFFFFu; + } + + static inline uint32_t compute(const uint8_t* data, uint16_t len) { + return update(init(), data, len); + } + +private: + static const uint32_t table_[256]; +}; + +class Crc32 { +public: + // crc32 (ieee 802.3), bitwise + // poly=0xedb88320 init=0xffffffff xorout=~ + + static inline uint32_t init() { return 0xFFFFFFFFu; } + + static inline uint32_t updateByte(uint32_t crc, uint8_t data) { + crc ^= data; + for (uint8_t b = 0; b < 8; b++) { + uint32_t mask = -(int32_t)(crc & 1u); + crc = (crc >> 1) ^ (0xEDB88320u & mask); + } + return crc; + } + + static inline uint32_t update(uint32_t crc, const uint8_t* data, uint16_t len) { + uint16_t sinceYield = 0; + for (uint16_t i = 0; i < len; i++) { + crc = updateByte(crc, data[i]); + #if CRC_YIELD_BYTES > 0 + if (++sinceYield >= CRC_YIELD_BYTES) { sinceYield = 0; tasks.yield(); } + #endif + } + return crc; + } + + static inline uint32_t final(uint32_t crc) { return ~crc; } + + static inline uint32_t compute(const uint8_t* data, uint16_t len) { + return final(update(init(), data, len)); + } + + // Salted ASCII helper (optional convenience) + static inline uint32_t saltedAscii(const char* saltAscii, const char* textAscii) { + if (!saltAscii) saltAscii = ""; + if (!textAscii) textAscii = ""; + uint32_t crc = init(); + crc = update(crc, (const uint8_t*)saltAscii, (uint16_t)strlen(saltAscii)); + crc = update(crc, (const uint8_t*)textAscii, (uint16_t)strlen(textAscii)); + return final(crc); + } +}; diff --git a/src/lib/nv/Nv.cpp b/src/lib/nv/Nv.cpp new file mode 100755 index 0000000..0874e48 --- /dev/null +++ b/src/lib/nv/Nv.cpp @@ -0,0 +1,191 @@ +// Nv.cpp +#include "Nv.h" + +#include + +#include "../tasks/OnTask.h" + +// -------------------- Backend type selection (per driver) -------------------- + +#if (NV_DRIVER == NV_EEPROM) + + #include "device/EepromArduino.h" + using NvBackend = NvDeviceEepromArduino; + +#elif (NV_DRIVER == NV_ESP) + + #include "device/EepromEmuEsp.h" + using NvBackend = NvDeviceEepromEmuEsp; + +#elif (NV_DRIVER == NV_M0) + + #include "device/EepromEmuM0.h" + using NvBackend = NvDeviceEepromEmuM0; + +#elif (NV_DRIVER == NV_2416) + + #include "device/24xxI2C.h" + using NvBackend = NvDevice24xxI2C<2048u>; + +#elif (NV_DRIVER == NV_2432) + + #include "device/24xxI2C.h" + using NvBackend = NvDevice24xxI2C<4096u>; + +#elif (NV_DRIVER == NV_2464) + + #include "device/24xxI2C.h" + using NvBackend = NvDevice24xxI2C<8192u>; + +#elif (NV_DRIVER == NV_24128) + + #include "device/24xxI2C.h" + using NvBackend = NvDevice24xxI2C<16384u>; + +#elif (NV_DRIVER == NV_24256) + + #include "device/24xxI2C.h" + using NvBackend = NvDevice24xxI2C<32768u>; + +#elif (NV_DRIVER == NV_AT24C32) + + #include "device/24xxI2C.h" + using NvBackend = NvDevice24xxI2C<4096u>; + +#elif (NV_DRIVER == NV_MB85RC32) + + #include "device/Mb85rcI2C.h" + using NvBackend = NvDeviceMb85rcI2C<4096u>; + +#elif (NV_DRIVER == NV_MB85RC64) + + #include "device/Mb85rcI2C.h" + using NvBackend = NvDeviceMb85rcI2C<8192u>; + +#elif (NV_DRIVER == NV_MB85RC256) + + #include "device/Mb85rcI2C.h" + using NvBackend = NvDeviceMb85rcI2C<32768u>; + +#elif (NV_DRIVER == OFF) + + #include "device/Null.h" + using NvBackend = NvDeviceNull; + +#else + #error "Unsupported NV_DRIVER" +#endif + + +// -------------------- Construct backend instance -------------------- +// I2C-backed drivers require ctor(TwoWire&, addr). Others default construct. +// HAL_WIRE and NV_I2C_ADDRESS are expected to be defined in NvsConfig.h for I2C drivers. + +#ifdef NV_DEVICE_I2C + + static NvBackend nvDevice(HAL_WIRE, NV_I2C_ADDRESS); + +#else + + static NvBackend nvDevice; + +#endif + +// -------------------- Optional shim + active pointer -------------------- + +#if (NV_SHIM == NV_SHIM_CACHED) + #include "device/ShimCached.h" + static NvShimCached nvShimCached(nvDevice); + static NvDevice* nvActive = &nvShimCached; + constexpr uint32_t NV_SVC_MS = 20; + +#elif (NV_SHIM == NV_SHIM_DELAYED) + #include "device/ShimDelayedCommit.h" + static NvShimDelayedCommit nvShimDelayedCommit(nvDevice, NV_WAIT_MS); + static NvDevice* nvActive = &nvShimDelayedCommit; + constexpr uint32_t NV_SVC_MS = 100; + +#else + static NvDevice* nvActive = &nvDevice; + constexpr uint32_t NV_SVC_MS = 100; +#endif + + +// -------------------- Volume instance -------------------- +// Bound to nvActive in NvSystem::init(). Partitions/format/mount happen higher up. + +static NvVolume nvVolume; + +// -------------------- NvSystem singleton -------------------- + +static NvSystem nvSystem; + +NvSystem& nv() { return nvSystem; } + + +// -------------------- NvSystem methods -------------------- + +// task callback +void nvServices() { + nv().poll(); +} + +bool NvSystem::init(uint8_t priorityLevel) { + if (ok_) return true; + + #if (NV_DRIVER == OFF) + // If OFF means “NV unavailable”, exit cleanly. + return false; + #endif + + // Init active layer (shim init may call down into device init) + if (!nvActive->init()) return false; + + // Bind volume to active device/shim (does not mount/format) + const NvVolume::Status st = nvVolume.init(*nvActive); + if (st != NvVolume::Status::Ok) return false; + + uint32_t pollingRate = NV_SVC_MS; + + // High endurance means FRAM and we double the polling rate since there is no write wait to worry about + if (device().endurance() == NvDevice::Endurance::High) { pollingRate /= 2U; } + + if (priorityLevel > 7) priorityLevel = 7; + + // start system service task + VF("MSG: Nv, start service task (rate "); V(pollingRate); VF("ms priority "); V((int)priorityLevel); VF(")... "); + // add task for system services, runs at pollingRate intervals + if (tasks.add(pollingRate, 0, true, priorityLevel, nvServices, "SvcNv")) { VLF("success"); } else { VLF("FAILED!"); return false; } + + ok_ = true; + return true; +} + +void NvSystem::wipe() { + static const uint8_t pattern[16] = { 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF }; + int16_t sizeBlocks = nv().device().sizeBytes()/16; + + VF("MSG: Setup, NV (EEPROM/FRAM/Flash/etc.) wiping device ."); + + for (int i = 0; i < sizeBlocks; i++) { + nv().device().writeBlock16(i, pattern); + if (i % 128 == 0) { VF("."); } + } + + nv().device().commit(); + VLF(" done."); + VLF(""); + VLF("MSG: Setup, !!!!!!! REMEMBER TO DISABLE THIS FEATURE IN Config.h !!!!!!!"); + VLF(""); +} + +void NvSystem::poll() { + if (!ok_) return; + if (!canService_()) return; + nvActive->poll(); +} + +NvDevice& NvSystem::device() { return nvDevice; } +NvDevice& NvSystem::active() { return *nvActive; } + +NvVolume& NvSystem::volume() { return nvVolume; } diff --git a/src/lib/nv/Nv.h b/src/lib/nv/Nv.h new file mode 100755 index 0000000..da5d0dc --- /dev/null +++ b/src/lib/nv/Nv.h @@ -0,0 +1,95 @@ +// ----------------------------------------------------------------------------------- +// Nv.h +// +// Non-volatile storage “front door” for the project. +// +// What you get +// - A single global NV system (`nv()`) that owns the selected storage backend +// (real EEPROM, FRAM, or flash/EEPROM-emulation), plus optional shims. +// - A volume manager (`NvVolume`) on top of that device, which provides named partitions. +// - A small KV store (`nv().kv()`) for persisting settings/structures by name. +// +// Typical usage pattern +// 1) nv().init(); // initialize backend + start background service +// 2) nv().volume().mount(...); // mount/format volume as needed (elsewhere) +// 3) nv().kv().init(nv().volume(),"KV");// bind KV store to the "KV" partition +// 4) nv().kv().getOrInit("NAME", obj); // load settings or write defaults +// +// Selecting KV16 vs KV32 +// - This header can build with either KV implementation, chosen at compile time by +// NV_USE_KV32 in NvConfig.h. +// - KV16 (NvKvPartition16) is the compact option for small EEPROM/FRAM. +// - KV32 (NvKvPartition32) supports larger entries/files and uses a wider key. +// - Call sites should NOT care which is used: prefer `KvPartition::Status::Ok` and +// access the store through `nv().kv()`. +// +// Background servicing +// - Background servicing is automatic using lib/OnTask, additionally +// setGate() can temporarily suspend this servicing when the system is busy. +// +// Notes for new users +// - If NV_DRIVER is OFF, nv().init() will fail (storage disabled). +// - This layer is intentionally small and deterministic: fixed-size partition, +// overwrite-in-place updates, and a rebuild-on-boot allocation scan. +// ----------------------------------------------------------------------------------- +#pragma once + +#include "../../Common.h" + +#include "device/NvDeviceBase.h" + +#include "NvConfig.h" +#include "NvVolume.h" + +#if NV_USE_KV32 == ON + #include "NvKvPartition32.h" + #define KvPartition KvPartition32 +#else + #include "NvKvPartition16.h" + #define KvPartition KvPartition16 +#endif + +class NvSystem { +public: + // Initializes backend (and optional shim), then binds NvVolume to nvActive. + bool init(uint8_t priorityLevel = 7); + + // Overwrite the entire associated device with 0xFF + void wipe(); + + // Gate on xBusy status + void setGate(bool *gate) { gate_ = gate; } + + bool ok() const { return ok_; } + + // Raw backend device (selected by NV_DRIVER) + NvDevice& device(); + + // Device you should use everywhere (raw or shimmed) + NvDevice& active(); + + // Volume manager bound to active() + NvVolume& volume(); + + void poll(); + + #if NV_USE_KV32 == ON + KvPartition32& kv() { return kv_; } + #else + KvPartition16& kv() { return kv_; } + #endif + +private: + bool canService_() const { return (gate_ == nullptr) ? true : !(*gate_); } + + #if NV_USE_KV32 == ON + KvPartition32 kv_; + #else + KvPartition16 kv_; + #endif + + bool ok_ = false; + bool *gate_ = nullptr; +}; + +NvSystem& nv(); diff --git a/src/lib/nv/NvConfig.h b/src/lib/nv/NvConfig.h new file mode 100755 index 0000000..55dae67 --- /dev/null +++ b/src/lib/nv/NvConfig.h @@ -0,0 +1,88 @@ +// NvConfig.h +#pragma once +#include + +// Provide driver-dependent defaults for: +// - E2END (sizeBytes = E2END + 1) +// - NV_I2C_ADDRESS (7-bit I2C addr) [I2C devices only] +// - NV_WAIT_MS (only meaningful for emulated EEPROM backends) +// + +#ifndef NV_DRIVER + #error "NV_DRIVER must be defined before including NvConfig.h" +#endif + +// OFF = KvPartition16 (16B blocks / v1), ON = KvPartition32 (virtual 32B blocks / v2) +#ifndef NV_USE_KV32 + #define NV_USE_KV32 OFF +#endif + +// NV_SHIM options +#define NV_SHIM_OFF 0 +#define NV_SHIM_CACHED 1 +#define NV_SHIM_DELAYED 2 + +// If your VM requires size to be multiple-of-16, enforce it (default ON). +#ifndef NVS_REQUIRE_SIZE_MULTIPLE_OF_16 + #define NVS_REQUIRE_SIZE_MULTIPLE_OF_16 0 +#endif + +// ---- Default I2C address (7-bit) ------------------------------------------------ +// Only meaningful for I2C devices. User may override by defining NV_I2C_ADDRESS earlier. +#if (NV_DRIVER == NV_2416) || (NV_DRIVER == NV_2432) || (NV_DRIVER == NV_2464) || \ + (NV_DRIVER == NV_24128) || (NV_DRIVER == NV_24256) || (NV_DRIVER == NV_AT24C32) || \ + (NV_DRIVER == NV_MB85RC32) || (NV_DRIVER == NV_MB85RC64) || (NV_DRIVER == NV_MB85RC256) + + #ifndef NV_I2C_ADDRESS + #if (NV_DRIVER == NV_AT24C32) + #define NV_I2C_ADDRESS 0x57 // common ZS042 module + #else + #define NV_I2C_ADDRESS 0x50 + #endif + #endif + + #ifndef NV_SHIM + #define NV_SHIM NV_SHIM_CACHED + #endif + + #ifndef NV_DEVICE_I2C + #define NV_DEVICE_I2C + #endif + +#endif + +// ---- Default shim selection for emulated EEPROM backends ----------------------- +#if (NV_DRIVER == NV_ESP) || (NV_DRIVER == NV_M0) + #ifndef NV_SHIM + #define NV_SHIM NV_SHIM_DELAYED + #endif +#endif + +// ---- Default shim selection for emulated EEPROM backends ----------------------- +#if NV_DRIVER == NV_EEPROM + #ifndef NV_SHIM + #define NV_SHIM NV_SHIM_CACHED + #endif +#endif + +// ---- Sanity checks -------------------------------------------------------------- +// Your system constraint: <= 65536 bytes +#if ((E2END + 1u) > 65536u) + #error "E2END+1 exceeds 65536 bytes" +#endif + +#if NVS_REQUIRE_SIZE_MULTIPLE_OF_16 + #if (((E2END + 1u) & 15u) != 0) + #error "NV size (E2END+1) must be a multiple of 16 bytes" + #endif +#endif + +#ifndef NV_SHIM + #define NV_SHIM NV_SHIM_OFF +#endif + +#if (NV_SHIM == NV_SHIM_DELAYED) + #ifndef NV_WAIT_MS + #define NV_WAIT_MS 5000u + #endif +#endif diff --git a/src/lib/nv/NvIvPartition.cpp b/src/lib/nv/NvIvPartition.cpp new file mode 100755 index 0000000..c10d596 --- /dev/null +++ b/src/lib/nv/NvIvPartition.cpp @@ -0,0 +1,101 @@ +// ------------------------------------------------------------------------------------------------- +// NvIvPartition.cpp +#include "NvIvPartition.h" + +bool IvPartition::init(NvVolume& vol, uint8_t partIdx) { + vol_ = &vol; + partIdx_ = partIdx; + ready_ = false; + partBytes_ = 0; + + if (!vol_->isMounted()) return false; + if (partIdx_ >= vol_->partitionCount()) return false; + + partBytes_ = vol_->partitionByteSize(partIdx_); + if (partBytes_ == 0) return false; + + // partitionByteSize is expected to be a multiple of block size; if not, refuse. + if ((partBytes_ % kBlockSize) != 0) return false; + + ready_ = true; + return true; +} + +// bind by ASCII partition name (requires SB1 present/valid) +bool IvPartition::init(NvVolume& vol, const char* partitionAscii) { + if (!partitionAscii || !partitionAscii[0]) return false; + if (!vol.isMounted()) return false; + + uint8_t idx = 0xFF; + if (!vol.partitionFind(partitionAscii, idx)) return false; + return init(vol, idx); +} + +// bind by name16 tag (requires SB1 present/valid) +bool IvPartition::init(NvVolume& vol, uint16_t partitionName16) { + if (partitionName16 == 0) return false; // 0 reserved/unnamed + if (!vol.isMounted()) return false; + + int8_t idx = vol.partitionFindByName16(partitionName16); + if (idx < 0) return false; + + return init(vol, (uint8_t)idx); +} + +IvPartition::NvVolStatus IvPartition::readBlock(uint16_t partBlock, uint8_t out16[kBlockSize]) { + if (!isReady()) return NvVolStatus::NotReady; + if (!out16) return NvVolStatus::IoError; + if (partBlock >= sizeBlocks()) return NvVolStatus::OutOfRange; + return vol_->partitionReadBlock(partIdx_, partBlock, out16); +} + +IvPartition::NvVolStatus IvPartition::writeBlock(uint16_t partBlock, const uint8_t in16[kBlockSize]) { + if (!isReady()) return NvVolStatus::NotReady; + if (!vol_->isWritable()) return NvVolStatus::ReadOnly; + if (!in16) return NvVolStatus::IoError; + if (partBlock >= sizeBlocks()) return NvVolStatus::OutOfRange; + return vol_->partitionWriteBlock(partIdx_, partBlock, in16); +} + +IvPartition::NvVolStatus IvPartition::readBytes(uint16_t offset, void* dst, uint16_t len) { + if (!isReady()) return NvVolStatus::NotReady; + if (len == 0) return NvVolStatus::Ok; + if (!dst) return NvVolStatus::IoError; + if ((uint32_t)offset + (uint32_t)len > (uint32_t)partBytes_) return NvVolStatus::OutOfRange; + + // Pass-through: NvVolume/backend owns any alignment/RMW/device constraints. + return vol_->partitionRead(partIdx_, offset, dst, len); +} + +IvPartition::NvVolStatus IvPartition::writeBytes(uint16_t offset, const void* src, uint16_t len) { + if (!isReady()) return NvVolStatus::NotReady; + if (!vol_->isWritable()) return NvVolStatus::ReadOnly; + if (len == 0) return NvVolStatus::Ok; + if (!src) return NvVolStatus::IoError; + if ((uint32_t)offset + (uint32_t)len > (uint32_t)partBytes_) return NvVolStatus::OutOfRange; + + // Pass-through: NvVolume/backend owns any alignment/RMW/device constraints. + return vol_->partitionWrite(partIdx_, offset, src, len); +} + +IvPartition::NvVolStatus IvPartition::clearBytes(uint16_t offset, uint16_t len) { + if (!isReady()) return NvVolStatus::NotReady; + if (!vol_->isWritable()) return NvVolStatus::ReadOnly; + if (len == 0) return NvVolStatus::Ok; + if ((uint32_t)offset + (uint32_t)len > (uint32_t)partBytes_) return NvVolStatus::OutOfRange; + + // Chunked zeros; NvVolume/backend handles any required alignment/merging. + uint8_t zchunk[32]; + memset(zchunk, 0, sizeof(zchunk)); + + uint16_t remaining = len; + uint16_t off = offset; + while (remaining) { + uint16_t take = (remaining > (uint16_t)sizeof(zchunk)) ? (uint16_t)sizeof(zchunk) : remaining; + NvVolStatus st = vol_->partitionWrite(partIdx_, off, zchunk, take); + if (st != NvVolStatus::Ok) return st; + off = (uint16_t)(off + take); + remaining = (uint16_t)(remaining - take); + } + return NvVolStatus::Ok; +} diff --git a/src/lib/nv/NvIvPartition.h b/src/lib/nv/NvIvPartition.h new file mode 100755 index 0000000..6a24643 --- /dev/null +++ b/src/lib/nv/NvIvPartition.h @@ -0,0 +1,87 @@ +// ------------------------------------------------------------------------------------------------- +// NvIvPartition.h +// +// Index/Value partition +// +// Provides a simple "flat byte array" API over a volume partition, plus typed helpers. +// +// - Addressing: partition-local offsets [0 .. partitionByteSize-1]. +// - Alignment/RMW: This layer does NOT perform block-based RMW. +// Any underlying device constraints (e.g., 16-byte aligned/multiple-of-16 I/O) must be +// satisfied/handled by NvVolume and/or the selected backend. +// - Endianness: typed helpers store the host representation as raw bytes. +// (If you need a stable cross-platform format, add explicit LE/BE conversions.) +// +// This layer does NOT add its own CRC. If you want integrity, store CRC fields in your payload +// or use the KV partition for "file-like" items with CRC. +// +// ------------------------------------------------------------------------------------------------- +#pragma once + +#include +#include + +#include "NvVolume.h" + +class IvPartition { +public: + using NvVolStatus = NvVolume::Status; + static constexpr uint16_t kBlockSize = NvVolume::kBlockSize; // 16 + + IvPartition() = default; + + // Bind to a mounted volume + partition index. + bool init(NvVolume& vol, uint8_t partIdx); + + // Bind by ASCII partition name (requires valid SB1 names block). + bool init(NvVolume& vol, const char* partitionAscii); + + // Bind by partition name16 tag (requires valid SB1 names block). + bool init(NvVolume& vol, uint16_t partitionName16); + + bool isReady() const { return (vol_ != nullptr) && ready_; } + uint8_t partIndex() const { return partIdx_; } + + uint16_t sizeBytes() const { return partBytes_; } + uint16_t sizeBlocks() const { return (uint16_t)(partBytes_ / kBlockSize); } + + // Explicit block ops (partition-local block index). + // These are useful for callers that already operate on block granularity. + NvVolStatus readBlock(uint16_t partBlock, uint8_t out16[kBlockSize]); + NvVolStatus writeBlock(uint16_t partBlock, const uint8_t in16[kBlockSize]); + + // Raw byte access (pass-through; NvVolume/backend handles any alignment/device constraints). + NvVolStatus readBytes(uint16_t offset, void* dst, uint16_t len); + NvVolStatus writeBytes(uint16_t offset, const void* src, uint16_t len); + + // Zero-fill a range (implemented via small chunked writes). + NvVolStatus clearBytes(uint16_t offset, uint16_t len); + + // Typed helpers + template + NvVolStatus read(uint16_t offset, T& outVal) { + return readBytes(offset, &outVal, (uint16_t)sizeof(T)); + } + + template + NvVolStatus write(uint16_t offset, const T& val) { + return writeBytes(offset, &val, (uint16_t)sizeof(T)); + } + + template + NvVolStatus readArray(uint16_t offset, T* outVals, uint16_t count) { + return readBytes(offset, outVals, (uint16_t)(sizeof(T) * count)); + } + + template + NvVolStatus writeArray(uint16_t offset, const T* vals, uint16_t count) { + return writeBytes(offset, vals, (uint16_t)(sizeof(T) * count)); + } + +private: + NvVolume* vol_ = nullptr; + uint8_t partIdx_ = 0; + bool ready_ = false; + + uint16_t partBytes_ = 0; +}; diff --git a/src/lib/nv/NvKvPartition16.cpp b/src/lib/nv/NvKvPartition16.cpp new file mode 100755 index 0000000..8ce7a60 --- /dev/null +++ b/src/lib/nv/NvKvPartition16.cpp @@ -0,0 +1,722 @@ +// ----------------------------------------------------------------------------------- +// NvKvPartition16.cpp +#include "NvKvPartition16.h" + +#if defined(ARDUINO) + #include +#elif defined(ARDUINO_ARCH_AVR) + #include +#endif + +namespace { +static inline uint8_t readProgByte16_(const char* p) { +#if defined(ARDUINO_ARCH_AVR) + return (uint8_t)pgm_read_byte(p); +#else + return (uint8_t)(*p); +#endif +} +} + +bool KvPartition16::initGeom_() { + if (!vol_) return false; + + const uint16_t pb = vol_->partitionBlockSize(partIdx_); + if (pb < 2u) return false; + if (pb > MAX_PART_BLOCKS) return false; + + uint32_t t = (uint32_t)pb * (uint32_t)KEY_TABLE_PERCENT + 500u; + t /= 1000u; + if (t < 1u) t = 1u; + + const uint16_t tableMax = (uint16_t)(pb / 2u); + if (t > tableMax) t = tableMax; + + const uint16_t minTableForAddr8 = (pb > 255u) ? (uint16_t)(pb - 255u) : 0u; + if (t < minTableForAddr8) t = minTableForAddr8; + if (t > tableMax) t = tableMax; + + g_.partBlocks = pb; + g_.tableBlocks = (uint16_t)t; + g_.slotCount = g_.tableBlocks; + + if (g_.tableBlocks == 0u || g_.tableBlocks >= g_.partBlocks) return false; + + g_.dataBegin = g_.tableBlocks; + { + uint16_t dc = (uint16_t)(g_.partBlocks - g_.dataBegin); + if (dc > 255u) dc = 255u; + g_.dataCount = dc; + } + + g_.mainPoolBase = g_.dataBegin; + return g_.dataCount != 0u; +} + +KvPartition16::Status KvPartition16::init(NvVolume& vol, uint8_t partIdx) { + if (!vol.isMounted()) return status_ = Status::NotBound; + if (partIdx >= vol.partitionCount()) return status_ = Status::BadGeom; + + vol_ = &vol; + partIdx_ = partIdx; + allocInit_ = false; + memset(allocBits_, 0, sizeof(allocBits_)); + + if (!initGeom_()) return status_ = Status::BadGeom; + + const Status s = initAllocMap_(); + if (s != Status::Ok) { + allocInit_ = false; + return status_ = s; + } + + return status_ = Status::Ok; +} + +KvPartition16::Status KvPartition16::init(NvVolume& vol, uint16_t partitionName16) { + if (partitionName16 == 0u) return status_ = Status::BadGeom; + if (!vol.isMounted()) return status_ = Status::NotBound; + + const int8_t idx = vol.partitionFindByName16(partitionName16); + if (idx < 0) return status_ = Status::NotBound; + + return init(vol, (uint8_t)idx); +} + +KvPartition16::Status KvPartition16::init(NvVolume& vol, const char* partitionAscii) { + if (!partitionAscii || !partitionAscii[0]) return status_ = Status::BadGeom; + if (!vol.isMounted()) return status_ = Status::NotBound; + + uint8_t idx = 0; + if (!vol.partitionFind(partitionAscii, idx)) return status_ = Status::NotBound; + + return init(vol, idx); +} + +KvPartition16::Status KvPartition16::readSlot_(uint16_t slotIdx, uint8_t A[16]) { + if (!vol_) return Status::NotBound; + if (slotIdx >= g_.slotCount) return Status::OutOfRange; + + const uint16_t bo = slotToBlockOfs_(slotIdx); + return (vol_->partitionReadBlock(partIdx_, bo, A) == NvVolume::Status::Ok) ? Status::Ok : Status::IoError; +} + +KvPartition16::Status KvPartition16::writeSlot_(uint16_t slotIdx, const uint8_t A[16]) { + if (!vol_) return Status::NotBound; + if (slotIdx >= g_.slotCount) return Status::OutOfRange; + + const uint16_t bo = slotToBlockOfs_(slotIdx); + return (vol_->partitionWriteBlock(partIdx_, bo, A) == NvVolume::Status::Ok) ? Status::Ok : Status::IoError; +} + +KvPartition16::Status KvPartition16::readSlotName24_(uint16_t slotIdx, uint32_t& outName24) { + if (!vol_) return Status::NotBound; + if (slotIdx >= g_.slotCount) return Status::OutOfRange; + + const uint16_t bo = slotToBlockOfs_(slotIdx); + uint8_t n3[3]; + if (vol_->partitionReadInBlock(partIdx_, bo, 0, n3, 3) != NvVolume::Status::Ok) return Status::IoError; + outName24 = le24_(n3) & 0x00FFFFFFu; + return Status::Ok; +} + +KvPartition16::Status KvPartition16::readDataBlockByAddr_(uint8_t addr8, uint8_t out16[16]) const { + if (!vol_) return Status::NotBound; + if (!dataAddrValid_(addr8)) return Status::Corrupt; + + const uint16_t b = dataAddrToBlock_(addr8); + return (vol_->partitionReadBlock(partIdx_, b, out16) == NvVolume::Status::Ok) ? Status::Ok : Status::IoError; +} + +KvPartition16::Status KvPartition16::writeDataBlockByAddr_(uint8_t addr8, const uint8_t in16[16]) { + if (!vol_) return Status::NotBound; + if (!dataAddrValid_(addr8)) return Status::OutOfRange; + + const uint16_t b = dataAddrToBlock_(addr8); + return (vol_->partitionWriteBlock(partIdx_, b, in16) == NvVolume::Status::Ok) ? Status::Ok : Status::IoError; +} + +KvPartition16::Status KvPartition16::decodeEntry_(const uint8_t A[16], Decoded& out) const { + out = Decoded{}; + + out.flags3 = hdrFlags3_(A[3]); + out.len5 = hdrLen5_(A[3]); + + const bool fInlineOnly = (out.flags3 & FLAG_INLINE_ONLY) != 0u; + const bool fInlinePlus = (out.flags3 & FLAG_INLINE_PLUS_BLOCK) != 0u; + const bool fTail = (out.flags3 & FLAG_TAIL) != 0u; + + if (fInlineOnly) { + if (fInlinePlus || fTail) return Status::Corrupt; + if (out.len5 > INLINE_CAP) return Status::Corrupt; + + out.mode = Mode::InlineOnly; + out.inlineLen = out.len5; + return Status::Ok; + } + + if (fInlinePlus) { + if (fTail) return Status::Corrupt; + if (out.len5 <= INLINE_PLUS_HEAD_CAP || out.len5 > (uint8_t)(INLINE_PLUS_HEAD_CAP + BLOCK_BYTES)) return Status::Corrupt; + + uint8_t body[9]; + bodyRead_(A, body); + + if (!dataAddrValid_(body[8])) return Status::Corrupt; + + out.mode = Mode::InlinePlusBlock; + out.inlineLen = INLINE_PLUS_HEAD_CAP; + out.oneBlockAddr = body[8]; + return Status::Ok; + } + + if (!fTail) return Status::Corrupt; + if (out.len5 < 1u || out.len5 > BLOCK_BYTES) return Status::Corrupt; + + uint8_t body[9]; + bodyRead_(A, body); + + out.mode = Mode::AddrList; + out.tailLen = out.len5; + out.blockCount = 0; + + for (uint8_t i = 0; i < ADDR_CAP; i++) { + const uint8_t a = body[i]; + if (a == ADDR_END) break; + if (!dataAddrValid_(a)) return Status::Corrupt; + + out.blocks[out.blockCount++] = a; + } + + if (out.blockCount == 0u) return Status::Corrupt; + out.blocks[out.blockCount] = ADDR_END; + return Status::Ok; +} + +uint16_t KvPartition16::payloadLenBytes_(const Decoded& d) const { + if (d.mode == Mode::InlineOnly) return d.inlineLen; + if (d.mode == Mode::InlinePlusBlock) return d.len5; + + if (d.blockCount == 0u) return 0u; + return (uint16_t)(((uint16_t)(d.blockCount - 1u) * BLOCK_BYTES) + d.tailLen); +} + +KvPartition16::Status KvPartition16::computeFullCrc24_(const uint8_t A[16], uint32_t& outCrc24) const { + if (!vol_) return Status::NotBound; + + Decoded d; + const Status ds = decodeEntry_(A, d); + if (ds != Status::Ok) return ds; + + uint8_t Ac[16]; + memcpy(Ac, A, 16); + Ac[13] = 0; + Ac[14] = 0; + Ac[15] = 0; + + uint32_t crc = Crc24::init(); + crc = Crc24::update(crc, Ac, 16); + + if (d.mode == Mode::InlinePlusBlock) { + uint8_t blk[16]; + const Status rs = readDataBlockByAddr_(d.oneBlockAddr, blk); + if (rs != Status::Ok) return rs; + crc = Crc24::update(crc, blk, 16); + } else if (d.mode == Mode::AddrList) { + for (uint8_t i = 0; i < d.blockCount; i++) { + uint8_t blk[16]; + const Status rs = readDataBlockByAddr_(d.blocks[i], blk); + if (rs != Status::Ok) return rs; + crc = Crc24::update(crc, blk, 16); + } + } + + outCrc24 = crc & 0x00FFFFFFu; + return Status::Ok; +} + +KvPartition16::Status KvPartition16::validateEntryCrc_(const uint8_t A[16]) const { + const uint32_t stored = loadCrc24LE_(A) & 0x00FFFFFFu; + uint32_t calc = 0; + const Status s = computeFullCrc24_(A, calc); + if (s != Status::Ok) return s; + return (((stored ^ calc) & 0x00FFFFFFu) == 0u) ? Status::Ok : Status::Corrupt; +} + +void KvPartition16::markUsed_(uint16_t blockIdx, bool used) { + if (blockIdx >= g_.partBlocks) return; + + const uint16_t byteIdx = (uint16_t)(blockIdx >> 3); + const uint8_t mask = (uint8_t)(1u << (blockIdx & 7u)); + if (byteIdx >= BITMAP_BYTES) return; + + if (used) allocBits_[byteIdx] |= mask; + else allocBits_[byteIdx] &= (uint8_t)~mask; +} + +bool KvPartition16::isUsed_(uint16_t blockIdx) const { + if (blockIdx >= g_.partBlocks) return true; + + const uint16_t byteIdx = (uint16_t)(blockIdx >> 3); + const uint8_t mask = (uint8_t)(1u << (blockIdx & 7u)); + if (byteIdx >= BITMAP_BYTES) return true; + + return (allocBits_[byteIdx] & mask) != 0u; +} + +void KvPartition16::freeEntryBlocksFromMap_(const uint8_t A[16]) { + Decoded d; + if (decodeEntry_(A, d) != Status::Ok) return; + + if (d.mode == Mode::InlinePlusBlock) { + if (dataAddrValid_(d.oneBlockAddr)) markUsed_(dataAddrToBlock_(d.oneBlockAddr), false); + return; + } + + if (d.mode == Mode::AddrList) { + for (uint8_t i = 0; i < d.blockCount; i++) { + if (dataAddrValid_(d.blocks[i])) markUsed_(dataAddrToBlock_(d.blocks[i]), false); + } + } +} + +KvPartition16::Status KvPartition16::findSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[16]) { + if (!vol_) return Status::NotBound; + name24 &= 0x00FFFFFFu; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + uint32_t n = 0; + const Status ns = readSlotName24_(i, n); + if (ns != Status::Ok) return ns; + if (n != name24) continue; + + const Status rs = readSlot_(i, A); + if (rs != Status::Ok) return rs; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::IoError) return vs; + if (vs != Status::Ok) continue; + + outSlot = i; + return Status::Ok; + } + + return Status::NotFound; +} + +KvPartition16::Status KvPartition16::findOrAllocSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[16], bool& outIsExisting) { + int16_t firstEmpty = -1; + int16_t firstCorruptMatch = -1; + name24 &= 0x00FFFFFFu; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + uint32_t n = 0; + const Status ns = readSlotName24_(i, n); + if (ns != Status::Ok) return ns; + + if (n == name24) { + const Status rs = readSlot_(i, A); + if (rs != Status::Ok) return rs; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::IoError) return vs; + + if (vs != Status::Ok) { + if (firstCorruptMatch < 0) firstCorruptMatch = (int16_t)i; + continue; + } + + outSlot = i; + outIsExisting = true; + return Status::Ok; + } + + if (n == EMPTY_NAME24 && firstEmpty < 0) firstEmpty = (int16_t)i; + } + + if (firstCorruptMatch >= 0) { + outSlot = (uint16_t)firstCorruptMatch; + outIsExisting = false; + return Status::Ok; + } + + if (firstEmpty >= 0) { + outSlot = (uint16_t)firstEmpty; + outIsExisting = false; + return Status::Ok; + } + + return Status::NoSpace; +} + +KvPartition16::Status KvPartition16::initAllocMap_() { + memset(allocBits_, 0, sizeof(allocBits_)); + + for (uint16_t b = 0; b < g_.dataBegin; b++) markUsed_(b, true); + for (uint16_t b = (uint16_t)(g_.dataBegin + g_.dataCount); b < g_.partBlocks; b++) markUsed_(b, true); + + uint8_t A[16]; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + Status s = readSlot_(i, A); + if (s != Status::Ok) { allocInit_ = false; return s; } + + const uint32_t n = le24_(&A[0]) & 0x00FFFFFFu; + if (n == EMPTY_NAME24) continue; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::IoError) { allocInit_ = false; return Status::IoError; } + if (vs != Status::Ok) continue; + + Decoded d; + if (decodeEntry_(A, d) != Status::Ok) continue; + + if (d.mode == Mode::InlinePlusBlock) { + markUsed_(dataAddrToBlock_(d.oneBlockAddr), true); + } else if (d.mode == Mode::AddrList) { + for (uint8_t k = 0; k < d.blockCount; k++) markUsed_(dataAddrToBlock_(d.blocks[k]), true); + } + } + + allocInit_ = true; + return Status::Ok; +} + +KvPartition16::Status KvPartition16::allocDataBlocks_(uint8_t count, uint8_t outAddrs[ADDR_CAP]) { + if (count == 0u || count > ADDR_CAP) return Status::OutOfRange; + if (!allocInit_) return Status::Corrupt; + + uint8_t got = 0; + + const uint16_t first = g_.dataBegin; + const uint16_t last = (uint16_t)(g_.dataBegin + g_.dataCount - 1u); + + for (int32_t b = (int32_t)last; b >= (int32_t)first; --b) { + const uint16_t bb = (uint16_t)b; + if (isUsed_(bb)) continue; + + markUsed_(bb, true); + outAddrs[got++] = (uint8_t)(bb - g_.dataBegin); + if (got == count) return Status::Ok; + } + + for (uint8_t i = 0; i < got; i++) markUsed_(dataAddrToBlock_(outAddrs[i]), false); + + return Status::NoSpace; +} + +KvPartition16::Status KvPartition16::get(uint32_t name24, void* dst, uint16_t dstCap, uint16_t& outLen) { + if (!vol_) return Status::NotBound; + name24 &= 0x00FFFFFFu; + if (name24 == EMPTY_NAME24) return Status::OutOfRange; + + uint8_t A[16]; + uint16_t slot = 0; + const Status fs = findSlot_(name24, slot, A); + (void)slot; + if (fs != Status::Ok) return fs; + + Decoded d; + const Status ds = decodeEntry_(A, d); + if (ds != Status::Ok) return ds; + + outLen = payloadLenBytes_(d); + if (!dst) return Status::Ok; + if (dstCap < outLen) return Status::OutOfRange; + + uint8_t* p = (uint8_t*)dst; + + if (d.mode == Mode::InlineOnly) { + uint8_t body[9]; + bodyRead_(A, body); + if (d.inlineLen) memcpy(p, body, d.inlineLen); + return Status::Ok; + } + + if (d.mode == Mode::InlinePlusBlock) { + uint8_t body[9]; + bodyRead_(A, body); + memcpy(p, body, INLINE_PLUS_HEAD_CAP); + + uint8_t blk[16]; + const Status rs = readDataBlockByAddr_(d.oneBlockAddr, blk); + if (rs != Status::Ok) return rs; + + const uint8_t tail = (uint8_t)(d.len5 - INLINE_PLUS_HEAD_CAP); + if (tail) memcpy(p + INLINE_PLUS_HEAD_CAP, blk, tail); + return Status::Ok; + } + + uint16_t written = 0; + for (uint8_t i = 0; i < d.blockCount; i++) { + uint8_t blk[16]; + const Status rs = readDataBlockByAddr_(d.blocks[i], blk); + if (rs != Status::Ok) return rs; + + const bool isLast = (i == (uint8_t)(d.blockCount - 1u)); + const uint8_t n = isLast ? d.tailLen : BLOCK_BYTES; + + memcpy(p + written, blk, n); + written = (uint16_t)(written + n); + } + + return (written == outLen) ? Status::Ok : Status::Corrupt; +} + +KvPartition16::Status KvPartition16::put(uint32_t name24, const void* src, uint16_t len) { + if (!vol_) return Status::NotBound; + if (!src && len) return Status::OutOfRange; + if (!allocInit_) return Status::Corrupt; + + name24 &= 0x00FFFFFFu; + if (name24 == EMPTY_NAME24) return Status::OutOfRange; + + if (len > (uint16_t)(ADDR_CAP * BLOCK_BYTES)) return Status::OutOfRange; + + uint8_t Aold[16]; + uint16_t slot = 0; + bool existing = false; + + Status s = findOrAllocSlot_(name24, slot, Aold, existing); + if (s != Status::Ok) return s; + + if (!existing) memset(Aold, 0xFF, sizeof(Aold)); + + bool freedOld = false; + { + const Status vs = validateEntryCrc_(Aold); + if (vs == Status::Ok) { + freeEntryBlocksFromMap_(Aold); + freedOld = true; + } else if (vs == Status::IoError) { + return Status::IoError; + } + } + + uint8_t A[16]; + memset(A, 0xFF, sizeof(A)); + wrle24_(&A[0], name24); + + const uint8_t* p = (const uint8_t*)src; + + if (len <= INLINE_CAP) { + A[3] = packHeader_(FLAG_INLINE_ONLY, (uint8_t)len); + + uint8_t body[9]; + memset(body, 0xFF, sizeof(body)); + if (len) memcpy(body, p, len); + bodyWrite_(A, body); + + uint32_t crc24 = 0; + Status cs = computeFullCrc24_(A, crc24); + if (cs != Status::Ok) { invalidateAlloc_(); return cs; } + storeCrc24LE_(A, crc24); + + Status ws = writeSlot_(slot, A); + if (ws == Status::IoError) invalidateAlloc_(); + return ws; + } + + if (len <= (uint16_t)(INLINE_PLUS_HEAD_CAP + BLOCK_BYTES)) { + uint8_t addrs[ADDR_CAP] = {0}; + s = allocDataBlocks_(1u, addrs); + if (s != Status::Ok) { + if (freedOld) invalidateAlloc_(); + return s; + } + + uint8_t blk[16]; + memset(blk, 0xFF, sizeof(blk)); + const uint8_t tail = (uint8_t)(len - INLINE_PLUS_HEAD_CAP); + if (tail) memcpy(blk, p + INLINE_PLUS_HEAD_CAP, tail); + + Status wsb = writeDataBlockByAddr_(addrs[0], blk); + if (wsb != Status::Ok) { + markUsed_(dataAddrToBlock_(addrs[0]), false); + invalidateAlloc_(); + return wsb; + } + + A[3] = packHeader_(FLAG_INLINE_PLUS_BLOCK, (uint8_t)len); + + uint8_t body[9]; + memset(body, 0xFF, sizeof(body)); + memcpy(body, p, INLINE_PLUS_HEAD_CAP); + body[8] = addrs[0]; + bodyWrite_(A, body); + + uint32_t crc24 = 0; + Status cs = computeFullCrc24_(A, crc24); + if (cs != Status::Ok) { invalidateAlloc_(); return cs; } + storeCrc24LE_(A, crc24); + + Status ws = writeSlot_(slot, A); + if (ws == Status::IoError) invalidateAlloc_(); + return ws; + } + + const uint8_t blocksNeeded = (uint8_t)((len + (BLOCK_BYTES - 1u)) / BLOCK_BYTES); + if (blocksNeeded == 0u || blocksNeeded > ADDR_CAP) return Status::OutOfRange; + + uint8_t addrs[ADDR_CAP] = {0}; + s = allocDataBlocks_(blocksNeeded, addrs); + if (s != Status::Ok) { + if (freedOld) invalidateAlloc_(); + return s; + } + + uint8_t tmp[16]; + for (uint8_t i = 0; i < blocksNeeded; i++) { + memset(tmp, 0xFF, sizeof(tmp)); + + const uint16_t ofs = (uint16_t)i * BLOCK_BYTES; + const uint16_t avail = (len > ofs) ? (uint16_t)(len - ofs) : 0u; + const uint16_t chunk = (avail >= BLOCK_BYTES) ? BLOCK_BYTES : avail; + if (chunk) memcpy(tmp, p + ofs, chunk); + + Status ws = writeDataBlockByAddr_(addrs[i], tmp); + if (ws != Status::Ok) { + for (uint8_t k = 0; k < blocksNeeded; k++) markUsed_(dataAddrToBlock_(addrs[k]), false); + invalidateAlloc_(); + return ws; + } + } + + const uint8_t tailLen = (uint8_t)((len & (BLOCK_BYTES - 1u)) ? (len & (BLOCK_BYTES - 1u)) : BLOCK_BYTES); + A[3] = packHeader_(FLAG_TAIL, tailLen); + + uint8_t body[9]; + memset(body, ADDR_END, sizeof(body)); + for (uint8_t i = 0; i < blocksNeeded; i++) body[i] = addrs[i]; + bodyWrite_(A, body); + + uint32_t crc24 = 0; + Status cs = computeFullCrc24_(A, crc24); + if (cs != Status::Ok) { invalidateAlloc_(); return cs; } + storeCrc24LE_(A, crc24); + + Status ws = writeSlot_(slot, A); + if (ws == Status::IoError) invalidateAlloc_(); + return ws; +} + +KvPartition16::Status KvPartition16::del(uint32_t name24) { + if (!vol_) return Status::NotBound; + if (!allocInit_) return Status::Corrupt; + + name24 &= 0x00FFFFFFu; + if (name24 == EMPTY_NAME24) return Status::OutOfRange; + + uint8_t A[16]; + uint16_t slot = 0; + const Status fs = findSlot_(name24, slot, A); + if (fs != Status::Ok) return fs; + + if (validateEntryCrc_(A) == Status::Ok) freeEntryBlocksFromMap_(A); + + memset(A, 0xFF, sizeof(A)); + + Status ws = writeSlot_(slot, A); + if (ws == Status::IoError) invalidateAlloc_(); + return ws; +} + +uint32_t KvPartition16::computeKey(const char* nameAscii) { + uint32_t crc = Crc24::init(); + crc = Crc24::update(crc, (const uint8_t*)KV_NAME_SALT, (uint16_t)strlen(KV_NAME_SALT)); + crc = Crc24::update(crc, (const uint8_t*)(nameAscii ? nameAscii : ""), (uint16_t)strlen(nameAscii ? nameAscii : "")); + return (crc & 0x00FFFFFFu); +} + +uint32_t KvPartition16::computeKey(const __FlashStringHelper* nameAsciiF) { +#if defined(ARDUINO) + if (!nameAsciiF) nameAsciiF = F(""); + + uint32_t crc = Crc24::init(); + crc = Crc24::update(crc, (const uint8_t*)KV_NAME_SALT, (uint16_t)strlen(KV_NAME_SALT)); + + const char* p = (const char*)nameAsciiF; + for (;;) { + const uint8_t c = readProgByte16_(p++); + if (c == 0) break; + crc = Crc24::updateByte(crc, c); + } + + return (crc & 0x00FFFFFFu); +#else + (void)nameAsciiF; + return computeKey(""); +#endif +} + +KvPartition16::Status KvPartition16::scrub(uint16_t* outDeleted) { + if (outDeleted) *outDeleted = 0; + + if (!vol_ || !vol_->isMounted()) return Status::NotBound; + if (!initGeom_()) return Status::BadGeom; + + uint8_t A[16]; + uint16_t deleted = 0; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + const Status rs = readSlot_(i, A); + if (rs != Status::Ok) return rs; + + const uint32_t n = le24_(&A[0]) & 0x00FFFFFFu; + if (n == EMPTY_NAME24) continue; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::Ok) continue; + if (vs == Status::IoError) return Status::IoError; + if (vs != Status::Corrupt) return vs; + + memset(A, 0xFF, sizeof(A)); + const Status ws = writeSlot_(i, A); + if (ws != Status::Ok) { + if (ws == Status::IoError) invalidateAlloc_(); + return ws; + } + + deleted++; + } + + const Status ms = initAllocMap_(); + if (ms != Status::Ok) { + if (ms == Status::IoError) invalidateAlloc_(); + return ms; + } + + if (outDeleted) *outDeleted = deleted; + return Status::Ok; +} + +KvPartition16::Status KvPartition16::stats(Stats& out) { + memset(&out, 0, sizeof(out)); + if (!vol_ || !vol_->isMounted()) return Status::NotBound; + if (!initGeom_()) return Status::BadGeom; + if (!allocInit_) return Status::Corrupt; + + out.dataBlocksTotal = g_.dataCount; + out.slotsTotal = g_.slotCount; + + uint16_t usedData = 0; + for (uint16_t a = 0; a < g_.dataCount; a++) { + const uint16_t b = (uint16_t)(g_.dataBegin + a); + const uint16_t byteIdx = (uint16_t)(b >> 3); + const uint8_t mask = (uint8_t)(1u << (b & 7u)); + if (byteIdx >= BITMAP_BYTES) break; + if (allocBits_[byteIdx] & mask) usedData++; + } + out.dataBlocksFree = (usedData <= g_.dataCount) ? (uint16_t)(g_.dataCount - usedData) : 0u; + + uint16_t empty = 0; + for (uint16_t i = 0; i < g_.slotCount; i++) { + uint32_t n = 0; + const Status ns = readSlotName24_(i, n); + if (ns != Status::Ok) return ns; + if (n == EMPTY_NAME24) empty++; + } + out.slotsFree = empty; + + return Status::Ok; +} diff --git a/src/lib/nv/NvKvPartition16.h b/src/lib/nv/NvKvPartition16.h new file mode 100755 index 0000000..7d15acf --- /dev/null +++ b/src/lib/nv/NvKvPartition16.h @@ -0,0 +1,302 @@ +// ----------------------------------------------------------------------------------- +// NvKvPartition16.h +// +// KV partition on 16-byte blocks (v1 format). +// +// This KV layer stores named blobs in a fixed-size partition. +// The partition is a 16-byte-block address space provided by NvVolume. +// Supported partition size: up to 8192 bytes (512 x 16B blocks). +// +// Layout +// - Table entries are stored at blocks [0 .. tableBlocks-1]. +// - Data blocks are stored at [dataBegin .. partBlocks-1], where dataBegin = tableBlocks. +// - tableBlocks targets KEY_TABLE_PERCENT (default 40%) and can slide upward +// to at most partBlocks/2 as needed so dataCount fits byte addressing. +// +// Terminology +// - Block: 16 bytes. +// - Data address (addr8): 8-bit index into the data region. +// addr8=0 -> physical block dataBegin +// addr8=1 -> physical block dataBegin+1 +// ... +// addr8=254 -> physical block dataBegin+254 +// - addr8 value 0xFF is reserved as ADDR_END sentinel. +// +// Keying +// - Each record key is name24: +// name24 = CRC24( KV_NAME_SALT + asciiName ) +// - CRC24 uses Crc24::init/update from Crc.h. +// +// Allocation model +// - Overwrite-in-place semantics; no compaction. +// - Data block allocation is top-down within the data region. +// - Addressable data region is limited to 255 blocks (0..254), clamped by partition size. +// +// =================================================================================================== +// v1 ENTRY FORMAT (16 bytes, one entry per key) +// a[0..2] : name24 (LE) +// a[3] : header = flags3 + len5 +// flags bits: +// FLAG_INLINE_ONLY : body contains inline payload only +// FLAG_INLINE_PLUS_BLOCK : body contains inline payload + one addr8 +// FLAG_TAIL : addr-list mode; len5 = bytes used in last block +// a[4..12] : body72 (9 bytes), interpreted by mode: +// - INLINE_ONLY: up to 9 inline bytes +// - INLINE_PLUS_BLOCK: 8 inline bytes + 1 addr8 +// - ADDR_LIST: up to 9 addr8 values (0xFF sentinel-terminated) +// a[13..15]: crc24 (LE), written last +// CRC covers entry (with crc bytes zeroed) and referenced payload blocks. +// +// Mode max payload sizes: +// - InlineOnly: 9 bytes +// - InlinePlusBlock: 8 + 16 = 24 bytes +// - AddrList: 9 * 16 = 144 bytes +// Overall max file size: 144 bytes +// ----------------------------------------------------------------------------------- +#pragma once + +#include +#include +#include + +#include "NvVolume.h" +#include "../math/Crc.h" + +class __FlashStringHelper; + +#ifndef MAX_BLOCKS + #define MAX_BLOCKS 2048u +#endif + +#ifndef KEY_TABLE_PERCENT + #define KEY_TABLE_PERCENT 400u // 40% table by default +#endif + +class KvPartition16 { +public: + enum class Status : uint8_t { + Ok = 0, + NotBound, + BadGeom, + IoError, + Corrupt, + NotFound, + NoSpace, + OutOfRange, + }; + + static constexpr uint8_t BLOCK_BYTES = 16u; + static constexpr uint8_t ADDR_END = 0xFFu; + static constexpr uint32_t EMPTY_NAME24 = 0x00FFFFFFu; + + static constexpr uint8_t FLAG_INLINE_ONLY = 0x01u; + static constexpr uint8_t FLAG_INLINE_PLUS_BLOCK = 0x02u; + static constexpr uint8_t FLAG_TAIL = 0x04u; + + static constexpr uint8_t INLINE_CAP = 9u; + static constexpr uint8_t INLINE_PLUS_HEAD_CAP = 8u; + static constexpr uint8_t ADDR_CAP = 9u; + static constexpr uint16_t MAX_PART_BLOCKS = 512u; + + struct Geom { + uint16_t partBlocks = 0; + uint16_t tableBlocks = 0; + uint16_t slotCount = 0; + uint16_t dataBegin = 0; + uint16_t dataCount = 0; + uint16_t mainPoolBase = 0; + }; + + struct Stats { + uint16_t dataBlocksTotal; + uint16_t dataBlocksFree; + uint16_t slotsTotal; + uint16_t slotsFree; + }; + + KvPartition16() = default; + + Status init(NvVolume& vol, uint8_t partIdx); + Status init(NvVolume& vol, uint16_t partitionName16); + Status init(NvVolume& vol, const char* partitionAscii); + + const Geom& geom() const { return g_; } + Status status() const { return status_; } + + Status get(uint32_t name24, void* dst, uint16_t dstCap, uint16_t& outLen); + Status put(uint32_t name24, const void* src, uint16_t len); + Status del(uint32_t name24); + + static uint32_t computeKey(const char* nameAscii); + static uint32_t computeKey(const __FlashStringHelper* nameAsciiF); + + Status get(const char* nameAscii, void* dst, uint16_t dstCap, uint16_t& outLen) { return get(name24_(nameAscii), dst, dstCap, outLen); } + Status put(const char* nameAscii, const void* src, uint16_t len) { return put(name24_(nameAscii), src, len); } + + Status put(const char* nameAscii, const char* zstr) { + if (!zstr) return put(nameAscii, (const void*)"", 1); + const uint16_t n = (uint16_t)strlen(zstr) + 1u; + return put(nameAscii, (const void*)zstr, n); + } + + Status del(const char* nameAscii) { return del(name24_(nameAscii)); } + + Status scrub(uint16_t* outDeleted); + + template + Status put(const char* nameAscii, const T& v) { return put(nameAscii, (const void*)&v, (uint16_t)sizeof(T)); } + + template + Status put(uint32_t name24, const T& v) { return put(name24, (const void*)&v, (uint16_t)sizeof(T)); } + + template + Status get(const char* nameAscii, T& out) { + T tmp = out; + uint16_t outLen = 0; + + Status s = get(nameAscii, (void*)&tmp, (uint16_t)sizeof(T), outLen); + if (s != Status::Ok) return s; + if (outLen != sizeof(T)) return Status::Corrupt; + + out = tmp; + return Status::Ok; + } + + template + Status get(uint32_t name24, T& out) { + T tmp = out; + uint16_t outLen = 0; + + Status s = get(name24, (void*)&tmp, (uint16_t)sizeof(T), outLen); + if (s != Status::Ok) return s; + if (outLen != sizeof(T)) return Status::Corrupt; + + out = tmp; + return Status::Ok; + } + + bool isValid() const { return (status_ == Status::Ok) && allocInit_; } + + template + bool getOrInit(const char* nameAscii, T& inOutDefaults) { + if (!nameAscii) return false; + if (!isValid()) return false; + + const Status gs = get(nameAscii, inOutDefaults); + if (gs == Status::Ok) return true; + if (gs == Status::IoError) return false; + + if (put(nameAscii, inOutDefaults) != Status::Ok) { initErrorFlag_ = true; } + return true; + } + + template + bool getOrInit(uint32_t name24, T& inOutDefaults) { + if ((name24 & 0x00FFFFFFu) == EMPTY_NAME24) return false; + if (!isValid()) return false; + + const Status gs = get(name24, inOutDefaults); + if (gs == Status::Ok) return true; + if (gs == Status::IoError) return false; + + if (put(name24, inOutDefaults) != Status::Ok) { initErrorFlag_ = true; } + return true; + } + + void resetInitErrorFlag() { initErrorFlag_ = false; } + bool getInitErrorFlag() { return initErrorFlag_; } + + Status stats(Stats& out); + +private: + enum class Mode : uint8_t { + InlineOnly = 0, // max 9 bytes + InlinePlusBlock = 1, // max 24 bytes + AddrList = 2, // max 9 * 16 = 144 bytes (sentinel terminated) + }; + + struct Decoded { + Mode mode = Mode::InlineOnly; + uint8_t flags3 = 0; + uint8_t len5 = 0; + + uint8_t inlineLen = 0; + uint8_t tailLen = 0; + + uint8_t oneBlockAddr = ADDR_END; + uint8_t blocks[ADDR_CAP + 1u] = {ADDR_END}; + uint8_t blockCount = 0; + }; + + static constexpr uint16_t BITMAP_BYTES = (MAX_BLOCKS + 7u) / 8u; + static constexpr const char* KV_NAME_SALT = "OnCueKVv1"; + + static inline uint8_t packHeader_(uint8_t flags3, uint8_t len5) { return (uint8_t)((flags3 & 0x07u) | ((len5 & 0x1Fu) << 3)); } + static inline uint8_t hdrFlags3_(uint8_t header) { return (uint8_t)(header & 0x07u); } + static inline uint8_t hdrLen5_(uint8_t header) { return (uint8_t)(header >> 3); } + + static inline uint32_t le24_(const uint8_t* p) { + return ((uint32_t)p[0]) | ((uint32_t)p[1] << 8) | ((uint32_t)p[2] << 16); + } + static inline void wrle24_(uint8_t* p, uint32_t v24) { + p[0] = (uint8_t)(v24 & 0xFFu); + p[1] = (uint8_t)((v24 >> 8) & 0xFFu); + p[2] = (uint8_t)((v24 >> 16) & 0xFFu); + } + + static inline uint32_t loadCrc24LE_(const uint8_t A[16]) { + return ((uint32_t)A[13]) | ((uint32_t)A[14] << 8) | ((uint32_t)A[15] << 16); + } + static inline void storeCrc24LE_(uint8_t A[16], uint32_t crc24) { + A[13] = (uint8_t)(crc24 & 0xFFu); + A[14] = (uint8_t)((crc24 >> 8) & 0xFFu); + A[15] = (uint8_t)((crc24 >> 16) & 0xFFu); + } + + static inline uint32_t name24_(const char* asciiName) { return computeKey(asciiName ? asciiName : ""); } + + uint16_t slotToBlockOfs_(uint16_t slotIdx) const { return slotIdx; } + uint16_t dataAddrToBlock_(uint8_t addr8) const { return (uint16_t)(g_.dataBegin + (uint16_t)addr8); } + bool dataAddrValid_(uint8_t addr8) const { return addr8 != ADDR_END && (uint16_t)addr8 < g_.dataCount; } + + Status readSlot_(uint16_t slotIdx, uint8_t A[16]); + Status writeSlot_(uint16_t slotIdx, const uint8_t A[16]); + Status readSlotName24_(uint16_t slotIdx, uint32_t& outName24); + + Status readDataBlockByAddr_(uint8_t addr8, uint8_t out16[16]) const; + Status writeDataBlockByAddr_(uint8_t addr8, const uint8_t in16[16]); + + void bodyRead_(const uint8_t A[16], uint8_t out[9]) const { memcpy(out, &A[4], 9); } + void bodyWrite_(uint8_t A[16], const uint8_t in[9]) const { memcpy(&A[4], in, 9); } + + Status decodeEntry_(const uint8_t A[16], Decoded& out) const; + uint16_t payloadLenBytes_(const Decoded& d) const; + + Status computeFullCrc24_(const uint8_t A[16], uint32_t& outCrc24) const; + Status validateEntryCrc_(const uint8_t A[16]) const; + + Status findSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[16]); + Status findOrAllocSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[16], bool& outIsExisting); + + void markUsed_(uint16_t blockIdx, bool used); + bool isUsed_(uint16_t blockIdx) const; + + Status initAllocMap_(); + Status allocDataBlocks_(uint8_t count, uint8_t outAddrs[ADDR_CAP]); + void invalidateAlloc_() { allocInit_ = false; } + + void freeEntryBlocksFromMap_(const uint8_t A[16]); + + bool initGeom_(); + +private: + NvVolume* vol_ = nullptr; + uint8_t partIdx_ = 0xFF; + Geom g_; + Status status_ = Status::NotBound; + + uint8_t allocBits_[BITMAP_BYTES] = {0}; + bool allocInit_ = false; + + bool initErrorFlag_ = false; +}; diff --git a/src/lib/nv/NvKvPartition32.cpp b/src/lib/nv/NvKvPartition32.cpp new file mode 100755 index 0000000..fedcd69 --- /dev/null +++ b/src/lib/nv/NvKvPartition32.cpp @@ -0,0 +1,653 @@ +// ----------------------------------------------------------------------------------- +// NvKvPartition32.cpp +#include "NvKvPartition32.h" + +#if defined(ARDUINO) + #include +#elif defined(ARDUINO_ARCH_AVR) + #include +#endif + +namespace { +static inline uint8_t readProgByte32_(const char* p) { +#if defined(ARDUINO_ARCH_AVR) + return (uint8_t)pgm_read_byte(p); +#else + return (uint8_t)(*p); +#endif +} +} + +bool KvPartition32::initGeom_() { + if (!vol_) return false; + + const uint16_t pb16 = vol_->partitionBlockSize(partIdx_); + if (pb16 == 0u) return false; + if ((pb16 & 1u) != 0u) return false; // virtual 32B blocks require even 16B count + + const uint16_t pb32 = (uint16_t)(pb16 / 2u); + if (pb32 < 2u) return false; + if (pb32 > MAX_BLOCKS_32) return false; + + uint32_t t = (uint32_t)pb32 * (uint32_t)KEY_TABLE_PERCENT + 500u; + t /= 1000u; + if (t < 1u) t = 1u; + + const uint16_t tableMax = (uint16_t)(pb32 / 2u); + if (t > tableMax) t = tableMax; + + const uint16_t minTableForAddr8 = (pb32 > 255u) ? (uint16_t)(pb32 - 255u) : 0u; + if (t < minTableForAddr8) t = minTableForAddr8; + if (t > tableMax) t = tableMax; + + g_.partBlocks = pb32; + g_.tableBlocks = (uint16_t)t; + g_.slotCount = g_.tableBlocks; + + if (g_.tableBlocks == 0u || g_.tableBlocks >= g_.partBlocks) return false; + + g_.dataBegin = g_.tableBlocks; + { + uint16_t dc = (uint16_t)(g_.partBlocks - g_.dataBegin); + if (dc > 255u) dc = 255u; + g_.dataCount = dc; + } + + g_.mainPoolBase = g_.dataBegin; + return g_.dataCount != 0u; +} + +KvPartition32::Status KvPartition32::init(NvVolume& vol, uint8_t partIdx) { + if (!vol.isMounted()) return status_ = Status::NotBound; + if (partIdx >= vol.partitionCount()) return status_ = Status::BadGeom; + + vol_ = &vol; + partIdx_ = partIdx; + allocInit_ = false; + memset(allocBits_, 0, sizeof(allocBits_)); + + if (!initGeom_()) return status_ = Status::BadGeom; + + const Status s = initAllocMap_(); + if (s != Status::Ok) { + allocInit_ = false; + return status_ = s; + } + + return status_ = Status::Ok; +} + +KvPartition32::Status KvPartition32::init(NvVolume& vol, uint16_t partitionName16) { + if (partitionName16 == 0u) return status_ = Status::BadGeom; + if (!vol.isMounted()) return status_ = Status::NotBound; + + const int8_t idx = vol.partitionFindByName16(partitionName16); + if (idx < 0) return status_ = Status::NotBound; + + return init(vol, (uint8_t)idx); +} + +KvPartition32::Status KvPartition32::init(NvVolume& vol, const char* partitionAscii) { + if (!partitionAscii || !partitionAscii[0]) return status_ = Status::BadGeom; + if (!vol.isMounted()) return status_ = Status::NotBound; + + uint8_t idx = 0; + if (!vol.partitionFind(partitionAscii, idx)) return status_ = Status::NotBound; + + return init(vol, idx); +} + +KvPartition32::Status KvPartition32::readBlock32_(uint16_t block32, uint8_t out32[32]) const { + if (!vol_) return Status::NotBound; + if (block32 >= g_.partBlocks) return Status::OutOfRange; + + const uint16_t bo16 = block32ToBlock16Ofs_(block32); + if (vol_->partitionReadBlock(partIdx_, bo16, out32) != NvVolume::Status::Ok) return Status::IoError; + if (vol_->partitionReadBlock(partIdx_, (uint16_t)(bo16 + 1u), out32 + 16) != NvVolume::Status::Ok) return Status::IoError; + return Status::Ok; +} + +KvPartition32::Status KvPartition32::writeBlock32_(uint16_t block32, const uint8_t in32[32]) { + if (!vol_) return Status::NotBound; + if (block32 >= g_.partBlocks) return Status::OutOfRange; + + const uint16_t bo16 = block32ToBlock16Ofs_(block32); + if (vol_->partitionWriteBlock(partIdx_, bo16, in32) != NvVolume::Status::Ok) return Status::IoError; + if (vol_->partitionWriteBlock(partIdx_, (uint16_t)(bo16 + 1u), in32 + 16) != NvVolume::Status::Ok) return Status::IoError; + return Status::Ok; +} + +KvPartition32::Status KvPartition32::readSlot_(uint16_t slotIdx, uint8_t A[32]) { + if (slotIdx >= g_.slotCount) return Status::OutOfRange; + return readBlock32_(slotToBlock32Ofs_(slotIdx), A); +} + +KvPartition32::Status KvPartition32::writeSlot_(uint16_t slotIdx, const uint8_t A[32]) { + if (slotIdx >= g_.slotCount) return Status::OutOfRange; + return writeBlock32_(slotToBlock32Ofs_(slotIdx), A); +} + +KvPartition32::Status KvPartition32::readSlotName24_(uint16_t slotIdx, uint32_t& outName24) { + if (!vol_) return Status::NotBound; + if (slotIdx >= g_.slotCount) return Status::OutOfRange; + + const uint16_t block32 = slotToBlock32Ofs_(slotIdx); + const uint16_t bo16 = block32ToBlock16Ofs_(block32); + + uint8_t n3[3]; + if (vol_->partitionReadInBlock(partIdx_, bo16, 0, n3, 3) != NvVolume::Status::Ok) return Status::IoError; + outName24 = le24_(n3) & 0x00FFFFFFu; + return Status::Ok; +} + +KvPartition32::Status KvPartition32::readDataBlockByAddr_(uint8_t addr8, uint8_t out32[32]) const { + if (!dataAddrValid_(addr8)) return Status::Corrupt; + return readBlock32_(dataAddrToBlock_(addr8), out32); +} + +KvPartition32::Status KvPartition32::writeDataBlockByAddr_(uint8_t addr8, const uint8_t in32[32]) { + if (!dataAddrValid_(addr8)) return Status::OutOfRange; + return writeBlock32_(dataAddrToBlock_(addr8), in32); +} + +KvPartition32::Status KvPartition32::decodeEntry_(const uint8_t A[32], Decoded& out) const { + out = Decoded{}; + + out.flags2 = hdrFlags2_(A[3]); + out.len6 = hdrLen6_(A[3]); + + const bool fInlineOnly = (out.flags2 & FLAG_INLINE_ONLY) != 0u; + const bool fTail = (out.flags2 & FLAG_TAIL) != 0u; + + if (fInlineOnly) { + if (fTail) return Status::Corrupt; + if (out.len6 > INLINE_CAP) return Status::Corrupt; + + out.mode = Mode::InlineOnly; + out.inlineLen = out.len6; + return Status::Ok; + } + + if (!fTail) return Status::Corrupt; + if (out.len6 < 1u || out.len6 > BLOCK_BYTES) return Status::Corrupt; + + uint8_t body[25]; + bodyRead_(A, body); + + out.mode = Mode::AddrList; + out.tailLen = out.len6; + out.blockCount = 0; + + for (uint8_t i = 0; i < ADDR_CAP; i++) { + const uint8_t a = body[i]; + if (a == ADDR_END) break; + if (!dataAddrValid_(a)) return Status::Corrupt; + out.blocks[out.blockCount++] = a; + } + + if (out.blockCount == 0u) return Status::Corrupt; + out.blocks[out.blockCount] = ADDR_END; + return Status::Ok; +} + +uint16_t KvPartition32::payloadLenBytes_(const Decoded& d) const { + if (d.mode == Mode::InlineOnly) return d.inlineLen; + + if (d.blockCount == 0u) return 0u; + return (uint16_t)(((uint16_t)(d.blockCount - 1u) * BLOCK_BYTES) + d.tailLen); +} + +KvPartition32::Status KvPartition32::computeFullCrc24_(const uint8_t A[32], uint32_t& outCrc24) const { + if (!vol_) return Status::NotBound; + + Decoded d; + const Status ds = decodeEntry_(A, d); + if (ds != Status::Ok) return ds; + + uint8_t Ac[32]; + memcpy(Ac, A, 32); + Ac[29] = 0; + Ac[30] = 0; + Ac[31] = 0; + + uint32_t crc = Crc24::init(); + crc = Crc24::update(crc, Ac, 32); + + if (d.mode == Mode::AddrList) { + for (uint8_t i = 0; i < d.blockCount; i++) { + uint8_t blk[32]; + const Status rs = readDataBlockByAddr_(d.blocks[i], blk); + if (rs != Status::Ok) return rs; + crc = Crc24::update(crc, blk, 32); + } + } + + outCrc24 = crc & 0x00FFFFFFu; + return Status::Ok; +} + +KvPartition32::Status KvPartition32::validateEntryCrc_(const uint8_t A[32]) const { + const uint32_t stored = loadCrc24LE_(A) & 0x00FFFFFFu; + uint32_t calc = 0; + const Status s = computeFullCrc24_(A, calc); + if (s != Status::Ok) return s; + return (((stored ^ calc) & 0x00FFFFFFu) == 0u) ? Status::Ok : Status::Corrupt; +} + +void KvPartition32::markUsed_(uint16_t blockIdx, bool used) { + if (blockIdx >= g_.partBlocks) return; + + const uint16_t byteIdx = (uint16_t)(blockIdx >> 3); + const uint8_t mask = (uint8_t)(1u << (blockIdx & 7u)); + if (byteIdx >= BITMAP_BYTES) return; + + if (used) allocBits_[byteIdx] |= mask; + else allocBits_[byteIdx] &= (uint8_t)~mask; +} + +bool KvPartition32::isUsed_(uint16_t blockIdx) const { + if (blockIdx >= g_.partBlocks) return true; + + const uint16_t byteIdx = (uint16_t)(blockIdx >> 3); + const uint8_t mask = (uint8_t)(1u << (blockIdx & 7u)); + if (byteIdx >= BITMAP_BYTES) return true; + + return (allocBits_[byteIdx] & mask) != 0u; +} + +void KvPartition32::freeEntryBlocksFromMap_(const uint8_t A[32]) { + Decoded d; + if (decodeEntry_(A, d) != Status::Ok) return; + + if (d.mode == Mode::AddrList) { + for (uint8_t i = 0; i < d.blockCount; i++) { + if (dataAddrValid_(d.blocks[i])) markUsed_(dataAddrToBlock_(d.blocks[i]), false); + } + } +} + +KvPartition32::Status KvPartition32::findSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[32]) { + if (!vol_) return Status::NotBound; + name24 &= 0x00FFFFFFu; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + uint32_t n = 0; + const Status ns = readSlotName24_(i, n); + if (ns != Status::Ok) return ns; + if (n != name24) continue; + + const Status rs = readSlot_(i, A); + if (rs != Status::Ok) return rs; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::IoError) return vs; + if (vs != Status::Ok) continue; + + outSlot = i; + return Status::Ok; + } + + return Status::NotFound; +} + +KvPartition32::Status KvPartition32::findOrAllocSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[32], bool& outIsExisting) { + int16_t firstEmpty = -1; + int16_t firstCorruptMatch = -1; + name24 &= 0x00FFFFFFu; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + uint32_t n = 0; + const Status ns = readSlotName24_(i, n); + if (ns != Status::Ok) return ns; + + if (n == name24) { + const Status rs = readSlot_(i, A); + if (rs != Status::Ok) return rs; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::IoError) return vs; + + if (vs != Status::Ok) { + if (firstCorruptMatch < 0) firstCorruptMatch = (int16_t)i; + continue; + } + + outSlot = i; + outIsExisting = true; + return Status::Ok; + } + + if (n == EMPTY_NAME24 && firstEmpty < 0) firstEmpty = (int16_t)i; + } + + if (firstCorruptMatch >= 0) { + outSlot = (uint16_t)firstCorruptMatch; + outIsExisting = false; + return Status::Ok; + } + + if (firstEmpty >= 0) { + outSlot = (uint16_t)firstEmpty; + outIsExisting = false; + return Status::Ok; + } + + return Status::NoSpace; +} + +KvPartition32::Status KvPartition32::initAllocMap_() { + memset(allocBits_, 0, sizeof(allocBits_)); + + for (uint16_t b = 0; b < g_.dataBegin; b++) markUsed_(b, true); + for (uint16_t b = (uint16_t)(g_.dataBegin + g_.dataCount); b < g_.partBlocks; b++) markUsed_(b, true); + + uint8_t A[32]; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + Status s = readSlot_(i, A); + if (s != Status::Ok) { allocInit_ = false; return s; } + + const uint32_t n = le24_(&A[0]) & 0x00FFFFFFu; + if (n == EMPTY_NAME24) continue; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::IoError) { allocInit_ = false; return Status::IoError; } + if (vs != Status::Ok) continue; + + Decoded d; + if (decodeEntry_(A, d) != Status::Ok) continue; + + if (d.mode == Mode::AddrList) { + for (uint8_t k = 0; k < d.blockCount; k++) markUsed_(dataAddrToBlock_(d.blocks[k]), true); + } + } + + allocInit_ = true; + return Status::Ok; +} + +KvPartition32::Status KvPartition32::allocDataBlocks_(uint8_t count, uint8_t outAddrs[ADDR_CAP]) { + if (count == 0u || count > ADDR_CAP) return Status::OutOfRange; + if (!allocInit_) return Status::Corrupt; + + uint8_t got = 0; + + const uint16_t first = g_.dataBegin; + const uint16_t last = (uint16_t)(g_.dataBegin + g_.dataCount - 1u); + + for (int32_t b = (int32_t)last; b >= (int32_t)first; --b) { + const uint16_t bb = (uint16_t)b; + if (isUsed_(bb)) continue; + + markUsed_(bb, true); + outAddrs[got++] = (uint8_t)(bb - g_.dataBegin); + if (got == count) return Status::Ok; + } + + for (uint8_t i = 0; i < got; i++) markUsed_(dataAddrToBlock_(outAddrs[i]), false); + return Status::NoSpace; +} + +KvPartition32::Status KvPartition32::get(uint32_t name24, void* dst, uint16_t dstCap, uint16_t& outLen) { + if (!vol_) return Status::NotBound; + name24 &= 0x00FFFFFFu; + if (name24 == EMPTY_NAME24) return Status::OutOfRange; + + uint8_t A[32]; + uint16_t slot = 0; + const Status fs = findSlot_(name24, slot, A); + (void)slot; + if (fs != Status::Ok) return fs; + + Decoded d; + const Status ds = decodeEntry_(A, d); + if (ds != Status::Ok) return ds; + + outLen = payloadLenBytes_(d); + if (!dst) return Status::Ok; + if (dstCap < outLen) return Status::OutOfRange; + + uint8_t* p = (uint8_t*)dst; + + if (d.mode == Mode::InlineOnly) { + uint8_t body[25]; + bodyRead_(A, body); + if (d.inlineLen) memcpy(p, body, d.inlineLen); + return Status::Ok; + } + + uint16_t written = 0; + for (uint8_t i = 0; i < d.blockCount; i++) { + uint8_t blk[32]; + const Status rs = readDataBlockByAddr_(d.blocks[i], blk); + if (rs != Status::Ok) return rs; + + const bool isLast = (i == (uint8_t)(d.blockCount - 1u)); + const uint8_t n = isLast ? d.tailLen : BLOCK_BYTES; + + memcpy(p + written, blk, n); + written = (uint16_t)(written + n); + } + + return (written == outLen) ? Status::Ok : Status::Corrupt; +} + +KvPartition32::Status KvPartition32::put(uint32_t name24, const void* src, uint16_t len) { + if (!vol_) return Status::NotBound; + if (!src && len) return Status::OutOfRange; + if (!allocInit_) return Status::Corrupt; + + name24 &= 0x00FFFFFFu; + if (name24 == EMPTY_NAME24) return Status::OutOfRange; + + if (len > (uint16_t)(ADDR_CAP * BLOCK_BYTES)) return Status::OutOfRange; + + uint8_t Aold[32]; + uint16_t slot = 0; + bool existing = false; + + Status s = findOrAllocSlot_(name24, slot, Aold, existing); + if (s != Status::Ok) return s; + + if (!existing) memset(Aold, 0xFF, sizeof(Aold)); + + bool freedOld = false; + { + const Status vs = validateEntryCrc_(Aold); + if (vs == Status::Ok) { + freeEntryBlocksFromMap_(Aold); + freedOld = true; + } else if (vs == Status::IoError) { + return Status::IoError; + } + } + + uint8_t A[32]; + memset(A, 0xFF, sizeof(A)); + wrle24_(&A[0], name24); + + const uint8_t* p = (const uint8_t*)src; + + if (len <= INLINE_CAP) { + A[3] = packHeader_(FLAG_INLINE_ONLY, (uint8_t)len); + + uint8_t body[25]; + memset(body, 0xFF, sizeof(body)); + if (len) memcpy(body, p, len); + bodyWrite_(A, body); + + uint32_t crc24 = 0; + Status cs = computeFullCrc24_(A, crc24); + if (cs != Status::Ok) { invalidateAlloc_(); return cs; } + storeCrc24LE_(A, crc24); + + Status ws = writeSlot_(slot, A); + if (ws == Status::IoError) invalidateAlloc_(); + return ws; + } + + const uint8_t blocksNeeded = (uint8_t)((len + (BLOCK_BYTES - 1u)) / BLOCK_BYTES); + if (blocksNeeded == 0u || blocksNeeded > ADDR_CAP) return Status::OutOfRange; + + uint8_t addrs[ADDR_CAP] = {0}; + s = allocDataBlocks_(blocksNeeded, addrs); + if (s != Status::Ok) { + if (freedOld) invalidateAlloc_(); + return s; + } + + uint8_t tmp[32]; + for (uint8_t i = 0; i < blocksNeeded; i++) { + memset(tmp, 0xFF, sizeof(tmp)); + + const uint16_t ofs = (uint16_t)i * BLOCK_BYTES; + const uint16_t avail = (len > ofs) ? (uint16_t)(len - ofs) : 0u; + const uint16_t chunk = (avail >= BLOCK_BYTES) ? BLOCK_BYTES : avail; + if (chunk) memcpy(tmp, p + ofs, chunk); + + Status ws = writeDataBlockByAddr_(addrs[i], tmp); + if (ws != Status::Ok) { + for (uint8_t k = 0; k < blocksNeeded; k++) markUsed_(dataAddrToBlock_(addrs[k]), false); + invalidateAlloc_(); + return ws; + } + } + + const uint8_t tailLen = (uint8_t)((len & (BLOCK_BYTES - 1u)) ? (len & (BLOCK_BYTES - 1u)) : BLOCK_BYTES); + A[3] = packHeader_(FLAG_TAIL, tailLen); + + uint8_t body[25]; + memset(body, ADDR_END, sizeof(body)); + for (uint8_t i = 0; i < blocksNeeded; i++) body[i] = addrs[i]; + bodyWrite_(A, body); + + uint32_t crc24 = 0; + Status cs = computeFullCrc24_(A, crc24); + if (cs != Status::Ok) { invalidateAlloc_(); return cs; } + storeCrc24LE_(A, crc24); + + Status ws = writeSlot_(slot, A); + if (ws == Status::IoError) invalidateAlloc_(); + return ws; +} + +KvPartition32::Status KvPartition32::del(uint32_t name24) { + if (!vol_) return Status::NotBound; + if (!allocInit_) return Status::Corrupt; + + name24 &= 0x00FFFFFFu; + if (name24 == EMPTY_NAME24) return Status::OutOfRange; + + uint8_t A[32]; + uint16_t slot = 0; + const Status fs = findSlot_(name24, slot, A); + if (fs != Status::Ok) return fs; + + if (validateEntryCrc_(A) == Status::Ok) freeEntryBlocksFromMap_(A); + + memset(A, 0xFF, sizeof(A)); + + Status ws = writeSlot_(slot, A); + if (ws == Status::IoError) invalidateAlloc_(); + return ws; +} + +uint32_t KvPartition32::computeKey(const char* nameAscii) { + uint32_t crc = Crc24::init(); + crc = Crc24::update(crc, (const uint8_t*)KV_NAME_SALT, (uint16_t)strlen(KV_NAME_SALT)); + crc = Crc24::update(crc, (const uint8_t*)(nameAscii ? nameAscii : ""), (uint16_t)strlen(nameAscii ? nameAscii : "")); + return (crc & 0x00FFFFFFu); +} + +uint32_t KvPartition32::computeKey(const __FlashStringHelper* nameAsciiF) { +#if defined(ARDUINO) + if (!nameAsciiF) nameAsciiF = F(""); + + uint32_t crc = Crc24::init(); + crc = Crc24::update(crc, (const uint8_t*)KV_NAME_SALT, (uint16_t)strlen(KV_NAME_SALT)); + + const char* p = (const char*)nameAsciiF; + for (;;) { + const uint8_t c = readProgByte32_(p++); + if (c == 0) break; + crc = Crc24::updateByte(crc, c); + } + + return (crc & 0x00FFFFFFu); +#else + (void)nameAsciiF; + return computeKey(""); +#endif +} + +KvPartition32::Status KvPartition32::scrub(uint16_t* outDeleted) { + if (outDeleted) *outDeleted = 0; + + if (!vol_ || !vol_->isMounted()) return Status::NotBound; + if (!initGeom_()) return Status::BadGeom; + + uint8_t A[32]; + uint16_t deleted = 0; + + for (uint16_t i = 0; i < g_.slotCount; i++) { + const Status rs = readSlot_(i, A); + if (rs != Status::Ok) return rs; + + const uint32_t n = le24_(&A[0]) & 0x00FFFFFFu; + if (n == EMPTY_NAME24) continue; + + const Status vs = validateEntryCrc_(A); + if (vs == Status::Ok) continue; + if (vs == Status::IoError) return Status::IoError; + if (vs != Status::Corrupt) return vs; + + memset(A, 0xFF, sizeof(A)); + const Status ws = writeSlot_(i, A); + if (ws != Status::Ok) { + if (ws == Status::IoError) invalidateAlloc_(); + return ws; + } + + deleted++; + } + + const Status ms = initAllocMap_(); + if (ms != Status::Ok) { + if (ms == Status::IoError) invalidateAlloc_(); + return ms; + } + + if (outDeleted) *outDeleted = deleted; + return Status::Ok; +} + +KvPartition32::Status KvPartition32::stats(Stats& out) { + memset(&out, 0, sizeof(out)); + if (!vol_ || !vol_->isMounted()) return Status::NotBound; + if (!initGeom_()) return Status::BadGeom; + if (!allocInit_) return Status::Corrupt; + + out.dataBlocksTotal = g_.dataCount; + out.slotsTotal = g_.slotCount; + + uint16_t usedData = 0; + for (uint16_t a = 0; a < g_.dataCount; a++) { + const uint16_t b = (uint16_t)(g_.dataBegin + a); + const uint16_t byteIdx = (uint16_t)(b >> 3); + const uint8_t mask = (uint8_t)(1u << (b & 7u)); + if (byteIdx >= BITMAP_BYTES) break; + if (allocBits_[byteIdx] & mask) usedData++; + } + out.dataBlocksFree = (usedData <= g_.dataCount) ? (uint16_t)(g_.dataCount - usedData) : 0u; + + uint16_t empty = 0; + for (uint16_t i = 0; i < g_.slotCount; i++) { + uint32_t n = 0; + const Status ns = readSlotName24_(i, n); + if (ns != Status::Ok) return ns; + if (n == EMPTY_NAME24) empty++; + } + out.slotsFree = empty; + + return Status::Ok; +} diff --git a/src/lib/nv/NvKvPartition32.h b/src/lib/nv/NvKvPartition32.h new file mode 100755 index 0000000..8286224 --- /dev/null +++ b/src/lib/nv/NvKvPartition32.h @@ -0,0 +1,295 @@ +// ----------------------------------------------------------------------------------- +// NvKvPartition32.h +// +// KV partition on virtual 32-byte blocks (v2 format). +// +// This KV layer stores named blobs in a fixed-size partition. +// The underlying NvVolume partition uses 16-byte blocks; this class combines pairs +// of consecutive 16-byte blocks into one virtual 32-byte block. +// +// Layout +// - Table entries are stored at blocks [0 .. tableBlocks-1]. +// - Data blocks are stored at [dataBegin .. partBlocks-1], where dataBegin = tableBlocks. +// - tableBlocks targets KEY_TABLE_PERCENT (default 40%) and can slide upward +// to at most partBlocks/2 as needed so dataCount fits byte addressing. +// +// Terminology +// - Block: 32 bytes (virtual). +// - Data address (addr8): 8-bit index into the data region. +// addr8=0 -> physical block dataBegin +// ... +// addr8=254 -> physical block dataBegin+254 +// - addr8 value 0xFF is reserved as ADDR_END sentinel. +// +// Keying +// - Each record key is name24: +// name24 = CRC24( KV_NAME_SALT + asciiName ) +// - CRC24 uses Crc24::init/update from Crc.h. +// +// Allocation model +// - Overwrite-in-place semantics; no compaction. +// - Data block allocation is top-down within the data region. +// - Addressable data region is limited to 255 blocks (0..254), clamped by partition size. +// +// =================================================================================================== +// v2 ENTRY FORMAT (32 bytes, one entry per key) +// a[0..2] : name24 (LE) +// a[3] : header = flags2 + len6 +// flags bits: +// FLAG_INLINE_ONLY : body contains inline payload only +// FLAG_TAIL : addr-list mode; len6 = bytes used in last block +// a[4..28] : body200 (25 bytes), interpreted by mode: +// - INLINE_ONLY: up to 25 inline bytes +// - ADDR_LIST: up to 25 addr8 values (0xFF sentinel-terminated) +// a[29..31]: crc24 (LE), written last +// CRC covers entry (with crc bytes zeroed) and referenced payload blocks. +// +// Mode max payload sizes: +// - InlineOnly: 25 bytes +// - AddrList: 25 * 32 = 800 bytes +// Overall max file size: 800 bytes +// ----------------------------------------------------------------------------------- +#pragma once + +#include +#include +#include + +#include "NvVolume.h" +#include "../math/Crc.h" + +class __FlashStringHelper; + +#ifndef MAX_BLOCKS_32 + #define MAX_BLOCKS_32 2048u +#endif + +#ifndef KEY_TABLE_PERCENT + #define KEY_TABLE_PERCENT 400u // 40% table by default +#endif + +class KvPartition32 { +public: + enum class Status : uint8_t { + Ok = 0, + NotBound, + BadGeom, + IoError, + Corrupt, + NotFound, + NoSpace, + OutOfRange, + }; + + static constexpr uint8_t BLOCK_BYTES = 32u; + static constexpr uint8_t ADDR_END = 0xFFu; + static constexpr uint32_t EMPTY_NAME24 = 0x00FFFFFFu; + + static constexpr uint8_t FLAG_INLINE_ONLY = 0x01u; + static constexpr uint8_t FLAG_TAIL = 0x02u; + + static constexpr uint8_t INLINE_CAP = 25u; + static constexpr uint8_t ADDR_CAP = 25u; + + struct Geom { + uint16_t partBlocks = 0; // count of 32-byte virtual blocks + uint16_t tableBlocks = 0; + uint16_t slotCount = 0; + uint16_t dataBegin = 0; + uint16_t dataCount = 0; + uint16_t mainPoolBase = 0; + }; + + struct Stats { + uint16_t dataBlocksTotal; + uint16_t dataBlocksFree; + uint16_t slotsTotal; + uint16_t slotsFree; + }; + + KvPartition32() = default; + + Status init(NvVolume& vol, uint8_t partIdx); + Status init(NvVolume& vol, uint16_t partitionName16); + Status init(NvVolume& vol, const char* partitionAscii); + + const Geom& geom() const { return g_; } + Status status() const { return status_; } + + Status get(uint32_t name24, void* dst, uint16_t dstCap, uint16_t& outLen); + Status put(uint32_t name24, const void* src, uint16_t len); + Status del(uint32_t name24); + + static uint32_t computeKey(const char* nameAscii); + static uint32_t computeKey(const __FlashStringHelper* nameAsciiF); + + Status get(const char* nameAscii, void* dst, uint16_t dstCap, uint16_t& outLen) { return get(name24_(nameAscii), dst, dstCap, outLen); } + Status put(const char* nameAscii, const void* src, uint16_t len) { return put(name24_(nameAscii), src, len); } + Status put(const char* nameAscii, const char* zstr) { + if (!zstr) return put(nameAscii, (const void*)"", 1); + const uint16_t n = (uint16_t)strlen(zstr) + 1u; + return put(nameAscii, (const void*)zstr, n); + } + Status del(const char* nameAscii) { return del(name24_(nameAscii)); } + + Status scrub(uint16_t* outDeleted); + + template + Status put(const char* nameAscii, const T& v) { return put(nameAscii, (const void*)&v, (uint16_t)sizeof(T)); } + + template + Status put(uint32_t name24, const T& v) { return put(name24, (const void*)&v, (uint16_t)sizeof(T)); } + + template + Status get(const char* nameAscii, T& out) { + T tmp = out; + uint16_t outLen = 0; + + Status s = get(nameAscii, (void*)&tmp, (uint16_t)sizeof(T), outLen); + if (s != Status::Ok) return s; + if (outLen != sizeof(T)) return Status::Corrupt; + + out = tmp; + return Status::Ok; + } + + template + Status get(uint32_t name24, T& out) { + T tmp = out; + uint16_t outLen = 0; + + Status s = get(name24, (void*)&tmp, (uint16_t)sizeof(T), outLen); + if (s != Status::Ok) return s; + if (outLen != sizeof(T)) return Status::Corrupt; + + out = tmp; + return Status::Ok; + } + + bool isValid() const { return (status_ == Status::Ok) && allocInit_; } + + template + bool getOrInit(const char* nameAscii, T& inOutDefaults) { + if (!nameAscii) return false; + if (!isValid()) return false; + + const Status gs = get(nameAscii, inOutDefaults); + if (gs == Status::Ok) return true; + if (gs == Status::IoError) return false; + + if (put(nameAscii, inOutDefaults) != Status::Ok) { initErrorFlag_ = true; } + return true; + } + + template + bool getOrInit(uint32_t name24, T& inOutDefaults) { + if ((name24 & 0x00FFFFFFu) == EMPTY_NAME24) return false; + if (!isValid()) return false; + + const Status gs = get(name24, inOutDefaults); + if (gs == Status::Ok) return true; + if (gs == Status::IoError) return false; + + if (put(name24, inOutDefaults) != Status::Ok) { initErrorFlag_ = true; } + return true; + } + + void resetInitErrorFlag() { initErrorFlag_ = false; } + bool getInitErrorFlag() { return initErrorFlag_; } + + Status stats(Stats& out); + +private: + enum class Mode : uint8_t { + InlineOnly = 0, // max 25 bytes + AddrList = 1, // max 25 * 32 = 800 bytes (sentinel terminated) + }; + + struct Decoded { + Mode mode = Mode::InlineOnly; + uint8_t flags2 = 0; + uint8_t len6 = 0; + + uint8_t inlineLen = 0; + uint8_t tailLen = 0; + + uint8_t blocks[ADDR_CAP + 1u] = {ADDR_END}; + uint8_t blockCount = 0; + }; + + static constexpr uint16_t BITMAP_BYTES = (MAX_BLOCKS_32 + 7u) / 8u; + static constexpr const char* KV_NAME_SALT = "OnCueKVv2"; + + static inline uint8_t packHeader_(uint8_t flags2, uint8_t len6) { return (uint8_t)((flags2 & 0x03u) | ((len6 & 0x3Fu) << 2)); } + static inline uint8_t hdrFlags2_(uint8_t header) { return (uint8_t)(header & 0x03u); } + static inline uint8_t hdrLen6_(uint8_t header) { return (uint8_t)(header >> 2); } + + static inline uint32_t le24_(const uint8_t* p) { + return ((uint32_t)p[0]) | ((uint32_t)p[1] << 8) | ((uint32_t)p[2] << 16); + } + static inline void wrle24_(uint8_t* p, uint32_t v24) { + p[0] = (uint8_t)(v24 & 0xFFu); + p[1] = (uint8_t)((v24 >> 8) & 0xFFu); + p[2] = (uint8_t)((v24 >> 16) & 0xFFu); + } + + static inline uint32_t loadCrc24LE_(const uint8_t A[32]) { + return ((uint32_t)A[29]) | ((uint32_t)A[30] << 8) | ((uint32_t)A[31] << 16); + } + static inline void storeCrc24LE_(uint8_t A[32], uint32_t crc24) { + A[29] = (uint8_t)(crc24 & 0xFFu); + A[30] = (uint8_t)((crc24 >> 8) & 0xFFu); + A[31] = (uint8_t)((crc24 >> 16) & 0xFFu); + } + + static inline uint32_t name24_(const char* asciiName) { return computeKey(asciiName ? asciiName : ""); } + + uint16_t block32ToBlock16Ofs_(uint16_t block32) const { return (uint16_t)(block32 * 2u); } + uint16_t slotToBlock32Ofs_(uint16_t slotIdx) const { return slotIdx; } + uint16_t dataAddrToBlock_(uint8_t addr8) const { return (uint16_t)(g_.dataBegin + (uint16_t)addr8); } + bool dataAddrValid_(uint8_t addr8) const { return addr8 != ADDR_END && (uint16_t)addr8 < g_.dataCount; } + + Status readBlock32_(uint16_t block32, uint8_t out32[32]) const; + Status writeBlock32_(uint16_t block32, const uint8_t in32[32]); + + Status readSlot_(uint16_t slotIdx, uint8_t A[32]); + Status writeSlot_(uint16_t slotIdx, const uint8_t A[32]); + Status readSlotName24_(uint16_t slotIdx, uint32_t& outName24); + + Status readDataBlockByAddr_(uint8_t addr8, uint8_t out32[32]) const; + Status writeDataBlockByAddr_(uint8_t addr8, const uint8_t in32[32]); + + void bodyRead_(const uint8_t A[32], uint8_t out[25]) const { memcpy(out, &A[4], 25); } + void bodyWrite_(uint8_t A[32], const uint8_t in[25]) const { memcpy(&A[4], in, 25); } + + Status decodeEntry_(const uint8_t A[32], Decoded& out) const; + uint16_t payloadLenBytes_(const Decoded& d) const; + + Status computeFullCrc24_(const uint8_t A[32], uint32_t& outCrc24) const; + Status validateEntryCrc_(const uint8_t A[32]) const; + + Status findSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[32]); + Status findOrAllocSlot_(uint32_t name24, uint16_t& outSlot, uint8_t A[32], bool& outIsExisting); + + void markUsed_(uint16_t blockIdx, bool used); + bool isUsed_(uint16_t blockIdx) const; + + Status initAllocMap_(); + Status allocDataBlocks_(uint8_t count, uint8_t outAddrs[ADDR_CAP]); + void invalidateAlloc_() { allocInit_ = false; } + + void freeEntryBlocksFromMap_(const uint8_t A[32]); + + bool initGeom_(); + +private: + NvVolume* vol_ = nullptr; + uint8_t partIdx_ = 0xFF; + Geom g_; + Status status_ = Status::NotBound; + + uint8_t allocBits_[BITMAP_BYTES] = {0}; + bool allocInit_ = false; + + bool initErrorFlag_ = false; +}; diff --git a/src/lib/nv/NvVolume.cpp b/src/lib/nv/NvVolume.cpp new file mode 100755 index 0000000..fed97e3 --- /dev/null +++ b/src/lib/nv/NvVolume.cpp @@ -0,0 +1,598 @@ +// NvVolume.cpp +#include "NvVolume.h" +#include "../math/Crc.h" + +uint16_t NvVolume::crc16_sb(const uint8_t* data, uint16_t lenNoCrc) { + return Crc16::compute(data, lenNoCrc); +} + +// Reserve 0 for "unnamed" sentinel +static uint16_t normalizeTag16(uint16_t t) { + if (t == 0x0000u) t = 0x0001u; + return t; +} + +uint16_t NvVolume::computeVolumeName16(const char* projectAscii, uint16_t storageVersion) { + const uint16_t base = normalizeTag16(Crc16::saltedAscii(kSalt, projectAscii)); + // Compatibility token: base(project) + storageVersion (non-cryptographic, deterministic). + return normalizeTag16((uint16_t)(base + storageVersion)); +} + +uint16_t NvVolume::computePartName16(const char* partitionAscii) { + return normalizeTag16(Crc16::saltedAscii(kSalt, partitionAscii)); +} + +// bytes11 points at sb[3..13] (11 bytes, 88 bits). bit0 is LSB of bytes11[0]. +uint32_t NvVolume::getBitsLSB(const uint8_t* bytes11, uint32_t bitIndex, uint32_t bitCount) { + uint32_t v = 0; + for (uint32_t i = 0; i < bitCount; ++i) { + const uint32_t b = bitIndex + i; + const uint32_t byte = b >> 3; + const uint32_t bit = b & 7u; + const uint32_t bitVal = (bytes11[byte] >> bit) & 1u; + v |= (bitVal << i); + } + return v; +} + +void NvVolume::setBitsLSB(uint8_t* bytes11, uint32_t bitIndex, uint32_t bitCount, uint32_t value) { + for (uint32_t i = 0; i < bitCount; ++i) { + const uint32_t b = bitIndex + i; + const uint32_t byte = b >> 3; + const uint32_t bit = b & 7u; + const uint8_t mask = (uint8_t)(1u << bit); + if ((value >> i) & 1u) bytes11[byte] |= mask; + else bytes11[byte] &= (uint8_t)~mask; + } +} + +NvVolume::Status NvVolume::init(NvDevice& dev) { + dev_ = &dev; + + uint32_t bytes = dev_->sizeBytes(); + if (bytes < kBlockSize) return status_ = Status::BadGeometry; + if (bytes > 65536u) return status_ = Status::BadGeometry; + if (bytes & (kBlockSize - 1u)) return status_ = Status::BadGeometry; + + const uint32_t blocks = bytes / kBlockSize; + if (blocks > kMaxBlocks) return status_ = Status::BadGeometry; + + blockCount_ = (uint16_t)blocks; + + mounted_ = false; + partCount_ = 0; + flags8_ = 0; + flags4_ = 0; + storedVolumeName16_ = 0; + hasPartNames_ = false; + for (uint8_t i = 0; i < kMaxParts; ++i) { + parts_[i] = Part{}; + partName16_[i] = 0; + } + + builderActive_ = false; + builderProject_ = nullptr; + builderStorageVersion_ = 0; + builderCount_ = 0; + for (uint8_t i = 0; i < kMaxParts; ++i) { + builderPartAscii_[i] = nullptr; + builderSizeBlocks_[i] = 0; + } + + status_ = Status::Ok; + return status_; +} + +NvVolume::Status NvVolume::mount() { + if (!dev_) return status_ = Status::NotReady; + if (blockCount_ < 2) return status_ = Status::BadGeometry; + + uint8_t sb0[16]; + Status rb = readBlockSt(*dev_, 0, sb0); + if (rb != Status::Ok) return status_ = rb; + + const uint16_t storedCrc0 = le16(&sb0[14]); + const uint16_t calcCrc0 = crc16_sb(&sb0[0], 14); + if (storedCrc0 != calcCrc0) { + mounted_ = false; partCount_ = 0; hasPartNames_ = false; + return status_ = Status::Unformatted; + } + + const uint8_t flags8 = sb0[2]; + const uint8_t ver = (uint8_t)(flags8 >> kFlagVersionShift); + if (ver != kSbVersionV1) { + mounted_ = false; partCount_ = 0; hasPartNames_ = false; + return status_ = Status::Unformatted; + } + + if (flags8 & kFlagReservedMask) { + mounted_ = false; partCount_ = 0; hasPartNames_ = false; + return status_ = Status::Unformatted; + } + + storedVolumeName16_ = le16(&sb0[0]); + + Status e = enumerateFromSb0(sb0); + if (e != Status::Ok) { + mounted_ = false; partCount_ = 0; hasPartNames_ = false; + return status_ = Status::Unformatted; // keep mount policy + } + + (void)loadNamesBlock(); // optional; keep prior semantics + + mounted_ = true; + return status_ = Status::Ok; +} + +NvVolume::Status NvVolume::mount(const char* projectAscii, uint16_t storageVersion) { + Status st = mount(); // call existing one + if (st != Status::Ok) return st; + + if (!projectAscii || !projectAscii[0]) { + mounted_ = false; partCount_ = 0; hasPartNames_ = false; + return status_ = Status::BadGeometry; + } + + const uint16_t expected = computeVolumeName16(projectAscii, storageVersion); + if (storedVolumeName16_ != expected) { + mounted_ = false; partCount_ = 0; hasPartNames_ = false; + return status_ = Status::Unformatted; // policy: treat as "not ours / incompatible" + } + + return status_ = Status::Ok; +} + +NvVolume::Status NvVolume::enumerateFromSb0(const uint8_t sb0[16]) { + flags8_ = sb0[2]; + + const uint8_t* packed = &sb0[3]; + flags4_ = (uint8_t)getBitsLSB(packed, 0, 4); + + uint16_t p[7]; + for (uint8_t i = 0; i < 7; ++i) { + p[i] = (uint16_t)getBitsLSB(packed, 4u + (uint32_t)i * 12u, 12u); + } + + uint16_t starts[7]; + uint8_t N = 0; + for (uint8_t i = 0; i < 7; ++i) { + const uint16_t v = (uint16_t)(p[i] & 0x0FFFu); + if (v == 0x0FFFu) break; + starts[N++] = v; + } + + const bool lastIsEndMarker = (flags8_ & kFlagLastIsEndMarker) != 0; + if (lastIsEndMarker && N < 2) return Status::BadGeometry; + + for (uint8_t i = 0; i < N; ++i) { + const uint16_t s = starts[i]; + if (s < 2) return Status::BadGeometry; + if (s >= blockCount_) return Status::OutOfRange; + if (i > 0 && starts[i - 1] >= s) return Status::BadGeometry; + } + + uint8_t partCount = N; + uint16_t limitBlock = blockCount_; + if (lastIsEndMarker) { + partCount = (uint8_t)(N - 1); + limitBlock = starts[N - 1]; + if (limitBlock >= blockCount_) return Status::OutOfRange; + } + + for (uint8_t i = 0; i < partCount; ++i) { + const uint16_t begin = starts[i]; + uint16_t end = 0; + + if (i + 1 < partCount) end = starts[i + 1]; + else end = lastIsEndMarker ? limitBlock : blockCount_; + + if (end <= begin) return Status::BadGeometry; + + parts_[i].beginBlock = begin; + parts_[i].endBlock = end; + } + + for (uint8_t i = partCount; i < kMaxParts; ++i) { + parts_[i].beginBlock = 0; + parts_[i].endBlock = 0; + } + + partCount_ = partCount; + return Status::Ok; +} + +NvVolume::Status NvVolume::loadNamesBlock() { + hasPartNames_ = false; + for (uint8_t i = 0; i < kMaxParts; ++i) partName16_[i] = 0; + + if (!dev_ || blockCount_ < 2) return Status::Ok; + + uint8_t sb1[16]; + Status rb = readBlockSt(*dev_, 1, sb1); + if (rb != Status::Ok) return Status::Ok; // optional block; treat as absent (including NotReady) + + const uint16_t stored = le16(&sb1[14]); + const uint16_t calc = crc16_sb(&sb1[0], 14); + if (stored != calc) return Status::Ok; + + for (uint8_t i = 0; i < kMaxParts; ++i) { + partName16_[i] = le16(&sb1[i * 2]); + } + hasPartNames_ = true; + return Status::Ok; +} + +NvVolume::Status NvVolume::writeNamesBlock(const uint16_t partName16[kMaxParts]) { + if (!dev_) return Status::NotReady; + if (!dev_->isWritable()) return Status::ReadOnly; + if (blockCount_ < 2) return Status::BadGeometry; + + uint8_t sb1[16] = {0}; + for (uint8_t i = 0; i < kMaxParts; ++i) { + wrle16(&sb1[i * 2], partName16[i]); + } + const uint16_t crc = crc16_sb(&sb1[0], 14); + wrle16(&sb1[14], crc); + + Status wb = writeBlockSt(*dev_, 1, sb1); + if (wb != Status::Ok) return wb; + + for (uint8_t i = 0; i < kMaxParts; ++i) partName16_[i] = partName16[i]; + hasPartNames_ = true; + return Status::Ok; +} + +NvVolume::Status NvVolume::scrubPartitionBlocksOnFormat_() { + if (!dev_) return Status::NotReady; + if (!dev_->isWritable()) return Status::ReadOnly; + + uint8_t ff[16]; + for (uint8_t i = 0; i < 16; ++i) ff[i] = 0xFF; + + for (uint8_t i = 0; i < partCount_; ++i) { + const Part& p = parts_[i]; + if (p.endBlock <= p.beginBlock) continue; + + uint16_t lo = p.beginBlock; + uint16_t hi = (uint16_t)(p.endBlock - 1u); + + if (lo >= blockCount_ || hi >= blockCount_) return Status::OutOfRange; + + // Don’t touch SB0/SB1 (physical blocks 0 and 1) + if (hi < 2u) continue; + if (lo < 2u) lo = 2u; + + for (uint16_t j = lo; j <= hi; ++j) { + Status s = writeBlockSt(*dev_, j, ff); + if (s != Status::Ok) return s; + } + } + + return Status::Ok; +} + +NvVolume::Status NvVolume::formatAll(const char* projectAscii, + uint16_t storageVersion, + const uint16_t starts[kMaxParts], + bool lastIsEndMarker, + const char* partAscii[kMaxParts]) { + if (!dev_) return status_ = Status::NotReady; + if (!dev_->isWritable()) return status_ = Status::ReadOnly; + if (blockCount_ < 3) return status_ = Status::BadGeometry; + + uint16_t prev = 0; + uint8_t N = 0; + for (uint8_t i = 0; i < kMaxParts; ++i) { + const uint16_t s = (uint16_t)(starts[i] & 0x0FFFu); + if (s == 0x0FFFu) break; + if (s < 2) return status_ = Status::BadGeometry; + if (s >= blockCount_) return status_ = Status::OutOfRange; + if (i > 0 && prev >= s) return status_ = Status::BadGeometry; + prev = s; + ++N; + } + if (lastIsEndMarker && N < 2) return status_ = Status::BadGeometry; + + uint8_t sb0[16] = {0}; + + const uint16_t volName16 = computeVolumeName16(projectAscii, storageVersion); + wrle16(&sb0[0], volName16); + + uint8_t flags8 = 0; + if (lastIsEndMarker) flags8 |= kFlagLastIsEndMarker; + flags8 |= (uint8_t)(kSbVersionV1 << kFlagVersionShift); + sb0[2] = flags8; + + uint8_t* packed = &sb0[3]; + for (uint8_t i = 0; i < 11; ++i) packed[i] = 0; + + setBitsLSB(packed, 0, 4, 0); + for (uint8_t i = 0; i < kMaxParts; ++i) { + setBitsLSB(packed, 4u + (uint32_t)i * 12u, 12u, (uint32_t)(starts[i] & 0x0FFFu)); + } + + const uint16_t crc0 = crc16_sb(&sb0[0], 14); + wrle16(&sb0[14], crc0); + + Status wb0 = writeBlockSt(*dev_, 0, sb0); + if (wb0 != Status::Ok) return status_ = wb0; + + uint16_t pn[7] = {0}; + for (uint8_t i = 0; i < kMaxParts; ++i) { + if (partAscii && partAscii[i] && partAscii[i][0]) pn[i] = computePartName16(partAscii[i]); + else pn[i] = 0; + } + + Status w1 = writeNamesBlock(pn); + if (w1 != Status::Ok) return status_ = w1; + + storedVolumeName16_ = volName16; + flags8_ = flags8; + flags4_ = 0; + + // Post-format self-check: this is our own SB0 buffer; propagate real cause. + Status e = enumerateFromSb0(sb0); + if (e != Status::Ok) { mounted_ = false; partCount_ = 0; return status_ = e; } + + Status scrub = scrubPartitionBlocksOnFormat_(); + if (scrub != Status::Ok) { mounted_ = false; partCount_ = 0; return status_ = scrub; } + + mounted_ = true; + return status_ = Status::Ok; +} + +NvVolume::Status NvVolume::formatBegin(const char* projectAscii, uint16_t storageVersion) { + if (!dev_) return status_ = Status::NotReady; + if (!dev_->isWritable()) return status_ = Status::ReadOnly; + + builderActive_ = true; + builderProject_ = projectAscii ? projectAscii : ""; + builderStorageVersion_ = storageVersion; + builderCount_ = 0; + for (uint8_t i = 0; i < kMaxParts; ++i) { + builderPartAscii_[i] = nullptr; + builderSizeBlocks_[i] = 0; + } + return status_ = Status::Ok; +} + +bool NvVolume::formatAddPartition(const char* partitionAscii, uint32_t sizeBytes) { + if (!builderActive_) return false; + if (builderCount_ >= kMaxParts) return false; + + if (sizeBytes == 0) { + for (uint8_t i = 0; i < builderCount_; ++i) { + if (builderSizeBlocks_[i] == 0) return false; + } + builderSizeBlocks_[builderCount_] = 0; + } else { + if (sizeBytes & (kBlockSize - 1u)) return false; + const uint32_t blocks = sizeBytes / kBlockSize; + if (blocks == 0 || blocks > 0xFFFFu) return false; + builderSizeBlocks_[builderCount_] = (uint16_t)blocks; + + for (uint8_t i = 0; i < builderCount_; ++i) { + if (builderSizeBlocks_[i] == 0) return false; + } + } + + builderPartAscii_[builderCount_] = partitionAscii; + ++builderCount_; + return true; +} + +NvVolume::Status NvVolume::formatCommit() { + if (!builderActive_) return status_ = Status::NotReady; + if (!dev_) return status_ = Status::NotReady; + if (!dev_->isWritable()) return status_ = Status::ReadOnly; + if (blockCount_ < 3) return status_ = Status::BadGeometry; + + if (builderCount_ == 0) return status_ = Status::BadGeometry; + + uint16_t starts[kMaxParts]; + for (uint8_t i = 0; i < kMaxParts; ++i) starts[i] = 0x0FFFu; + + uint16_t cur = 2; + for (uint8_t i = 0; i < builderCount_; ++i) { + starts[i] = cur; + + uint16_t sz = builderSizeBlocks_[i]; + if (sz == 0) { + if (cur >= blockCount_) return status_ = Status::OutOfRange; + cur = blockCount_; + break; + } else { + const uint32_t next = (uint32_t)cur + (uint32_t)sz; + if (next > (uint32_t)blockCount_) return status_ = Status::OutOfRange; + cur = (uint16_t)next; + } + } + + const char* partAscii[kMaxParts] = {nullptr}; + for (uint8_t i = 0; i < builderCount_; ++i) partAscii[i] = builderPartAscii_[i]; + + Status s = formatAll(builderProject_, builderStorageVersion_, starts, false, partAscii); + + builderActive_ = false; + builderProject_ = nullptr; + builderStorageVersion_ = 0; + builderCount_ = 0; + for (uint8_t i = 0; i < kMaxParts; ++i) { + builderPartAscii_[i] = nullptr; + builderSizeBlocks_[i] = 0; + } + + return status_ = s; +} + +uint16_t NvVolume::partitionBlockSize(uint8_t idx) const { + if (!mounted_ || idx >= partCount_) return 0; + const Part& p = parts_[idx]; + return (uint16_t)(p.endBlock - p.beginBlock); +} + +uint16_t NvVolume::partitionByteSize(uint8_t idx) const { + const uint16_t blocks = partitionBlockSize(idx); + return (uint16_t)(blocks << 4); +} + +int8_t NvVolume::partitionFindByName16(uint16_t tag) const { + if (!mounted_) return -1; + if (!hasPartNames_) return -1; + if (tag == 0) return -1; + + for (uint8_t i = 0; i < partCount_; ++i) { + if (partName16_[i] == tag) return (int8_t)i; + } + return -1; +} + +bool NvVolume::partitionFind(const char* partitionAscii, uint8_t& outIdx) const { + outIdx = 0xFF; + if (!mounted_ || !hasPartNames_ || !partitionAscii || !partitionAscii[0]) return false; + + const uint16_t tag = computePartName16(partitionAscii); + if (tag == 0) return false; + + const int8_t idx = partitionFindByName16(tag); + if (idx < 0) return false; + + outIdx = (uint8_t)idx; + return true; +} + +bool NvVolume::partitionFind(const char* partitionAscii, PartitionView16& out) const { + out = PartitionView16{}; + uint8_t idx = 0xFF; + if (!partitionFind(partitionAscii, idx)) return false; + + out.idx = idx; + out.beginBlock = parts_[idx].beginBlock; + out.endBlock = parts_[idx].endBlock; + return true; +} + +bool NvVolume::isReadable() const { + return dev_ && dev_->isReadable(); +} + +bool NvVolume::isWritable() const { + return dev_ && dev_->isWritable(); +} + +static inline NvVolume::Status checkRange_(uint16_t partBytes, uint16_t offset, uint16_t len) { + if (len == 0) return NvVolume::Status::Ok; + const uint32_t end = (uint32_t)offset + (uint32_t)len; + return (end <= (uint32_t)partBytes) ? NvVolume::Status::Ok : NvVolume::Status::OutOfRange; +} + +NvVolume::Status NvVolume::partitionRead(uint8_t partIdx, uint16_t offset, void* dst, uint16_t len) { + if (!dev_ || !mounted_) return Status::NotReady; + if (partIdx >= partCount_) return Status::OutOfRange; + if (len == 0) return Status::Ok; + if (!dst) return Status::IoError; + + const Part& p = parts_[partIdx]; + const uint16_t partBytes = (uint16_t)((p.endBlock - p.beginBlock) << 4); + + const Status chk = checkRange_(partBytes, offset, len); + if (chk != Status::Ok) return chk; + + const uint16_t absAddr = (uint16_t)((p.beginBlock << 4) + offset); + return mapIo(dev_->read(absAddr, dst, len)); +} + +NvVolume::Status NvVolume::partitionWrite(uint8_t partIdx, uint16_t offset, const void* src, uint16_t len) { + if (!dev_ || !mounted_) return Status::NotReady; + if (partIdx >= partCount_) return Status::OutOfRange; + if (!dev_->isWritable()) return Status::ReadOnly; + if (len == 0) return Status::Ok; + if (!src) return Status::IoError; + + const Part& p = parts_[partIdx]; + const uint16_t partBytes = (uint16_t)((p.endBlock - p.beginBlock) << 4); + + const Status chk = checkRange_(partBytes, offset, len); + if (chk != Status::Ok) return chk; + + const uint16_t absAddr = (uint16_t)((p.beginBlock << 4) + offset); + return mapIo(dev_->write(absAddr, src, len)); +} + +NvVolume::Status NvVolume::partitionReadBlock(uint8_t partIdx, uint16_t partBlockOfs, uint8_t out16[16]) { + if (!dev_ || !mounted_) return Status::NotReady; + if (partIdx >= partCount_) return Status::OutOfRange; + + const Part& p = parts_[partIdx]; + const uint16_t blocks = (uint16_t)(p.endBlock - p.beginBlock); + if (partBlockOfs >= blocks) return Status::OutOfRange; + + const uint16_t absBlock = (uint16_t)(p.beginBlock + partBlockOfs); + return readBlockSt(*dev_, absBlock, out16); +} + +NvVolume::Status NvVolume::partitionWriteBlock(uint8_t partIdx, uint16_t partBlockOfs, const uint8_t in16[16]) { + if (!dev_ || !mounted_) return Status::NotReady; + if (partIdx >= partCount_) return Status::OutOfRange; + if (!dev_->isWritable()) return Status::ReadOnly; + + const Part& p = parts_[partIdx]; + const uint16_t blocks = (uint16_t)(p.endBlock - p.beginBlock); + if (partBlockOfs >= blocks) return Status::OutOfRange; + + const uint16_t absBlock = (uint16_t)(p.beginBlock + partBlockOfs); + return writeBlockSt(*dev_, absBlock, in16); +} + +NvVolume::Status NvVolume::partitionReadInBlock(uint8_t partIdx, uint16_t partBlockOfs, + uint8_t byteOfs, void* dst, uint8_t len) { + if (!dev_ || !mounted_) return Status::NotReady; + if (partIdx >= partCount_) return Status::OutOfRange; + if (!dst || len == 0) return Status::OutOfRange; + if (byteOfs >= 16 || (uint16_t)byteOfs + (uint16_t)len > 16) return Status::Unaligned; + + const Part& p = parts_[partIdx]; + const uint16_t blocks = (uint16_t)(p.endBlock - p.beginBlock); + if (partBlockOfs >= blocks) return Status::OutOfRange; + + const uint16_t absBlock = (uint16_t)(p.beginBlock + partBlockOfs); + const uint16_t absAddr = (uint16_t)((absBlock << 4) + byteOfs); + + return mapIo(dev_->read(absAddr, dst, (uint16_t)len)); +} + +NvVolume::Status NvVolume::partitionWriteInBlock(uint8_t partIdx, uint16_t partBlockOfs, + uint8_t byteOfs, const void* src, uint8_t len) { + if (!dev_ || !mounted_) return Status::NotReady; + if (partIdx >= partCount_) return Status::OutOfRange; + if (!dev_->isWritable()) return Status::ReadOnly; + if (!src || len == 0) return Status::OutOfRange; + if (byteOfs >= 16 || (uint16_t)byteOfs + (uint16_t)len > 16) return Status::Unaligned; + + const Part& p = parts_[partIdx]; + const uint16_t blocks = (uint16_t)(p.endBlock - p.beginBlock); + if (partBlockOfs >= blocks) return Status::OutOfRange; + + const uint16_t absBlock = (uint16_t)(p.beginBlock + partBlockOfs); + const uint16_t absAddr = (uint16_t)((absBlock << 4) + byteOfs); + + return mapIo(dev_->write(absAddr, src, (uint16_t)len)); +} + +NvVolume::Status NvVolume::invalidateNextMount() { + if (!dev_) return status_ = Status::NotReady; + if (!dev_->isWritable()) return status_ = Status::ReadOnly; + if (blockCount_ < 1) return status_ = Status::BadGeometry; + + uint8_t sb0[16]; + Status rb = readBlockSt(*dev_, 0, sb0); + if (rb != Status::Ok) return status_ = rb; + + // Poison ONLY the stored CRC (bytes 14..15). No need to touch protected payload. + sb0[14] ^= 0x01u; + + Status wb = writeBlockSt(*dev_, 0, sb0); + if (wb != Status::Ok) return status_ = wb; + + return status_ = Status::Ok; +} diff --git a/src/lib/nv/NvVolume.h b/src/lib/nv/NvVolume.h new file mode 100755 index 0000000..48ba1fd --- /dev/null +++ b/src/lib/nv/NvVolume.h @@ -0,0 +1,184 @@ +// NvVolume.h +#pragma once +#include +#include + +#include "device/NvDeviceBase.h" + +// Superblocks v1: +// - block size = 16 bytes +// - block 0 = SB0 (layout) +// - block 1 = SB1 (partition name tags, optional) +// - partitions start at block >= 2 +class NvVolume { +public: + enum class Status : uint8_t { + Ok = 0, + NotReady, + BadGeometry, + Unaligned, + OutOfRange, + ReadOnly, + IoError, + Unformatted, + }; + + static constexpr uint16_t kBlockSize = 16; + static constexpr uint16_t kMaxBlocks = 4096; // 12-bit block indices + static constexpr uint8_t kMaxParts = 7; + static constexpr uint8_t kSbVersionV1 = 1; + + // SB0 flags8: + // bit1 LAST_IS_END_MARKER + // bits2..3 reserved (0) + // bits4..7 version (v1=1) + static constexpr uint8_t kFlagLastIsEndMarker = 0x02; + static constexpr uint8_t kFlagReservedMask = 0x0C; // bits2..3 must be 0 + static constexpr uint8_t kFlagVersionShift = 4; + + struct Part { + uint16_t beginBlock = 0; // inclusive + uint16_t endBlock = 0; // exclusive + }; + + struct PartitionView16 { + uint8_t idx = 0xFF; + uint16_t beginBlock = 0; + uint16_t endBlock = 0; + uint16_t blockCount() const { return (uint16_t)(endBlock - beginBlock); } + uint16_t byteCount() const { return (uint16_t)(blockCount() << 4); } + }; + + NvVolume() = default; + + // ---------------- device binding / identity ---------------- + Status init(NvDevice& dev); + + // Name tags (hashing lives in Crc16 now) + static uint16_t computeVolumeName16(const char* projectAscii, uint16_t storageVersion); + static uint16_t computePartName16(const char* partitionAscii); + + // ---------------- mount / format ---------------- + // Mount validates SB0 CRC/version/layout and loads partitions. + // SB1 (partition names) is optional; if its CRC fails, names are treated as absent. + Status mount(); + Status mount(const char* projectAscii, uint16_t storageVersion); + + // causes the next volume mount to fail, triggering a reset to defaults normally + Status invalidateNextMount(); + + // Format SB0 + SB1. + // Note the internal 16-bit tag is ONLY very probably unique for a unique name (65536 combinations.) + Status formatAll(const char* projectAscii, + uint16_t storageVersion, + const uint16_t starts[kMaxParts], + bool lastIsEndMarker, + const char* partAscii[kMaxParts]); + + // Builder API (chained calls style; strings must remain valid until formatCommit()). + Status formatBegin(const char* projectAscii, uint16_t storageVersion); + bool formatAddPartition(const char* partitionAscii, uint32_t sizeBytes); // sizeBytes==0 => remainder + Status formatCommit(); + + // ---------------- volume query ---------------- + bool isMounted() const { return mounted_; } + uint16_t blockCount() const { return blockCount_; } + uint32_t byteCount() const { return (uint32_t)blockCount_ << 4; } + bool hasPartNames() const { return hasPartNames_; } + + bool isReadable() const; + bool isWritable() const; + + // ---------------- partition query ---------------- + uint8_t partitionCount() const { return partCount_; } + Part partition(uint8_t idx) const { return (idx < partCount_) ? parts_[idx] : Part{}; } + uint16_t partitionName16(uint8_t idx) const { return (idx < partCount_) ? partName16_[idx] : 0; } + + uint16_t partitionBlockSize(uint8_t idx) const; + uint16_t partitionByteSize(uint8_t idx) const; + + // ---------------- partition find (requires valid SB1) ---------------- + // Find by already-computed 16-bit name tag (tag==0 reserved => "unnamed"). + int8_t partitionFindByName16(uint16_t tag) const; + + // Find by ASCII name (internally hashed to 16-bit tag). + // Note the internal 16-bit tag is ONLY very probably unique for a unique name (65536 combinations.) + bool partitionFind(const char* partitionAscii, uint8_t& outIdx) const; + bool partitionFind(const char* partitionAscii, PartitionView16& out) const; + + // ---------------- partition I/O ---------------- + // Aligned I/O (offset/len must be multiples of 16). + Status partitionRead(uint8_t partIdx, uint16_t offset, void* dst, uint16_t len); + Status partitionWrite(uint8_t partIdx, uint16_t offset, const void* src, uint16_t len); + + // Block-scoped I/O (never cross a 16B block). + Status partitionReadBlock(uint8_t partIdx, uint16_t partBlockOfs, uint8_t out16[16]); + Status partitionWriteBlock(uint8_t partIdx, uint16_t partBlockOfs, const uint8_t in16[16]); + Status partitionReadInBlock(uint8_t partIdx, uint16_t partBlockOfs, uint8_t byteOfs, void* dst, uint8_t len); + Status partitionWriteInBlock(uint8_t partIdx, uint16_t partBlockOfs, uint8_t byteOfs, const void* src, uint8_t len); + +private: + static constexpr const char* kSalt = "OnCueV1"; + + static inline uint16_t le16(const uint8_t* p) { return (uint16_t)p[0] | (uint16_t)(p[1] << 8); } + static inline void wrle16(uint8_t* p, uint16_t v) { p[0] = (uint8_t)(v & 0xFFu); p[1] = (uint8_t)(v >> 8); } + + static uint16_t crc16_sb(const uint8_t* data, uint16_t lenNoCrc); + + static uint32_t getBitsLSB(const uint8_t* bytes11, uint32_t bitIndex, uint32_t bitCount); + static void setBitsLSB(uint8_t* bytes11, uint32_t bitIndex, uint32_t bitCount, uint32_t value); + + Status enumerateFromSb0(const uint8_t sb0[16]); + Status loadNamesBlock(); + Status writeNamesBlock(const uint16_t partName16[kMaxParts]); + + static inline Status mapIo(NvDevice::IoStatus st) { + switch (st) { + case NvDevice::IoStatus::Ok: return Status::Ok; + case NvDevice::IoStatus::ReadOnly: return Status::ReadOnly; + case NvDevice::IoStatus::OutOfRange: return Status::OutOfRange; + case NvDevice::IoStatus::NotReady: return Status::NotReady; + default: return Status::IoError; + } + } + + // Status-returning 16-byte block helpers (used internally for better error propagation). + static inline Status readBlockSt(NvDevice& d, uint16_t blockIndex, uint8_t out16[16]) { + return mapIo(d.readBlock16(blockIndex, out16)); + } + + static inline Status writeBlockSt(NvDevice& d, uint16_t blockIndex, const uint8_t in16[16]) { + if (d.supportsWriteBlock16()) { + return mapIo(d.writeBlock16(blockIndex, in16)); + } + return mapIo(d.write((uint16_t)(blockIndex << 4), in16, 16)); + } + + Status scrubPartitionBlocksOnFormat_(); + +private: + NvDevice* dev_ = nullptr; + + uint16_t blockCount_ = 0; + + bool mounted_ = false; + Status status_ = Status::NotReady; + + uint16_t storedVolumeName16_ = 0; + uint8_t flags8_ = 0; + uint8_t flags4_ = 0; + + uint8_t partCount_ = 0; + Part parts_[kMaxParts]; + + uint16_t partName16_[kMaxParts] = {0}; + bool hasPartNames_ = false; + + // builder staging + bool builderActive_ = false; + const char* builderProject_ = nullptr; + uint16_t builderStorageVersion_ = 0; + uint8_t builderCount_ = 0; + const char* builderPartAscii_[kMaxParts] = {nullptr}; + uint16_t builderSizeBlocks_[kMaxParts] = {0}; // 0 => remainder +}; diff --git a/src/lib/nv/device/24xxI2C.h b/src/lib/nv/device/24xxI2C.h new file mode 100755 index 0000000..094405c --- /dev/null +++ b/src/lib/nv/device/24xxI2C.h @@ -0,0 +1,147 @@ +// NvDevice24xxI2C.h +#pragma once + +#include "NvDeviceBase.h" +#include + +#ifndef EEPROM_ACK_SKIP_MS + #define EEPROM_ACK_SKIP_MS 20 +#endif + +#ifndef EEPROM_ACK_TIMEOUT_MS + #define EEPROM_ACK_TIMEOUT_MS 50 +#endif + +template +class NvDevice24xxI2C : public NvDevice { +public: + explicit NvDevice24xxI2C(TwoWire& wire, uint8_t addr7) + : wire_(&wire), addr_(addr7) {} + + bool init() override { + wire_->begin(); + wire_->beginTransmission(addr_); + bool success = (wire_->endTransmission() == 0); + VF("MSG: NvDevice, 24xx "); V(sizeBytes()/1024); VF("KB I2C EEPROM "); + if (success) { VLF("found"); } else { VLF("NOT FOUND!"); } + return success; + } + + uint32_t sizeBytes() const override { return DeviceBytes; } + + // Device write cycle endurance + // Low < 100K writes, Mid > ~100K writes, High > ~100M writes (FRAM) + Endurance endurance() const override { return Endurance::Mid; } + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + if (!dst) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + + uint8_t* out = (uint8_t*)dst; + + if (!waitForDevice_()) return IoStatus::NotReady; + + for (uint16_t i = 0; i < len; i++) { + const uint16_t a = (uint16_t)(addr + i); + + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(a >> 8)); + wire_->write((uint8_t)(a & 0xFF)); + if (wire_->endTransmission(false) != 0) return IoStatus::IoError; + + if (wire_->requestFrom((int)addr_, (int)1) != 1) return IoStatus::IoError; + if (!wire_->available()) return IoStatus::IoError; + out[i] = (uint8_t)wire_->read(); + } + return IoStatus::Ok; + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + if (!src) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + + const uint8_t* in = (const uint8_t*)src; + + for (uint16_t i = 0; i < len; i++) { + const uint16_t a = (uint16_t)(addr + i); + + if (!waitForDevice_()) return IoStatus::NotReady; + + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(a >> 8)); + wire_->write((uint8_t)(a & 0xFF)); + wire_->write(in[i]); + if (wire_->endTransmission() != 0) return IoStatus::IoError; + + lastWriteTimeMs_ = millis(); + } + return IoStatus::Ok; + } + + bool supportsReadBlock16() const override { return true; } + + IoStatus readBlock16(uint16_t blockIdx, void* dst16) override { + if (!dst16) return IoStatus::OutOfRange; + + const uint16_t addr = (uint16_t)(blockIdx << 4); + if ((uint32_t)addr + 16u > sizeBytes()) return IoStatus::OutOfRange; + + if (!waitForDevice_()) return IoStatus::NotReady; + + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(addr >> 8)); + wire_->write((uint8_t)(addr & 0xFF)); + if (wire_->endTransmission(false) != 0) return IoStatus::IoError; + + if (wire_->requestFrom((int)addr_, (int)16) != 16) return IoStatus::IoError; + + uint8_t* out = (uint8_t*)dst16; + for (uint8_t i = 0; i < 16; i++) { + if (!wire_->available()) return IoStatus::IoError; + out[i] = (uint8_t)wire_->read(); + } + return IoStatus::Ok; + } + + bool supportsWriteBlock16() const override { return true; } + + IoStatus writeBlock16(uint16_t blockIdx, const void* src16) override { + if (!src16) return IoStatus::OutOfRange; + + const uint16_t addr = (uint16_t)(blockIdx << 4); + if ((uint32_t)addr + 16u > sizeBytes()) return IoStatus::OutOfRange; + + if (!waitForDevice_()) return IoStatus::NotReady; + + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(addr >> 8)); + wire_->write((uint8_t)(addr & 0xFF)); + + const uint8_t* p = (const uint8_t*)src16; + for (uint8_t i = 0; i < 16; i++) wire_->write(p[i]); + + if (wire_->endTransmission() != 0) return IoStatus::IoError; + + lastWriteTimeMs_ = millis(); + return IoStatus::Ok; + } + +private: + bool waitForDevice_() { + if (lastWriteTimeMs_ == 0) return true; + + const uint32_t now = millis(); + if ((uint32_t)(now - lastWriteTimeMs_) >= (uint32_t)EEPROM_ACK_SKIP_MS) return true; + + const uint32_t t0 = now; + while ((uint32_t)(millis() - t0) < (uint32_t)EEPROM_ACK_TIMEOUT_MS) { + wire_->beginTransmission(addr_); + if (wire_->endTransmission() == 0) return true; + } + return false; + } + + uint32_t lastWriteTimeMs_ = 0; + TwoWire* wire_ = nullptr; + uint8_t addr_ = 0; +}; diff --git a/src/lib/nv/device/DeviceNull.h b/src/lib/nv/device/DeviceNull.h new file mode 100755 index 0000000..3856da3 --- /dev/null +++ b/src/lib/nv/device/DeviceNull.h @@ -0,0 +1,71 @@ +// ------------------------------------------------------------------------------------------------- +// NvDeviceNull.h +// +// Null NV backend (no persistence). +// +// - Reads return 0xFF bytes (erased-state semantics). +// - Writes succeed but are discarded. +// - Useful for bring-up, unit tests, or builds without NV hardware. +// +// Contract: +// - sizeBytes() reports a usable aligned size (multiple of 16). +// - read/write are flat device-relative addressing with 16-bit addr/len. +// +// ------------------------------------------------------------------------------------------------- +#pragma once + +#include +#include + +#include "NvDeviceBase.h" + +class NvDeviceNull final : public NvDevice { +public: + // Ensure the reported size is a multiple of 16 (minimum 16). + explicit NvDeviceNull(uint32_t sizeBytes = kDefaultSizeBytes) + : sizeBytes_(align16_(sizeBytes)) {} + + void setSizeBytes(uint32_t sizeBytes) { sizeBytes_ = align16_(sizeBytes); } + + // ---- NvDevice ---- + bool init() override { return true; } + + uint32_t sizeBytes() const override { return sizeBytes_; } + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + if (len == 0) return IoStatus::Ok; + if (!dst) return IoStatus::IoError; + + const uint32_t end = (uint32_t)addr + (uint32_t)len; + if (end > sizeBytes_) return IoStatus::OutOfRange; + + memset(dst, 0xFF, (size_t)len); + return IoStatus::Ok; + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + if (len == 0) return IoStatus::Ok; + if (!src) return IoStatus::IoError; + + const uint32_t end = (uint32_t)addr + (uint32_t)len; + if (end > sizeBytes_) return IoStatus::OutOfRange; + + // Discard writes. + return IoStatus::Ok; + } + + // Advertise block16 accelerators as supported. They will call through + // the base default implementations (which call read/write). + bool supportsWriteBlock16() const override { return true; } + bool supportsReadBlock16() const override { return true; } + +private: + static constexpr uint32_t kDefaultSizeBytes = 64u * 1024u; + + static uint32_t align16_(uint32_t n) { + if (n < 16u) n = 16u; + return (n + 15u) & ~15u; + } + + uint32_t sizeBytes_ = kDefaultSizeBytes; +}; diff --git a/src/lib/nv/device/EepromArduino.h b/src/lib/nv/device/EepromArduino.h new file mode 100755 index 0000000..0f5acab --- /dev/null +++ b/src/lib/nv/device/EepromArduino.h @@ -0,0 +1,68 @@ +// NvDeviceEepromArduino.h +#pragma once +#include "NvDeviceBase.h" +#include + +#if !defined(NV_DRIVER) || (NV_DRIVER != NV_EEPROM) + #error "NvDeviceEepromArduino.h included but NV_DRIVER != NV_EEPROM" +#endif + +#ifndef E2END + #error "E2END must be defined for NV_EEPROM (set it in HAL or NvsConfig.h)" +#endif + +class NvDeviceEepromArduino : public NvDevice { +public: + bool init() override { + // Nothing to init for classic EEPROM + VLF("MSG: NvDevice, using built-in EEPROM via Arduino library"); + return true; + } + + // reports usable aligned size (multiple of 16) + uint32_t sizeBytes() const override { + const uint32_t n = (uint32_t)(E2END + 1u); + return n & ~15u; + } + + // Device write cycle endurance + // Low < 100K writes, Mid > ~100K writes, High > ~100M writes (FRAM) + Endurance endurance() const override { return Endurance::Mid; } + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + if (!dst) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + + uint8_t* p = (uint8_t*)dst; + for (uint16_t i = 0; i < len; i++) { + p[i] = EEPROM.read((int)(addr + i)); + } + return IoStatus::Ok; + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + if (!src) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + if (!isWritable()) return IoStatus::ReadOnly; + + const uint8_t* p = (const uint8_t*)src; + for (uint16_t i = 0; i < len; i++) { + const int a = (int)(addr + i); + const uint8_t v = p[i]; + + // AVR internal EEPROM: update() is supported and avoids needless wear. + #if defined(ARDUINO_ARCH_AVR) + EEPROM.update(a, v); + #elif defined(HAL_EEPROM_HAS_UPDATE) + EEPROM.update(a, v); + #else + EEPROM.write(a, v); + #endif + } + return IoStatus::Ok; + } + + bool hasCommit() const override { return false; } + IoStatus commit() override { return IoStatus::Ok; } + bool commitDone() const override { return true; } +}; diff --git a/src/lib/nv/device/EepromEmuEsp.h b/src/lib/nv/device/EepromEmuEsp.h new file mode 100755 index 0000000..636d46a --- /dev/null +++ b/src/lib/nv/device/EepromEmuEsp.h @@ -0,0 +1,81 @@ +// NvDeviceEepromEmuEsp.h +#pragma once +#include "NvDeviceBase.h" + +#if defined(ESP32) || defined(ESP8266) + #include +#endif + +class NvDeviceEepromEmuEsp : public NvDevice { +public: + bool init() override { + #if defined(ESP32) + const uint32_t sz = sizeBytes(); + VLF("MSG: NvDevice, using built-in EEPROM 4KB via. ESP32 core library"); + return EEPROM.begin((int)sz); + #elif defined(ESP8266) + const uint32_t sz = sizeBytes(); + VLF("MSG: NvDevice, using built-in EEPROM 4KB via. ESP8266 core library"); + EEPROM.begin((int)sz); + return true; + #else + VLF("MSG: NvDevice, NOT FOUND!"); + return false; + #endif + } + + uint32_t sizeBytes() const override { + return 4096u; + } + + // Device write cycle endurance + // Low < 100K writes, Mid > ~100K writes, High > ~100M writes (FRAM) + Endurance endurance() const override { return Endurance::Low; } + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + #if !(defined(ESP32) || defined(ESP8266)) + (void)addr; (void)dst; (void)len; + return IoStatus::IoError; + #else + if (!dst) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + + uint8_t* p = (uint8_t*)dst; + for (uint16_t i = 0; i < len; i++) { + p[i] = EEPROM.read((int)(addr + i)); + } + return IoStatus::Ok; + #endif + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + #if !(defined(ESP32) || defined(ESP8266)) + (void)addr; (void)src; (void)len; + return IoStatus::IoError; + #else + if (!src) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + if (!isWritable()) return IoStatus::ReadOnly; + + const uint8_t* p = (const uint8_t*)src; + for (uint16_t i = 0; i < len; i++) { + EEPROM.write((int)(addr + i), p[i]); + } + return IoStatus::Ok; + #endif + } + + bool hasCommit() const override { return true; } + + IoStatus commit() override { + #if !(defined(ESP32) || defined(ESP8266)) + return IoStatus::IoError; + #else + bool ok = EEPROM.commit(); + VLF("MSG: Nv, commit"); + return ok ? IoStatus::Ok : IoStatus::IoError; + #endif + } + + bool commitDone() const override { return true; } // synchronous in Arduino EEPROM +}; diff --git a/src/lib/nv/device/EepromEmuM0.h b/src/lib/nv/device/EepromEmuM0.h new file mode 100755 index 0000000..40d6343 --- /dev/null +++ b/src/lib/nv/device/EepromEmuM0.h @@ -0,0 +1,88 @@ +// NvDeviceEepromEmuM0.h +#pragma once + +#include "NvDeviceBase.h" + +// Arduino SAMD (M0/M0 Pro/Zero) EEPROM emulation. +// Depending on core, the header may be or . +#if defined(ARDUINO_ARCH_SAMD) + #include +#endif + +#ifndef E2END + #error "E2END must be defined for NvDeviceEepromEmuM0" +#endif + +class NvDeviceEepromEmuM0 : public NvDevice { +public: + bool init() override { + #if !defined(ARDUINO_ARCH_SAMD) + VLF("MSG: NvDevice, NOT FOUND!"); + return false; + #else + const uint32_t sz = sizeBytes(); + // Match your old guard: M0 emu is expected to be exactly 1024 bytes. + if (sz != 1024u) return false; + + // Many SAMD EEPROM emu implementations don't require begin(size). + // If your core provides EEPROM.begin(size), you can add it here. + VLF("MSG: NvDevice, using built-in 1KB EEPROM via. M0 core library"); + return true; + #endif + } + + uint32_t sizeBytes() const override { + return (uint32_t)(E2END + 1u); + } + + // Device write cycle endurance + // Low < 100K writes, Mid > ~100K writes, High > ~100M writes (FRAM) + Endurance endurance() const override { return Endurance::Low; } + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + #if !defined(ARDUINO_ARCH_SAMD) + (void)addr; (void)dst; (void)len; + return IoStatus::IoError; + #else + if (!dst) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + + uint8_t* p = (uint8_t*)dst; + for (uint16_t i = 0; i < len; i++) { + p[i] = EEPROM.read((int)(addr + i)); + } + return IoStatus::Ok; + #endif + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + #if !defined(ARDUINO_ARCH_SAMD) + (void)addr; (void)src; (void)len; + return IoStatus::IoError; + #else + if (!src) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + if (!isWritable()) return IoStatus::ReadOnly; + + const uint8_t* p = (const uint8_t*)src; + for (uint16_t i = 0; i < len; i++) { + EEPROM.write((int)(addr + i), p[i]); + } + return IoStatus::Ok; + #endif + } + + bool hasCommit() const override { return true; } + + IoStatus commit() override { + #if !defined(ARDUINO_ARCH_SAMD) + return IoStatus::IoError; + #else + // Most SAMD EEPROM emulation layers expose EEPROM.commit(). + EEPROM.commit(); + return IoStatus::Ok; + #endif + } + + bool commitDone() const override { return true; } // commit is synchronous +}; diff --git a/src/lib/nv/device/Mb85rcI2C.h b/src/lib/nv/device/Mb85rcI2C.h new file mode 100755 index 0000000..c959e12 --- /dev/null +++ b/src/lib/nv/device/Mb85rcI2C.h @@ -0,0 +1,127 @@ +// NvDeviceMb85rcI2c.h +#pragma once + +#include "NvDeviceBase.h" +#include + +template +class NvDeviceMb85rcI2C : public NvDevice { +public: + explicit NvDeviceMb85rcI2C(TwoWire& wire, uint8_t addr7) + : wire_(&wire), addr_(addr7) {} + + bool init() override { + wire_->begin(); + wire_->beginTransmission(addr_); + bool success = (wire_->endTransmission() == 0); + VF("MSG: NvDevice, MB85RC "); V(sizeBytes()/1024); VF("KB I2C FRAM "); + if (success) { VLF("found"); } else { VLF("NOT FOUND!"); } + return success; + } + + uint32_t sizeBytes() const override { return DeviceBytes; } + + // Device write cycle endurance + // Low < 100K writes, Mid > ~100K writes, High > ~100M writes (FRAM) + Endurance endurance() const override { return Endurance::High; } + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + if (!dst) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + + uint8_t* out = (uint8_t*)dst; + + // 24xx-like simple loop (byte-at-a-time). You can later optimize to chunked streaming reads if desired. + for (uint16_t i = 0; i < len; i++) { + const uint16_t a = (uint16_t)(addr + i); + + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(a >> 8)); + wire_->write((uint8_t)(a & 0xFF)); + if (wire_->endTransmission(false) != 0) return IoStatus::IoError; + + if (wire_->requestFrom((int)addr_, (int)1) != 1) return IoStatus::IoError; + if (!wire_->available()) return IoStatus::IoError; + out[i] = (uint8_t)wire_->read(); + } + + return IoStatus::Ok; + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + if (!src) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > sizeBytes()) return IoStatus::OutOfRange; + if (!isWritable()) return IoStatus::ReadOnly; + + const uint8_t* in = (const uint8_t*)src; + + // 24xx-like simple loop (byte-at-a-time) + for (uint16_t i = 0; i < len; i++) { + const uint16_t a = (uint16_t)(addr + i); + + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(a >> 8)); + wire_->write((uint8_t)(a & 0xFF)); + wire_->write(in[i]); + if (wire_->endTransmission() != 0) return IoStatus::IoError; + } + + return IoStatus::Ok; + } + + bool supportsReadBlock16() const override { return true; } + + // Accelerator: 16-byte block read in one I2C transaction + IoStatus readBlock16(uint16_t blockIdx, void* dst16) override { + if (!dst16) return IoStatus::OutOfRange; + const uint16_t addr = (uint16_t)(blockIdx << 4); + if ((uint32_t)addr + 16u > sizeBytes()) return IoStatus::OutOfRange; + + // Set address pointer, keep bus (repeated start) + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(addr >> 8)); + wire_->write((uint8_t)(addr & 0xFF)); + if (wire_->endTransmission(false) != 0) return IoStatus::IoError; + + // Read 16 bytes + if (wire_->requestFrom((int)addr_, (int)16) != 16) return IoStatus::IoError; + + uint8_t* out = (uint8_t*)dst16; + for (uint8_t i = 0; i < 16; i++) { + if (!wire_->available()) return IoStatus::IoError; + out[i] = (uint8_t)wire_->read(); + } + + return IoStatus::Ok; + } + + bool supportsWriteBlock16() const override { return true; } + + // Accelerator: 16-byte block write in one I2C transaction + IoStatus writeBlock16(uint16_t blockIdx, const void* src16) override { + if (!src16) return IoStatus::OutOfRange; + const uint16_t addr = (uint16_t)(blockIdx << 4); + if ((uint32_t)addr + 16u > sizeBytes()) return IoStatus::OutOfRange; + if (!isWritable()) return IoStatus::ReadOnly; + + wire_->beginTransmission(addr_); + wire_->write((uint8_t)(addr >> 8)); + wire_->write((uint8_t)(addr & 0xFF)); + + const uint8_t* p = (const uint8_t*)src16; + for (uint8_t i = 0; i < 16; i++) wire_->write(p[i]); + if (wire_->endTransmission() != 0) return IoStatus::IoError; + + return IoStatus::Ok; + } + + // FRAM has no commit barrier. + bool hasCommit() const override { return false; } + IoStatus commit() override { return IoStatus::Ok; } + bool commitDone() const override { return true; } + +private: + + TwoWire* wire_ = nullptr; + uint8_t addr_ = 0; +}; diff --git a/src/lib/nv/device/NvDeviceBase.h b/src/lib/nv/device/NvDeviceBase.h new file mode 100755 index 0000000..a14e4cb --- /dev/null +++ b/src/lib/nv/device/NvDeviceBase.h @@ -0,0 +1,57 @@ +// NvDeviceBase.h +#pragma once +#include +#include +class NvDevice { +public: + enum class IoStatus : uint8_t { Ok, OutOfRange, NotReady, IoError, ReadOnly }; + enum class Endurance : uint8_t { Low, Mid, High }; + + virtual ~NvDevice() = default; + + virtual bool init() { return true; } + + // reports usable aligned size (multiple of 16) + virtual uint32_t sizeBytes() const = 0; + + // Device write cycle endurance + // Low < 100K writes, Mid > ~100K writes, High > ~100M writes (FRAM) + virtual Endurance endurance() const { return Endurance::Low; } + + // Required primitives: flat 16-bit byte addressing, device-relative + virtual IoStatus read(uint16_t addr, void* dst, uint16_t len) = 0; + virtual IoStatus write(uint16_t addr, const void* src, uint16_t len) = 0; + + // Optional service hook + virtual void poll() {} + + // Optional commit barrier (ESP/flash emu) + virtual bool hasCommit() const { return false; } + virtual IoStatus commit() { return IoStatus::Ok; } + virtual bool commitDone() const { return true; } + + // ---- Optional 16-byte block accelerator (NOT a second "public API" conceptually) ---- + // Wrappers may call this for efficiency; higher-level code can ignore it entirely. + + virtual bool supportsWriteBlock16() const { return false; } + + virtual IoStatus writeBlock16(uint16_t blockIdx, const void* src16) { + const uint32_t addr = ((uint32_t)blockIdx) << 4; + if (addr + 16u > sizeBytes()) return IoStatus::OutOfRange; + return write((uint16_t)addr, src16, 16); + } + + virtual bool supportsReadBlock16() const { return false; } + + virtual IoStatus readBlock16(uint16_t blockIdx, void* dst16) { + const uint32_t addr = ((uint32_t)blockIdx) << 4; + if (addr + 16u > sizeBytes()) return IoStatus::OutOfRange; + return read((uint16_t)addr, dst16, 16); + } + + // Optional capability hints + virtual bool isReadable() const { return true; } + virtual bool isWritable() const { return true; } + +protected: +}; diff --git a/src/lib/nv/device/ShimCached.h b/src/lib/nv/device/ShimCached.h new file mode 100755 index 0000000..1d74149 --- /dev/null +++ b/src/lib/nv/device/ShimCached.h @@ -0,0 +1,365 @@ +// ShimCached.h +#pragma once + +#include "NvDeviceBase.h" + +#include +#include +#include + +// NvShimCached +// +// Cache wrapper (old intent + fast): +// +// - Two 1-bit-per-byte bitsets: +// dirtyReadBits[i] == 1 => cache[i] invalid; must read backend before use +// dirtyWriteBits[i] == 1 => cache[i] differs; must write back +// +// - poll() scans a small window (20 bytes) each call +// - On dirty write: +// * if writeback delay elapsed: prefer 16B page-base flush IF page is fully valid in cache +// AND page has any dirty-write bytes; otherwise fall back to 1-byte write at scanIndex. +// - On dirty read: fetch 1 byte into cache +// - "Clean" is determined by a full wrap scan pass (cacheClean/cacheCleanThisPass) +// - Backends may "pretend" they support 16B page writes: we always call writeBlock16() +// for the 16B path; base-class default emulates it with write(...,16). + +#ifndef NVS_ALLOW_CACHED_ON_COMMIT_BACKENDS + // 0 = refuse to wrap flash/EEPROM emulation layers that already cache+commit internally. + #define NVS_ALLOW_CACHED_ON_COMMIT_BACKENDS 0 +#endif + +// 1-bit-per-byte bitset helpers +static inline bool bitGet_(const uint8_t* bits, uint32_t bitIndex) { + return (bits[bitIndex >> 3] >> (bitIndex & 7)) & 1u; +} +static inline void bitSet_(uint8_t* bits, uint32_t bitIndex, bool value) { + const uint8_t mask = (uint8_t)(1u << (bitIndex & 7)); + uint8_t& b = bits[bitIndex >> 3]; + b = value ? (uint8_t)(b | mask) : (uint8_t)(b & (uint8_t)~mask); +} + +class NvShimCached : public NvDevice { +public: + explicit NvShimCached(NvDevice& inner) : backend(inner) {} + ~NvShimCached() override { cleanup_(); } + + bool init() override { + cleanup_(); + + if (!backend.init()) return false; + + #if !NVS_ALLOW_CACHED_ON_COMMIT_BACKENDS + // Avoid wrapping flash/EEPROM emulation layers that already do their own caching/commit. + if (backend.hasCommit()) return false; + #endif + + capacityBytes = backend.sizeBytes(); + if (capacityBytes == 0) return false; + + // We assume upstream respects "aligned usable size (multiple of 16)" contract. + if ((capacityBytes & 0x0Fu) != 0u) return false; + + // NOTE: public read/write use uint16_t address, so > 64K is not representable. + if (capacityBytes > 65536u) return false; + + cache = new (std::nothrow) uint8_t[capacityBytes]; + if (!cache) { cleanup_(); return false; } + + bitsetBytes = (uint16_t)((capacityBytes + 7u) / 8u); + + dirtyReadBits = new (std::nothrow) uint8_t[bitsetBytes]; + dirtyWriteBits = new (std::nothrow) uint8_t[bitsetBytes]; + if (!dirtyReadBits || !dirtyWriteBits) { cleanup_(); return false; } + + // Heuristic: scan ~1 flag-byte per 512B of device, clamped [4..32]. + uint32_t count = capacityBytes >> 9; + if (count < 4) count = 4; + if (count > 32) count = 32; + flagScanCount = (uint8_t)count; + + // Semantics: + // dirtyReadBits[i] == 1 -> cache[i] invalid, must fetch before use + // dirtyWriteBits[i] == 1 -> cache[i] differs, must write back + memset(dirtyReadBits, 0xFF, bitsetBytes); // all bytes invalid initially + memset(dirtyWriteBits, 0x00, bitsetBytes); // nothing dirty initially + + scanIndex = (uint32_t)-1; + pendingWriteCount = 0; + + writebackDelayMs = 0; + writebackReadyMs = 0; + + // cache isn't "clean" because read-cache is invalid everywhere + cacheClean = false; + cacheCleanThisPass = true; + + VLF("MSG: NvDevice, memory cache active"); + return true; + } + + // ---- Capabilities / identity pass-through ---- + + uint32_t sizeBytes() const override { return capacityBytes; } + Endurance endurance() const override { return backend.endurance(); } + bool isReadable() const override { return backend.isReadable(); } + bool isWritable() const override { return backend.isWritable(); } + + // ---- Delay control (old: commitReadyTimeMs = now + waitMs) ---- + + void setWritebackDelayMs(uint32_t ms) { writebackDelayMs = ms; } + + // ---- Required primitives ---- + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + if (!dst) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > capacityBytes) return IoStatus::OutOfRange; + + uint8_t* out = (uint8_t*)dst; + for (uint16_t i = 0; i < len; i++) { + uint8_t v = 0; + IoStatus st = readByteCached_((uint32_t)addr + i, v); + if (st != IoStatus::Ok) return st; + out[i] = v; + } + return IoStatus::Ok; + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + if (!src) return IoStatus::OutOfRange; + if ((uint32_t)addr + (uint32_t)len > capacityBytes) return IoStatus::OutOfRange; + if (!isWritable()) return IoStatus::ReadOnly; + + const uint8_t* in = (const uint8_t*)src; + for (uint16_t i = 0; i < len; i++) { + // Cache staging cannot fail here (bounds already checked). + (void)writeByteCached_((uint32_t)addr + i, in[i]); + } + + // Set delay gate once per call (avoid per-byte millis()). + writebackReadyMs = millis() + writebackDelayMs; + return IoStatus::Ok; + } + + // Commit API is irrelevant when wrapping EEPROM/FRAM-like devices; keep consistent. + bool hasCommit() const override { return false; } + IoStatus commit() override { return IoStatus::Ok; } + + // "Done" for this wrapper: no dirty bytes waiting to be written back. + // (We keep an incremental counter; if you ever need "old exact recount", add a debug validator.) + bool commitDone() const override { return pendingWriteCount == 0; } + + // ---- Background servicing ---- + + void poll() override { + backend.poll(); + if (capacityBytes == 0) return; + + // cacheClean, return. + if (cacheClean) return; + + const bool canWriteback = + (writebackDelayMs == 0) || ((int32_t)(millis() - writebackReadyMs) >= 0); + + bool doWrite = false; + bool doRead = false; + + // rapidly move ahead through clean cache + const uint32_t flagBytes = bitsetBytes; + uint32_t flagIndex = (((scanIndex + 1u) >= capacityBytes) ? 0u : (scanIndex + 1u)) >> 3; + + for (uint8_t i = 0; i < flagScanCount; i++) { + if ((dirtyWriteBits[flagIndex] | dirtyReadBits[flagIndex]) != 0u) break; + + flagIndex++; + if (flagIndex >= flagBytes) { scanIndex = capacityBytes - 1u; break; } + + scanIndex = (flagIndex << 3) - 1u; + } + + // Scan small window each call + for (uint8_t step = 0; step < 16; step++) { + scanIndex++; + if (scanIndex >= capacityBytes) { + if (cacheCleanThisPass) cacheClean = true; + scanIndex = 0; + cacheCleanThisPass = true; + } + + const bool w = bitGet_(dirtyWriteBits, scanIndex); + const bool r = bitGet_(dirtyReadBits, scanIndex); + + if (w) { + // Work exists, so this pass is not clean. + cacheCleanThisPass = false; + + // If writeback delay not elapsed, keep scanning (but never mark clean). + if (!canWriteback) continue; + + doWrite = true; + break; + } + + if (r) { + cacheCleanThisPass = false; + doRead = true; + break; + } + } + + if (doWrite) { + flushOneWriteFast_(scanIndex); // page-base flush preferred + return; + } + + if (doRead) { + uint8_t tmp = 0; + (void)readByteCached_(scanIndex, tmp); + return; + } + } + +private: + void cleanup_() { + delete[] cache; cache = nullptr; + delete[] dirtyReadBits; dirtyReadBits = nullptr; + delete[] dirtyWriteBits; dirtyWriteBits = nullptr; + + capacityBytes = 0; + bitsetBytes = 0; + scanIndex = 0; + + writebackDelayMs = 0; + writebackReadyMs = 0; + + pendingWriteCount = 0; + + cacheClean = true; + cacheCleanThisPass = true; + } + + // ---- Core byte cache ops ---- + + IoStatus readByteCached_(uint32_t address, uint8_t& out) { + if (address >= capacityBytes) { out = 0; return IoStatus::OutOfRange; } + + if (bitGet_(dirtyReadBits, address)) { + uint8_t v = 0; + IoStatus st = backend.read((uint16_t)address, &v, 1); + if (st != IoStatus::Ok) { out = 0; return st; } + + cache[address] = v; + bitSet_(dirtyReadBits, address, false); + } + + out = cache[address]; + return IoStatus::Ok; + } + + // Cache staging write; always succeeds if address is in range. + // If the backend is temporarily NotReady and we can't read the old value, + // we still stage the write (dirty) and rely on poll() to flush later. + IoStatus writeByteCached_(uint32_t address, uint8_t value) { + if (address >= capacityBytes) return IoStatus::OutOfRange; + + // Any write makes cache "not clean". + cacheClean = false; + cacheCleanThisPass = false; + + // Read old value (through cache) to avoid marking dirty if same. + uint8_t oldValue = 0; + IoStatus stOld = readByteCached_(address, oldValue); + if (stOld == IoStatus::Ok && oldValue == value) return IoStatus::Ok; + + // If stOld != Ok, proceed anyway: stage new value and mark dirty. + cache[address] = value; + + // Mark write dirty (only count 0->1 transitions) + if (!bitGet_(dirtyWriteBits, address)) { + bitSet_(dirtyWriteBits, address, true); + pendingWriteCount++; + } + + // Ensure subsequent reads never overwrite our cached value. + bitSet_(dirtyReadBits, address, false); + + return IoStatus::Ok; + } + + // ---- Old intent + fast writeback ---- + // + // When a dirty byte is found at idx, prefer flushing the *page base* (idx&~0x0F) + // via writeBlock16 if: + // - the full page is valid in cache (dirtyRead == 0 for all 16 bytes) + // - and the page contains any dirtyWrite bytes + // Otherwise fall back to a 1-byte write at idx. + + void flushOneWriteFast_(uint32_t idx) { + if (!backend.isWritable()) return; + if (idx >= capacityBytes) return; + + const uint32_t base = idx & ~0x0Fu; + + if (base + 16u <= capacityBytes) { + // base is 16B aligned => also 8-bit aligned in bitset + const uint32_t bi = base >> 3; // base / 8 + + // Page is fully valid if both corresponding bitset bytes are 0. + const bool pageValid = (dirtyReadBits[bi] == 0u) && (dirtyReadBits[bi + 1] == 0u); + + if (pageValid) { + // Only flush page if any byte in it is dirty-write. + const bool anyDirtyW = (dirtyWriteBits[bi] != 0u) || (dirtyWriteBits[bi + 1] != 0u); + + if (anyDirtyW) { + const uint16_t blockIdx = (uint16_t)(base >> 4); + IoStatus st = backend.writeBlock16(blockIdx, &cache[base]); + if (st != IoStatus::Ok) return; // NotReady/IoError => leave bits set, try later + + // Clear dirty-write bits for the page; adjust pending count accurately. + for (uint8_t k = 0; k < 16; k++) { + const uint32_t a = base + k; + if (bitGet_(dirtyWriteBits, a)) { + bitSet_(dirtyWriteBits, a, false); + if (pendingWriteCount) pendingWriteCount--; + } + } + return; + } + } + } + + // Fallback: 1-byte write at idx (old behavior fallback) + if (bitGet_(dirtyReadBits, idx)) return; // cache byte isn't valid; don't write garbage + + IoStatus st = backend.write((uint16_t)idx, &cache[idx], 1); + if (st != IoStatus::Ok) return; + + if (bitGet_(dirtyWriteBits, idx)) { + bitSet_(dirtyWriteBits, idx, false); + if (pendingWriteCount) pendingWriteCount--; + } + } + +private: + // ---- Member state ---- + NvDevice& backend; + + uint32_t capacityBytes = 0; + uint8_t* cache = nullptr; + + uint8_t* dirtyReadBits = nullptr; + uint8_t* dirtyWriteBits = nullptr; + uint16_t bitsetBytes = 0; + + uint8_t flagScanCount = 4; + uint32_t scanIndex = 0; + + uint32_t writebackDelayMs = 0; + uint32_t writebackReadyMs = 0; + + uint32_t pendingWriteCount = 0; + + bool cacheClean = true; + bool cacheCleanThisPass = true; +}; diff --git a/src/lib/nv/device/ShimDelayedCommit.h b/src/lib/nv/device/ShimDelayedCommit.h new file mode 100755 index 0000000..41a57af --- /dev/null +++ b/src/lib/nv/device/ShimDelayedCommit.h @@ -0,0 +1,118 @@ +// ShimDelayedCommit.h +#pragma once + +#include "NvDeviceBase.h" + +#include + +// A tiny wrapper for backends that already buffer writes internally (ESP/STM32 EEPROM emu). +// It batches commit() calls with a quiet-period timer, but does NOT add a second cache. +// +// Behavior: +// - write() passes through to backend.write() and schedules a commit for (now + delayMs). +// - poll() calls backend.poll() then triggers commit when quiet expires or hard deadline expires. +// - commitDone() is unified: "no pending scheduled commit AND backend reports done". +// +class NvShimDelayedCommit : public NvDevice { +public: + explicit NvShimDelayedCommit(NvDevice& backendRef, uint32_t delayMs) : backend(backendRef) { this->delayMs = delayMs; } + + bool init() override { + if (!backend.init()) return false; + + // This wrapper is only meaningful for commit-capable backends. + if (!backend.hasCommit()) return false; + + capacityBytes = backend.sizeBytes(); + if (capacityBytes == 0 || capacityBytes > 65536u) return false; + + VLF("MSG: NvDevice, delayed commit active"); + + return true; + } + + uint32_t sizeBytes() const override { return capacityBytes; } + Endurance endurance() const override { return backend.endurance(); } + bool isReadable() const override { return backend.isReadable(); } + bool isWritable() const override { return backend.isWritable(); } + + // Quiet period (ms) after last write before committing. + // delay=0 means commit on next poll. + void setDelayMs(uint32_t ms) { delayMs = ms; } + + // Hard bound: commit no later than this many ms after the first write in a burst. + // maxLatencyMs=0 disables (pure debounce behavior). + // defaults to 3 hours + void setMaxLatencyMs(uint32_t ms) { maxLatencyMs = ms; } + + IoStatus read(uint16_t addr, void* dst, uint16_t len) override { + return backend.read(addr, dst, len); + } + + IoStatus write(uint16_t addr, const void* src, uint16_t len) override { + if (!isWritable()) return IoStatus::ReadOnly; + + IoStatus st = backend.write(addr, src, len); + if (st != IoStatus::Ok) { DLF("WRN: write failed!"); return st; } + + const uint32_t now = millis(); + + // If this is the first write since we were idle, start the hard deadline window. + if (!commitScheduled && (maxLatencyMs != 0)) { + commitHardDueMs = now + maxLatencyMs; + } + + // Schedule commit after quiet period (debounce). + commitScheduled = true; + commitDueMs = now + delayMs; + return IoStatus::Ok; + } + + bool hasCommit() const override { return backend.hasCommit(); } + + // Explicit commit barrier: commit immediately and clear any pending schedule. + IoStatus commit() override { + commitScheduled = false; + commitDueMs = 0; + commitHardDueMs = 0; + return backend.commit(); + } + + // Unified "done" concept: no scheduled commit pending AND backend reports done. + bool commitDone() const override { + if (commitScheduled) return false; + return backend.commitDone(); + } + + // Optional: expose whether a commit is scheduled (useful for debugging). + bool commitPending() const { return commitScheduled; } + + void poll() override { + backend.poll(); + if (!commitScheduled) { return; } + + const uint32_t now = millis(); + + const bool quietExpired = ((int32_t)(now - commitDueMs) >= 0); + const bool hardExpired = (maxLatencyMs != 0) && ((int32_t)(now - commitHardDueMs) >= 0); + + if (quietExpired || hardExpired) { + (void)backend.commit(); + commitScheduled = false; + commitDueMs = 0; + commitHardDueMs = 0; + } + } + +private: + NvDevice& backend; + uint32_t capacityBytes = 0; + + bool commitScheduled = false; + + uint32_t delayMs = 0; + uint32_t commitDueMs = 0; + + uint32_t maxLatencyMs = 10800000; // 3 hours in ms = 3*60*60*1000 + uint32_t commitHardDueMs = 0; // set on first write of a burst +}; diff --git a/src/lib/pushButton/PushButton.cpp b/src/lib/pushButton/PushButton.cpp new file mode 100755 index 0000000..cfa58b9 --- /dev/null +++ b/src/lib/pushButton/PushButton.cpp @@ -0,0 +1,131 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// Handle push buttons + +#include "PushButton.h" + +#include "../gpioEx/GpioEx.h" + +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif + +Button::Button(int pin, int initState, int32_t trigger) { + this->pin = pin; + + long activeState = (trigger & 0b00000000000000000000000000000001); + int16_t analogThresholdValue = (trigger & 0b00000000000000000000011111111110) >> 1; + int16_t analogDownValue = (trigger & 0b01111111111000000000000000000000) >> 21; + hysteresis = (trigger & 0b00000000000111111111100000000000) >> 11; + + isAnalogThreshold = analogThresholdValue != 0; + isAnalogValue = analogDownValue != 0; + + if (isAnalogThreshold) { analogCompareValue = analogThresholdValue; debounceMs = 30; } else + if (isAnalogValue) { analogCompareValue = analogDownValue; debounceMs = 30; } else debounceMs = hysteresis; + + if (activeState == LOW) { UP = HIGH; DOWN = LOW; } else + if (activeState == HIGH) { UP = LOW; DOWN = HIGH; } + + if (isAnalogThreshold || isAnalogValue) { + if (analogCompareValue > ANALOG_READ_RANGE) { + analogCompareValue = ANALOG_READ_RANGE; + DF("WRN: Button::Button(), AnalogCompareValue for pin "); D(pin); DF(" is above analog range setting to "); DL(analogCompareValue); + } + if (analogCompareValue - hysteresis < 0) { + hysteresis = analogCompareValue; + DF("WRN: Button::Button(), AnalogCompareValue - hysteresis for pin "); D(pin); DF(" is below analog range setting Hysteresis to "); DL(hysteresis); + } else + if (analogCompareValue + hysteresis > ANALOG_READ_RANGE) { + hysteresis = ANALOG_READ_RANGE - analogCompareValue; + DF("WRN: Button::Button(), AnalogCompareValue + Hysteresis for pin "); D(pin); DF(" is above analog range setting Hysteresis to "); DL(hysteresis); + } + } + + pinModeEx(pin, initState); +} + +void Button::poll() { + int lastState = state; + if (isAnalogThreshold || isAnalogValue) { + #ifdef ESP32 + int analogValue = round((analogReadMilliVolts(pin)/3300.0F)*(float)ANALOG_READ_RANGE); + #else + int analogValue = analogRead(pin); + #endif + + if (isAnalogThreshold) { + if (DOWN == HIGH) { if (analogValue >= analogCompareValue + hysteresis) state = HIGH; else state = LOW; } + if (DOWN == LOW) { if (analogValue <= analogCompareValue - hysteresis) state = LOW; else state = HIGH; } + } else + + if (isAnalogValue) { + if (analogValue >= analogCompareValue - hysteresis && analogValue <= analogCompareValue + hysteresis) state = DOWN; else state = UP; + } + + } else state = digitalReadEx(pin); + if (lastState != state) { + avgPulseDuration = ((avgPulseDuration*49.0) + (double)(millis() - stableStartMs))/50.0; + stableStartMs = millis(); + } + stableMs = (long)(millis() - stableStartMs); + if (stableMs > 3000UL) { + avgPulseDuration = ((avgPulseDuration*4.0) + 2000.0)/5.0; + } + if (stableMs > debounceMs) { + if (lastStableState == UP && state == DOWN) { + static unsigned long lastPressTime = 0; + if (pressed && millis() - lastPressTime < 500) doublePressed = true; + pressed = true; + lastPressTime = millis(); + } + lastStableState = state; + } +} + +bool Button::isDown() { + if (stableMs > debounceMs && state == DOWN) return true; else return false; +} + +bool Button::wasPressed(bool peek) { + if (pressed) { + if (!peek) pressed = false; + return true; + } else return false; +} + +bool Button::wasDoublePressed(bool peek) { + if (doublePressed) { + if (!peek) { + doublePressed = false; + pressed = false; + } return true; + } else return false; +} + +bool Button::wasClicked(bool peek) { + if (isUp()) return wasPressed(peek); else return false; +} + +void Button::clearPress() { + pressed = false; +} + +bool Button::isUp() { + if (stableMs > debounceMs && state == UP) return true; else return false; +} + +long Button::timeDown() { + if (stableMs > debounceMs && state == DOWN) return stableMs; else return 0; +} + +long Button::timeUp() { + if (stableMs > debounceMs && state == UP) return stableMs; else return 0; +} + +bool Button::hasTone() { + if (fabs(avgPulseDuration - 40.0) < TONE_FREQ_THRESHOLD) return true; else return false; +} + +double Button::toneFreq() { + if (avgPulseDuration >= 0.1 && avgPulseDuration <= 10000.0) return 0.5/(avgPulseDuration/1000.0); else return NAN; +} diff --git a/src/lib/pushButton/PushButton.h b/src/lib/pushButton/PushButton.h new file mode 100755 index 0000000..74845cf --- /dev/null +++ b/src/lib/pushButton/PushButton.h @@ -0,0 +1,68 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// Handle push buttons +#pragma once + +#include "../../Common.h" + +// threshold in milliseconds (for tone detection, 40ms/7 = +/-17.5%) +#define TONE_FREQ_THRESHOLD 7.0 + +class Button { + public: + // create object + // \param pin MCU pin + // \param initState initialization state: INPUT, INPUT_PULLUP, or INPUT_PULLDOWN + // \param trigger triggered state: HIGH or LOW, optional + // analog threshold/hysteresis |THLD(t) |HIST(h) are 10 bit values + Button(int pin, int initState, int32_t trigger); + + // must be repeatedly called to check status of button + void poll(); + + // is the button down + bool isDown(); + + // was the button down since last checked + bool wasPressed(bool peek = false); + + // was the button down since last checked + bool wasDoublePressed(bool peek = false); + + // was the button down since last checked + bool wasClicked(bool peek = false); + + // clear pressed state + void clearPress(); + + // is the button up + bool isUp(); + + // number of ms down + long timeDown(); + + // number of ms up + long timeUp(); + + // check to see if this button has the SHC tone (12.5 Hz nominal) + bool hasTone(); + + // in Hz (0.05 to 5000Hz range) returns NAN otherwise + double toneFreq(); + + private: + int pin; + int state = HIGH; + int lastStableState = HIGH; + int analogCompareValue; + int hysteresis; + unsigned long debounceMs = 0; + unsigned long stableStartMs = 0; + unsigned long stableMs = 0; + bool isAnalogThreshold = false; + bool isAnalogValue = false; + bool pressed = false; + bool doublePressed = false; + double avgPulseDuration = 2000.0; + int UP = HIGH; + int DOWN = LOW; +}; diff --git a/src/lib/sense/Sense.cpp b/src/lib/sense/Sense.cpp new file mode 100755 index 0000000..586672f --- /dev/null +++ b/src/lib/sense/Sense.cpp @@ -0,0 +1,170 @@ +// ----------------------------------------------------------------------------------- +// combined digital and analog threshold read, 10 bit analog read resolution only +// analog mode reads uses software schmitt trigger with threshold/hysteresis-band +// digital mode reads have basic hf EMI/RFI noise filtering + +#include "Sense.h" + +#include "../tasks/OnTask.h" +#include "../gpioEx/GpioEx.h" + +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif + +SenseInput::SenseInput(int pin, int initState, int32_t trigger) { + this->pin = pin; + + activeState = (trigger & 0b0000000000000000000001); + threshold = (trigger & 0b0000000000011111111110) >> 1; + hysteresis = (trigger & 0b0111111111100000000000) >> 11; + isAnalog = threshold != 0; + + VF("pin="); V(pin); V(", "); + VF("active="); V(activeState ? "HIGH" : "LOW"); V(", "); + VF("ths="); V(threshold); V(", "); + VF("hys="); VL(hysteresis); + + if (isAnalog) { + if (threshold < 0) { + threshold = 0; + DF("WRN: SenseInput::SenseInput(), Threshold for pin "); D(pin); DLF(" is below Analog range setting to "); DL(threshold); + } else + if (threshold > ANALOG_READ_RANGE) { + threshold = ANALOG_READ_RANGE; + DF("WRN: SenseInput::SenseInput(), Threshold for pin "); D(pin); DF(" is above Analog range setting to "); DL(threshold); + } + if (threshold - hysteresis < 0) { + hysteresis = threshold; + DF("WRN: SenseInput::SenseInput(), Threshold - Hysteresis for pin "); D(pin); DF(" is below Analog range setting Hysteresis to "); DL(hysteresis); + } else + if (threshold + hysteresis > ANALOG_READ_RANGE) { + hysteresis = ANALOG_READ_RANGE - threshold; + DF("WRN: SenseInput::SenseInput(), Threshold + Hysteresis for pin "); D(pin); DF(" is above Analog range setting Hysteresis to "); DL(hysteresis); + } + } + + pinModeEx(pin, initState); + + reset(); +} + +int SenseInput::isOn() { + int value = lastValue; + if (isAnalog) { + #ifdef ESP32 + int sample = round((analogReadMilliVolts(pin)/3300.0F)*(float)ANALOG_READ_RANGE); + #else + int sample = analogRead(pin); + #endif + + if (sample >= threshold + hysteresis) value = HIGH; + if (sample <= threshold - hysteresis) value = LOW; + } else { + int sample = digitalReadEx(pin); + if (stableSample != sample) { stableStartMs = millis(); stableSample = sample; } + long stableMs = (long)(millis() - stableStartMs); + if (stableMs >= hysteresis) value = stableSample; + } + if (reverseState) { if (value == LOW) value = HIGH; else value = LOW; } + lastValue = value; + return value == activeState; +} + +int SenseInput::changed() { + int value = lastChangedValue; + if (isAnalog) { + #ifdef ESP32 + int sample = round((analogReadMilliVolts(pin)/3300.0F)*(float)ANALOG_READ_RANGE); + #else + int sample = analogRead(pin); + #endif + + if (sample >= threshold + hysteresis) value = HIGH; + if (sample < threshold - hysteresis) value = LOW; + } else { + int sample = digitalReadEx(pin); + if (stableSample != sample) { stableStartMs = millis(); stableSample = sample; } + long stableMs = (long)(millis() - stableStartMs); + if (stableMs >= hysteresis) value = stableSample; + } + if (reverseState) { if (value == LOW) value = HIGH; else value = LOW; } + bool result = lastChangedValue != value; + lastChangedValue = value; + return result; +} + +void SenseInput::poll() { + int value = lastValue; + if (!isAnalog) { + int sample = digitalReadEx(pin); + if (stableSample != sample) { stableStartMs = millis(); stableSample = sample; } + long stableMs = (long)(millis() - stableStartMs); + if (stableMs > hysteresis) value = stableSample; + if (reverseState) { if (value == LOW) value = HIGH; else value = LOW; } + } + lastValue = value; +} + +void SenseInput::reset() { + if (isAnalog) { + #ifdef ESP32 + int sample = round((analogReadMilliVolts(pin)/3300.0F)*(float)ANALOG_READ_RANGE); + #else + int sample = analogRead(pin); + #endif + if (sample > threshold) lastValue = HIGH; else lastValue = LOW; + } else { + lastValue = digitalReadEx(pin); + } + + stableSample = lastValue; +} + +// Manage sense pins + +uint8_t Sense::add(int pin, int initState, int32_t trigger, bool force) { + if ((pin == OFF || trigger == OFF) && !force) return 0; + if (senseCount >= SENSE_MAX) { + DF("WRN: Sense::add(), senseCount exceeded ignoring pin "); DL(pin); + return 0; + } + if (trigger < 0 || trigger >= SENSE_MAX_TRIGGER) { + DF("WRN: Sense::add(), trigger value invalid ignoring pin "); DL(pin); + return 0; + } + if (initState != INPUT && initState != INPUT_PULLUP && initState != INPUT_PULLDOWN) { + DF("WRN: Sense::add(), initState value invalid ignoring pin "); DL(pin); + return 0; + } + VF("MSG: Sense"); V(senseCount); V(", init "); + senseInput[senseCount] = new SenseInput(pin, initState, trigger); + senseCount++; + return senseCount; +} + +int Sense::isOn(uint8_t handle) { + if (handle == 0) return false; + return senseInput[handle - 1]->isOn(); +} + +long Sense::stableMillis(uint8_t handle) { + if (handle == 0) return false; + return senseInput[handle - 1]->stableMillis(); +} + +int Sense::changed(uint8_t handle) { + if (handle == 0) return false; + return senseInput[handle - 1]->changed(); +} + +void Sense::reverse(uint8_t handle, bool state) { + if (handle == 0) return; + senseInput[handle - 1]->reverse(state); +} + +void Sense::poll() { + for (int i = 0; i < senseCount; i++) { senseInput[i]->poll(); Y; } +} + +Sense sense; diff --git a/src/lib/sense/Sense.h b/src/lib/sense/Sense.h new file mode 100755 index 0000000..7cbc63e --- /dev/null +++ b/src/lib/sense/Sense.h @@ -0,0 +1,92 @@ +// ----------------------------------------------------------------------------------- +// combined digital and analog threshold read, 10 bit analog read resolution only +// analog mode reads uses software schmitt trigger with threshold/hysteresis-band +// digital mode reads have basic hf EMI/RFI noise filtering +#pragma once + +#include "../../Common.h" + +#ifndef THLD + #define THLD(v) ((v)<<1) // 10 bit analog threshold, bits 1 through 10 +#endif +#ifndef HYST + #define HYST(v) ((v)<<11) // 10 bit hysteresis, bits 11 through 20 +#endif +#ifndef INPUT_PULLDOWN + #define INPUT_PULLDOWN INPUT +#endif + +// default provision for up to 16 sense inputs is allowed, to change use: +// #define SENSE_MAX 30 (for example) +#ifndef SENSE_MAX + #define SENSE_MAX 8 +#endif + +// largest possible trigger value == 2^21 +#define SENSE_MAX_TRIGGER 2097152 + +class SenseInput { + public: + SenseInput(int pin, int initState, int32_t trigger); + + int isOn(); + inline long stableMillis() { return isAnalog ? 0 : (long)(millis() - stableStartMs); } + + int changed(); + inline void reverse(bool state) { reverseState = state; } + + void poll(); + + private: + void reset(); + + int pin; + int activeState = OFF; + bool reverseState = false; + bool isAnalog; + int threshold; + int hysteresis; + int triggerMode; + int lastValue = LOW; + int lastChangedValue = LOW; + int lastResult = LOW; + int stableSample = 0; + unsigned long stableStartMs = 0; +}; + +class Sense { + public: + // add sense input + // \param pin MCU pin to watch + // \param initState initialization state: INPUT, INPUT_PULLUP, or INPUT_PULLDOWN + // \param trigger triggered state: HIGH or LOW, optional + // analog threshold/hysteresis |THLD(t) |HIST(h) are 10 bit values + // \param force allocates even if pin is OFF + // \returns handle to this sense if successful, 0 otherwise + uint8_t add(int pin, int initState, int32_t trigger, bool force = false); + + // read the sense associated input pin and return state as configured + // \param handle sense handle + int isOn(uint8_t handle); + + // time in milliseconds since the input last changed state + // \param handle sense handle + long stableMillis(uint8_t handle); + + // check the sense associated input pin and return true if it has changed since last read + // \param handle sense handle + int changed(uint8_t handle); + + // reverse the on state for this pin + // \param handle sense handle + void reverse(uint8_t handle, bool state); + + // call repeatedly to check inputs for changes + void poll(); + + private: + uint8_t senseCount = 0; + SenseInput *senseInput[SENSE_MAX]; +}; + +extern Sense sense; diff --git a/src/lib/serial/Serial_IP_Ethernet.cpp b/src/lib/serial/Serial_IP_Ethernet.cpp new file mode 100755 index 0000000..d5ff482 --- /dev/null +++ b/src/lib/serial/Serial_IP_Ethernet.cpp @@ -0,0 +1,123 @@ +// ----------------------------------------------------------------------------------- +// IP communication routines + +#include "Serial_IP_Ethernet.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && SERIAL_SERVER != OFF + + void IPSerial::begin(long port, unsigned long clientTimeoutMs, bool persist) { + if (active) return; + + // special case where the port is the most common baud rate + // so a standard call to begin(baud_rate) can still work + if ((port < 9000 || port >= 10000 || port == 9600) && clientTimeoutMs == 2000 && persist == false) port = 9999; + + this->port = port; + this->clientTimeoutMs = clientTimeoutMs; + this->persist = persist; + + if (!ethernetManager.init()) { + DLF("WRN: IPSerial, failed to start Ethernet"); + return; + } + + cmdSvr = new EthernetServer(port); + cmdSvr->begin(); + VF("MSG: IPSerial, started EthernetServer on port "); VL(port); + + active = true; + + delay(1000); + } + + void IPSerial::restart() { + VLF("IPSerial, restarted EthernetServer"); + cmdSvr->begin(); + } + + void IPSerial::end() { + if (cmdSvrClient.connected()) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: end(), STOP cmdSvrClient"); + #endif + cmdSvrClient.stop(); + } + } + + void IPSerial::flush(void) { + if (!ethernetManager.active || !cmdSvrClient) return; + cmdSvrClient.flush(); + } + + int IPSerial::available(void) { + if (!ethernetManager.active) return 0; + + if (!cmdSvrClient) { + cmdSvrClient = cmdSvr->available(); + if (cmdSvrClient) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: available(), NEW cmdSvrClient"); + #endif + clientEndTimeMs = millis() + clientTimeoutMs; + cmdSvrClient.setTimeout(1000); + } + } else { + if (!cmdSvrClient.connected()) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: available(), not connected STOP cmdSvrClient"); + #endif + cmdSvrClient.stop(); + return 0; + } + if ((long)(clientEndTimeMs - millis()) < 0) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: available(), timed out STOP cmdSvrClient"); + #endif + cmdSvrClient.stop(); + return 0; + } + } + + int i = cmdSvrClient.available(); + + #if DEBUG_CMDSERVER == ON + if (i > 0) { VF("MSG: available(), recv. buffer has "); V(i); VLF(" chars"); } + #endif + + return i; + } + + int IPSerial::peek(void) { + if (!ethernetManager.active || !cmdSvrClient) return -1; + return cmdSvrClient.peek(); + } + + int IPSerial::read(void) { + if (!ethernetManager.active || !cmdSvrClient) return -1; + if (persist) clientEndTimeMs = millis() + clientTimeoutMs; + int c = cmdSvrClient.read(); + #if DEBUG_CMDSERVER == ON + VF("MSG: read(), found: "); VL((char)c); + #endif + return c; + } + + size_t IPSerial::write(uint8_t data) { + if (!ethernetManager.active || !cmdSvrClient) return 0; + return cmdSvrClient.write(data); + } + + size_t IPSerial::write(const uint8_t *data, size_t count) { + if (!ethernetManager.active || !cmdSvrClient) return 0; + return cmdSvrClient.write(data, count); + } + + #if SERIAL_SERVER == STANDARD || SERIAL_SERVER == BOTH + IPSerial SerialIP; + #endif + + #if SERIAL_SERVER == PERSISTENT || SERIAL_SERVER == BOTH + IPSerial SerialPIP1; + #endif + +#endif diff --git a/src/lib/serial/Serial_IP_Ethernet.h b/src/lib/serial/Serial_IP_Ethernet.h new file mode 100755 index 0000000..5385317 --- /dev/null +++ b/src/lib/serial/Serial_IP_Ethernet.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------------- +// Polling serial IP for Ethernet +#pragma once + +#include "../../Common.h" +#include "../ethernet/EthernetManager.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && SERIAL_SERVER != OFF + + #ifdef ESP8266 + #ifndef ETHERNET_W5500 + #error "The ESP8266 Ethernet option supports the W5500 only" + #endif + #include // https://github.com/adafruit/Ethernet2 + #else + #if OPERATIONAL_MODE == ETHERNET_TEENSY41 + #include + #else + #include // built-in library or my https://github.com/hjd1964/Ethernet for ESP32 and ASCOM Alpaca support + #endif + #endif + + class IPSerial : public Stream { + public: + void begin(long port, unsigned long clientTimeoutMs = 2000, bool persist = false); + void restart(); + void end(); + + void flush(void); + int available(void); + int peek(void); + int read(void); + size_t write(uint8_t data); + size_t write(const uint8_t* data, size_t count); + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + + using Print::write; + + private: + EthernetServer *cmdSvr; + EthernetClient cmdSvrClient; + + int port = -1; + unsigned long clientTimeoutMs; + unsigned long clientEndTimeMs = 0; + bool active = false; + bool persist = false; + }; + + #if SERIAL_SERVER == STANDARD || SERIAL_SERVER == BOTH + extern IPSerial SerialIP; + #define SERIAL_SIP SerialIP + #endif + + #if SERIAL_SERVER == PERSISTENT || SERIAL_SERVER == BOTH + extern IPSerial SerialPIP1; + #define SERIAL_PIP1 SerialPIP1 + #endif + +#endif diff --git a/src/lib/serial/Serial_IP_Ethernet_Client.cpp b/src/lib/serial/Serial_IP_Ethernet_Client.cpp new file mode 100755 index 0000000..09061e8 --- /dev/null +++ b/src/lib/serial/Serial_IP_Ethernet_Client.cpp @@ -0,0 +1,113 @@ +// ----------------------------------------------------------------------------------- +// IP communication routines + +#include "Serial_IP_Ethernet_Client.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && SERIAL_CLIENT != OFF + + #include "../tasks/OnTask.h" + + void pollIPClient() { SerialIPClient.poll(); } + + bool IPSerialClient::begin(long port, unsigned long clientTimeoutMs, bool persist) { + if (active) return true; + + // special case where the port is the most common baud rate + // so a standard call to begin(baud_rate) can still work + if ((port < 9000 || port >= 10000 || port == 9600) && clientTimeoutMs == 2000 && persist == false) port = 9999; + + this->port = port; + this->clientTimeoutMs = clientTimeoutMs; + this->persist = persist; + + if (!ethernetManager.init()) { + DLF("WRN: IPSerialClient, failed to start Ethernet"); + return false; + } + + delay(1000); + + onStep = IPAddress(ethernetManager.sta->target); + VF("MSG: IPSerialClient, target "); V(onStep[0]); V("."); V(onStep[1]); V("."); V(onStep[2]); V("."); V(onStep[3]); V(":"); VL(port); + + if (!persist) { + lastActivityTimeMs = millis(); + VF("MSG: Setup, start IPSerialClient monitor task (rate 1s priority 7)... "); + if (tasks.add(1000, 0, true, 7, pollIPClient, "PollIP")) { VLF("success"); } else { VLF("FAILED!"); } + } + + active = true; + return true; + } + + void IPSerialClient::end() { + if (!active) return; + + cmdSvrClient.stop(); + VLF("MSG: IPSerialClient, connection to target closed"); + + tasks.remove(tasks.getHandleByName("PollIP")); + + active = false; + } + + bool IPSerialClient::isConnected() { + lastActivityTimeMs = millis(); + if (Ethernet.linkStatus() != LinkOFF) { + if (!cmdSvrClient.connected()) { + cmdSvrClient.stop(); + if (cmdSvrClient.connect(onStep, port)) { + VLF("MSG: IPSerialClient, connection to target restarted"); + return true; + } else { + DLF("WRN: IPSerialClient, connection to target failed"); + return false; + } + } else return true; + } else { + DLF("WRN: IPSerialClient, connection to target failed no cable"); + return false; + } + } + + void IPSerialClient::flush(void) { + if (!active || !isConnected()) return; + + return cmdSvrClient.flush(); + } + + int IPSerialClient::available(void) { + if (!active || !isConnected()) return 0; + + return cmdSvrClient.available(); + } + + int IPSerialClient::peek(void) { + if (!active || !isConnected()) return -1; + + return cmdSvrClient.peek(); + } + + int IPSerialClient::read(void) { + if (!active || !isConnected()) return -1; + char c = cmdSvrClient.read(); + return c; + } + + size_t IPSerialClient::write(uint8_t data) { + if (!active || !isConnected()) return 0; + return cmdSvrClient.write(data); + } + + size_t IPSerialClient::write(const uint8_t *data, size_t quantity) { + if (!active || !isConnected()) return 0; + return cmdSvrClient.write(data, quantity); + } + + void IPSerialClient::poll() { + if (millis() - lastActivityTimeMs > clientTimeoutMs && cmdSvrClient.connected()) cmdSvrClient.stop(); + } + + IPSerialClient SerialIPClient; + +#endif diff --git a/src/lib/serial/Serial_IP_Ethernet_Client.h b/src/lib/serial/Serial_IP_Ethernet_Client.h new file mode 100755 index 0000000..398e8e6 --- /dev/null +++ b/src/lib/serial/Serial_IP_Ethernet_Client.h @@ -0,0 +1,60 @@ +// ----------------------------------------------------------------------------------- +// Polling serial IP for Ethernet +#pragma once + +#include "../../Common.h" +#include "../ethernet/EthernetManager.h" + +#if OPERATIONAL_MODE >= ETHERNET_FIRST && OPERATIONAL_MODE <= ETHERNET_LAST && SERIAL_CLIENT != OFF + + #ifdef ESP8266 + #ifndef ETHERNET_W5500 + #error "The ESP8266 Ethernet option supports the W5500 only" + #endif + #include // https://github.com/adafruit/Ethernet2 + #else + #if OPERATIONAL_MODE == ETHERNET_TEENSY41 + #include + #else + #include // built-in library or my https://github.com/hjd1964/Ethernet for ESP32 and ASCOM Alpaca support + #endif + #endif + + class IPSerialClient : public Stream { + public: + bool begin(long port, unsigned long clientTimeoutMs = 2000, bool persist = false); + void end(); + bool isConnected(); + + void flush(void); + int available(void); + int peek(void); + int read(void); + int availableForWrite() { return 1; } + size_t write(uint8_t data); + size_t write(const uint8_t* data, size_t count); + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + + using Print::write; + + void poll(); + + private: + EthernetClient cmdSvrClient; + IPAddress onStep; + + int port = -1; + unsigned long lastActivityTimeMs = 0; + unsigned long clientTimeoutMs; + unsigned long clientEndTimeMs = 0; + bool active = false; + bool persist = false; + }; + + extern IPSerialClient SerialIPClient; + #define SERIAL_IP SerialIPClient + +#endif diff --git a/src/lib/serial/Serial_IP_Wifi.cpp b/src/lib/serial/Serial_IP_Wifi.cpp new file mode 100755 index 0000000..b368d4b --- /dev/null +++ b/src/lib/serial/Serial_IP_Wifi.cpp @@ -0,0 +1,125 @@ +// ----------------------------------------------------------------------------------- +// IP communication routines + +#include "Serial_IP_Wifi.h" + +#if OPERATIONAL_MODE == WIFI && SERIAL_SERVER != OFF + + void IPSerial::begin(long port, unsigned long clientTimeoutMs, bool persist) { + if (active) return; + + // special case where the port is the most common baud rate + // so a standard call to begin(baud_rate) can still work + if ((port < 9000 || port >= 10000 || port == 9600) && clientTimeoutMs == 2000 && persist == false) port = 9999; + + this->port = port; + this->clientTimeoutMs = clientTimeoutMs; + this->persist = persist; + + if (!wifiManager.init()) { + DLF("WRN: IPSerial, failed to start WiFi"); + return; + } + + cmdSvr = new WiFiServer(port); + delay(1000); + + cmdSvr->begin(); + cmdSvr->setNoDelay(true); + VF("MSG: IPSerial, started WiFiServer on port "); VL(port); + + active = true; + + delay(1000); + } + + void IPSerial::end() { + if (cmdSvrClient.connected()) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: end(), STOP cmdSvrClient"); + #endif + cmdSvrClient.stop(); + } + } + + void IPSerial::flush(void) { + if (!active || !cmdSvrClient) return; + cmdSvrClient.flush(); + } + + int IPSerial::available(void) { + if (!active) return 0; + + if (!cmdSvrClient) { + if (cmdSvr->hasClient()) { + cmdSvrClient = cmdSvr->available(); + clientEndTimeMs = millis() + clientTimeoutMs; + } + if (cmdSvrClient) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: available(), NEW cmdSvrClient"); + #endif + } else return 0; + } else { + if (!cmdSvrClient.connected()) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: available(), not connected STOP cmdSvrClient"); + #endif + cmdSvrClient.stop(); + return 0; + } + if ((long)(clientEndTimeMs - millis()) < 0) { + #if DEBUG_CMDSERVER == ON + VLF("MSG: available(), timed out STOP cmdSvrClient"); + #endif + cmdSvrClient.stop(); + return 0; + } + } + + int i = cmdSvrClient.available(); + + #if DEBUG_CMDSERVER == ON + if (i > 0) { VF("MSG: available(), recv. buffer has "); V(i); VLF(" chars"); } + #endif + + return i; + } + + int IPSerial::peek(void) { + if (!active || !cmdSvrClient) return -1; + return cmdSvrClient.peek(); + } + + int IPSerial::read(void) { + if (!active || !cmdSvrClient) return -1; + if (persist) clientEndTimeMs = millis() + clientTimeoutMs; + int c = cmdSvrClient.read(); + #if DEBUG_CMDSERVER == ON + VF("MSG: read(), found: "); VL((char)c); + #endif + return c; + } + + size_t IPSerial::write(uint8_t data) { + if (!active || !cmdSvrClient) return 0; + return cmdSvrClient.write(data); + } + + size_t IPSerial::write(const uint8_t *data, size_t count) { + if (!active || !cmdSvrClient) return 0; + return cmdSvrClient.write(data, count); + } + + #if SERIAL_SERVER == STANDARD || SERIAL_SERVER == BOTH + IPSerial SerialIP; + #define SERIAL_SIP SerialIP + #endif + + #if SERIAL_SERVER == PERSISTENT || SERIAL_SERVER == BOTH + IPSerial SerialPIP1; + IPSerial SerialPIP2; + IPSerial SerialPIP3; + #endif + +#endif diff --git a/src/lib/serial/Serial_IP_Wifi.h b/src/lib/serial/Serial_IP_Wifi.h new file mode 100755 index 0000000..7b6703d --- /dev/null +++ b/src/lib/serial/Serial_IP_Wifi.h @@ -0,0 +1,65 @@ +// ----------------------------------------------------------------------------------- +// Polling serial IP for ESP32 +#pragma once + +#include "../../Common.h" +#include "../wifi/WifiManager.h" + +#if OPERATIONAL_MODE == WIFI && SERIAL_SERVER != OFF + + #if defined(ESP32) + #include + #include + #include + #elif defined(ESP8266) + #include + #include + #include + #else + #error "Configuration (Config.h): No Wifi support is present for this device" + #endif + + class IPSerial : public Stream { + public: + void begin(long port, unsigned long clientTimeoutMs = 2000, bool persist = false); + void end(); + + void flush(void); + int available(void); + int peek(void); + int read(void); + size_t write(uint8_t data); + size_t write(const uint8_t* data, size_t count); + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + + using Print::write; + + private: + WiFiServer *cmdSvr; + WiFiClient cmdSvrClient; + + int port = -1; + unsigned long clientTimeoutMs; + unsigned long clientEndTimeMs = 0; + bool active = false; + bool persist = false; + }; + + #if SERIAL_SERVER == STANDARD || SERIAL_SERVER == BOTH + extern IPSerial SerialIP; + #define SERIAL_SIP SerialIP + #endif + + #if SERIAL_SERVER == PERSISTENT || SERIAL_SERVER == BOTH + extern IPSerial SerialPIP1; + extern IPSerial SerialPIP2; + extern IPSerial SerialPIP3; + #define SERIAL_PIP1 SerialPIP1 + #define SERIAL_PIP2 SerialPIP2 + #define SERIAL_PIP3 SerialPIP3 + #endif + +#endif diff --git a/src/lib/serial/Serial_IP_Wifi_Client.cpp b/src/lib/serial/Serial_IP_Wifi_Client.cpp new file mode 100755 index 0000000..b36679f --- /dev/null +++ b/src/lib/serial/Serial_IP_Wifi_Client.cpp @@ -0,0 +1,117 @@ +// ----------------------------------------------------------------------------------- +// IP communication routines + +// original work by jesco-t + +#include "Serial_IP_Wifi_Client.h" + +#if OPERATIONAL_MODE == WIFI && SERIAL_CLIENT == ON + + #include "../tasks/OnTask.h" + + void pollIPClient() { SerialIPClient.poll(); } + + bool IPSerialClient::begin(long port, unsigned long clientTimeoutMs, bool persist) { + if (active) return true; + + // special case where the port is the most common baud rate + // so a standard call to begin(baud_rate) can still work + if ((port < 9000 || port >= 10000 || port == 9600) && clientTimeoutMs == 2000 && persist == false) port = 9996; + + this->port = port; + this->clientTimeoutMs = clientTimeoutMs; + this->persist = persist; + + if (!wifiManager.init()) { + DLF("WRN: IPSerialClient, failed to start WiFi"); + return false; + } + + delay(1000); + + onStep = IPAddress(wifiManager.sta->target); + VF("MSG: IPSerialClient, target "); V(onStep[0]); V("."); V(onStep[1]); V("."); V(onStep[2]); V("."); V(onStep[3]); V(":"); VL(port); + + if (!persist) { + lastActivityTimeMs = millis(); + VF("MSG: Setup, start IPSerialClient monitor task (rate 1s priority 7)... "); + if (tasks.add(1000, 0, true, 7, pollIPClient, "PollIP")) { VLF("success"); } else { VLF("FAILED!"); } + } + + active = true; + return true; + } + + void IPSerialClient::end() { + if (!active) return; + + cmdSvrClient.stop(); + VLF("MSG: IPSerialClient, connection to target closed"); + + tasks.remove(tasks.getHandleByName("PollIP")); + + WiFi.disconnect(); + VLF("MSG: IPSerialClient, WiFi disconnected"); + + active = false; + } + + bool IPSerialClient::isConnected() { + lastActivityTimeMs = millis(); + if (WiFi.status() == WL_CONNECTED) { + if (!cmdSvrClient.connected()) { + cmdSvrClient.stop(); + if (cmdSvrClient.connect(onStep, port)) { + VLF("MSG: IPSerialClient, connection to target restarted"); + return true; + } else { + DLF("WRN: IPSerialClient, connection to target failed"); + return false; + } + } else return true; + } else { active = false; return false; } + } + + void IPSerialClient::flush(void) { + if (!active || !isConnected()) return; + + return cmdSvrClient.flush(); + } + + int IPSerialClient::available(void) { + if (!active || !isConnected()) return 0; + + return cmdSvrClient.available(); + } + + int IPSerialClient::peek(void) { + if (!active || !isConnected()) return -1; + + return cmdSvrClient.peek(); + } + + int IPSerialClient::read(void) { + if (!active || !isConnected()) return -1; + + return cmdSvrClient.read(); + } + + size_t IPSerialClient::write(uint8_t data) { + if (!active || !isConnected()) return 0; + + return cmdSvrClient.write(data); + } + + size_t IPSerialClient::write(const uint8_t *data, size_t quantity) { + if (!active || !isConnected()) return 0; + + return cmdSvrClient.write(data, quantity); + } + + void IPSerialClient::poll() { + if (millis() - lastActivityTimeMs > clientTimeoutMs && cmdSvrClient.connected()) cmdSvrClient.stop(); + } + + IPSerialClient SerialIPClient; + +#endif diff --git a/src/lib/serial/Serial_IP_Wifi_Client.h b/src/lib/serial/Serial_IP_Wifi_Client.h new file mode 100755 index 0000000..554e073 --- /dev/null +++ b/src/lib/serial/Serial_IP_Wifi_Client.h @@ -0,0 +1,60 @@ +// ----------------------------------------------------------------------------------- +// IP communication routines + +// original work by jesco-t + +#pragma once + +#include "../../Common.h" +#include "../wifi/WifiManager.h" + +#if OPERATIONAL_MODE == WIFI && SERIAL_CLIENT == ON + + #if defined(ESP32) + #include + #include + #include + #elif defined(ESP8266) + #include + #include + #include + #else + #error "Configuration (Config.h): No Wifi support is present for this device" + #endif + + class IPSerialClient : public Stream { + public: + bool begin(long port, unsigned long clientTimeoutMs = 2000, bool persist = false); + void end(); + bool isConnected(); + + void flush(void); + int available(void); + int peek(void); + int read(void); + int availableForWrite() { return 1; } + size_t write(uint8_t data); + size_t write(const uint8_t* data, size_t count); + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + + using Print::write; + + void poll(); + + private: + WiFiClient cmdSvrClient; + IPAddress onStep; + + int port = -1; + unsigned long lastActivityTimeMs = 0; + unsigned long clientTimeoutMs; + bool active = false; + bool persist = false; + }; + + extern IPSerialClient SerialIPClient; + #define SERIAL_IP SerialIPClient +#endif \ No newline at end of file diff --git a/src/lib/serial/Serial_Local.cpp b/src/lib/serial/Serial_Local.cpp new file mode 100755 index 0000000..119270b --- /dev/null +++ b/src/lib/serial/Serial_Local.cpp @@ -0,0 +1,78 @@ +// ----------------------------------------------------------------------------------- +// Local Serial for sending internal commands + +#include "Serial_Local.h" + +#if defined(SERIAL_LOCAL_MODE) && SERIAL_LOCAL_MODE == ON + +void SerialLocal::begin(long baud) { + // init the buffers + xmit_index = 0; + xmit_buffer[0] = 0; + recv_head = 0; + recv_tail = 0; + recv_buffer[0] = 0; + (void)(baud); + + #ifdef ESP32 + mutex = xSemaphoreCreateMutex(); + #endif +} + +void SerialLocal::end() { } + +void SerialLocal::transmit(const char *data) { + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #endif + + int data_len = strlen(data); + for (int i = 0; i < data_len; i++) { + recv_buffer[recv_tail] = data[i]; + recv_tail++; + } + + #ifdef ESP32 + xSemaphoreGive(mutex); + #endif +} + +char *SerialLocal::receive() { + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #endif + + int i = 0; + xmit_result[i] = 0; + while (xmit_buffer[xmit_index] != 0) { + xmit_result[i++] = xmit_buffer[xmit_index++]; + xmit_index &= 0b1111111; + } + xmit_result[i] = 0; + + #ifdef ESP32 + xSemaphoreGive(mutex); + #endif + return xmit_result; +} + +int SerialLocal::read(void) { + if (!available()) return -1; + + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #endif + + recv_buffer[recv_tail] = 0; + char c = recv_buffer[recv_head]; + if (c) recv_head++; + + #ifdef ESP32 + xSemaphoreGive(mutex); + #endif + return c; +} + +SerialLocal serialLocal; + +#endif diff --git a/src/lib/serial/Serial_Local.h b/src/lib/serial/Serial_Local.h new file mode 100755 index 0000000..3acb1a8 --- /dev/null +++ b/src/lib/serial/Serial_Local.h @@ -0,0 +1,128 @@ +// ----------------------------------------------------------------------------------- +// Local Serial for sending internal commands +#pragma once + +#include "../../Common.h" + +#if defined(SERIAL_LOCAL_MODE) && SERIAL_LOCAL_MODE == ON + +class SerialLocal : public Stream { + public: + inline void begin() { begin(9600); } + void begin(long baud); + void end(); + + void setTimeout(long timeMs) { UNUSED(timeMs); } + + // sends a command for processing + void transmit(const char *data); + + // receive has the last commands response, if one exists + char *receive(); + + int read(void); + + inline int receiveAvailable() { + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #endif + + int i = 0; + int x = xmit_index; + while (xmit_buffer[xmit_index] != 0) { + i++; + xmit_index++; + xmit_index &= 0b1111111; + } + xmit_index = x; + + #ifdef ESP32 + xSemaphoreGive(mutex); + #endif + return i; + } + + inline int available(void) { + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #endif + + uint8_t p = recv_head; + uint8_t c = 0; + while (recv_buffer[p] && p != recv_tail && c < 255) { c++; p++; } + + #ifdef ESP32 + xSemaphoreGive(mutex); + #endif + return c; + } + + inline int peek(void) { + if (!available()) return -1; + + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #endif + + char c = recv_buffer[recv_head]; + + #ifdef ESP32 + xSemaphoreGive(mutex); + #endif + return c; + } + + inline void flush(void) { + #ifdef ESP32 + while (receiveAvailable()) { yield(); } + #endif + } + + inline size_t write(uint8_t data) { + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #endif + + xmit_buffer[xmit_tail] = data; + xmit_tail++; + xmit_tail &= 0b1111111; + xmit_buffer[xmit_tail] = 0; + + #ifdef ESP32 + xSemaphoreGive(mutex); + #endif + return 1; + } + + size_t write(const uint8_t* data, size_t count) { + for (int i = 0; i < (int)count; i++) { if (!write(data[i])) return i; } + return count; + } + + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + + using Print::write; + + private: + char recv_buffer[256] = ""; + uint8_t recv_head = 0; + uint8_t recv_tail = 0; + + char xmit_buffer[128] = ""; + char xmit_result[128] = ""; + uint8_t xmit_index = 0; + uint8_t xmit_tail = 0; + + #ifdef ESP32 + SemaphoreHandle_t mutex; + #endif +}; + +extern SerialLocal serialLocal; + +#define SERIAL_LOCAL serialLocal + +#endif diff --git a/src/lib/serial/Serial_MEGA2560.cpp b/src/lib/serial/Serial_MEGA2560.cpp new file mode 100755 index 0000000..3b6ebba --- /dev/null +++ b/src/lib/serial/Serial_MEGA2560.cpp @@ -0,0 +1,219 @@ +// ----------------------------------------------------------------------------------- +// Low overhead communication routines for Serial0, Serial1 + +#include "Serial_MEGA2560.h" + +#if defined(SERIAL_MEGA2560) && SERIAL_MEGA2560 == ON + +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + +#define MSB(i) (i >> 8) +#define LSB(i) (i & 0xFF) + +#ifdef HAL_POLLING_MEGA2560_SERIAL_A + +void PollingSerialA::begin(long baud) { + // init the buffers + xmit_index = 0; xmit_buffer[0] = 0; recv_head = 0; recv_tail = 0; recv_buffer[0] = 0; + + // Set baud rate + uint16_t ubrr = F_CPU/16/baud - 1; + UBRR0H = MSB(ubrr); + UBRR0L = LSB(ubrr); + + // Disable U2X mode + UCSR0A = 0; + + // Enable receiver and transmitter + UCSR0B = (1<= (char)32) { + recv_buffer[recv_tail] = c; + recv_buffer[++recv_tail] = (char)0; + return (char)0; + } else return c; + } else return (char)0; + } else return (char)0; +} + +bool SerialST4Master::trans(char *data_in) { + static bool reset = false; + static unsigned long resetFinishTime = 0; + + static int index = 18; + + uint8_t this_bit; + + static uint8_t this_data_in; + static uint8_t r_parity; + static uint8_t this_data_out; + static uint8_t s_parity; + + // force a reset of ST4 comms + if (reset) { + if ((long)(millis() - resetFinishTime) > 0) reset = false; else return false; + index = 18; + } + + switch (--index) { + case 17: // send start bit + + if (!send_error) { + if (xmit_buffer[xmit_head] != (char)0) { + this_data_out = xmit_buffer[xmit_head++]; + } else this_data_out = (char)0; + } + + s_parity = 0; + r_parity = 0; + frame_error = false; + send_error = false; + recv_error = false; + digitalWriteF(ST4_CLOCK_OUT_PIN, LOW); + digitalWriteF(ST4_DATA_OUT_PIN, LOW); + break; + case 16: // recv start bit + digitalWriteF(ST4_CLOCK_OUT_PIN, HIGH); + if (digitalReadF(ST4_DATA_IN_PIN) != LOW) { + DLF("WRN: SerialST4.poll(), frame/start error"); + frame_error = true; + resetFinishTime = millis() + 1000; + reset = true; + return false; + } + break; + case 15: case 13: case 11: case 9: case 7: case 5: case 3: case 1: // send data bits + this_bit = bitRead(this_data_out, index>>1); + s_parity += this_bit; + digitalWriteF(ST4_CLOCK_OUT_PIN, LOW); + digitalWriteF(ST4_DATA_OUT_PIN, this_bit); + break; + case 14: case 12: case 10: case 8: case 6: case 4: case 2: case 0: // recv data bits + digitalWriteF(ST4_CLOCK_OUT_PIN, HIGH); + this_bit = digitalReadF(ST4_DATA_IN_PIN); + r_parity += this_bit; + bitWrite(this_data_in, index>>1, this_bit); + break; + case -1: // send parity bit + digitalWriteF(ST4_CLOCK_OUT_PIN, LOW); + digitalWriteF(ST4_DATA_OUT_PIN, s_parity&1); + break; + case -2: // recv parity bit + digitalWriteF(ST4_CLOCK_OUT_PIN, HIGH); + if ((r_parity&1) != digitalReadF(ST4_DATA_IN_PIN)) recv_error = true; + break; + case -3: // send local parity ck bit + digitalWriteF(ST4_CLOCK_OUT_PIN, LOW); + digitalWriteF(ST4_DATA_OUT_PIN, recv_error); + break; + case -4: // recv remote parity ck bit + digitalWriteF(ST4_CLOCK_OUT_PIN, HIGH); + if (digitalReadF(ST4_DATA_IN_PIN) == HIGH) send_error = true; + break; + case -5: // send stop bit + digitalWriteF(ST4_CLOCK_OUT_PIN, LOW); + digitalWriteF(ST4_DATA_OUT_PIN, LOW); + break; + case -6: // recv stop bit + digitalWriteF(ST4_CLOCK_OUT_PIN, HIGH); + if (digitalReadF(ST4_DATA_IN_PIN) != LOW) frame_error = true; + + if (frame_error) { + DLF("WRN: SerialST4.poll(), frame/stop error"); + resetFinishTime = millis() + ST4_MAX_BIT_TIME/1000 + 1; + reset = true; + return false; + } + +// if (this_data_in >= 32 || this_data_out >= 32) { +// D("Got "); if (this_data_in >= 32) { D((char)this_data_in); } else D(" "); +// D(" Sent "); if (this_data_out >= 32) { DL((char)this_data_out); } else DL(" "); +// } + + *data_in = this_data_in; + + if (send_error) { DLF("WRN: SerialST4.poll(), send parity error"); } + if (recv_error) { DLF("WRN: SerialST4.poll(), recv parity error"); } + + index = 18; + return true; + break; + } + + return false; +} + +void SerialST4Master::begin() { + xmit_head = 0; + xmit_tail = 0; + xmit_buffer[0] = 0; + recv_head = 0; + recv_tail = 0; + recv_buffer[0] = 0; +} + +void SerialST4Master::begin(long baud) { + begin(); + (void)(baud); +} + +void SerialST4Master::end() { + xmit_head = 0; + xmit_tail = 0; + xmit_buffer[0] = 0; + recv_head = 0; + recv_tail = 0; + recv_buffer[0] = 0; +} + +size_t SerialST4Master::write(uint8_t data) { + unsigned long t_start = millis(); + uint8_t xh = xmit_head; + xh--; + while (xmit_tail == xh) { + Y; // yield from the command channel at priority level 5 (lets poll() run) + if ((millis() - t_start) > timeout) return 0; + } + xmit_buffer[xmit_tail] = data; xmit_tail++; + xmit_buffer[xmit_tail] = 0; + return 1; +} + +size_t SerialST4Master::write(const uint8_t *data, size_t quantity) { + // fail if trying to write more than the buffer can hold + if ((int)quantity > 254) return 0; + + for (int i = 0; i < (int)quantity; i++) { if (!write(data[i])) return 0; } + return 1; +} + +int SerialST4Master::available(void) { + int a=0; + for (uint8_t b = recv_head; recv_buffer[b] != (char)0; b++) a++; + return a; +} + +int SerialST4Master::read(void) { + char c = recv_buffer[recv_head]; if (c != 0) recv_head++; + if (c == 0) c = -1; + return c; +} + +int SerialST4Master::peek(void) { + int c = recv_buffer[recv_head]; + if (c == 0) c= -1; + + return c; +} + +void SerialST4Master::flush(void) { + unsigned long startMs = millis(); + int c; + do { + Y; // yield from the command channel at priority level 5 (lets poll() run) + c = xmit_buffer[xmit_head]; + } while (c != 0 || (millis() - startMs < timeout)); +} + +SerialST4Master serialST4; + +#endif diff --git a/src/lib/serial/Serial_ST4_Master.h b/src/lib/serial/Serial_ST4_Master.h new file mode 100755 index 0000000..1f5d374 --- /dev/null +++ b/src/lib/serial/Serial_ST4_Master.h @@ -0,0 +1,72 @@ +// ------------------------------------------------------------------------------------ +// Serial ST4 master +#pragma once + +/* +ST4 port data communication scheme: + +5V power ---- ESP32-S, Teensy4.0, Teensy3.2, etc. +Gnd --------- + +HC Signal OnStep +RAw ---- Data ---> Recv. data +DEs <--- Clock ---- Clock +DEn <--- Data ---- Send data +RAe ---- 12.5Hz Square wave ---> 100% sure SHC is present, switches DEs & DEn to OUTPUT + +Data is exchanged on clock edges similar to SPI so is timing insensitive (runs with interrupts enabled.) + +One data byte is exchanged (in both directions w/basic error detection and recovery.) A value 0x00 byte +means "no data" and is ignored on both sides. +*/ + +#include "../../Common.h" + +#if defined(SERIAL_ST4_MASTER) && SERIAL_ST4_MASTER == ON + +#include "Stream.h" + +class SerialST4Master : public Stream { + public: + void begin(); + void begin(long baud); + + void end(); + + // call periodically to move data to/from buffers across the SerialST4 interface + // recvd chars between 0 and 32 are returned directly and bypass the buffer + // 24 calls moves one byte of data in both directions so 100us/call results in a data rate of about 4200 bps (350 cps) + char poll(); + + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *, size_t); + virtual int available(void); + virtual int read(void); + virtual int peek(void); + virtual void flush(void); + + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + using Print::write; + + private: + bool trans(char *data_in); + + char xmit_buffer[256] = ""; + uint8_t xmit_head = 0; + uint8_t xmit_tail = 0; + char recv_buffer[256] = ""; + uint8_t recv_tail = 0; + bool frame_error = false; + bool send_error = false; + bool recv_error = false; + uint8_t recv_head = 0; + uint32_t timeout = 80; +}; + +extern SerialST4Master serialST4; +#define SERIAL_ST4 serialST4 + +#endif diff --git a/src/lib/serial/Serial_ST4_Slave.cpp b/src/lib/serial/Serial_ST4_Slave.cpp new file mode 100755 index 0000000..ed806bd --- /dev/null +++ b/src/lib/serial/Serial_ST4_Slave.cpp @@ -0,0 +1,298 @@ +// ----------------------------------------------------------------------------------- +// Serial ST4 slave + +#include "Serial_ST4_Slave.h" + +#if defined(SERIAL_ST4_SLAVE) && SERIAL_ST4_SLAVE == ON + +#include "../tasks/OnTask.h" + +// interval in milliseconds for generating the tone signal +#define ST4_TONE_INTERVAL 40 + +// interval in milliseconds for a clock timeout +#define ST4_MAX_BIT_TIME 100 + +#define ST4_CLOCK_PIN ST4_S_PIN +#define ST4_DATA_IN_PIN ST4_N_PIN +#define ST4_DATA_OUT_PIN ST4_W_PIN +#define ST4_TONE_PIN ST4_E_PIN + +IRAM_ATTR void dataClock() { SerialST4.poll(); } + +void SerialST4Slave::begin(long baudRate = 9600) { + if (isActive) return; + + xmit_head = 0; + xmit_tail = 0; + xmit_buffer[0] = 0; + recv_head = 0; + recv_tail = 0; + recv_buffer[0] = 0; + + VF("MSG: SerialST4Slave, start shcTone task (rate 40ms priority 0)... "); + handle = tasks.add(0, 0, true, 0, shcTone, "tone"); + if (handle) { + pinMode(ST4_CLOCK_PIN, INPUT_PULLUP); + pinMode(ST4_DATA_IN_PIN, INPUT_PULLUP); + pinMode(ST4_TONE_PIN, OUTPUT); + pinMode(ST4_DATA_OUT_PIN, OUTPUT); + + if (!tasks.requestHardwareTimer(handle, 0)) { VLF("(no hardware timer!)"); } else { VLF("success"); } + tasks.setPeriod(handle, ST4_TONE_INTERVAL); + + attachInterrupt(digitalPinToInterrupt(ST4_CLOCK_PIN), dataClock, CHANGE); + + isActive = true; + } else { + VLF("FAILED!"); + } +} + +void SerialST4Slave::end() { + if (!isActive) return; + + pinMode(ST4_TONE_PIN, INPUT_PULLUP); + pinMode(ST4_DATA_OUT_PIN, INPUT_PULLUP); + + detachInterrupt(digitalPinToInterrupt(ST4_CLOCK_PIN)); + + tasks.remove(handle); + VLF("MSG: SerialST4Slave, stopped shcTone task"); + + xmit_head = 0; + xmit_tail = 0; + xmit_buffer[0] = 0; + recv_head = 0; + recv_tail = 0; + recv_buffer[0] = 0; + + isActive = false; +} + +void SerialST4Slave::paused(bool state) { + if (!isActive) return; + + if (state) tasks.setPeriod(handle, ST4_TONE_INTERVAL); else tasks.setPeriod(handle, 0); +} + +bool SerialST4Slave::active() { + if (!isActive) return false; + + static unsigned long comp = 0; + bool result = false; + noInterrupts(); + if (comp != lastTimeInMilliseconds) { + result = true; + comp = lastTimeInMilliseconds; + } + interrupts(); + return result; +} + +size_t SerialST4Slave::write(uint8_t data) { + if (!isActive) return 0; + + // wait for room in buffer to become available or give up + unsigned long t_start = millis(); + + uint8_t xh = xmit_head; + xh--; + while (xmit_tail == xh) { + if ((millis() - t_start) > _timeout) return 0; + } + + // is this a control code command? is the buffer not empty? + if (data > 0 && data < 32 && xmit_buffer[xmit_head] != 0) { + noInterrupts(); + // insert the command into the buffer + uint8_t hd = xmit_head; + uint8_t hs = xmit_head; + hs--; + for (int i = 0; i < 254; i++) { + xmit_buffer[--hd] = xmit_buffer[--hs]; + if (xmit_buffer[hs] == 0) break; + } + xmit_head++; + xmit_buffer[xmit_head] = data; + xmit_head--; + xmit_buffer[++xmit_tail] = 0; + interrupts(); + } else { + noInterrupts(); + xmit_buffer[xmit_tail] = data; + xmit_buffer[++xmit_tail] = 0; + interrupts(); + } + return 1; +} + +size_t SerialST4Slave::write(const uint8_t *data, size_t quantity) { + if (!isActive) return 0; + + // fail if trying to write more than the buffer can hold + if ((int)quantity > 254) return 0; + + for (int i = 0; i < (int)quantity; i++) { if (!write(data[i])) return 0; } + return 1; +} + +int SerialST4Slave::available(void) { + if (!isActive) return 0; + + int a = 0; + noInterrupts(); + for (uint8_t b = recv_head; recv_buffer[b] != (char)0; b++) a++; + interrupts(); + + return a; +} + +int SerialST4Slave::read(void) { + if (!isActive) return -1; + + noInterrupts(); + int c = recv_buffer[recv_head]; if (c != 0) recv_head++; + interrupts(); + if (c == 0) c = -1; + + return c; +} + +int SerialST4Slave::peek(void) { + if (!isActive) return -1; + + noInterrupts(); + int c = recv_buffer[recv_head]; + interrupts(); + if (c == 0) c = -1; + + return c; +} + +void SerialST4Slave::flush(void) { + if (!isActive) return; + + unsigned long startMs = millis(); + int c; + do { + noInterrupts(); + c = xmit_buffer[xmit_head]; + interrupts(); + } while (c != 0 && (millis()-startMs) < _timeout); +} + +IRAM_ATTR void SerialST4Slave::poll() { + uint8_t clockState = digitalRead(ST4_CLOCK_PIN); + + static int index = 9; + + static bool frame_error = false; + static bool send_error = false; + static bool recv_error = false; + static uint8_t s_parity = 0; + static uint8_t r_parity = 0; + uint8_t this_bit; + + unsigned long timeInMilliseconds = millis(); + if ((long)((timeInMilliseconds - lastTimeInMilliseconds) - (unsigned long)ST4_MAX_BIT_TIME) > 0) { + DLF("WRN: SerialST4.poll(), timeout error"); + index = 9; + } + lastTimeInMilliseconds = timeInMilliseconds; + + if (clockState == LOW) { if (--index == -4) index = 8; } + + switch(index*2 + (int)(clockState == LOW)) { + case 17: // setup for another frame and send start bit (clock LOW) + + if (!send_error) { + data_out = SerialST4.xmit_buffer[SerialST4.xmit_head]; + if (data_out != 0) SerialST4.xmit_head++; + } else { DLF("WRN: SerialST4.poll(), send parity error"); } + + r_parity = 0; + s_parity = 0; + recv_error = false; + send_error = false; + frame_error = false; + digitalWrite(ST4_DATA_OUT_PIN, LOW); + break; + case 16: // recv start bit (clock HIGH) + if (digitalRead(ST4_DATA_IN_PIN) != LOW) { + frame_error = true; + index = 9; + DLF("WRN: SerialST4.poll(), frame/start error"); + } + break; + case 15: case 13: case 11: case 9: case 7: case 5: case 3: case 1: // send data bit (clock LOW) + this_bit = bitRead(data_out, index); + s_parity += this_bit; + digitalWrite(ST4_DATA_OUT_PIN, this_bit); + break; + case 14: case 12: case 10: case 8: case 6: case 4: case 2: case 0: // recv data bit (clock HIGH) + this_bit = digitalRead(ST4_DATA_IN_PIN); + r_parity += this_bit; + bitWrite(data_in, index, this_bit); + break; + case -1: // send parity bit (clock LOW) + digitalWrite(ST4_DATA_OUT_PIN, s_parity&1); + break; + case -2: // recv parity bit (clock HIGH) + if ((r_parity&1) != digitalRead(ST4_DATA_IN_PIN)) recv_error = true; + break; + case -3: // send local parity check (clock LOW) + digitalWrite(ST4_DATA_OUT_PIN, recv_error); + break; + case -4: // recv remote parity (clock HIGH) + if (digitalRead(ST4_DATA_IN_PIN) == HIGH) send_error = true; + break; + case -5: // send stop bit (clock LOW) + digitalWrite(ST4_DATA_OUT_PIN, LOW); + break; + case -6: // recv stop bit (clock HIGH) + if (digitalRead(ST4_DATA_IN_PIN) != LOW) frame_error = true; + + if (!frame_error) { + +// if (data_in >= 32 || data_out >= 32) { +// D("Got "); if (data_in >= 32) { D((char)data_in); } else D(" "); +// D(" Sent "); if (data_out >= 32) { DL((char)data_out); } else DL(" "); +// } + + if (!recv_error) { + if (data_in != 0) { + SerialST4.recv_buffer[SerialST4.recv_tail] = (char)data_in; + SerialST4.recv_buffer[++SerialST4.recv_tail] = (char)0; + } + } else { DLF("WRN: SerialST4.poll(), recv parity error"); } + + } else { DLF("WRN: SerialST4.poll(), frame/stop error"); } + + break; + } +} + +// this routine keeps a 12.5Hz "tone" on the RAe pin (always) but also on the +// RAw pin when the data comms clock from OnStep isn't running +IRAM_ATTR void shcTone() { + static volatile bool tone_state = false; + + if (tone_state) { + tone_state = false; + digitalWrite(ST4_TONE_PIN, HIGH); + if (millis() - SerialST4.lastTimeInMilliseconds > 2000U) { + digitalWrite(ST4_DATA_OUT_PIN, HIGH); + } + } else { + tone_state = true; + digitalWrite(ST4_TONE_PIN, LOW); + if (millis() - SerialST4.lastTimeInMilliseconds > 2000U) { + digitalWrite(ST4_DATA_OUT_PIN, LOW); + } + } +} + +SerialST4Slave SerialST4; + +#endif diff --git a/src/lib/serial/Serial_ST4_Slave.h b/src/lib/serial/Serial_ST4_Slave.h new file mode 100755 index 0000000..bfeeb93 --- /dev/null +++ b/src/lib/serial/Serial_ST4_Slave.h @@ -0,0 +1,84 @@ +// ----------------------------------------------------------------------------------- +// Serial ST4 slave +#pragma once + +/* +ST4 port data communication scheme: + +5V power ---- ESP32-S, Teensy4.0, Teensy3.2, etc. +Gnd --------- + +HC Signal OnStep +RAw ---- Data ---> Recv. data +DEs <--- Clock ---- Clock +DEn <--- Data ---- Send data +RAe ---- 12.5Hz Square wave ---> 100% sure SHC is present, switches DEs & DEn to OUTPUT + +Data is exchanged on clock edges similar to SPI so is timing insensitive (runs with interrupts enabled.) + +One data byte is exchanged (in both directions w/basic error detection and recovery.) A value 0x00 byte +means "no data" and is ignored on both sides. +*/ + +#include "../../Common.h" + +#if defined(SERIAL_ST4_SLAVE) && SERIAL_ST4_SLAVE == ON + +#include + +#if !defined(ST4_W_PIN) && !defined(ST4_S_PIN) && !defined(ST4_N_PIN) && !defined(ST4_E_PIN) + #warning "ST4 interface pins aren't defined, using defaults." + #define ST4_W_PIN 2 + #define ST4_S_PIN 3 + #define ST4_N_PIN 4 + #define ST4_E_PIN 5 +#endif + +void dataClock(); +void shcTone(); + +class SerialST4Slave : public Stream { + public: + void begin(long baudRate); + void end(); + void paused(bool state); + bool active(); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *, size_t); + virtual int available(void); + virtual int read(void); + virtual int peek(void); + virtual void flush(void); + + inline int availableForWrite(void) { return 0; } + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + using Print::write; + + void poll(); + + volatile char xmit_buffer[256] = ""; + volatile uint8_t xmit_head = 0; + volatile uint8_t xmit_tail = 0; + volatile char recv_buffer[256] = ""; + volatile uint8_t recv_tail = 0; + volatile unsigned long lastTimeInMilliseconds = 0; + + private: + uint8_t recv_head = 0; + volatile uint8_t data_in = 0; + volatile uint8_t data_out = 0; + long time_out = 500; + bool isActive = false; + uint8_t handle = 0; +}; + +extern SerialST4Slave SerialST4; +#ifdef SERIAL_ST4 + #undef SERIAL_ST4 +#endif +#define SERIAL_ST4 SerialST4 + +#endif diff --git a/src/lib/softSpi/Pins.h b/src/lib/softSpi/Pins.h new file mode 100755 index 0000000..400eb1b --- /dev/null +++ b/src/lib/softSpi/Pins.h @@ -0,0 +1,5 @@ +// ----------------------------------------------------------------------------------- +// Step driver mode control pins +#pragma once + +#include diff --git a/src/lib/softSpi/SoftSpi.cpp b/src/lib/softSpi/SoftSpi.cpp new file mode 100755 index 0000000..7cf9841 --- /dev/null +++ b/src/lib/softSpi/SoftSpi.cpp @@ -0,0 +1,73 @@ +// ----------------------------------------------------------------------------------- +// Simple software SPI routines (CPOL=1, CPHA=1) just for TMC stepper drivers + +#include "SoftSpi.h" + +#include "../gpioEx/GpioEx.h" + +bool SoftSpi::init(int16_t mosi, int16_t sck, int16_t cs, int16_t miso) { + this->miso = miso; + this->mosi = mosi; + this->sck = sck; + this->cs = cs; + + #if DEBUG == VERBOSE + VF("MSG: SoftSpi, init MOSI="); if (mosi == OFF) { V("OFF"); } else { V(mosi); } + VF(", SCK="); if (sck == OFF) { V("OFF"); } else { V(sck); } + VF(", CS="); if (cs == OFF) { V("OFF"); } else { V(cs); } + VF(", MISO="); if (miso == OFF) { VL("OFF"); } else { VL(miso); } + #endif + + if (mosi == OFF || sck == OFF || cs == OFF) return false; else return true; +} + +void SoftSpi::begin() { + pinModeEx(cs, OUTPUT); + digitalWriteEx(cs, HIGH); + delayMicroseconds(1); + pinMode(sck, OUTPUT); + digitalWriteF(sck, HIGH); + pinModeEx(miso, INPUT_PULLUP); + pinMode(mosi, OUTPUT); + delayMicroseconds(1); + digitalWriteEx(cs, LOW); + delayMicroseconds(1); +} + +void SoftSpi::pause() { + digitalWriteEx(cs, HIGH); + delayMicroseconds(1); + digitalWriteEx(cs, LOW); + delayMicroseconds(1); +} + +void SoftSpi::end() { + digitalWriteEx(cs, HIGH); + delayMicroseconds(1); +} + +uint8_t SoftSpi::transfer(uint8_t data_out) { + uint8_t data_in = 0; + for(int i = 7; i >= 0; i--) { + digitalWriteF(sck, LOW); + digitalWriteF(mosi, bitRead(data_out, i)); + delayMicroseconds(1); + digitalWriteF(sck, HIGH); + if (miso != OFF) bitWrite(data_in, i, digitalReadF(miso)); else bitWrite(data_in, i, 0); + delayMicroseconds(1); + } + return data_in; +} + +uint32_t SoftSpi::transfer32(uint32_t data_out) { + uint32_t data_in = 0; + for(int i = 31; i >= 0; i--) { + digitalWriteF(sck, LOW); + digitalWriteF(mosi, bitRead(data_out, i)); + delayMicroseconds(1); + digitalWriteF(sck, HIGH); + if (miso != OFF) bitWrite(data_in, i, digitalReadF(miso)); else bitWrite(data_in, i, 0); + delayMicroseconds(1); + } + return data_in; +} diff --git a/src/lib/softSpi/SoftSpi.h b/src/lib/softSpi/SoftSpi.h new file mode 100755 index 0000000..93a8093 --- /dev/null +++ b/src/lib/softSpi/SoftSpi.h @@ -0,0 +1,26 @@ +// ----------------------------------------------------------------------------------- +// Simple software SPI routines (CPOL=1, CPHA=1) just for TMC stepper drivers +#pragma once + +#include "../../Common.h" + +class SoftSpi { + public: + // check pins and report status + bool init(int16_t mosi, int16_t sck, int16_t cs, int16_t miso); + // setup pins and activate CS + void begin(); + // cycle CS to reset conversation + void pause(); + // set CS high to disable conversation + void end(); + // send/receive an 8 bit value + uint8_t transfer(uint8_t data_out); + // send/receive an 32 bit value + uint32_t transfer32(uint32_t data_out); + // returns true if this interface only supports sending data + bool outOnly() { return miso == OFF; } + + private: + int16_t miso, mosi, sck, cs; +}; diff --git a/src/lib/sound/Sound.cpp b/src/lib/sound/Sound.cpp new file mode 100755 index 0000000..135be8c --- /dev/null +++ b/src/lib/sound/Sound.cpp @@ -0,0 +1,67 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// Sound + +#include "Sound.h" + +#ifdef STATUS_BUZZER + +#include "../tasks/OnTask.h" +#include "../gpioEx/GpioEx.h" + +void Sound::init() { + #if STATUS_BUZZER == ON + pinModeEx(STATUS_BUZZER_PIN, OUTPUT); + digitalWriteEx(STATUS_BUZZER_PIN, !STATUS_BUZZER_ON_STATE); + #endif + ready = true; +} + +#if STATUS_BUZZER == ON + uint8_t _buzzerHandle = 0; + void buzzerOff() { + digitalWriteEx(STATUS_BUZZER_PIN, !STATUS_BUZZER_ON_STATE); + tasks.setDurationComplete(_buzzerHandle); + _buzzerHandle = 0; + } +#endif + +void Sound::alert() { + if (ready && enabled) { + #if STATUS_BUZZER == ON + digitalWriteEx(STATUS_BUZZER_PIN, STATUS_BUZZER_ON_STATE); + if (_buzzerHandle) tasks.remove(_buzzerHandle); + _buzzerHandle = tasks.add(1000, 0, false, 6, buzzerOff); + #endif + #if STATUS_BUZZER >= 0 + tone(STATUS_BUZZER_PIN, STATUS_BUZZER, 1000); + #endif + } +} + +void Sound::beep() { + if (ready && enabled) { + #if STATUS_BUZZER == ON + digitalWriteEx(STATUS_BUZZER_PIN, STATUS_BUZZER_ON_STATE); + if (_buzzerHandle) tasks.remove(_buzzerHandle); + _buzzerHandle = tasks.add(250, 0, false, 6, buzzerOff); + #endif + #if STATUS_BUZZER >= 0 + tone(STATUS_BUZZER_PIN, STATUS_BUZZER, 250); + #endif + } +} + +void Sound::click() { + if (ready && enabled) { + #if STATUS_BUZZER == ON + digitalWriteEx(STATUS_BUZZER_PIN, STATUS_BUZZER_ON_STATE); + if (_buzzerHandle) tasks.remove(_buzzerHandle); + _buzzerHandle = tasks.add(50, 0, false, 6, buzzerOff); + #endif + #if STATUS_BUZZER >= 0 + tone(STATUS_BUZZER_PIN, STATUS_BUZZER, 50); + #endif + } +} + +#endif diff --git a/src/lib/sound/Sound.h b/src/lib/sound/Sound.h new file mode 100755 index 0000000..66ec7f4 --- /dev/null +++ b/src/lib/sound/Sound.h @@ -0,0 +1,21 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// Sound +#pragma once + +#include "../../Common.h" + +#ifdef STATUS_BUZZER + +class Sound { + public: + void init(); // get sound pins ready + void alert(); // sound/buzzer + void click(); // sound/beep + void beep(); // sound/click + + bool enabled = STATUS_BUZZER_DEFAULT == ON; + private: + bool ready = false; +}; + +#endif diff --git a/src/lib/tasks/HAL_ATMEGA328_HWTIMER.h b/src/lib/tasks/HAL_ATMEGA328_HWTIMER.h new file mode 100755 index 0000000..c114889 --- /dev/null +++ b/src/lib/tasks/HAL_ATMEGA328_HWTIMER.h @@ -0,0 +1,81 @@ +//-------------------------------------------------------------------------------------------------- +// ATMEGA328 hardware timers + +// provides one 16 bit timer (Timer1) with 16 bit software pre-scaler, running at 2MHz +// timer configured as ~0 to 0.032 seconds (granularity of timer is 0.5uS) /8 pre-scaler + +#define TIMER_RATE_MHZ 2L // AtMega328 motor timers run at 2 MHz +#define TIMER_RATE_16MHZ_TICKS 8L // 16L/TIMER_RATE_MHZ, 8x slower than the default 16MHz "sub-micros" + +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) + // prepare hw timer for interval in sub-microseconds (1/16us) + volatile uint16_t _nextPeriod1 = 2000, _nextPeriod2 = 2000, _nextPeriod3 = 2000, _nextPeriod4 = 2000; + volatile uint16_t _nextRep1 = 0, _nextRep2 = 0, _nextRep3 = 0, _nextRep4 = 0; + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { + // maximum time is about 134 seconds for this design + uint32_t counts, reps = 0; + if (period != 0 && period <= 2144000000) { + if (period < 16) period = 16; // minimum time is 1us + period /= TIMER_RATE_16MHZ_TICKS; + reps = period/65536 + 1; + counts = period/reps - 1; // has -1 since this is dropped right into a timer register + } else counts = 2000; // set for a 1ms period, stopped + + noInterrupts(); + _nextPeriod1 = counts; _nextRep1 = reps; + interrupts(); + } +#else + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { (void)(num); (void)(period); } +#endif + +#ifdef TASKS_HWTIMER1_ENABLE + void (*HAL_HWTIMER1_FUN)() = NULL; // points to task/process callback function + + bool HAL_HWTIMER1_INIT(uint8_t priority) { + noInterrupts(); + OCR1A = 2000; // startup one millisecond + TCCR1B = (1 << WGM12) | (1 << CS11); + TCCR1A = 0; + TIMSK1 = (1 << OCIE1A); + interrupts(); + return true; + } + + void HAL_HWTIMER1_DONE() { + HAL_HWTIMER1_FUN = NULL; + TIMSK1 = 0; + } + + #define HAL_HWTIMER1_SET_PERIOD() (OCR1A = _nextPeriod1) + ISR(TIMER1_COMPA_vect) { + TASKS_HWTIMER1_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep1 > 1) { count++; if (count%_nextRep1 != 0) goto done; } + if (_nextRep1) HAL_HWTIMER1_FUN(); + HAL_HWTIMER1_SET_PERIOD(); + done: {} + TASKS_HWTIMER1_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER2_ENABLE + void (*HAL_HWTIMER2_FUN)() = NULL; + #define HAL_HWTIMER2_SET_PERIOD() + bool HAL_HWTIMER2_INIT(uint8_t priority) { return false; } + void HAL_HWTIMER2_DONE() { } +#endif + +#ifdef TASKS_HWTIMER3_ENABLE + void (*HAL_HWTIMER3_FUN)() = NULL; + #define HAL_HWTIMER3_SET_PERIOD() + bool HAL_HWTIMER3_INIT(uint8_t priority) { return false;} + void HAL_HWTIMER3_DONE() { } +#endif + +#ifdef TASKS_HWTIMER4_ENABLE + void (*HAL_HWTIMER4_FUN)() = NULL; + #define HAL_HWTIMER4_SET_PERIOD() + bool HAL_HWTIMER4_INIT(uint8_t priority) { return false; } + void HAL_HWTIMER4_DONE() { } +#endif \ No newline at end of file diff --git a/src/lib/tasks/HAL_EMPTY_HWTIMER.h b/src/lib/tasks/HAL_EMPTY_HWTIMER.h new file mode 100755 index 0000000..c46b738 --- /dev/null +++ b/src/lib/tasks/HAL_EMPTY_HWTIMER.h @@ -0,0 +1,139 @@ +//-------------------------------------------------------------------------------------------------- +// Empty hardware timers + +// minimum structure + + // a useful example for 16bit counter/timer hardware to +#define TIMER_RATE_MHZ 4L // allow sufficient resolution (0.25us) and range (134 seconds) +#define TIMER_RATE_16MHZ_TICKS 4L // timer hardware 4x slower than the default 16MHz "sub-micros" (4MHz) + +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) + // prepare hw timer for interval in sub-microseconds (1/16us) + volatile uint16_t _nextPeriod1, _nextPeriod2, _nextPeriod3, _nextPeriod4; + volatile uint16_t _nextRep1, _nextRep2, _nextRep3, _nextRep4; + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { + // maximum time is about 134 seconds for this design + uint32_t counts, reps = 0; + if (period != 0 && period <= 2144000000) { + if (period < 16) period = 16; // minimum time is 1us + period /= TIMER_RATE_16MHZ_TICKS; + reps = period/65536 + 1; + counts = period/reps - 1; // has -1 since this is dropped right into a timer register + } else counts = 2000; // set for a 1ms period, stopped + + noInterrupts(); + switch (num) { + case 1: _nextPeriod1 = counts; _nextRep1 = reps; break; + case 2: _nextPeriod2 = counts; _nextRep2 = reps; break; + case 3: _nextPeriod3 = counts; _nextRep3 = reps; break; + case 4: _nextPeriod4 = counts; _nextRep4 = reps; break; + } + interrupts(); + } +#else + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { (void)(num); (void)(period); } +#endif + +#ifdef TASKS_HWTIMER1_ENABLE + void (*HAL_HWTIMER1_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER1_WRAPPER(void); // forward definition of the timer ISR + + bool HAL_HWTIMER1_INIT(uint8_t priority) { + // <--- code to init/start timer goes here + return true; + } + + void HAL_HWTIMER1_DONE() { + // <--- code to stop timer goes here + HAL_HWTIMER1_FUN = NULL; + } + + #define HAL_HWTIMER1_SET_PERIOD() {} // <--- code to set timer period goes here + void HAL_HWTIMER1_WRAPPER() { + TASKS_HWTIMER1_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep1 > 1) { count++; if (count%_nextRep1 != 0) goto done; } + if (_nextRep1) HAL_HWTIMER1_FUN(); + HAL_HWTIMER1_SET_PERIOD(); + done: {} + TASKS_HWTIMER1_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER2_ENABLE + void (*HAL_HWTIMER2_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER2_WRAPPER(void); // forward definition of the timer ISR + + bool HAL_HWTIMER2_INIT(uint8_t priority) { + // <--- code to init/start timer goes here + return true; + } + + void HAL_HWTIMER2_DONE() { + // <--- code to stop timer goes here + HAL_HWTIMER2_FUN = NULL; + } + + #define HAL_HWTIMER2_SET_PERIOD() {} // <--- code to set timer period goes here + void HAL_HWTIMER2_WRAPPER() { + TASKS_HWTIMER2_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep2 > 1) { count++; if (count%_nextRep2 != 0) goto done; } + if (_nextRep2) HAL_HWTIMER2_FUN(); + HAL_HWTIMER2_SET_PERIOD(); + done: {} + TASKS_HWTIMER2_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER3_ENABLE + void (*HAL_HWTIMER3_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER3_WRAPPER(void); // forward definition of the timer ISR + + bool HAL_HWTIMER3_INIT(uint8_t priority) { + // <--- code to init/start timer goes here + return true; + } + + void HAL_HWTIMER3_DONE() { + // <--- code to stop timer goes here + HAL_HWTIMER3_FUN = NULL; + } + + #define HAL_HWTIMER3_SET_PERIOD() {} // <--- code to set timer period goes here + void HAL_HWTIMER3_WRAPPER() { + TASKS_HWTIMER3_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep3 > 1) { count++; if (count%_nextRep3 != 0) goto done; } + if (_nextRep3) HAL_HWTIMER3_FUN(); + HAL_HWTIMER3_SET_PERIOD(); + done: {} + TASKS_HWTIMER3_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER4_ENABLE + void (*HAL_HWTIMER4_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER4_WRAPPER(void); // forward definition of the timer ISR + + bool HAL_HWTIMER4_INIT(uint8_t priority) { + // init code goes here + return false; + } + + void HAL_HWTIMER4_DONE() { + // <--- code to stop timer goes here + HAL_HWTIMER4_FUN = NULL; + } + + #define HAL_HWTIMER4_SET_PERIOD() {} // <--- code to set timer period goes here + void HAL_HWTIMER4_WRAPPER() { + TASKS_HWTIMER4_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep4 > 1) { count++; if (count%_nextRep4 != 0) goto done; } + if (_nextRep4) HAL_HWTIMER4_FUN(); + HAL_HWTIMER4_SET_PERIOD(); + done: {} + TASKS_HWTIMER4_PROFILER_SUFFIX; + } +#endif diff --git a/src/lib/tasks/HAL_ESP32_HWTIMER.h b/src/lib/tasks/HAL_ESP32_HWTIMER.h new file mode 100755 index 0000000..e6a414a --- /dev/null +++ b/src/lib/tasks/HAL_ESP32_HWTIMER.h @@ -0,0 +1,216 @@ +//-------------------------------------------------------------------------------------------------- +// ESP32 hardware timers + +// mux to block code from running simultaniously +portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; + +// provides two 32 bit interval timers with 16 bit software pre-scalers, running at 16MHz +// each timer configured as ~0 to x seconds (granularity of timer is 0.062uS) +// timer use doesn't collide with PWM or tone() on these platforms + +// notes for future improvements: +// the ESP32 timers flexible enough that we can get rid of the software pre-scalers +// also, we can more directly access the timer objects for example: +// #define HAL_HWTIMER1_SET_PERIOD() itimer1->dev->alarm_low = _nextPeriod1 + +#define TIMER_RATE_MHZ 16L // ESP32 motor timers run at 16MHz so use full resolution +#define TIMER_RATE_16MHZ_TICKS 1L // 16L/TIMER_RATE_MHZ, for the default 16MHz "sub-micros" (16MHz) + +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) + // prepare hw timer for interval in sub-microseconds (1/16us) + volatile uint32_t _nextPeriod1 = 16000, _nextPeriod2 = 16000, _nextPeriod3 = 16000, _nextPeriod4 = 16000; + volatile uint16_t _nextRep1 = 0, _nextRep2 = 0, _nextRep3 = 0, _nextRep4 = 0; + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { + // maximum time is about 134 seconds for this design + uint32_t counts, reps = 0; + if (period != 0 && period <= 2144000000) { + if (period < 16) period = 16; // minimum time is 1us + period /= TIMER_RATE_16MHZ_TICKS; + reps = period/4194304UL + 1; + counts = period/reps; + } else counts = 16000; // set for a 1ms period, stopped + + noInterrupts(); + switch (num) { + case 1: _nextPeriod1 = counts; _nextRep1 = reps; break; + case 2: _nextPeriod2 = counts; _nextRep2 = reps; break; + case 3: _nextPeriod3 = counts; _nextRep3 = reps; break; + case 4: _nextPeriod4 = counts; _nextRep4 = reps; break; + } + interrupts(); + } +#else + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { (void)(num); (void)(period); } +#endif + +#ifdef TASKS_HWTIMER1_ENABLE + hw_timer_t *itimer1 = NULL; + + void (*HAL_HWTIMER1_FUN)() = NULL; // points to task/process callback function + IRAM_ATTR void HAL_HWTIMER1_WRAPPER(); + + bool HAL_HWTIMER1_INIT(uint8_t priority) { + // timer#, divider, count up + itimer1 = timerBegin(0, 5, true); // ESP32 timer frequency is 80MHz, 80/5 = 16MHz + timerAttachInterrupt(itimer1, &HAL_HWTIMER1_WRAPPER, true); + timerAlarmWrite(itimer1, 1000*16, true); // startup one millisecond + timerAlarmEnable(itimer1); + return true; + } + + void HAL_HWTIMER1_DONE() { + HAL_HWTIMER1_FUN = NULL; + timerDetachInterrupt(itimer1); + timerEnd(itimer1); + } + + #define HAL_HWTIMER1_SET_PERIOD() timerAlarmWrite(itimer1, _nextPeriod1, true) + IRAM_ATTR void HAL_HWTIMER1_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER1_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep1 > 1) { count++; if (count % _nextRep1 != 0) goto done; } + if (_nextRep1) HAL_HWTIMER1_FUN(); + HAL_HWTIMER1_SET_PERIOD(); + done: {} + TASKS_HWTIMER1_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +#ifdef TASKS_HWTIMER2_ENABLE + hw_timer_t *itimer2 = NULL; + + void (*HAL_HWTIMER2_FUN)() = NULL; // points to task/process callback function + IRAM_ATTR void HAL_HWTIMER2_WRAPPER(); + + bool HAL_HWTIMER2_INIT(uint8_t priority) { + // timer#, divider, count up + itimer2 = timerBegin(1, 5, true); // ESP32 timer frequency is 80MHz, 80/5 = 16MHz + timerAttachInterrupt(itimer2, &HAL_HWTIMER2_WRAPPER, true); + timerAlarmWrite(itimer2, 1000*16, true); // startup one millisecond + timerAlarmEnable(itimer2); + return true; + } + + void HAL_HWTIMER2_DONE() { + HAL_HWTIMER2_FUN = NULL; + timerDetachInterrupt(itimer2); + timerEnd(itimer2); + } + + #define HAL_HWTIMER2_SET_PERIOD() timerAlarmWrite(itimer2, _nextPeriod2, true) + IRAM_ATTR void HAL_HWTIMER2_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER2_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep2 > 1) { count++; if (count % _nextRep2 != 0) goto done; } + if (_nextRep2) HAL_HWTIMER2_FUN(); + HAL_HWTIMER2_SET_PERIOD(); + done: {} + TASKS_HWTIMER2_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +#ifdef TASKS_HWTIMER3_ENABLE + hw_timer_t *itimer3 = NULL; + + void (*HAL_HWTIMER3_FUN)() = NULL; // points to task/process callback function + IRAM_ATTR void HAL_HWTIMER3_WRAPPER(); + + bool HAL_HWTIMER3_INIT(uint8_t priority) { + // timer#, divider, count up + itimer3 = timerBegin(2, 5, true); // ESP32 timer frequency is 80MHz, 80/5 = 16MHz + timerAttachInterrupt(itimer3, &HAL_HWTIMER3_WRAPPER, true); + timerAlarmWrite(itimer3, 1000*16, true); // startup one millisecond + timerAlarmEnable(itimer3); + return true; + } + + void HAL_HWTIMER3_DONE() { + HAL_HWTIMER3_FUN = NULL; + timerDetachInterrupt(itimer3); + timerEnd(itimer3); + } + + #define HAL_HWTIMER3_SET_PERIOD() timerAlarmWrite(itimer3, _nextPeriod3, true) + IRAM_ATTR void HAL_HWTIMER3_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER3_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep3 > 1) { count++; if (count % _nextRep3 != 0) goto done; } + if (_nextRep3) HAL_HWTIMER3_FUN(); + HAL_HWTIMER3_SET_PERIOD(); + done: {} + TASKS_HWTIMER3_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +#ifdef TASKS_HWTIMER4_ENABLE + hw_timer_t *itimer4 = NULL; + + void (*HAL_HWTIMER4_FUN)() = NULL; // points to task/process callback function + IRAM_ATTR void HAL_HWTIMER4_WRAPPER(); + + bool HAL_HWTIMER4_INIT(uint8_t priority) { + // timer#, divider, count up + itimer4 = timerBegin(3, 5, true); // ESP32 timer frequency is 80MHz, 80/5 = 16MHz + timerAttachInterrupt(itimer4, &HAL_HWTIMER4_WRAPPER, true); + timerAlarmWrite(itimer4, 1000*16, true); // startup one millisecond + timerAlarmEnable(itimer4); + return true; + } + + void HAL_HWTIMER4_DONE() { + HAL_HWTIMER4_FUN = NULL; + timerDetachInterrupt(itimer4); + timerEnd(itimer4); + } + + #define HAL_HWTIMER4_SET_PERIOD() timerAlarmWrite(itimer4, _nextPeriod4, true) + IRAM_ATTR void HAL_HWTIMER4_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER4_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep4 > 1) { count++; if (count % _nextRep4 != 0) goto done; } + if (_nextRep4) HAL_HWTIMER4_FUN(); + HAL_HWTIMER4_SET_PERIOD(); + done: {} + TASKS_HWTIMER4_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +// disable hardware timer based tasks by clearing interrupt flag(s) +void timerAlarmsDisable() { +#ifdef TASKS_HWTIMER1_ENABLE + if (itimer1 != NULL) timerAlarmDisable(itimer1); +#endif +#ifdef TASKS_HWTIMER2_ENABLE + if (itimer2 != NULL) timerAlarmDisable(itimer2); +#endif +#ifdef TASKS_HWTIMER3_ENABLE + if (itimer3 != NULL) timerAlarmDisable(itimer3); +#endif +#ifdef TASKS_HWTIMER4_ENABLE + if (itimer4 != NULL) timerAlarmDisable(itimer4); +#endif +} + +// enable hardware timer based tasks by setting interrupt flag(s) +void timerAlarmsEnable() { +#ifdef TASKS_HWTIMER1_ENABLE + if (itimer1 != NULL) timerAlarmEnable(itimer1); +#endif +#ifdef TASKS_HWTIMER2_ENABLE + if (itimer2 != NULL) timerAlarmEnable(itimer2); +#endif +#ifdef TASKS_HWTIMER3_ENABLE + if (itimer3 != NULL) timerAlarmEnable(itimer3); +#endif +#ifdef TASKS_HWTIMER4_ENABLE + if (itimer4 != NULL) timerAlarmEnable(itimer4); +#endif +} diff --git a/src/lib/tasks/HAL_ESP32_V3_HWTIMER.h b/src/lib/tasks/HAL_ESP32_V3_HWTIMER.h new file mode 100755 index 0000000..42e43bb --- /dev/null +++ b/src/lib/tasks/HAL_ESP32_V3_HWTIMER.h @@ -0,0 +1,208 @@ +//-------------------------------------------------------------------------------------------------- +// ESP32 hardware timers + +// mux to block code from running simultaniously +portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; + +// provides two 32 bit interval timers with 16 bit software pre-scalers, running at 16MHz +// each timer configured as ~0 to x seconds (granularity of timer is 0.062uS) +// timer use doesn't collide with PWM or tone() on these platforms + +// notes for future improvements: +// the ESP32 timers flexible enough that we can get rid of the software pre-scalers +// also, we can more directly access the timer objects for example: +// #define HAL_HWTIMER1_SET_PERIOD() itimer1->dev->alarm_low = _nextPeriod1 + +#define TIMER_RATE_MHZ 16L // ESP32 motor timers run at 16MHz so use full resolution +#define TIMER_RATE_16MHZ_TICKS 1L // 16L/TIMER_RATE_MHZ, for the default 16MHz "sub-micros" (16MHz) + +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) + // prepare hw timer for interval in sub-microseconds (1/16us) + volatile uint32_t _nextPeriod1 = 16000, _nextPeriod2 = 16000, _nextPeriod3 = 16000, _nextPeriod4 = 16000; + volatile uint16_t _nextRep1 = 0, _nextRep2 = 0, _nextRep3 = 0, _nextRep4 = 0; + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { + // maximum time is about 134 seconds for this design + uint32_t counts, reps = 0; + if (period != 0 && period <= 2144000000) { + if (period < 16) period = 16; // minimum time is 1us + period /= TIMER_RATE_16MHZ_TICKS; + reps = period/4194304UL + 1; + counts = period/reps; + } else counts = 16000; // set for a 1ms period, stopped + + noInterrupts(); + switch (num) { + case 1: _nextPeriod1 = counts; _nextRep1 = reps; break; + case 2: _nextPeriod2 = counts; _nextRep2 = reps; break; + case 3: _nextPeriod3 = counts; _nextRep3 = reps; break; + case 4: _nextPeriod4 = counts; _nextRep4 = reps; break; + } + interrupts(); + } +#else + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { (void)(num); (void)(period); } +#endif + +#ifdef TASKS_HWTIMER1_ENABLE + hw_timer_t *itimer1 = NULL; + + void (*HAL_HWTIMER1_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER1_WRAPPER(); + + bool HAL_HWTIMER1_INIT(uint8_t priority) { + itimer1 = timerBegin(16000000); // ESP32 timer frequency is 16MHz + timerAttachInterrupt(itimer1, &HAL_HWTIMER1_WRAPPER); + timerAlarm(itimer1, 1000*16, true, 0); // startup one millisecond + return true; + } + + void HAL_HWTIMER1_DONE() { + HAL_HWTIMER1_FUN = NULL; + timerDetachInterrupt(itimer1); + timerEnd(itimer1); + } + + #define HAL_HWTIMER1_SET_PERIOD() timerAlarm(itimer1, _nextPeriod1, true, 0) + IRAM_ATTR void HAL_HWTIMER1_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER1_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep1 > 1) { count++; if (count % _nextRep1 != 0) goto done; } + if (_nextRep1) HAL_HWTIMER1_FUN(); + HAL_HWTIMER1_SET_PERIOD(); + done: {} + TASKS_HWTIMER1_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +#ifdef TASKS_HWTIMER2_ENABLE + hw_timer_t *itimer2 = NULL; + + void (*HAL_HWTIMER2_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER2_WRAPPER(); + + bool HAL_HWTIMER2_INIT(uint8_t priority) { + itimer2 = timerBegin(16000000); // ESP32 timer frequency is 16MHz + timerAttachInterrupt(itimer2, &HAL_HWTIMER2_WRAPPER); + timerAlarm(itimer2, 1000*16, true, 0); // startup one millisecond + return true; + } + + void HAL_HWTIMER2_DONE() { + HAL_HWTIMER2_FUN = NULL; + timerDetachInterrupt(itimer2); + timerEnd(itimer2); + } + + #define HAL_HWTIMER2_SET_PERIOD() timerAlarm(itimer2, _nextPeriod2, true, 0) + IRAM_ATTR void HAL_HWTIMER2_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER2_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep2 > 1) { count++; if (count % _nextRep2 != 0) goto done; } + if (_nextRep2) HAL_HWTIMER2_FUN(); + HAL_HWTIMER2_SET_PERIOD(); + done: {} + TASKS_HWTIMER2_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +#ifdef TASKS_HWTIMER3_ENABLE + hw_timer_t *itimer3 = NULL; + + void (*HAL_HWTIMER3_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER3_WRAPPER(); + + bool HAL_HWTIMER3_INIT(uint8_t priority) { + itimer3 = timerBegin(16000000); // ESP32 timer frequency is 16MHz + timerAttachInterrupt(itimer3, &HAL_HWTIMER3_WRAPPER); + timerAlarm(itimer3, 1000*16, true, 0); // startup one millisecond + return true; + } + + void HAL_HWTIMER3_DONE() { + HAL_HWTIMER3_FUN = NULL; + timerDetachInterrupt(itimer3); + timerEnd(itimer3); + } + + #define HAL_HWTIMER3_SET_PERIOD() timerAlarm(itimer3, _nextPeriod3, true, 0) + IRAM_ATTR void HAL_HWTIMER3_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER3_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep3 > 1) { count++; if (count % _nextRep3 != 0) goto done; } + if (_nextRep3) HAL_HWTIMER3_FUN(); + HAL_HWTIMER3_SET_PERIOD(); + done: {} + TASKS_HWTIMER3_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +#ifdef TASKS_HWTIMER4_ENABLE + hw_timer_t *itimer4 = NULL; + + void (*HAL_HWTIMER4_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER4_WRAPPER(); + + bool HAL_HWTIMER4_INIT(uint8_t priority) { + itimer4 = timerBegin(16000000); // ESP32 timer frequency is 16MHz + timerAttachInterrupt(itimer4, &HAL_HWTIMER4_WRAPPER); + timerAlarm(itimer4, 1000*16, true, 0); // startup one millisecond + return true; + } + + void HAL_HWTIMER4_DONE() { + HAL_HWTIMER4_FUN = NULL; + timerDetachInterrupt(itimer4); + timerEnd(itimer4); + } + + #define HAL_HWTIMER4_SET_PERIOD() timerAlarm(itimer4, _nextPeriod4, true, 0) + IRAM_ATTR void HAL_HWTIMER4_WRAPPER() { + portENTER_CRITICAL_ISR(&timerMux); + TASKS_HWTIMER4_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep4 > 1) { count++; if (count % _nextRep4 != 0) goto done; } + if (_nextRep4) HAL_HWTIMER4_FUN(); + HAL_HWTIMER4_SET_PERIOD(); + done: {} + TASKS_HWTIMER4_PROFILER_SUFFIX; + portEXIT_CRITICAL_ISR ( &timerMux ); + } +#endif + +// disable hardware timer based tasks by clearing interrupt flag(s) +void timerAlarmsDisable() { +#ifdef TASKS_HWTIMER1_ENABLE + if (itimer1 != NULL) timerStop(itimer1); +#endif +#ifdef TASKS_HWTIMER2_ENABLE + if (itimer2 != NULL) timerStop(itimer2); +#endif +#ifdef TASKS_HWTIMER3_ENABLE + if (itimer3 != NULL) timerStop(itimer3); +#endif +#ifdef TASKS_HWTIMER4_ENABLE + if (itimer4 != NULL) timerStop(itimer4); +#endif +} + +// enable hardware timer based tasks by setting interrupt flag(s) +void timerAlarmsEnable() { +#ifdef TASKS_HWTIMER1_ENABLE + if (itimer1 != NULL) timerStart(itimer1); +#endif +#ifdef TASKS_HWTIMER2_ENABLE + if (itimer2 != NULL) timerStart(itimer2); +#endif +#ifdef TASKS_HWTIMER3_ENABLE + if (itimer3 != NULL) timerStart(itimer3); +#endif +#ifdef TASKS_HWTIMER4_ENABLE + if (itimer4 != NULL) timerStart(itimer4); +#endif +} diff --git a/src/lib/tasks/HAL_HWTIMERS.h b/src/lib/tasks/HAL_HWTIMERS.h new file mode 100755 index 0000000..d85552a --- /dev/null +++ b/src/lib/tasks/HAL_HWTIMERS.h @@ -0,0 +1,54 @@ +//-------------------------------------------------------------------------------------------------- +// Selects hardware timer HAL according to platform + +// these must be present even if the hardware timer isn't brought in +#ifndef TASKS_HWTIMER1_ENABLE + void (*HAL_HWTIMER1_FUN)() = NULL; + #define HAL_HWTIMER1_SET_PERIOD() + bool HAL_HWTIMER1_INIT(uint8_t priority) { (void)(priority); return false; } + void HAL_HWTIMER1_DONE() { } +#endif +#ifndef TASKS_HWTIMER2_ENABLE + void (*HAL_HWTIMER2_FUN)() = NULL; + #define HAL_HWTIMER2_SET_PERIOD() + bool HAL_HWTIMER2_INIT(uint8_t priority) { (void)(priority); return false; } + void HAL_HWTIMER2_DONE() { } +#endif +#ifndef TASKS_HWTIMER3_ENABLE + void (*HAL_HWTIMER3_FUN)() = NULL; + #define HAL_HWTIMER3_SET_PERIOD() + bool HAL_HWTIMER3_INIT(uint8_t priority) { (void)(priority); return false;} + void HAL_HWTIMER3_DONE() { } +#endif +#ifndef TASKS_HWTIMER4_ENABLE + void (*HAL_HWTIMER4_FUN)() = NULL; + #define HAL_HWTIMER4_SET_PERIOD() + bool HAL_HWTIMER4_INIT(uint8_t priority) { (void)(priority); return false; } + void HAL_HWTIMER4_DONE() { } +#endif + +// bring in hardware timer support +#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + #include "HAL_ATMEGA328_HWTIMER.h" +#elif defined(__AVR_ATmega1280__) ||defined(__AVR_ATmega2560__) + #include "HAL_MEGA2560_HWTIMER.h" +#elif defined(STM32H723xx) || defined(STM32H743xx) || defined(STM32H750xx) || \ + defined(STM32F446xx) || defined(STM32F411xE) || \ + defined(STM32F401xC) || defined(STM32F407xx) || \ + defined(STM32F303xC) || defined(STM32F103xB) + #include "HAL_STM32_HWTIMER.h" +// Teensy3.0, 3.1, 3.2, 3.5, 3.6, 4.0, and 4.1 +#elif defined(_mk20dx128_h_) || defined(__MK20DX128__) || \ + defined(__MK20DX256__) || defined(__MK64FX512__) || \ + defined(__MK66FX1M0__) || defined(__IMXRT1052__) || \ + defined(__IMXRT1062__) + #include "HAL_TEENSY_HWTIMER.h" +#elif defined(ESP32) + #if ESP_ARDUINO_VERSION >= 0x30000 + #include "HAL_ESP32_V3_HWTIMER.h" + #else + #include "HAL_ESP32_HWTIMER.h" + #endif +#else + #include "HAL_EMPTY_HWTIMER.h" +#endif diff --git a/src/lib/tasks/HAL_MEGA2560_HWTIMER.h b/src/lib/tasks/HAL_MEGA2560_HWTIMER.h new file mode 100755 index 0000000..641b55f --- /dev/null +++ b/src/lib/tasks/HAL_MEGA2560_HWTIMER.h @@ -0,0 +1,162 @@ +//-------------------------------------------------------------------------------------------------- +// MEGA2560 hardware timers + +// provides four 16 bit timers (Timer1, Timer3, Timer4, Timer5) with 16 bit software pre-scalers, running at 2MHz +// each timer configured as ~0 to 0.032 seconds (granularity of timer is 0.5uS) /8 pre-scaler +// note that timer use my collide with PWM or tone() depending on which pin is being controlled + +#define TIMER_RATE_MHZ 2L // Mega2560 motor timers run at 2 MHz +#define TIMER_RATE_16MHZ_TICKS 8L // 16L/TIMER_RATE_MHZ, 8x slower than the default 16MHz "sub-micros" + +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) + // prepare hw timer for interval in sub-microseconds (1/16us) + volatile uint16_t _nextPeriod1 = 2000, _nextPeriod2 = 2000, _nextPeriod3 = 2000, _nextPeriod4 = 2000; + volatile uint16_t _nextRep1 = 0, _nextRep2 = 0, _nextRep3 = 0, _nextRep4 = 0; + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { + // maximum time is about 134 seconds for this design + uint32_t counts, reps=0; + if (period != 0 && period <= 2144000000) { + if (period < 16) period = 16; // minimum time is 1us + period /= TIMER_RATE_16MHZ_TICKS; + reps = period/65536 + 1; + counts = period/reps - 1; // has -1 since this is dropped right into a timer register + } else counts = 2000; // set for a 1ms period, stopped + + noInterrupts(); + switch (num) { + case 1: _nextPeriod1 = counts; _nextRep1 = reps; break; + case 2: _nextPeriod2 = counts; _nextRep2 = reps; break; + case 3: _nextPeriod3 = counts; _nextRep3 = reps; break; + case 4: _nextPeriod4 = counts; _nextRep4 = reps; break; + } + interrupts(); + } +#else + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { (void)(num); (void)(period); } +#endif + +// start hw timers +#ifdef TASKS_HWTIMER1_ENABLE + void (*HAL_HWTIMER1_FUN)() = NULL; // points to task/process callback function + + bool HAL_HWTIMER1_INIT(uint8_t priority) { + // period 0.5... us per count 32.767 ms max + (void)priority; + noInterrupts(); + OCR1A = 2000; // startup one millisecond + TCCR1B = (1 << WGM12) | (1 << CS11); + TCCR1A = 0; + TIMSK1 = (1 << OCIE1A); + interrupts(); + return true; + } + + void HAL_HWTIMER1_DONE() { + TIMSK1 = 0; // stop hw timer (clear register interrupt flag) + HAL_HWTIMER1_FUN = NULL; + } + + #define HAL_HWTIMER1_SET_PERIOD() OCR1A = _nextPeriod1 + ISR(TIMER1_COMPA_vect) { + TASKS_HWTIMER1_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep1 > 1) { count++; if (count%_nextRep1 != 0) goto done; } + if (_nextRep1) HAL_HWTIMER1_FUN(); + HAL_HWTIMER1_SET_PERIOD(); + done: {} + TASKS_HWTIMER1_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER2_ENABLE + void (*HAL_HWTIMER2_FUN)() = NULL; // points to task/process callback function + + bool HAL_HWTIMER2_INIT(uint8_t priority) { + (void)priority; + noInterrupts(); + OCR3A = 2000; // startup one millisecond + TCCR3B = (1 << WGM12) | (1 << CS11); + TCCR3A = 0; + TIMSK3 = (1 << OCIE3A); + interrupts(); + return true; + } + + void HAL_HWTIMER2_DONE() { + TIMSK3 = 0; // stop hw timer (clear register interrupt flag) + HAL_HWTIMER2_FUN = NULL; + } + + #define HAL_HWTIMER2_SET_PERIOD() OCR3A = _nextPeriod2 + ISR(TIMER3_COMPA_vect) { + TASKS_HWTIMER2_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep2 > 1) { count++; if (count%_nextRep2 != 0) goto done; } + if (_nextRep2) HAL_HWTIMER2_FUN(); + HAL_HWTIMER2_SET_PERIOD(); + done: {} + TASKS_HWTIMER2_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER3_ENABLE + void (*HAL_HWTIMER3_FUN)() = NULL; // points to task/process callback function + + bool HAL_HWTIMER3_INIT(uint8_t priority) { + (void)priority; + noInterrupts(); + OCR4A = 2000; // startup one millisecond + TCCR4B = (1 << WGM12) | (1 << CS11); + TCCR4A = 0; + TIMSK4 = (1 << OCIE4A); + interrupts(); + return true; + } + + void HAL_HWTIMER3_DONE() { + TIMSK4 = 0; // stop hw timer (clear register interrupt flag) + HAL_HWTIMER3_FUN = NULL; + } + + #define HAL_HWTIMER3_SET_PERIOD() OCR4A = _nextPeriod3 + ISR(TIMER4_COMPA_vect) { + TASKS_HWTIMER3_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep3 > 1) { count++; if (count%_nextRep3 != 0) goto done; } + if (_nextRep3) HAL_HWTIMER3_FUN(); + HAL_HWTIMER3_SET_PERIOD(); + done: {} + TASKS_HWTIMER3_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER4_ENABLE + void (*HAL_HWTIMER4_FUN)() = NULL; // points to task/process callback function + + bool HAL_HWTIMER4_INIT(uint8_t priority) { + (void)priority; + noInterrupts(); + OCR5A = 2000; // startup one millisecond + TCCR5B = (1 << WGM12) | (1 << CS11); + TCCR5A = 0; + TIMSK5 = (1 << OCIE5A); + interrupts(); + return true; + } + + void HAL_HWTIMER4_DONE() { + TIMSK5 = 0; // stop hw timer (clear register interrupt flag) + HAL_HWTIMER4_FUN = NULL; + } + + #define HAL_HWTIMER4_SET_PERIOD() OCR5A = _nextPeriod4 + ISR(TIMER5_COMPA_vect) { + TASKS_HWTIMER4_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep4 > 1) { count++; if (count%_nextRep4 != 0) goto done; } + if (_nextRep4) HAL_HWTIMER4_FUN(); + HAL_HWTIMER4_SET_PERIOD(); + done: {} + TASKS_HWTIMER4_PROFILER_SUFFIX; + } +#endif diff --git a/src/lib/tasks/HAL_PROFILER.h b/src/lib/tasks/HAL_PROFILER.h new file mode 100755 index 0000000..565f5ca --- /dev/null +++ b/src/lib/tasks/HAL_PROFILER.h @@ -0,0 +1,48 @@ +//-------------------------------------------------------------------------------------------------- +// Configures the profiler according to platform + +#ifdef TASKS_PROFILER_ENABLE + volatile unsigned long _task_max_runtime[4] = {0, 0, 0, 0}; + volatile unsigned long _task_total_runtime[4] = {0, 0, 0, 0}; + volatile unsigned long _task_total_runtime_count[4] = {0, 0, 0, 0}; + #ifdef TASKS_HWTIMER1_ENABLE + #define TASKS_HWTIMER1_PROFILER_PREFIX unsigned long runtime_t0 = micros() + #define TASKS_HWTIMER1_PROFILER_SUFFIX { long at = micros()-runtime_t0; _task_total_runtime[0] += at; _task_total_runtime_count[0]++; if (labs(at) > _task_max_runtime[0]) _task_max_runtime[0] = labs(at); } + #endif + #ifdef TASKS_HWTIMER2_ENABLE + #define TASKS_HWTIMER2_PROFILER_PREFIX unsigned long runtime_t0 = micros() + #define TASKS_HWTIMER2_PROFILER_SUFFIX { long at = micros()-runtime_t0; _task_total_runtime[1] += at; _task_total_runtime_count[1]++; if (labs(at) > _task_max_runtime[1]) _task_max_runtime[1] = labs(at); } + #endif + #ifdef TASKS_HWTIMER3_ENABLE + #define TASKS_HWTIMER3_PROFILER_PREFIX unsigned long runtime_t0 = micros() + #define TASKS_HWTIMER3_PROFILER_SUFFIX { long at = micros()-runtime_t0; _task_total_runtime[2] += at; _task_total_runtime_count[2]++; if (labs(at) > _task_max_runtime[2]) _task_max_runtime[2] = labs(at); } + #endif + #ifdef TASKS_HWTIMER4_ENABLE + #define TASKS_HWTIMER4_PROFILER_PREFIX unsigned long runtime_t0 = micros() + #define TASKS_HWTIMER4_PROFILER_SUFFIX { long at = micros()-runtime_t0; _task_total_runtime[3] += at; _task_total_runtime_count[3]++; if (labs(at) > _task_max_runtime[3]) _task_max_runtime[3] = labs(at); } + #endif + + #define TASKS_PROFILER_PREFIX \ + long at = time_to_next_task - period; \ + average_arrival_time += at; \ + average_arrival_time_count++; \ + if (labs(at + period) > max_arrival_time) max_arrival_time = labs(at + period); \ + unsigned long runtime_t0 = micros(); + #define TASKS_PROFILER_SUFFIX \ + at = micros()-runtime_t0; \ + total_runtime += at; \ + total_runtime_count++; \ + if (labs(at) > max_runtime) max_runtime = labs(at); + +#else + #define TASKS_HWTIMER1_PROFILER_PREFIX + #define TASKS_HWTIMER1_PROFILER_SUFFIX + #define TASKS_HWTIMER2_PROFILER_PREFIX + #define TASKS_HWTIMER2_PROFILER_SUFFIX + #define TASKS_HWTIMER3_PROFILER_PREFIX + #define TASKS_HWTIMER3_PROFILER_SUFFIX + #define TASKS_HWTIMER4_PROFILER_PREFIX + #define TASKS_HWTIMER4_PROFILER_SUFFIX + #define TASKS_PROFILER_PREFIX + #define TASKS_PROFILER_SUFFIX +#endif diff --git a/src/lib/tasks/HAL_STM32_HWTIMER.h b/src/lib/tasks/HAL_STM32_HWTIMER.h new file mode 100755 index 0000000..4f42a0a --- /dev/null +++ b/src/lib/tasks/HAL_STM32_HWTIMER.h @@ -0,0 +1,202 @@ +//---------------------------------------------------------------------------------------------------- +// STM32 hardware timers + +// provides four 16 bit timers with 16 bit software pre-scalers, running at 4MHz +// each timer configured as ~0 to 0.016 seconds (granularity of timer is 0.25uS) +// note that timer use may collide with PWM depending on which pin is being controlled +// I tried to stay away from tone() and use the most basic timers first where possible + +#define TIMER_RATE_MHZ 4L // STM32 motor timers run at 4 MHz +#define TIMER_RATE_16MHZ_TICKS 4L // 16L/TIMER_RATE_MHZ, 4x slower than the default 16MHz "sub micros" + +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) + // prepare hw timer for interval in sub-microseconds (1/16us) + volatile uint16_t _nextPeriod1 = 4000, _nextPeriod2 = 4000, _nextPeriod3 = 4000, _nextPeriod4 = 4000; + volatile uint16_t _nextRep1 = 0, _nextRep2 = 0, _nextRep3 = 0, _nextRep4 = 0; + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { + // maximum time is about 134 seconds for this design + uint32_t counts, reps=0; + if (period != 0 && period <= 2144000000) { + if (period < 16) period = 16; // minimum time is 1us + period /= TIMER_RATE_16MHZ_TICKS; + reps = period/65536 + 1; + counts = period/reps - 1; // has -1 since this is dropped right into a timer register + } else counts = 4000; // set for a 1ms period, stopped + + noInterrupts(); + switch (num) { + case 1: _nextPeriod1 = counts; _nextRep1 = reps; break; + case 2: _nextPeriod2 = counts; _nextRep2 = reps; break; + case 3: _nextPeriod3 = counts; _nextRep3 = reps; break; + case 4: _nextPeriod4 = counts; _nextRep4 = reps; break; + } + interrupts(); + } +#else + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { (void)(num); (void)(period); } +#endif + +#define TIMER_CHANNEL 1 // always use timer channel 1 +#define F_COMP 4000000 // timer clocks at 4MHz + +// search "TIMER_TONE" in github.com/stm32duino/Arduino_Core_STM32/blob/main/variants to find the timer use +#if defined(STM32H723xx) || defined(STM32H743xx) || defined(STM32H750xx) + #define STM32_TIMER1 TIM17 // this can use any type of timer AFAIK + #define STM32_TIMER2 TIM16 // tone uses TIM 6, servo TIM7, PWM is unknown + #define STM32_TIMER3 TIM15 // TIM 1/8 are advanced, TIM2/5 are 32bit (no library support for 32 bit) + #define STM32_TIMER4 TIM3 // TIM 3/4/12/13/14/15/16/17 are general purpose and TIM 6/7 are basic +#elif defined(STM32F446xx) + #define STM32_TIMER1 TIM1 // for STM32F446 (tone uses timer6, servo uses timer2, serial uses timer7) + #define STM32_TIMER2 TIM10 + #define STM32_TIMER3 TIM11 // same use as prior to this point (but different order) + #define STM32_TIMER4 TIM14 +#elif defined(STM32F411xE) || defined(STM32F401xC) + #define STM32_TIMER1 TIM11 // for STM32F401 and STM32F411 (tone uses timer10, servo uses timer11) + #define STM32_TIMER2 TIM9 + #define STM32_TIMER3 TIM4 // same use as prior to this point (but different order) + #define STM32_TIMER4 TIM5 +#elif defined(STM32F407xx) + #define STM32_TIMER1 TIM3 // TIM1 is used for PWM on Axis1 and Axis2 step pins (for DC servo control) + #define STM32_TIMER2 TIM10 // TIM9 is used for PWM reticle and the buzzer (I think) + #define STM32_TIMER3 TIM11 // supports timers TIM1 to TIM14, TIM 1/8 are advanced, TIM2/5 are 32bit (no library support for 32 bit) + #define STM32_TIMER4 TIM14 // the rest and general purpose or basic TIM 6/7 (this can use any type AFAIK) +#elif defined(STM32F303xC) + #define STM32_TIMER1 TIM17 // for STM32F303 (tone uses timer6, servo uses timer2) + #define STM32_TIMER2 TIM16 + #define STM32_TIMER3 TIM15 // same use as prior to this point (but different order) + #define STM32_TIMER4 TIM7 +#elif defined(STM32F103xB) + #define STM32_TIMER1 TIM4 // for STM32F103 (tone uses timer3, servo uses timer2) + #define STM32_TIMER2 TIM2 // don't use the tone() function + #define STM32_TIMER3 TIM1 // same use as prior to this point (but different order) + #define STM32_TIMER4 TIM3 +#endif +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) +bool HAL_HWTIMER_INIT(uint8_t priority, HardwareTimer * hwtimer, void (*wrapper)()) { + hwtimer->pause(); + hwtimer->setMode(TIMER_CHANNEL, TIMER_OUTPUT_COMPARE); + hwtimer->setCaptureCompare(TIMER_CHANNEL, 1); // Interrupt 1 count after each update + hwtimer->attachInterrupt(TIMER_CHANNEL, wrapper); + + // period 0.25... us per count (72/18 = 4MHz) 16.384 ms max + uint32_t prescaleFactor = hwtimer->getTimerClkFreq()/F_COMP; // for example, 72000000/4000000 = 18 + hwtimer->setPrescaleFactor(prescaleFactor); + hwtimer->setOverflow(4000); // startup one millisecond + + // the sub-priority is which interrupt to handle first if both fire at the + // same time, which isn't supported in this HAL so all are set to 0 + // setInterruptPriority(priority, sub-priority) + hwtimer->setInterruptPriority(priority, 0); + hwtimer->resume(); // start the timer counting + hwtimer->refresh(); // refresh the timer's count, prescale, and overflow + return true; + } +#endif + +#ifdef TASKS_HWTIMER1_ENABLE + HardwareTimer *hwtimer1 = new HardwareTimer(STM32_TIMER1); + + void (*HAL_HWTIMER1_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER1_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER1_INIT(uint8_t priority) { + return HAL_HWTIMER_INIT(priority, hwtimer1, HAL_HWTIMER1_WRAPPER); + } + + void HAL_HWTIMER1_DONE() { + hwtimer1->pause(); + HAL_HWTIMER1_FUN = NULL; + } + + #define HAL_HWTIMER1_SET_PERIOD() WRITE_REG(STM32_TIMER1->ARR, _nextPeriod1) + void HAL_HWTIMER1_WRAPPER() { + TASKS_HWTIMER1_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep1 > 1) { count++; if (count%_nextRep1 != 0) goto done; } + if (_nextRep1) HAL_HWTIMER1_FUN(); + HAL_HWTIMER1_SET_PERIOD(); + done: {} + TASKS_HWTIMER1_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER2_ENABLE + HardwareTimer *hwtimer2 = new HardwareTimer(STM32_TIMER2); + + void (*HAL_HWTIMER2_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER2_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER2_INIT(uint8_t priority) { + return HAL_HWTIMER_INIT(priority, hwtimer2, HAL_HWTIMER2_WRAPPER); + } + + void HAL_HWTIMER2_DONE() { + hwtimer2->pause(); + HAL_HWTIMER2_FUN = NULL; + } + + #define HAL_HWTIMER2_SET_PERIOD() WRITE_REG(STM32_TIMER2->ARR, _nextPeriod2) + void HAL_HWTIMER2_WRAPPER() { + TASKS_HWTIMER2_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep2 > 1) { count++; if (count%_nextRep2 != 0) goto done; } + if (_nextRep2) HAL_HWTIMER2_FUN(); + HAL_HWTIMER2_SET_PERIOD(); + done: {} + TASKS_HWTIMER2_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER3_ENABLE + HardwareTimer *hwtimer3 = new HardwareTimer(STM32_TIMER3); + + void (*HAL_HWTIMER3_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER3_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER3_INIT(uint8_t priority) { + return HAL_HWTIMER_INIT(priority, hwtimer3, HAL_HWTIMER3_WRAPPER); + } + + void HAL_HWTIMER3_DONE() { + hwtimer3->pause(); + HAL_HWTIMER3_FUN = NULL; + } + + #define HAL_HWTIMER3_SET_PERIOD() WRITE_REG(STM32_TIMER3->ARR, _nextPeriod3) + void HAL_HWTIMER3_WRAPPER() { + TASKS_HWTIMER3_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep3 > 1) { count++; if (count%_nextRep3 != 0) goto done; } + if (_nextRep3) HAL_HWTIMER3_FUN(); + HAL_HWTIMER3_SET_PERIOD(); + done: {} + TASKS_HWTIMER3_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER4_ENABLE + HardwareTimer *hwtimer4 = new HardwareTimer(STM32_TIMER4); + + void (*HAL_HWTIMER4_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER4_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER4_INIT(uint8_t priority) { + return HAL_HWTIMER_INIT(priority, hwtimer4, HAL_HWTIMER4_WRAPPER); + } + + void HAL_HWTIMER4_DONE() { + hwtimer4->pause(); + HAL_HWTIMER4_FUN = NULL; + } + + #define HAL_HWTIMER4_SET_PERIOD() WRITE_REG(STM32_TIMER4->ARR, _nextPeriod4) + void HAL_HWTIMER4_WRAPPER() { + TASKS_HWTIMER4_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep4 > 1) { count++; if (count%_nextRep4 != 0) goto done; } + if (_nextRep4) HAL_HWTIMER4_FUN(); + HAL_HWTIMER4_SET_PERIOD(); + done: {} + TASKS_HWTIMER4_PROFILER_SUFFIX; + } +#endif diff --git a/src/lib/tasks/HAL_TEENSY_HWTIMER.h b/src/lib/tasks/HAL_TEENSY_HWTIMER.h new file mode 100755 index 0000000..31c6721 --- /dev/null +++ b/src/lib/tasks/HAL_TEENSY_HWTIMER.h @@ -0,0 +1,150 @@ +//-------------------------------------------------------------------------------------------------- +// Teensyduino hardware timers + +// provides four 32 bit pit timers with 16 bit software pre-scalers +// each timer configured as ~0 to x seconds (granularity of timer is 0.062uS) +// timer use doesn't collide with PWM or tone() on these platforms + +#define F_BUS2 16000000L // Pretend the timers are running at 0.062us/tick +#define TIMER_RATE_MHZ (F_BUS2/1000000.0) // Teensy PIT timers run at F_BUS Hz but we use the interval timer library +#define TIMER_RATE_16MHZ_TICKS (16.0/TIMER_RATE_MHZ) // 16.0/TIMER_RATE_MHZ for the default 16MHz "sub micros" + +#if defined(TASKS_HWTIMER1_ENABLE) || defined(TASKS_HWTIMER2_ENABLE) || defined(TASKS_HWTIMER3_ENABLE) || defined(TASKS_HWTIMER4_ENABLE) + // prepare hw timer for interval in sub-microseconds (1/16us) + volatile double _nextPeriod1 = 1000.0, _nextPeriod2 = 1000.0, _nextPeriod3 = 1000.0, _nextPeriod4 = 1000.0; + volatile uint16_t _nextRep1 = 0, _nextRep2 = 0, _nextRep3 = 0, _nextRep4 = 0; + const double timerRate16MHzTicks = TIMER_RATE_16MHZ_TICKS; + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { + double counts; + uint16_t reps = 0; + if (period != 0 && period <= 2144000000) { + if (period < 16) period = 16; // minimum time is 1us + double fperiod = period/timerRate16MHzTicks; + reps = fperiod/4194304.0 + 1.0; + counts = fperiod/reps/16.0; + } else counts = 1000.0; // set for a 1ms period, stopped + + noInterrupts(); + switch (num) { + case 1: _nextPeriod1 = counts; _nextRep1 = reps; break; + case 2: _nextPeriod2 = counts; _nextRep2 = reps; break; + case 3: _nextPeriod3 = counts; _nextRep3 = reps; break; + case 4: _nextPeriod4 = counts; _nextRep4 = reps; break; + } + interrupts(); + } +#else + void HAL_HWTIMER_PREPARE_PERIOD(uint8_t num, unsigned long period) { (void)(num); (void)(period); } +#endif + +#ifdef TASKS_HWTIMER1_ENABLE + IntervalTimer itimer1; + + void (*HAL_HWTIMER1_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER1_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER1_INIT(uint8_t priority) { + itimer1.priority(priority); + return itimer1.begin(HAL_HWTIMER1_WRAPPER, 1000); // startup one millisecond + } + + void HAL_HWTIMER1_DONE() { + HAL_HWTIMER1_FUN = NULL; + itimer1.end(); + } + + #define HAL_HWTIMER1_SET_PERIOD() itimer1.update(_nextPeriod1) + void HAL_HWTIMER1_WRAPPER() { + TASKS_HWTIMER1_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep1 > 1) { count++; if (count%_nextRep1 != 0) goto done; } + if (_nextRep1) HAL_HWTIMER1_FUN(); + HAL_HWTIMER1_SET_PERIOD(); + done: {} + TASKS_HWTIMER1_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER2_ENABLE + IntervalTimer itimer2; + + void (*HAL_HWTIMER2_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER2_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER2_INIT(uint8_t priority) { + itimer2.priority(priority); + return itimer2.begin(HAL_HWTIMER2_WRAPPER, 1000); // startup one millisecond + } + + void HAL_HWTIMER2_DONE() { + HAL_HWTIMER2_FUN = NULL; + itimer2.end(); + } + + #define HAL_HWTIMER2_SET_PERIOD() itimer2.update(_nextPeriod2) + void HAL_HWTIMER2_WRAPPER() { + TASKS_HWTIMER2_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep2 > 1) { count++; if (count%_nextRep2 != 0) goto done; } + if (_nextRep2) HAL_HWTIMER2_FUN(); + HAL_HWTIMER2_SET_PERIOD(); + done: {} + TASKS_HWTIMER2_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER3_ENABLE + IntervalTimer itimer3; + + void (*HAL_HWTIMER3_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER3_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER3_INIT(uint8_t priority) { + itimer3.priority(priority); + return itimer3.begin(HAL_HWTIMER3_WRAPPER, 1000); // startup one millisecond + } + + void HAL_HWTIMER3_DONE() { + HAL_HWTIMER3_FUN = NULL; + itimer3.end(); + } + + #define HAL_HWTIMER3_SET_PERIOD() itimer3.update(_nextPeriod3) + void HAL_HWTIMER3_WRAPPER() { + TASKS_HWTIMER3_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep3 > 1) { count++; if (count%_nextRep3 != 0) goto done; } + if (_nextRep3) HAL_HWTIMER3_FUN(); + HAL_HWTIMER3_SET_PERIOD(); + done: {} + TASKS_HWTIMER3_PROFILER_SUFFIX; + } +#endif + +#ifdef TASKS_HWTIMER4_ENABLE + IntervalTimer itimer4; + + void (*HAL_HWTIMER4_FUN)() = NULL; // points to task/process callback function + void HAL_HWTIMER4_WRAPPER(); // forward definition of the timer ISR + + bool HAL_HWTIMER4_INIT(uint8_t priority) { + itimer4.priority(priority); + return itimer4.begin(HAL_HWTIMER4_WRAPPER, 1000); // startup one millisecond + } + + void HAL_HWTIMER4_DONE() { + HAL_HWTIMER4_FUN = NULL; + itimer4.end(); + } + + #define HAL_HWTIMER4_SET_PERIOD() itimer4.update(_nextPeriod4) + void HAL_HWTIMER4_WRAPPER() { + TASKS_HWTIMER4_PROFILER_PREFIX; + static uint16_t count = 0; + if (_nextRep4 > 1) { count++; if (count%_nextRep4 != 0) goto done; } + if (_nextRep4) HAL_HWTIMER4_FUN(); + HAL_HWTIMER4_SET_PERIOD(); + done: {} + TASKS_HWTIMER4_PROFILER_SUFFIX; + } +#endif diff --git a/src/lib/tasks/OnTask.cpp b/src/lib/tasks/OnTask.cpp new file mode 100755 index 0000000..bf5ee1c --- /dev/null +++ b/src/lib/tasks/OnTask.cpp @@ -0,0 +1,660 @@ +/* + * Title OnTask + * by Howard Dutton + * + * Copyright (C) 2021 Howard Dutton + * www.stellarjourney.com + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * +*/ + +// this line allows configuration of OnTask from the a given project, if this library is used stand-alone remove it +#include "../../Common.h" + +// Enabling the profiler inserts time logging code into the process calls. This provides the average +// and largest start time delta and run time for each process. To enable the profiler uncomment: +//#define TASKS_PROFILER_ENABLE + +// Normally tasks are skipped if they become too late, to instead delay them for later processing uncomment: +//#define TASKS_QUEUE_MISSED + +#include "OnTask.h" + +#include "HAL_PROFILER.h" +#include "HAL_HWTIMERS.h" + +#define roundPeriod(x) ((unsigned long)((x)+(double)0.5L)) + +unsigned char _task_postpone = false; +volatile unsigned long _task_ppsAverageSubMicros = 16000000UL; +double _task_masterFrequencyRatio = 1.0L; + +// Task object +Task::Task(uint32_t period, uint32_t duration, bool repeat, uint8_t priority, void (*callback)()) { + idle = period == 0; + this->period = period; + period_units = PU_MILLIS; + this->duration = duration; + this->repeat = repeat; + if (!repeat) immediate = false; + this->priority = priority; + this->callback = callback; + + start_time = millis(); + uint32_t spread_phase = 0; + if (repeat && period >= 4) { + uint32_t h = (uint32_t)(uintptr_t)callback ^ ((uint32_t)(uintptr_t)this >> 4) ^ start_time; + spread_phase = h % period; + } + next_task_time = start_time + spread_phase; + + strcpy(processName, ""); +} + +Task::~Task() { + switch (hardware_timer) { + case 1: HAL_HWTIMER1_DONE(); break; + case 2: HAL_HWTIMER2_DONE(); break; + case 3: HAL_HWTIMER3_DONE(); break; + case 4: HAL_HWTIMER4_DONE(); break; + } +} + +bool Task::requestHardwareTimer(uint8_t num, uint8_t hwPriority) { + if (num < 1 || num > 4) { DLF("ERR: Task::requestHardwareTimer(), timer number out of range"); return false; } + if (repeat != true) { DLF("ERR: Task::requestHardwareTimer(), repeat must be true"); return false; } + if (priority != 0) { DLF("ERR: Task::requestHardwareTimer(), s/w priority must be 0 (highest)"); return false; } + switch (num) { + case 1: + if (HAL_HWTIMER1_FUN != NULL) { DLF("ERR: Task::requestHardwareTimer(), channel1 already in use"); return false; } + break; + case 2: + if (HAL_HWTIMER2_FUN != NULL) { DLF("ERR: Task::requestHardwareTimer(), channel2 already in use"); return false; } + break; + case 3: + if (HAL_HWTIMER3_FUN != NULL) { DLF("ERR: Task::requestHardwareTimer(), channel3 already in use"); return false; } + break; + case 4: + if (HAL_HWTIMER4_FUN != NULL) { DLF("ERR: Task::requestHardwareTimer(), channel4 already in use"); return false; } + break; + } + + unsigned long hardware_timer_period = period; + if (period_units == PU_NONE) hardware_timer_period = 0; else + if (period_units == PU_MILLIS) hardware_timer_period *= 16000U; else + if (period_units == PU_MICROS) hardware_timer_period *= 16U; + + HAL_HWTIMER_PREPARE_PERIOD(num, roundPeriod((double)hardware_timer_period*_task_masterFrequencyRatio)); + + bool success = true; + switch (num) { + case 1: + HAL_HWTIMER1_FUN = callback; + if (!HAL_HWTIMER1_INIT(hwPriority)) { success = false; HAL_HWTIMER1_FUN = NULL; } + break; + case 2: + HAL_HWTIMER2_FUN = callback; + if (!HAL_HWTIMER2_INIT(hwPriority)) { success = false; HAL_HWTIMER2_FUN = NULL; } + break; + case 3: + HAL_HWTIMER3_FUN = callback; + if (!HAL_HWTIMER3_INIT(hwPriority)) { success = false; HAL_HWTIMER3_FUN = NULL; } + break; + case 4: + HAL_HWTIMER4_FUN = callback; + if (!HAL_HWTIMER4_INIT(hwPriority)) { success = false; HAL_HWTIMER4_FUN = NULL; } + break; + } + if (!success) { DF("ERR: Task::requestHardwareTimer(), HAL_HWTIMER"); D(num); DLF("_INIT() failed"); return false; } + hardware_timer = num; + + period = hardware_timer_period; + period_units = PU_SUB_MICROS; + next_period = period; + next_period_units = PU_NONE; + return true; +} + +void Task::setCallback(void (*callback)()) { + this->callback = callback; + noInterrupts(); + switch (hardware_timer) { + case 1: + HAL_HWTIMER1_FUN = callback; + break; + case 2: + HAL_HWTIMER2_FUN = callback; + break; + case 3: + HAL_HWTIMER3_FUN = callback; + break; + case 4: + HAL_HWTIMER4_FUN = callback; + break; + } + interrupts(); +} + +void Task::setTimingMode(TimingMode mode) { + timingMode = mode; +} + +bool Task::poll() { + if (hardware_timer || running) return false; + + if (period != 0) { + unsigned long t, time_to_next_task; + + if (period_units == PU_MICROS) t = micros(); else t = millis(); + if (immediate) { immediate = false; next_task_time = t; } + time_to_next_task = next_task_time - t; + + if ((long)time_to_next_task < 0) { + running = true; + + TASKS_PROFILER_PREFIX; + callback(); + TASKS_PROFILER_SUFFIX; + + running = false; + + if (_task_postpone) { _task_postpone = false; return false; } + + // set timing for guaranteed minimum (or gap) period + if (timingMode != TM_BALANCED) { + if (timingMode == TM_GAP) { + if (period_units == PU_MICROS) t = micros(); else t = millis(); + } + time_to_next_task = 0; + } + + // adopt next period + if (next_period_units != PU_NONE) { + time_to_next_task = 0; + period = roundPeriod((double)next_period*_task_masterFrequencyRatio); + period_units = next_period_units; + next_period_units = PU_NONE; + } + + // set adjusted period + #ifndef TASKS_QUEUE_MISSED + if ((long)(period + time_to_next_task) < 0) time_to_next_task = -period; + #endif + next_task_time = t + (long)(period + time_to_next_task); + if (!repeat) period = 0; + } + } else immediate = true; + + return false; +} + +void Task::refreshPeriod() { + if (hardware_timer) { + next_period_units = PU_SUB_MICROS; + setHardwareTimerPeriod(); + } +} + +void Task::setPeriod(unsigned long period, PeriodUnits units) { + if (hardware_timer) { + next_period = period; + next_period_units = units; + setHardwareTimerPeriod(); + } else { + if (units == PU_SUB_MICROS) { period = lroundf(period/16.0F); units = PU_MICROS; } + if (units == PU_MILLIS && period < 1000) { period *= 1000; units = PU_MICROS; } + if (this->period == 0) { + idle = true; + this->period = period; + period_units = units; + next_period_units = PU_NONE; + if (period_units == PU_MICROS) next_task_time = micros() + period; else next_task_time = millis() + period; + } else { + idle = false; + next_period = period; + next_period_units = units; + } + } +} + +void Task::setFrequency(float freq) { + if (freq > 0.0F) { + freq = 1.0F / freq; // seconds per call + float f = freq * 16000000.0F; // sub-micros per call + if (f <= 4294967295.0F) { + setPeriod(lroundf(f), PU_SUB_MICROS); + return; + } + f = freq * 1000000.0F; // micros per call + if (f <= 4294967295.0F) { + setPeriod(lroundf(f), PU_MICROS); + return; + } + f = freq * 1000.0F; // millis per call + if (f <= 4294967295.0F) { + setPeriod(lroundf(f)); + return; + } + } + setPeriod(0); +} + +void Task::setDuration(unsigned long duration) { + this->duration = duration; +} + +bool Task::isDurationComplete() { + return (duration > 0 && ((long)(millis() - (start_time + duration)) >= 0)); +} + +void Task::setDurationComplete() { + duration = 1; + start_time--; +} + +void Task::setRepeat(bool repeat) { + if (hardware_timer) return; + this->repeat = repeat; +} + +void Task::setPriority(uint8_t priority) { + if (hardware_timer) return; + this->priority = priority; +} + +uint8_t Task::getPriority() { + return priority; +} + +void Task::setNameStr(const char name[]) { + strncpy(processName, name, 7); + processName[7] = 0; +} + +char* Task::getNameStr() { + return processName; +} + +#ifdef TASKS_PROFILER_ENABLE +float Task::getArrivalAvg() { + if (hardware_timer) return 0; + if (average_arrival_time_count == 0) return 0; + float value = -average_arrival_time/average_arrival_time_count; + average_arrival_time = 0; + average_arrival_time_count = 0; + return value; +} +float Task::getArrivalMax() { + if (hardware_timer) return 0; + float value = max_arrival_time; + max_arrival_time = 0; + return value; +} +float Task::getRuntimeTotal() { + if (hardware_timer) { noInterrupts(); total_runtime = _task_total_runtime[hardware_timer-1]; _task_total_runtime[hardware_timer-1] = 0; total_runtime_count=_task_total_runtime_count[hardware_timer-1]; interrupts(); }; + float value = total_runtime; + if (value > 2147483647) value = 0.0; + total_runtime = 0; + return value; +} +long Task::getRuntimeTotalCount() { + if (hardware_timer) { noInterrupts(); total_runtime_count = _task_total_runtime_count[hardware_timer-1]; _task_total_runtime_count[hardware_timer-1] = 0; interrupts(); }; + long value = total_runtime_count; + total_runtime_count = 0; + return value; +} +float Task::getRuntimeMax() { + if (hardware_timer) { noInterrupts(); max_runtime = _task_max_runtime[hardware_timer-1]; _task_max_runtime[hardware_timer-1] = 0; interrupts(); }; + float value = max_runtime; + max_runtime = 0; + return value; +} +#endif + +void Task::setHardwareTimerPeriod() { + // adopt next period + if (next_period_units != PU_NONE) { + if (next_period_units == PU_MILLIS) next_period *= 16000UL; else if (next_period_units == PU_MICROS) next_period *= 16UL; + next_period_units = PU_NONE; + period_units = PU_SUB_MICROS; + + unsigned long lastPeriod = period; + + period = roundPeriod((double)next_period*_task_masterFrequencyRatio); + HAL_HWTIMER_PREPARE_PERIOD(hardware_timer, period); + + // if the current period is > 0.1 seconds and the new period < lastPeriod adopt the new rate immediately + if (lastPeriod > 1600000UL && period < lastPeriod) { + switch (hardware_timer) { + case 1: HAL_HWTIMER1_SET_PERIOD(); break; + case 2: HAL_HWTIMER2_SET_PERIOD(); break; + case 3: HAL_HWTIMER3_SET_PERIOD(); break; + case 4: HAL_HWTIMER4_SET_PERIOD(); break; + } + } + } +} + +void tasksMonitor() { + static unsigned long _lastPpsAverageSubMicros = 16000000UL; + if (_lastPpsAverageSubMicros != _task_ppsAverageSubMicros) { + _task_masterFrequencyRatio = (double)_task_ppsAverageSubMicros/16000000.0L; + + uint8_t handle = tasks.getFirstHandle(); + for (int i = 0; i < TASKS_MAX; i++) { + if (handle == 0) break; + tasks.refreshPeriod(handle); + handle = tasks.getNextHandle(handle); + } + + _lastPpsAverageSubMicros = _task_ppsAverageSubMicros; + } +} + +Tasks::Tasks() { + // clear tasks + for (uint8_t c = 0; c < TASKS_MAX; c++) { + task[c] = NULL; + allocated[c] = false; + } + + // start the task monitor + tasks.add(1000, 0, true, 7, tasksMonitor, "TaskMon"); +} + +Tasks::~Tasks() { + // find active tasks and remove + for (uint8_t c = 0; c < TASKS_MAX; c++) { + if (allocated[c]) delete task[c]; + } +} + +uint8_t Tasks::add(uint32_t period, uint32_t duration, bool repeat, uint8_t priority, void (*callback)()) { + // check priority + if (priority > 7) return false; + if (priority > highest_priority) highest_priority = priority; + + // find the next free task + int8_t e = -1; + for (uint8_t c = 0; c < TASKS_MAX; c++) { + if (!allocated[c]) { e = c; break; } + } + // no tasks available + if (e == -1) return false; + + // create the task handler + task[e] = new Task(period, duration, repeat, priority, callback); + if (task[e] != NULL) allocated[e] = true; else return false; + + updateEventRange(); + return e + 1; +} + +uint8_t Tasks::add(uint32_t period, uint32_t duration, bool repeat, uint8_t priority, void (*callback)(), const char name[]) { + uint8_t handle = add(period, duration, repeat, priority, callback); + setNameStr(handle, name); + return handle; +} + +bool Tasks::requestHardwareTimer(uint8_t handle) { + return requestHardwareTimer(handle, 128); +} + +bool Tasks::requestHardwareTimer(uint8_t handle, uint8_t hwPriority) { + if (handle != 0 && allocated[handle - 1] && TASKS_HWTIMERS > 0) { + for (int num = 0; num < TASKS_HWTIMERS; num++) { + if (!hardware_timer_allocated[num]) { + hardware_timer_allocated[num] = task[handle - 1]->requestHardwareTimer(num + 1, hwPriority); + return hardware_timer_allocated[num]; + } + } + } + return false; +} + +bool Tasks::setCallback(uint8_t handle, void (*callback)()) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setCallback(callback); + return true; + } else return false; +} + +bool Tasks::setTimingMode(uint8_t handle, TimingMode mode) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setTimingMode(mode); + return true; + } else return false; +} + +void Tasks::remove(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + uint8_t hw_timer_number = task[handle - 1]->hardware_timer; + if (hw_timer_number) hardware_timer_allocated[hw_timer_number - 1] = false; + delete task[handle - 1]; + allocated[handle - 1] = false; + updateEventRange(); + updatePriorityRange(); + } +} + +void Tasks::refreshPeriod(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->refreshPeriod(); + } +} + +void Tasks::setPeriod(uint8_t handle, unsigned long period) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setPeriod(period); + } +} + +void Tasks::setPeriodMicros(uint8_t handle, unsigned long period) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setPeriod(period, PU_MICROS); + } +} + +void Tasks::setPeriodSubMicros(uint8_t handle, unsigned long period) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setPeriod(period, PU_SUB_MICROS); + } +} + +void Tasks::setFrequency(uint8_t handle, double freq) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setFrequency(freq); + } +} + +IRAM_ATTR void Tasks::setPeriodRatioSubMicros(unsigned long value) { + _task_ppsAverageSubMicros = value; +} + +void Tasks::setDuration(uint8_t handle, unsigned long duration) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setDuration(duration); + } +} + +void Tasks::setDurationComplete(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setDurationComplete(); + } +} + +void Tasks::setRepeat(uint8_t handle, bool repeat) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setRepeat(repeat); + } +} + +void Tasks::setPriority(uint8_t handle, uint8_t priority) { + if (handle != 0 && allocated[handle - 1]) { + if (priority > 7) return; + task[handle - 1]->setPriority(priority); + updateEventRange(); + updatePriorityRange(); + } +} + +void Tasks::setNameStr(uint8_t handle, const char name[]) { + if (handle != 0 && allocated[handle - 1]) { + task[handle - 1]->setNameStr(name); + } +} + +char* Tasks::getNameStr(uint8_t handle) { + static char empty[1]; + strcpy(empty, ""); + if (handle != 0 && allocated[handle - 1]) { + return task[handle - 1]->getNameStr(); + } else return empty; +} + +uint8_t Tasks::getFirstHandle() { + return getNextHandle(0); +} + +uint8_t Tasks::getNextHandle(uint8_t handle) { + if (handle > highest_task) return 0; + do { + if (allocated[handle]) { + return handle + 1; + } + } while (++handle <= highest_task); + return 0; +} + +uint8_t Tasks::getHandleByName(const char name[]) { + char *candidate; + uint8_t handle = getFirstHandle(); + while (handle) { + candidate = getNameStr(handle); + if (strstr(candidate, name)) { + if (strlen(candidate) == strlen(name)) break; + } + handle = getNextHandle(handle); + } + + return handle; +} + +#ifdef TASKS_PROFILER_ENABLE + double Tasks::getArrivalAvg(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + return task[handle - 1]->getArrivalAvg(); + } else return 0; + } + double Tasks::getArrivalMax(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + return task[handle - 1]->getArrivalMax(); + } else return 0; + } + double Tasks::getRuntimeTotal(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + return task[handle - 1]->getRuntimeTotal(); + } else return 0; + } + long Tasks::getRuntimeTotalCount(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + return task[handle - 1]->getRuntimeTotalCount(); + } else return 0; + } + double Tasks::getRuntimeMax(uint8_t handle) { + if (handle != 0 && allocated[handle - 1]) { + return task[handle - 1]->getRuntimeMax(); + } else return 0; + } +#endif + +void Tasks::yield() { + static uint8_t highest_priority_active = 8; + ::yield(); + for (uint8_t priority = 0; priority <= highest_priority; priority++) { + uint8_t last_priority = highest_priority_active; + if (priority < highest_priority_active) { + highest_priority_active = priority; + for (uint8_t i = 0; i <= highest_task; i++) { + if (++number[priority] > highest_task) number[priority] = 0; + if (allocated[number[priority]]) { + if (task[number[priority]]->getPriority() == priority) { + if (task[number[priority]]->isDurationComplete()) { remove(number[priority] + 1); highest_priority_active = last_priority; return; } + if (task[number[priority]]->poll()) { highest_priority_active = last_priority; return; } + } + } + } + highest_priority_active = last_priority; + } + } +} + +void Tasks::yield(unsigned long milliseconds) { + unsigned long endTime = millis() + milliseconds; + while ((long)(millis() - endTime) < 0) this->yield(); +} + +void Tasks::yieldMicros(unsigned long microseconds) { + unsigned long endTime = micros() + microseconds; + while ((long)(micros() - endTime) < 0) this->yield(); +} + +void Tasks::yieldAll() { + for (uint8_t priority = 0; priority <= highest_priority; priority++) { + for (uint8_t i = 0; i <= highest_task; i++) { + if (++number[priority] > highest_task) number[priority] = 0; + if (allocated[number[priority]]) { + if (task[number[priority]]->getPriority() == priority) { + if (task[number[priority]]->isDurationComplete()) { remove(number[priority] + 1); return; } + if (task[number[priority]]->poll()) return; + } + } + } + } +} + +void Tasks::yieldAll(unsigned long milliseconds) { + unsigned long endTime = millis() + milliseconds; + while ((long)(millis() - endTime) < 0) this->yieldAll(); +} + +void Tasks::yieldAllMicros(unsigned long microseconds) { + unsigned long endTime = micros() + microseconds; + while ((long)(micros() - endTime) < 0) this->yieldAll(); +} + +void Tasks::updatePriorityRange() { + // scan for highest priority + highest_priority = 0; + for (uint8_t e = 0; e <= highest_task; e++) { + if (allocated[e]) { + uint8_t p = task[e]->getPriority(); + if (p > highest_priority) highest_priority = p; + } + } +} + +void Tasks::updateEventRange() { + // scan for highest task handle + highest_task = 0; + for (int8_t e = TASKS_MAX - 1; e >= 0 ; e--) { + if (allocated[e]) { + highest_task = e; + break; + } + } +} + +Tasks tasks; diff --git a/src/lib/tasks/OnTask.h b/src/lib/tasks/OnTask.h new file mode 100755 index 0000000..cc48305 --- /dev/null +++ b/src/lib/tasks/OnTask.h @@ -0,0 +1,380 @@ +/* + * Title OnTask + * by Howard Dutton + * + * Copyright (C) 2021 Howard Dutton + * www.stellarjourney.com + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * DESCRIPTION: + * + * Software timer based processes must use "tasks.yield();" (or the "Y;" macro, which is the same exact thing) + * during any processing that requires a significant amount of time. This allows for the processing of other tasks + * while it is working. What a "significant amount of time" is depends on the program's requirements. It might + * be measured in micro-seconds, or milli-seconds, or seconds. Yield should not be used for hardware timer based + * processes + * +*/ + +#pragma once +#include + +#ifndef IRAM_ATTR + #define IRAM_ATTR +#endif + +// To enable the option to use a given hardware timer (1..4), if available, uncomment that line: +#ifndef TASKS_HWTIMERS + #define TASKS_HWTIMERS -1 +#endif +#if TASKS_HWTIMERS < -1 || TASKS_HWTIMERS > 4 + #error "OnTask: TASKS_HWTIMERS must be OFF, or 1 to 4." +#endif +#if TASKS_HWTIMERS >= 1 + #define TASKS_HWTIMER1_ENABLE +#endif +#if TASKS_HWTIMERS >= 2 + #define TASKS_HWTIMER2_ENABLE +#endif +#if TASKS_HWTIMERS >= 3 + #define TASKS_HWTIMER3_ENABLE +#endif +#if TASKS_HWTIMERS == 4 + #define TASKS_HWTIMER4_ENABLE +#endif + +// default provision is for 8 tasks, up to 255 are allowed, to change use: +// #define TASKS_MAX 200 (for example) +#ifndef TASKS_MAX + #define TASKS_MAX 8 +#endif + +// ESP32 override cli/sei and use muxes to block the h/w timer ISR's instead +#ifdef ESP32 + // on the ESP32 noInterrupts()/interrupts() are #defined to be cli()/sei() + // cli()/sei() are empty and do nothing, below we use them for our purposes + // to the block execution of all four timer interrupts + extern portMUX_TYPE timerMux; + #undef cli + // disable hardware timer based tasks by FreeRTOS mux + #define cli() portENTER_CRITICAL(&timerMux) + #undef sei + // enable hardware timer based tasks by FreeRTOS mux + #define sei() portEXIT_CRITICAL(&timerMux) + // disable hardware timer based tasks by clearing interrupt flag(s) + extern void timerAlarmsDisable(); + // enable hardware timer based tasks by setting interrupt flag(s) + extern void timerAlarmsEnable(); +#endif + +// runs tasks at their prescribed interval +// \note allows only higher priority tasks to run +// \note each call can trigger running at most a single process +#define Y tasks.yield() + +// short macro to allow momentary postponement of the current task +extern unsigned char __task_postpone; +// postpone currently executing task (scheduler picks task up ASAP again on exit) +#define task_postpone() __task_postpone = true; + +enum PeriodUnits: uint8_t {PU_NONE, PU_MILLIS, PU_MICROS, PU_SUB_MICROS}; + +// Timing modes +// TM_BALANCED (default) to maintain the specified frequency/period where a task that runs late is next run early to compensate +// TM_MINIMUM to run the task at an interval not less than the specified frequency/period from task start to next start +// TM_GAP to run the task at an interval not less than the specified frequency/period from task exit to next start +enum TimingMode: uint8_t {TM_BALANCED, TM_MINIMUM, TM_GAP}; + +class Task { + public: + Task(uint32_t period, uint32_t duration, bool repeat, uint8_t priority, void (*callback)()); + ~Task(); + + bool requestHardwareTimer(uint8_t num, uint8_t hwPriority); + uint8_t hardware_timer = 0; + + void setCallback(void (*callback)()); + + void setTimingMode(TimingMode mode); + + // run task at the prescribed interval + // note: tasks are timed in such a way as to achieve an accurate average frequency, if + // the task occurs late the next call is scheduled earlier to make up the difference + bool poll(); + + void refreshPeriod(); + void setPeriod(unsigned long period, PeriodUnits units = PU_MILLIS); + void setFrequency(float freq); + + void setDuration(unsigned long duration); + bool isDurationComplete(); + void setDurationComplete(); + + void setRepeat(bool repeat); + + void setPriority(uint8_t priority); + uint8_t getPriority(); + + void setNameStr(const char name[]); + char *getNameStr(); + + #ifdef TASKS_PROFILER_ENABLE + float getArrivalAvg(); + float getArrivalMax(); + float getRuntimeTotal(); + long getRuntimeTotalCount(); + float getRuntimeMax(); + #endif + + volatile bool immediate = true; + + private: + + void setHardwareTimerPeriod(); + + char processName[8]; + unsigned long period = 0; + unsigned long next_period = 0; + unsigned long duration = 0; + bool repeat = false; + uint8_t priority = 0; + bool idle = true; + bool running = false; + PeriodUnits period_units = PU_MILLIS; + PeriodUnits next_period_units = PU_NONE; + unsigned long start_time = 0; + unsigned long last_task_time = 0; + unsigned long next_task_time = 0; + TimingMode timingMode = TM_BALANCED; + void (*volatile callback)() = NULL; + + #ifdef TASKS_PROFILER_ENABLE + volatile double average_arrival_time = 0; + volatile unsigned long average_arrival_time_count = 0; + volatile long max_arrival_time = 0; + volatile double total_runtime = 0; + volatile unsigned long total_runtime_count = 0; + volatile long max_runtime = 0; + #endif +}; + +class Tasks { + public: + Tasks(); + ~Tasks(); + + // add process task + // \param period between process calls in milliseconds, use 0 to disable + // software timers are limited to <= 49 days, hardware timers are limited to <= 139 seconds + // \param duration in milliseconds the task is valid for, use 0 for unlimited (deletes the task on completion) + // \param repeat true if the task is allowed to repeat, false to run once (sets period to 0 on completion) + // \param priority software level highest 0 to 7 lowest. hardware tasks are always higher priority than software tasks + // for software timed tasks level 0 must be complete before level 1 are serviced, etc. + // tasks run round robin in ea priority level, the most recently serviced task will be the last visited again + // only higher priority tasks are allowed to run when a lower priority task yields + // \param callback function to handle this tasks processing + // \return handle to the task on success, or 0 on failure + uint8_t add(uint32_t period, uint32_t duration, bool repeat, uint8_t priority, void (*callback)()); + + // add process task + // \param period between process calls in milliseconds, use 0 to disable + // software timers are limited to <= 49 days, hardware timers are limited to <= 139 seconds + // \param duration in milliseconds the task is valid for, use 0 for unlimited (deletes the task on completion) + // \param repeat true if the task is allowed to repeat, false to run once (sets period to 0 on completion) + // \param priority software level highest 0 to 7 lowest. hardware tasks are always higher priority than software tasks + // for software timed tasks level 0 must be complete before level 1 are serviced, etc. + // tasks run round robin in ea priority level, the most recently serviced task will be the last visited again + // only higher priority tasks are allowed to run when a lower priority task yields + // \param callback function to handle this tasks processing + // \param name an optional short (max length 7) char str describing the task + // \return handle to the task on success, or 0 on failure + uint8_t add(uint32_t period, uint32_t duration, bool repeat, uint8_t priority, void (*callback)(), const char name[]); + + // allocates a hardware timer, if available, for this task. Note: for the associated task: *repeat* must be true, + // *priority* must be 0 (all are higher than s/w task priority 0.) + // \param handle task handle + // \param hwPriority optional hardware interrupt priority, default is 128 if not specified + // \return true if successful, or false (in which case the standard polling timer will still be available) + bool requestHardwareTimer(uint8_t handle, uint8_t hwPriority); + bool requestHardwareTimer(uint8_t handle); + + // change task callback + // \param handle task handle + // \param callback function to handle this tasks processing + // \return true if successful, or false if unable to find the associated task + bool setCallback(uint8_t handle, void (*callback)()); + + // change task timing mode + // \param handle task handle + // \param mode either TM_BALANCED (default) or TM_MINIMUM + // \return true if successful, or false if unable to find the associated task + bool setTimingMode(uint8_t handle, TimingMode mode); + + // remove process task. Note: do not remove a task if the task process is running + // \param handle task handle + // \return nothing + void remove(uint8_t handle); + + // refresh process period, causes setPeriodRatioSubMicros() ratio to be recognized by hardware timers + // handle: task handle + void refreshPeriod(uint8_t handle); + + // set process period ms. Note: the period/frequency change takes effect on the next task cycle, + // if the period is > the hardware timers maximum period the task is disabled + // \param handle task handle + // \param period in milliseconds, use 0 for disabled + // \return nothing + void setPeriod(uint8_t handle, unsigned long period); + + // set process period us + // \param handle: task handle + // \param period: in milliseconds, use 0 for disabled + // \return nothing + // \note the period/frequency change takes effect on the next task cycle + // \note if the period is > the hardware timers maximum period the task is disabled + void setPeriodMicros(uint8_t handle, unsigned long period); + + // set process period sub-us + // \param handle: task handle + // \param period: in sub-microseconds (1/16 microsecond units), use 0 for disabled + // \return nothing + // \note the period/frequency change takes effect on the next task cycle + // \note if the period is > the hardware timers maximum period the task is disabled + void setPeriodSubMicros(uint8_t handle, unsigned long period); + + // change process period Hz + // \param handle: task handle + // \param freq: in Hertz, use 0 for disabled + // \return nothing + // \note the period/frequency change takes effect on the next task cycle + // \note when setting a frequency the most appropriate setPeriod is used automatically + // \note if the period is > ~49 days (or > the hardware timers maximum period) the task is disabled + void setFrequency(uint8_t handle, double freq); + + // set the period ratio to compensate for MCU clock inaccuracy + // \param value: in submicros per second (default 16000000) + // \note values above 16M cause the timers to compensate by running slower + // \note values below 16M cause the timers to compensate by running faster + void setPeriodRatioSubMicros(unsigned long value); + + // set process to run immediately on the next pass (within its priority level) + // \param handle: task handle + IRAM_ATTR inline void immediate(uint8_t handle) { if (handle != 0 && allocated[handle - 1]) { task[handle - 1]->immediate = true; } } + + // change process duration + // \param handle: task handle + // \param duration: in milliseconds use 0 to disable + void setDuration(uint8_t handle, unsigned long duration); + + // late removal of process task + // \param handle: task handle + // \param note removes a task process, even if it is running + void setDurationComplete(uint8_t handle); + + // change process repeat + // \param handle: task handle + // \param repeat: true to repeat indefinately + void setRepeat(uint8_t handle, bool repeat); + + // change process priority level + // \param handle: task handle + // \param priority: highest 0 to 7 lowest + void setPriority(uint8_t handle, uint8_t priority); + + // gets process priority level, highest 0 to 7 lowest or -1 on failure + // \param handle: task handle + inline int getPriority(uint8_t handle) { if (handle != 0 && allocated[handle - 1]) { return task[handle - 1]->getPriority(); } else return -1; } + + // set the process name + // \param handle: task handle + // \param name: up to 7 chars + void setNameStr(uint8_t handle, const char name[]); + + // get the process name + // \param handle: task handle + char *getNameStr(uint8_t handle); + + // get first task, returns handle or 0 if no tasks are found + uint8_t getFirstHandle(); + + // get next task, returns handle or 0 if no tasks are found + uint8_t getNextHandle(uint8_t handle); + + // search for task by name, returns handle or 0 if not found + // \param name: up to 7 chars + uint8_t getHandleByName(const char name[]); + + #ifdef TASKS_PROFILER_ENABLE + double getArrivalAvg(uint8_t handle); + double getArrivalMax(uint8_t handle); + double getRuntimeTotal(uint8_t handle); + long getRuntimeTotalCount(uint8_t handle); + double getRuntimeMax(uint8_t handle); + #endif + + // runs higher priority tasks at their prescribed interval + // \note processes that are already running are ignored so it's ok to yield() within a process + // \note processes assigned to hardware timers run outside this mechanism + // \note each call can trigger running at most a single process + void yield(); + + // runs higher priority tasks at their prescribed interval + // \param milliseconds: time to repeatedly run any tasks that need servicing + // \note processes that are already running are ignored so it's ok to yield() within a process + // \note processes assigned to hardware timers run outside this mechanism + void yield(unsigned long milliseconds); + + // runs higher priority tasks at their prescribed interval + // \param microseconds: time to repeatedly run any tasks that need servicing + // \note processes that are already running are ignored so it's ok to yield() within a process + // \note processes assigned to hardware timers run outside this mechanism + void yieldMicros(unsigned long microseconds); + + // runs all other tasks at their prescribed interval + // \note processes that are already running are ignored so it's ok to yield() within a process + // \note processes assigned to hardware timers run outside this mechanism + // \note each call can trigger running at most a single process + void yieldAll(); + + // runs all other tasks at their prescribed interval + // \param milliseconds: time to repeatedly run any tasks that need servicing + // \note processes that are already running are ignored so it's ok to yield() within a process + // \note processes assigned to hardware timers run outside this mechanism + void yieldAll(unsigned long milliseconds); + + // runs all other tasks at their prescribed interval + // \param microseconds: time to repeatedly run any tasks that need servicing + // \note processes that are already running are ignored so it's ok to yield() within a process + // \note processes assigned to hardware timers run outside this mechanism + void yieldAllMicros(unsigned long microseconds); + + private: + // keep track of the range of priorities so we don't waste cycles looking at empty ones + void updatePriorityRange(); + // keep track of the range of tasks so we don't waste cycles looking at empty ones + void updateEventRange(); + + uint8_t highest_task = 0; // the highest task# assigned + uint8_t highest_priority = 0; // the highest task priority + uint8_t count = 0; + uint8_t num_tasks = 0; // the total number of tasks + uint8_t number[8] = {255, 255, 255, 255, 255, 255, 255, 255}; // the task# we are servicing at this priority level + bool allocated[TASKS_MAX]; + bool hardware_timer_allocated[4] = {false, false, false, false}; + Task *task[TASKS_MAX]; +}; + +extern Tasks tasks; diff --git a/src/lib/tasks/OnTaskExample.ino.txt b/src/lib/tasks/OnTaskExample.ino.txt new file mode 100755 index 0000000..e1973ae --- /dev/null +++ b/src/lib/tasks/OnTaskExample.ino.txt @@ -0,0 +1,189 @@ +/* + * Title OnTask + * by Howard Dutton + * + * Copyright (C) 2021 Howard Dutton + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * Author: Howard Dutton + * http://www.stellarjourney.com + * hjd1964@gmail.com + * + */ + +// ----------------------------------------------------------------------------------- +// some constants +#define OFF 0 +#define ON 1 + +// ----------------------------------------------------------------------------------- +// configure settings in this section to experiment, enable/disable the various tasks + +#define SERIAL_BAUD 115200 + +// feature name ON/OFF state +#define CLOCK_TICK ON +#define CLOCK_EXTRA_NOTHING ON +#define SHOW_CLOCK_EVERY_SEC OFF +#define SHOW_MESSAGE_FOUR_TIMES ON +#define SHOW_MESSAGE_10SEC ON +#define SHOW_MESSAGE_HALFSEC_FOR_4SEC ON +#define PROFILER_VT100 ON +#define SHOW_TASKS_PROFILER_EVERY_SEC ON + +// uncomment the line below to enable using an hardware timer for the CLOCK_TICK +#define TASKS_HWTIMER1_ENABLE + +// ----------------------------------------------------------------------------------- + +#if SHOW_TASKS_PROFILER_EVERY_SEC == ON + #define TASKS_PROFILER_ENABLE +#endif + +#include "OnTask.h" + +#if SHOW_TASKS_PROFILER_EVERY_SEC == ON + #include "profiler.h" +#endif + +// handles to keep track of tasks +int clockCountHandle=0; +int fourTimesHandle =0; +int profilerHandle =0; + +void setup() { + Serial.begin(SERIAL_BAUD); + Serial1.begin(SERIAL_BAUD); + + Serial.println(); + Serial.println("Creating tasks..."); + Serial.println(); + + // this helps show what the parameters are: + // handle = tasks.add(period_ms, duration_ms, repeat_true_or_false, priority_0to7, callback_function); + // success = tasks.requestHardwareTimer(handle, hardware_timer_number_1to4, hardware_timer_priority_0to255); + + // create an task, the callback function "clockCount" is the process for this handler +#if CLOCK_TICK == ON + clockCountHandle = tasks.add(1, 0, true, 0, clockCount, "ClkCnt"); + Serial.print("ClockCount running every millisecond"); + // move this task to a hardware timer + bool success = tasks.requestHardwareTimer(clockCountHandle,1); + if (success) Serial.println(" on HardwareTimer1"); else Serial.println(); +#endif + + // create/start another task to throw some extra work at the task manager +#if CLOCK_EXTRA_NOTHING == ON + tasks.add(1, 0, true, 0, clockExtraNothing, "EtraNth"); +#endif + + // create/start another task to show the clock at 1 second intervals +#if SHOW_CLOCK_EVERY_SEC == ON + tasks.add(1000, 0, true, 2, showClock, "ShowClk"); + Serial.println("ShowClock: running every second"); +#endif + + // start another task with a period of 1 second which it then adds 2 seconds to each time it runs (before it stops itself) +#if SHOW_MESSAGE_FOUR_TIMES == ON + fourTimesHandle=tasks.add(1000, 0, true, 2, fourTimes, "FourTms"); + Serial.println("FourTimes: running every 1,2,4,6 seconds"); +#endif + + // create/start another task with a period of 10 seconds +#if SHOW_MESSAGE_10SEC == ON + tasks.add(10000, 0, true, 4, longCalc, "LngCalc"); + Serial.println("TenSec: running every 10 seconds"); +#endif + + // create/start another task with a period of 0.5 seconds and a duration of 4 seconds +#if SHOW_MESSAGE_HALFSEC_FOR_4SEC == ON + tasks.add(2000, 4000, true, 3, twoSec, "TwoSecs"); + Serial.println("TwoSec: running every 2 seconds, for 4 seconds"); + Serial.println(); +#endif + + // create/start a task to show the profiler at work +#if SHOW_TASKS_PROFILER_EVERY_SEC == ON + profilerHandle = tasks.add(250, 0, true, 2, profiler, "Profilr"); + Serial.println("Profiler: running every second"); + Serial.println(); +#endif +} + +void loop() { + tasks.yield(); +} + +// ----------------------------------------------------------------------------------- +// task processes + +// runs 1000 times a second in a hardware timer, if available, for a millisecond clock +volatile long cs=0; +void clockCount() { + cs++; +} + +// runs 1000 times a second in the task scheduler, to waste some time +void clockExtraNothing() { + noInterrupts(); + cs++; + cs--; + interrupts(); +} + +// show the clock time +void showClock() { + // Y macros allow any higher priority processes to run + Serial.print("[ShowClock ] timer vs. millis() = "); Y; + noInterrupts(); + long centiSec = cs; + interrupts(); + Serial.print((long)(millis()-centiSec)); Y; + Serial.println(" ms"); +} + +// show a long process working in the background +void longCalc() { + Serial.println("[LongCalc ] started (sending progress to Serial1)"); + for (long j=0; j<100; j++) { + for (long i=0; i<300; i++) { + delayMicroseconds(50); Y; // burn some time + } + Serial1.print("."); + } + Serial1.println(); Y; + Serial.println("[LongCalc ] done"); +} + +// run four times then stop, first at 1 seconds, then 2 seconds, etc. +void fourTimes() { + static int period = 0; + if (period <= 4000) { + period += 2000; + tasks.setPeriod(fourTimesHandle,period); + Serial.print("[FourTimes ] next pass in "); Y; + Serial.print(period); Y; + Serial.println(" ms"); + } else { + tasks.setPeriod(fourTimesHandle,0); + Serial.println("[FourTimes ] done"); + } +} + +// run four times then stop, first at 1 seconds, then 2 seconds, etc. +void twoSec() { + Serial.println("[TwoSec ] ran"); Y; +} diff --git a/src/lib/tls/PPS.cpp b/src/lib/tls/PPS.cpp new file mode 100755 index 0000000..b73efe0 --- /dev/null +++ b/src/lib/tls/PPS.cpp @@ -0,0 +1,38 @@ +// ----------------------------------------------------------------------------------- +// Pulse Per Second precision timer skew + +#include "PPS.h" + +#if defined(TIME_LOCATION_PPS_SENSE) && (TIME_LOCATION_PPS_SENSE) != OFF + +#include "../tasks/OnTask.h" + +void ppsIsr() { + unsigned long t = micros(); + unsigned long oneSecond = t - pps.lastMicros; + if (oneSecond > 1000000UL - PPS_WINDOW_MICROS && oneSecond < 1000000UL + PPS_WINDOW_MICROS) { + pps.averageSubMicros = (pps.averageSubMicros*(PPS_SECS_TO_AVERAGE - 1) + (oneSecond*16))/PPS_SECS_TO_AVERAGE; + pps.synced = true; + tasks.setPeriodRatioSubMicros((long)pps.averageSubMicros + pps.averageSubMicrosSkew); + } else { + pps.synced = false; + tasks.setPeriodRatioSubMicros(16000000UL); + } + pps.lastMicros = t; +} + +void Pps::init() { + VLF("MSG: PPS, attaching ISR to sense input"); + pinMode(PPS_SENSE_PIN, INPUT_PULLUP); + #if (TIME_LOCATION_PPS_SENSE) == HIGH + attachInterrupt(digitalPinToInterrupt(PPS_SENSE_PIN), ppsIsr, RISING); + #elif (TIME_LOCATION_PPS_SENSE) == LOW + attachInterrupt(digitalPinToInterrupt(PPS_SENSE_PIN), ppsIsr, FALLING); + #elif (TIME_LOCATION_PPS_SENSE) == BOTH + attachInterrupt(digitalPinToInterrupt(PPS_SENSE_PIN), ppsIsr, CHANGE); + #endif +} + +Pps pps; + +#endif diff --git a/src/lib/tls/PPS.h b/src/lib/tls/PPS.h new file mode 100755 index 0000000..ffe8515 --- /dev/null +++ b/src/lib/tls/PPS.h @@ -0,0 +1,30 @@ +// handle PPS interrupt +#pragma once + +#include "../../Common.h" + +#if defined(TIME_LOCATION_PPS_SENSE) && (TIME_LOCATION_PPS_SENSE) != OFF + +#define PPS_SECS_TO_AVERAGE 40 // running average of 40 samples (1 per second) +#define PPS_WINDOW_MICROS 20000 // +/- window in microseconds to meet synced criteria (2%) + +#if !defined(PPS_SENSE_PIN) || PPS_SENSE_PIN == OFF + #error "Configuration (Config.h): PPS_SENSE_PIN must be defined for TIME_LOCATION_PPS_SENSE ON" +#endif + +class Pps { + public: + // attach interrupt and start PPS + void init(); + + volatile bool synced = false; + + volatile unsigned long averageSubMicros = 16000000UL; + long averageSubMicrosSkew = 0; + volatile unsigned long lastMicros = 0UL; + private: +}; + +extern Pps pps; + +#endif diff --git a/src/lib/tls/Tls.h b/src/lib/tls/Tls.h new file mode 100755 index 0000000..e21efe4 --- /dev/null +++ b/src/lib/tls/Tls.h @@ -0,0 +1,13 @@ + +// ----------------------------------------------------------------------------------- +// Time/Location source +#pragma once + +#include "ds3231/DS3231.h" +#include "ds3234/DS3234.h" +#include "gps/GPS.h" +#include "ntp/NTP.h" +#include "sd3031/SD3031.h" +#include "teensy/Teensy.h" + +#include "PPS.h" diff --git a/src/lib/tls/TlsBase.cpp b/src/lib/tls/TlsBase.cpp new file mode 100755 index 0000000..96a93d5 --- /dev/null +++ b/src/lib/tls/TlsBase.cpp @@ -0,0 +1,2 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source diff --git a/src/lib/tls/TlsBase.h b/src/lib/tls/TlsBase.h new file mode 100755 index 0000000..da47de4 --- /dev/null +++ b/src/lib/tls/TlsBase.h @@ -0,0 +1,45 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source +#pragma once + +#include "../../Common.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE != OFF + +#include "../calendars/Calendars.h" + +class TimeLocationSource { + public: + // initialize (also enables the RTC PPS if available) + virtual bool init(); + + // restart the device + virtual void restart() {} + + // true if date/time ready + inline bool isReady() { return ready; } + + // set the RTC's time + virtual void set(JulianDate ut1); + virtual void set(int year, int month, int day, int hour, int minute, int second); + + // get the RTC's time, returns true if current + virtual bool get(JulianDate &ut1); + + // get the location + virtual void getSite(double &latitude, double &longitude, float &elevation) { UNUSED(latitude); UNUSED(longitude); UNUSED(elevation); } + + // secondary way to enable PPS + virtual void ppsEnable() {} + + // update from GPS or NTP + virtual void poll() {} + + // not used, date/time is stored as UT1 + double DUT1 = 0.0L; + + protected: + bool ready = false; +}; + +#endif diff --git a/src/lib/tls/ds3231/DS3231.cpp b/src/lib/tls/ds3231/DS3231.cpp new file mode 100755 index 0000000..55e02cb --- /dev/null +++ b/src/lib/tls/ds3231/DS3231.cpp @@ -0,0 +1,111 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source DS3231 RTC support +// uses the default I2C port in most cases; though HAL_WIRE can redirect to another port (as is done for the Teensy3.5/3.6) + +#include "DS3231.h" + +#if (defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == DS3231) || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == DS3231) + +#ifdef TLS_TIMELIB + #include // https://github.com/PaulStoffregen/Time/archive/master.zip +#endif + +#include // https://github.com/Makuna/Rtc/archive/master.zip +RtcDS3231 rtcDS3231(HAL_WIRE); + +#include "../PPS.h" + +bool TlsDs3231::init() { + HAL_WIRE.begin(); + HAL_WIRE_SET_CLOCK(); + + HAL_WIRE.beginTransmission(0x68); + bool error = HAL_WIRE.endTransmission() != 0; + if (!error) { + rtcDS3231.Begin(); + HAL_WIRE_SET_CLOCK(); + + if (!rtcDS3231.GetIsRunning()) rtcDS3231.SetIsRunning(true); + + // see if the RTC is present + if (rtcDS3231.GetIsRunning()) { + #if TIME_LOCATION_SOURCE == DS3231 + ppsEnable(); + #endif + + #ifdef TLS_TIMELIB + RtcDateTime now = rtcDS3231.GetDateTime(); + setTime(now.Hour(), now.Minute(), now.Second(), now.Day(), now.Month(), now.Year()); + #endif + + ready = true; + } else { DLF("WRN: tls.init(), DS3231 GetIsRunning() false"); } + } else { DLF("WRN: tls.init(), DS3231 (I2C 0x68) not found"); } + #ifdef HAL_WIRE_RESET_AFTER_CONNECT + HAL_WIRE.end(); + HAL_WIRE.begin(); + HAL_WIRE_SET_CLOCK(); + #endif + return ready; +} + +void TlsDs3231::set(JulianDate ut1) { + if (!ready) return; + + GregorianDate greg = calendars.julianDayToGregorian(ut1); + + double f1 = fabs(ut1.hour) + TLS_CLOCK_SKEW; + int h = floor(f1); + double m = (f1 - h)*60.0; + double s = (m - floor(m))*60.0; + + set(greg.year, greg.month, greg.day, h, floor(m), floor(s)); +} + +void TlsDs3231::set(int year, int month, int day, int hour, int minute, int second) { + #ifdef TLS_TIMELIB + setTime(hour, minute, second, day, month, year); + #endif + RtcDateTime updateTime = RtcDateTime(year, month, day, hour, minute, second); + rtcDS3231.SetDateTime(updateTime); +} + +bool TlsDs3231::get(JulianDate &ut1) { + if (!ready) return false; + + unsigned long t = 0.5; + #if defined(TIME_LOCATION_PPS_SENSE) && (TIME_LOCATION_PPS_SENSE) != OFF + // wait until we're roughly in the middle of a second + if (pps.synced) { + do { t = micros() - pps.lastMicros; } while (t < 400000 || t > 600000); + } + #endif + + GregorianDate greg; + RtcDateTime now = rtcDS3231.GetDateTime(); + if (now.Year() >= 2018 && now.Year() <= 3000 && now.Month() >= 1 && now.Month() <= 12 && now.Day() >= 1 && now.Day() <= 31 && + now.Hour() <= 23 && now.Minute() <= 59 && now.Second() <= 59) { + greg.year = now.Year(); + greg.month = now.Month(); + greg.day = now.Day(); + ut1 = calendars.gregorianToJulianDay(greg); + ut1.hour = now.Hour() + now.Minute()/60.0 + (now.Second() + t/1000000.0)/3600.0; + } else { + greg.year = 2000; + greg.month = 1; + greg.day = 1; + ut1 = calendars.gregorianToJulianDay(greg); + ut1.hour = 0.0; + } + + return true; +} + +void TlsDs3231::ppsEnable() { + // frequency 0 (1Hz) on the SQW pin + rtcDS3231.SetSquareWavePin(DS3231SquareWavePin_ModeClock); + rtcDS3231.SetSquareWavePinClockFrequency(DS3231SquareWaveClock_1Hz); +} + +#endif diff --git a/src/lib/tls/ds3231/DS3231.h b/src/lib/tls/ds3231/DS3231.h new file mode 100755 index 0000000..dc4b430 --- /dev/null +++ b/src/lib/tls/ds3231/DS3231.h @@ -0,0 +1,36 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source DS3231 RTC support +// uses the default I2C port in most cases; though HAL_WIRE can redirect to another port (as is done for the Teensy3.5/3.6) +#pragma once + +#include "../TlsBase.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == DS3231 || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == DS3231) + +#ifndef TLS_CLOCK_SKEW + #define TLS_CLOCK_SKEW 0.000139 // in hours, +5 seconds ahead when setting time +#endif + +class TlsDs3231 : public TimeLocationSource { + public: + // initialize (also enables the RTC PPS if available) + bool init(); + + // set the RTC's time + void set(JulianDate ut1); + void set(int year, int month, int day, int hour, int minute, int second); + + // get the RTC's time + bool get(JulianDate &ut1); + + // secondary way to enable PPS + void ppsEnable(); + + // not used, date/time is stored as UT1 + double DUT1 = 0.0L; + + private: +}; + +#endif diff --git a/src/lib/tls/ds3234/DS3234.cpp b/src/lib/tls/ds3234/DS3234.cpp new file mode 100755 index 0000000..0a2dea4 --- /dev/null +++ b/src/lib/tls/ds3234/DS3234.cpp @@ -0,0 +1,99 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source DS3234 RTC support +// uses the default SPI port + +#include "DS3234.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == DS3234 || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == DS3234) + + +#ifdef TLS_TIMELIB + #include // https://github.com/PaulStoffregen/Time/archive/master.zip +#endif + +#include +#include // https://github.com/Makuna/Rtc/archive/master.zip +RtcDS3234 rtcDS3234(SPI, DS3234_CS_PIN); + +bool TlsDs3234::init() { + SPI.begin(); + rtcDS3234.Begin(); + if (!rtcDS3234.GetIsRunning()) rtcDS3234.SetIsRunning(true); + + // see if the RTC is present + if (rtcDS3234.GetIsRunning()) { + // frequency 0 (1Hz) on the SQW pin + rtcDS3234.SetSquareWavePin(DS3234SquareWavePin_ModeClock); + rtcDS3234.SetSquareWavePinClockFrequency(DS3234SquareWaveClock_1Hz); + + #ifdef TLS_TIMELIB + RtcDateTime now = rtcDS3234.GetDateTime(); + setTime(now.Hour(), now.Minute(), now.Second(), now.Day(), now.Month(), now.Year()); + #endif + + ready = true; + } else DLF("WRN: tls.init(), DS3234 GetIsRunning() false"); + #ifdef SSPI_SHARED + SPI.end(); + #endif + return ready; +} + +void TlsDs3234::set(JulianDate ut1) { + if (!ready) return; + + GregorianDate greg = calendars.julianDayToGregorian(ut1); + + double f1 = fabs(ut1.hour) + TLS_CLOCK_SKEW; + int h = floor(f1); + double m = (f1 - h)*60.0; + double s = (m - floor(m))*60.0; + + set(greg.year, greg.month, greg.day, h, floor(m), floor(s)); +} + +void TlsDs3234::set(int year, int month, int day, int hour, int minute, int second) { + #ifdef TLS_TIMELIB + setTime(hour, minute, second, day, month, year); + #endif + #ifdef SSPI_SHARED + SPI.begin(); + #endif + RtcDateTime updateTime = RtcDateTime(year, month, day, hour, minute, second); + rtcDS3234.SetDateTime(updateTime); + #ifdef SSPI_SHARED + SPI.end(); + #endif +} + +bool TlsDs3234::get(JulianDate &ut1) { + if (!ready) return false; + + #ifdef SSPI_SHARED + SPI.begin(); + #endif + GregorianDate greg; + RtcDateTime now = rtcDS3234.GetDateTime(); + if (now.Year() >= 2018 && now.Year() <= 3000 && now.Month() >= 1 && now.Month() <= 12 && now.Day() >= 1 && now.Day() <= 31 && + now.Hour() <= 23 && now.Minute() <= 59 && now.Second() <= 59) { + greg.year = now.Year(); + greg.month = now.Month(); + greg.day = now.Day(); + ut1 = calendars.gregorianToJulianDay(greg); + ut1.hour = now.Hour() + now.Minute()/60.0 + now.Second()/3600.0; + } else { + greg.year = 2000; + greg.month = 1; + greg.day = 1; + ut1 = calendars.gregorianToJulianDay(greg); + ut1.hour = 0.0; + } + #ifdef SSPI_SHARED + SPI.end(); + #endif + + return true; +} + +#endif \ No newline at end of file diff --git a/src/lib/tls/ds3234/DS3234.h b/src/lib/tls/ds3234/DS3234.h new file mode 100755 index 0000000..9ec32d6 --- /dev/null +++ b/src/lib/tls/ds3234/DS3234.h @@ -0,0 +1,37 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source DS3234 RTC support +// uses the default SPI port +#pragma once + +#include "../TlsBase.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == DS3234 || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == DS3234) + +#if !defined(DS3234_CS_PIN) || DS3234_CS_PIN == OFF + #error "Configuration (Config.h): DS3234_CS_PIN must be defined for TIME_LOCATION_SOURCE DS3234" +#endif + +#ifndef TLS_CLOCK_SKEW +#define TLS_CLOCK_SKEW 0.000139 // in hours, +5 seconds ahead when setting time +#endif + +class TlsDs3234 : public TimeLocationSource { + public: + // initialize (also enables the RTC PPS if available) + bool init(); + + // set the RTC's time + void set(JulianDate ut1); + void set(int year, int month, int day, int hour, int minute, int second); + + // get the RTC's time + bool get(JulianDate &ut1); + + // not used, date/time is stored as UT1 + double DUT1 = 0.0L; + + private: +}; + +#endif \ No newline at end of file diff --git a/src/lib/tls/gps/GPS.cpp b/src/lib/tls/gps/GPS.cpp new file mode 100755 index 0000000..b2f77fa --- /dev/null +++ b/src/lib/tls/gps/GPS.cpp @@ -0,0 +1,208 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source GPS support +// uses the specified serial port + +#include "GPS.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == GPS + +#ifdef TLS_TIMELIB + #include // https://github.com/PaulStoffregen/Time/archive/master.zip +#endif + +#ifndef SERIAL_GPS + #error "SERIAL_GPS must be set to the serial port object if TIME_LOCATION_SOURCE GPS is used" +#endif + +#ifndef SERIAL_GPS_BAUD + #error "SERIAL_GPS_BAUD must be set to the baud rate if TIME_LOCATION_SOURCE GPS is used" +#endif + +#if SERIAL_GPS == SoftSerial || SERIAL_GPS == HardSerial + #ifndef SERIAL_GPS_RX + #error "SERIAL_GPS_RX must be set to the serial port RX pin if SoftSerial or HardSerial is used" + #endif + #ifndef SERIAL_GPS_TX + #error "SERIAL_GPS_TX must be set to the serial port TX pin if SoftSerial or HardSerial is used" + #endif +#endif + +#include "../PPS.h" +#include "../../tasks/OnTask.h" + +#include // https://github.com/mikalhart/TinyGPSPlus/releases +TinyGPSPlus gps; + +// provide for using software serial +#if SERIAL_GPS == SoftSerial + #include + #undef SERIAL_GPS + SoftwareSerial SWSerialGPS(SERIAL_GPS_RX, SERIAL_GPS_TX); + #define SERIAL_GPS SWSerialGPS + #define SERIAL_GPS_RXTX_SET +#endif + +// provide for using hardware serial +#if SERIAL_GPS == HardSerial + #include + #undef SERIAL_GPS + HardwareSerial HWSerialGPS(SERIAL_GPS_RX, SERIAL_GPS_TX); + #define SERIAL_GPS HWSerialGPS + #define SERIAL_GPS_RXTX_SET +#endif + +TlsGPS *tlsGps; + +void gpsPoll() { + #if (TIME_LOCATION_PPS_SENSE) != OFF + if (pps.synced) { + #endif + + if (!tlsGps->isReady()) tlsGps->poll(); + + #if (TIME_LOCATION_PPS_SENSE) != OFF + } + #endif +} + +// initialize +bool TlsGPS::init() { + tlsGps = this; + + #if defined(SERIAL_GPS_RX) && defined(SERIAL_GPS_TX) && !defined(SERIAL_GPS_RXTX_SET) + SERIAL_GPS.begin(SERIAL_GPS_BAUD, SERIAL_8N1, SERIAL_GPS_RX, SERIAL_GPS_TX); + #else + SERIAL_GPS.begin(SERIAL_GPS_BAUD); + #endif + activeLink = true; + + // check to see if the GPS is present + tasks.yield(1000); + if (!SERIAL_GPS.available()) { + DLF("WRN: TLS, GPS serial RX interface is quiet!"); + return false; + } else { + unsigned long timeout = millis() + 1000U; + while (SERIAL_GPS.available() > 0) { + if (gps.encode(SERIAL_GPS.read())) break; + if ((long)(millis() - timeout) > 0) { + DLF("WRN: TLS, GPS serial RX interface no NMEA sentences detected!"); + return false; + } + Y; + } + } + + VF("MSG: TLS, GPS start monitor task (rate 10ms priority 7)... "); + if (tasks.add(10, 0, true, 7, gpsPoll, "gpsPoll")) { VLF("success"); active = true; } else { VLF("FAILED!"); } + + return active; +} + +void TlsGPS::set(JulianDate ut1) { + ut1 = ut1; +} + +void TlsGPS::set(int year, int month, int day, int hour, int minute, int second) { + #ifdef TLS_TIMELIB + setTime(hour, minute, second, day, month, year); + #else + (void)year; (void)month; (void)day; (void)hour; (void)minute; (void)second; + #endif +} + +bool TlsGPS::get(JulianDate &ut1) { + if (!ready) return false; + if (!timeIsValid()) return false; + + // get the date/time right now if the serial link is active + bool sentence = true; + if (activeLink) { + while (SERIAL_GPS.available() > 0) { + sentence = gps.encode(SERIAL_GPS.read()); + } + } + + GregorianDate greg; + greg.year = gps.date.year(); + greg.month = gps.date.month(); + greg.day = gps.date.day(); + ut1 = calendars.gregorianToJulianDay(greg); + // DUT1 = UT1 − UTC + // UT1 = DUT1 + UTC + + double second = gps.time.second() + gps.time.age()/1000.0; + #if defined(TIME_LOCATION_PPS_SENSE) && (TIME_LOCATION_PPS_SENSE) != OFF + if (pps.synced) { + unsigned long t; + // wait until we're roughly in the middle of a second + do { t = micros() - pps.lastMicros; } while (t < 400000 || t > 600000); + second = floor(gps.time.second() + gps.time.age()/1000.0) + t/1000000.0; + } + #endif + + ut1.hour = gps.time.hour() + gps.time.minute()/60.0 + (second + DUT1)/3600.0; + + // adjust date/time for DUT1 as needed + if (ut1.hour >= 24.0L) { ut1.hour -= 24.0L; ut1.day += 1.0L; } else + if (ut1.hour < 0.0L) { ut1.hour += 24.0L; ut1.day -= 1.0L; } + + // shutdown the link if the date/time is current + if (sentence && activeLink) { + VLF("MSG: TLS, closing GPS serial port"); + SERIAL_GPS.end(); + activeLink = false; + } + + return sentence; +} + +void TlsGPS::getSite(double &latitude, double &longitude, float &elevation) { + if (!ready) return; + if (!siteIsValid()) return; + + latitude = gps.location.lat(); + longitude = -gps.location.lng(); + elevation = gps.altitude.meters(); +} + +void TlsGPS::poll() { + while (SERIAL_GPS.available() > 0) { + gps.encode(SERIAL_GPS.read()); + } + + if (gps.location.isValid() && siteIsValid()) { + if (gps.date.isValid() && gps.time.isValid() && timeIsValid()) { + if (waitIsValid()) { + VLF("MSG: TLS, GPS date/time/location is ready"); + + VLF("MSG: TLS, stopping GPS monitor task"); + tasks.setDurationComplete(tasks.getHandleByName("gpsPoll")); + + #ifdef TLS_TIMELIB + setTime(gps.time.hour(), gps.time.minute(), gps.time.second(), gps.date.day(), gps.date.month(), gps.date.year()); + #endif + + ready = true; + } + } + } +} + +// starts keeping track of the wait once (PPS is synced, if applicable) and GPS has a lock +bool TlsGPS::waitIsValid() { + if (startTime == 0) startTime = millis(); + unsigned long t = millis() - startTime; + return (t/1000UL)/60UL >= GPS_MIN_WAIT_MINUTES; +} + +bool TlsGPS::timeIsValid() { + if (gps.date.year() <= 3000 && gps.date.month() >= 1 && gps.date.month() <= 12 && gps.date.day() >= 1 && gps.date.day() <= 31 && + gps.time.hour() <= 23 && gps.time.minute() <= 59 && gps.time.second() <= 59) return true; else return false; +} + +bool TlsGPS::siteIsValid() { + if (gps.location.lat() >= -90 && gps.location.lat() <= 90 && gps.location.lng() >= -360 && gps.location.lng() <= 360) return true; else return false; +} + +#endif diff --git a/src/lib/tls/gps/GPS.h b/src/lib/tls/gps/GPS.h new file mode 100755 index 0000000..5494ccc --- /dev/null +++ b/src/lib/tls/gps/GPS.h @@ -0,0 +1,55 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source GPS support +// uses the specified serial port +#pragma once + +#include "../TlsBase.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == GPS + +#ifndef GPS_TIMEOUT_MINUTES + #define GPS_TIMEOUT_MINUTES 10 // wait up to 10 minutes to get lock, use 0 to disable timeout +#endif +#ifndef GPS_MIN_WAIT_MINUTES + #define GPS_MIN_WAIT_MINUTES 2 // minimum wait for stabilization in minutes, use 0 to disable +#endif + +class TlsGPS : public TimeLocationSource { + public: + // initialize (also enables the RTC PPS if available) + // pass a hook to the tls you created so it can be called + bool init(); + + // set the RTC's time + void set(JulianDate ut1); + void set(int year, int month, int day, int hour, int minute, int second); + + // get the RTC's time + bool get(JulianDate &ut1); + + // get the location + void getSite(double &latitude, double &longitude, float &elevation); + + // update from GPS + void poll(); + + // for conversion from UTC to UT1 + double DUT1 = 0.0L; + + private: + // validate wait time + bool waitIsValid(); + + // validate time + bool timeIsValid(); + + // validate site + bool siteIsValid(); + + unsigned long startTime = 0; + + bool active = false; + bool activeLink = false; +}; + +#endif diff --git a/src/lib/tls/ntp/NTP.cpp b/src/lib/tls/ntp/NTP.cpp new file mode 100755 index 0000000..9bc5faa --- /dev/null +++ b/src/lib/tls/ntp/NTP.cpp @@ -0,0 +1,143 @@ +// ----------------------------------------------------------------------------------------------------------------- +// NTP time code +// from here: https://github.com/PaulStoffregen/Time + +#include "NTP.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == NTP + +#include // https://github.com/PaulStoffregen/Time/archive/master.zip + +#if OPERATIONAL_MODE == WIFI + WiFiUDP Udp; +#else + EthernetUDP Udp; +#endif + +#include "../../tasks/OnTask.h" + +IPAddress timeServer = IPAddress TIME_IP_ADDR; + +// local port to listen for UDP packets +unsigned int localPort = 8888; + +TlsNTP *tlsNtp; + +void ntpWrapper() { + if (!tlsNtp->isReady()) tlsNtp->poll(); +} + +// initialize +bool TlsNTP::init() { + tlsNtp = this; + + VF("MSG: TLS, start NTP monitor task (rate 5 min priority 7)... "); + handle = tasks.add(5*60*1000L, 0, true, 7, ntpWrapper, "ntp"); + if (handle) { + VLF("success"); + active = true; + } else { + VLF("FAILED!"); + active = false; + } + + // flag that start time is unknown + startTime = 0; + + ready = false; + + return active; +} + +void TlsNTP::restart() { +} + +void TlsNTP::set(JulianDate ut1) { + ut1 = ut1; +} + +void TlsNTP::set(int year, int month, int day, int hour, int minute, int second) { + #ifdef TLS_TIMELIB + setTime(hour, minute, second, day, month, year); + #else + (void)year; (void)month; (void)day; (void)hour; (void)minute; (void)second; + #endif +} + +bool TlsNTP::get(JulianDate &ut1) { + if (!ready) return false; + + if (year() >= 0 && year() <= 3000 && month() >= 1 && month() <= 12 && day() >= 1 && day() <= 31 && + hour() <= 23 && minute() <= 59 && second() <= 59) { + GregorianDate greg; greg.year = year(); greg.month = month(); greg.day = day(); + ut1 = calendars.gregorianToJulianDay(greg); + ut1.hour = hour() + minute()/60.0 + (second() + DUT1)/3600.0; + } + + return true; +} + +void TlsNTP::poll() { + Udp.begin(localPort); + + // discard any previously received packets + unsigned long tOut = millis() + 3000U; + while ((Udp.parsePacket() > 0) && ((long)(millis() - tOut) < 0)) Y; + + VLF("MSG: TLS, transmit NTP Request"); + sendNTPpacket(timeServer); + + unsigned long beginWait = millis(); + while (millis() - beginWait < 1500U) { + int size = Udp.parsePacket(); + if (size >= NTP_PACKET_SIZE) { + VLF("MSG: TLS, receive NTP Response"); + // read packet into the buffer + Udp.read(packetBuffer, NTP_PACKET_SIZE); + unsigned long secsSince1900; + // convert four bytes starting at location 40 to a long integer + secsSince1900 = (unsigned long)packetBuffer[40] << 24; + secsSince1900 |= (unsigned long)packetBuffer[41] << 16; + secsSince1900 |= (unsigned long)packetBuffer[42] << 8; + secsSince1900 |= (unsigned long)packetBuffer[43]; + time_t ntpTime = secsSince1900 - 2208988800UL; + setTime(ntpTime); + DLF("MSG: TLS, next NTP query in 24 hours"); + tasks.setPeriod(handle, 24L*60L*60L*1000L); + ready = true; + + Udp.stop(); + return; + } + Y; + } + DLF("MSG: TLS, no NTP Response :-("); + DLF("MSG: TLS, next NTP query in 5 minutes"); + tasks.setPeriod(handle, 5*60*1000L); + + Udp.stop(); +} + +// send an NTP request to the time server at the given address +void TlsNTP::sendNTPpacket(IPAddress &address) { + // set all bytes in the buffer to 0 + memset(packetBuffer, 0, NTP_PACKET_SIZE); + // Initialize values needed to form NTP request + // (see URL above for details on the packets) + packetBuffer[0] = 0b11100011; // LI, Version, Mode + packetBuffer[1] = 0; // Stratum, or type of clock + packetBuffer[2] = 6; // Polling Interval + packetBuffer[3] = 0xEC; // Peer Clock Precision + // 8 bytes of zero for Root Delay & Root Dispersion + packetBuffer[12] = 49; + packetBuffer[13] = 0x4E; + packetBuffer[14] = 49; + packetBuffer[15] = 52; + // all NTP fields have been given values, now + // you can send a packet requesting a timestamp: + Udp.beginPacket(address, 123); //NTP requests are to port 123 + Udp.write(packetBuffer, NTP_PACKET_SIZE); + Udp.endPacket(); +} + +#endif diff --git a/src/lib/tls/ntp/NTP.h b/src/lib/tls/ntp/NTP.h new file mode 100755 index 0000000..bd5d6f5 --- /dev/null +++ b/src/lib/tls/ntp/NTP.h @@ -0,0 +1,60 @@ +// ----------------------------------------------------------------------------------------------------------------- +// NTP time code +// from here: https://github.com/PaulStoffregen/Time +#pragma once + +#include "../TlsBase.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == NTP + +#if OPERATIONAL_MODE == WIFI + #include +#elif OPERATIONAL_MODE == ETHERNET_TEENSY41 + #include +#else + #include +#endif + +#ifndef NTP_TIMEOUT_SECONDS + #define NTP_TIMEOUT_SECONDS 300 // wait up to 5 minutes to get date/time, use 0 to disable timeout +#endif + +class TlsNTP : public TimeLocationSource { + public: + // initialize (also enables the RTC PPS if available) + bool init(); + + // restart UDP server + void restart(); + + // set the RTC's time + void set(JulianDate ut1); + void set(int year, int month, int day, int hour, int minute, int second); + + // get the RTC's time + bool get(JulianDate &ut1); + + // update from NTP + void poll(); + + // for conversion from UTC to UT1 + double DUT1 = 0.0L; + + private: + // send an NTP request to the time server at the given address + void sendNTPpacket(IPAddress &address); + + // NTP time is in the first 48 bytes of message + static const int NTP_PACKET_SIZE = 48; + + // buffer to hold incoming & outgoing packets + uint8_t packetBuffer[NTP_PACKET_SIZE]; + + unsigned long startTime = 0; + + bool active = false; + uint8_t handle = 0; + +}; + +#endif diff --git a/src/lib/tls/sd3031/SD3031.cpp b/src/lib/tls/sd3031/SD3031.cpp new file mode 100755 index 0000000..9e6c3ed --- /dev/null +++ b/src/lib/tls/sd3031/SD3031.cpp @@ -0,0 +1,85 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source SD3031 RTC support +// uses the default I2C port in most cases; though HAL_WIRE can redirect to another port (as is done for the Teensy3.5/3.6) + +#include "SD3031.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == SD3031 || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == SD3031) + +#ifdef TLS_TIMELIB + #include // https://github.com/PaulStoffregen/Time/archive/master.zip +#endif + +#include // https://github.com/cdjq/DFRobot_SD3031 +DFRobot_SD3031 rtcSD3031(&HAL_WIRE); + +bool TlsSd3031::init() { + HAL_WIRE.begin(); + HAL_WIRE_SET_CLOCK(); + + bool error = rtcSD3031.begin(); + if (!error) { + HAL_WIRE_SET_CLOCK(); + + rtcSD3031.setHourSystem(rtcSD3031.e24hours); + + #ifdef TLS_TIMELIB + sTimeData_t dateTime = rtcSD3031.getRTCTime(); + setTime(dateTime.hour, dateTime.minute, dateTime.second, dateTime.day, dateTime.month, dateTime.year); + #endif + + ready = true; + } else { DLF("WRN: tls.init(), SD3031 (I2C 0x32) not found"); } + #ifdef HAL_WIRE_RESET_AFTER_CONNECT + HAL_WIRE.end(); + HAL_WIRE.begin(); + HAL_WIRE_SET_CLOCK(); + #endif + return ready; +} + +void TlsSd3031::set(JulianDate ut1) { + if (!ready) return; + + GregorianDate greg = calendars.julianDayToGregorian(ut1); + + double f1 = fabs(ut1.hour) + TLS_CLOCK_SKEW; + int h = floor(f1); + double m = (f1 - h)*60.0; + double s = (m - floor(m))*60.0; + + set(greg.year, greg.month, greg.day, h, floor(m), floor(s)); +} + +void TlsSd3031::set(int year, int month, int day, int hour, int minute, int second) { + #ifdef TLS_TIMELIB + setTime(hour, minute, second, day, month, year); + #endif + rtcSD3031.setTime(year, month, day, hour, minute, second); +} + +bool TlsSd3031::get(JulianDate &ut1) { + if (!ready) return false; + + sTimeData_t dateTime = rtcSD3031.getRTCTime(); + + if (dateTime.year >= 2018 && dateTime.year <= 3000 && + dateTime.month >= 1 && dateTime.month <= 12 && + dateTime.day >= 1 && dateTime.day <= 31 && + dateTime.hour <= 23 && dateTime.minute <= 59 && dateTime.second <= 59) { + GregorianDate greg; + greg.year = dateTime.year; greg.month = dateTime.month; greg.day = dateTime.day; + ut1 = calendars.gregorianToJulianDay(greg); + ut1.hour = dateTime.hour + dateTime.minute/60.0 + dateTime.second/3600.0; + } + + return true; +} + +void TlsSd3031::ppsEnable() { + // frequency 0 (1Hz) on the SQW pin + rtcSD3031.enableFrequency(rtcSD3031.eHz_1Second); +} + +#endif diff --git a/src/lib/tls/sd3031/SD3031.h b/src/lib/tls/sd3031/SD3031.h new file mode 100755 index 0000000..df87169 --- /dev/null +++ b/src/lib/tls/sd3031/SD3031.h @@ -0,0 +1,36 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source SD3031 RTC support +// uses the default I2C port in most cases; though HAL_WIRE can redirect to another port (as is done for the Teensy3.5/3.6) +#pragma once + +#include "../TlsBase.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == SD3031 || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == SD3031) + +#ifndef TLS_CLOCK_SKEW + #define TLS_CLOCK_SKEW 0.0 // in hours +#endif + +class TlsSd3031 : public TimeLocationSource { + public: + // initialize (also enables the RTC PPS if available) + bool init(); + + // set the RTC's time + void set(JulianDate ut1); + void set(int year, int month, int day, int hour, int minute, int second); + + // get the RTC's time + bool get(JulianDate &ut1); + + // secondary way to enable PPS + void ppsEnable(); + + // not used, date/time is stored as UT1 + double DUT1 = 0.0L; + + private: +}; + +#endif diff --git a/src/lib/tls/teensy/Teensy.cpp b/src/lib/tls/teensy/Teensy.cpp new file mode 100755 index 0000000..9996671 --- /dev/null +++ b/src/lib/tls/teensy/Teensy.cpp @@ -0,0 +1,49 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source TEENSY 3.2 RTC support + +#include "Teensy.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == TEENSY || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == TEENSY) + +#include // https://github.com/PaulStoffregen/Time/archive/master.zip + +bool TlsTeensy::init() { + ready = true; + return ready; +} + +void TlsTeensy::set(JulianDate ut1) { + GregorianDate greg = calendars.julianDayToGregorian(ut1); + + double f1 = fabs(ut1.hour) + TLS_CLOCK_SKEW; + int h = floor(f1); + double m = (f1 - h)*60.0; + double s = (m - floor(m))*60.0; + + set(greg.year, greg.month, greg.day, h, floor(m), floor(s)); +} + +void TlsTeensy::set(int year, int month, int day, int hour, int minute, int second) { + setTime(hour, minute, second, day, month, year); + unsigned long TeensyTime = now(); // get time in epoch + Teensy3Clock.set(TeensyTime); // set Teensy time +} + +bool TlsTeensy::get(JulianDate &ut1) { + if (!ready) return false; + + unsigned long TeensyTime = Teensy3Clock.get(); // get time from Teensy RTC + setTime(TeensyTime); // set system time + + if (year() >= 0 && year() <= 3000 && month() >= 1 && month() <= 12 && day() >= 1 && day() <= 31 && + hour() <= 23 && minute() <= 59 && second() <= 59) { + GregorianDate greg; greg.year = year(); greg.month = month(); greg.day = day(); + ut1 = calendars.gregorianToJulianDay(greg); + ut1.hour = hour() + minute()/60.0 + second()/3600.0; + } + + return true; +} + +#endif \ No newline at end of file diff --git a/src/lib/tls/teensy/Teensy.h b/src/lib/tls/teensy/Teensy.h new file mode 100755 index 0000000..d974820 --- /dev/null +++ b/src/lib/tls/teensy/Teensy.h @@ -0,0 +1,32 @@ +// ----------------------------------------------------------------------------------- +// Time/Location source TEENSY 3.2 RTC support +#pragma once + +#include "../TlsBase.h" + +#if defined(TIME_LOCATION_SOURCE) && TIME_LOCATION_SOURCE == TEENSY || \ + (defined(TIME_LOCATION_SOURCE_FALLBACK) && TIME_LOCATION_SOURCE_FALLBACK == TEENSY) + +#ifndef TLS_CLOCK_SKEW + #define TLS_CLOCK_SKEW 0.000139 // in hours, +5 seconds ahead when setting time +#endif + +class TlsTeensy : public TimeLocationSource { + public: + // initialize (also enables the RTC PPS if available) + bool init(); + + // set the RTC's time + void set(JulianDate ut1); + void set(int year, int month, int day, int hour, int minute, int second); + + // get the RTC's time + bool get(JulianDate &ut1); + + // not used, date/time is stored as UT1 + double DUT1 = 0.0L; + + private: +}; + +#endif diff --git a/src/lib/watchdog/Watchdog.cpp b/src/lib/watchdog/Watchdog.cpp new file mode 100755 index 0000000..9729f36 --- /dev/null +++ b/src/lib/watchdog/Watchdog.cpp @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// General purpose "virtual" watchdog +#include "Watchdog.h" + +#if defined(WATCHDOG) && WATCHDOG != OFF || true + +#ifndef __AVR_ATmega2560__ + #include "../tasks/OnTask.h" + void watchdogWrapper() { watchdog.poll(); } +#else + void (*mega2560restart) (void) = 0; + #include +#endif + +void Watchdog::enable(int seconds) { + #ifndef __AVR_ATmega2560__ + if (this->seconds == -1) { + this->seconds = seconds; + + VF("MSG: Watchdog, start monitor task (100ms rate priority 0)... "); + uint8_t taskHandle = tasks.add(100, 0, true, 0, watchdogWrapper, "wdog"); + + if (taskHandle) { + VLF("success"); + if (!tasks.requestHardwareTimer(taskHandle, 0)) { DLF("WRN: Watchdog::enable(), didn't get h/w timer (using s/w timer!!!)"); } + } else { + VLF("FAILED!"); + return; + } + } + #else + if (seconds == 1) wdt_enable(WDTO_1S); else + if (seconds == 2) wdt_enable(WDTO_2S); else + if (seconds == 4) wdt_enable(WDTO_4S); else + if (seconds == 8) wdt_enable(WDTO_8S); else return; + #endif + enabled = true; +} + +void Watchdog::disable() { + #ifndef __AVR_ATmega2560__ + enabled = false; + #else + wdt_disable(); + #endif +} + +void Watchdog::reset() { + #ifndef __AVR_ATmega2560__ + count = 0; + #else + wdt_reset(); + #endif +} + +void Watchdog::poll() { + if (!enabled) return; + if (count++ > 10 * seconds) { HAL_RESET(); } +} + +Watchdog watchdog; + +#endif diff --git a/src/lib/watchdog/Watchdog.h b/src/lib/watchdog/Watchdog.h new file mode 100755 index 0000000..37c79da --- /dev/null +++ b/src/lib/watchdog/Watchdog.h @@ -0,0 +1,31 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// General purpose "virtual" watchdog +#pragma once + +#include "../../Common.h" + +#if defined(WATCHDOG) && WATCHDOG != OFF || true + +class Watchdog { + public: + // initialize and start the watchdog with timeout (to reset MCU) of seconds + void enable(int seconds); + + // call at least once per timeout period or the MCU will be reset if the WD is enabled + void reset(); + + // disable the watchdog + void disable(); + + // for internal use + void poll(); + + private: + volatile int16_t count = 0; + volatile int16_t seconds = -1; + volatile bool enabled = false; +}; + +extern Watchdog watchdog; + +#endif diff --git a/src/lib/wifi/WifiManager.cpp b/src/lib/wifi/WifiManager.cpp new file mode 100755 index 0000000..7413486 --- /dev/null +++ b/src/lib/wifi/WifiManager.cpp @@ -0,0 +1,251 @@ +// wifi manager, used by the webserver and wifi serial IP +#include "WifiManager.h" + +#if OPERATIONAL_MODE == WIFI + +#include "../tasks/OnTask.h" +#include "../nv/Nv.h" + +#if STA_AUTO_RECONNECT == true + void reconnectStationWrapper() { wifiManager.reconnectStation(); } +#endif + +bool WifiManager::init() { + if (!active) { + + readSettings(); + + setStation(stationNumber); + + IPAddress ap_ip = IPAddress(settings.ap.ip); + IPAddress ap_gw = IPAddress(settings.ap.gw); + IPAddress ap_sn = IPAddress(settings.ap.sn); + + IPAddress sta_ip = IPAddress(sta->ip); + IPAddress sta_gw = IPAddress(sta->gw); + IPAddress sta_sn = IPAddress(sta->sn); + + char name[32] = HOST_NAME; + strtohostname(name); + + #if MDNS_SERVER == ON + WiFi.hostname(name); + #endif + + if (settings.accessPointEnabled == false && settings.stationEnabled == false) { + VF("MSG: WiFi, re-enabling access point mode to prevent lock-out!"); + settings.accessPointEnabled = true; + } + + TryAgain: + if (settings.accessPointEnabled && !settings.stationEnabled) { + VF("MSG: WiFi, starting Soft AP for SSID "); V(settings.ap.ssid); V(" PWD "); V(settings.ap.pwd); V(" CH "); VL(settings.ap.channel); + WiFi.softAP(settings.ap.ssid, settings.ap.pwd, settings.ap.channel); + #if defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32C3) + WiFi.setTxPower(WIFI_POWER_8_5dBm); + #endif + WiFi.mode(WIFI_AP); + } else + if (!settings.accessPointEnabled && settings.stationEnabled) { + VF("MSG: WiFi, starting Station for SSID "); V(sta->ssid); V(" PWD "); VL(staPwd->password); + WiFi.begin(sta->ssid, staPwd->password); + #if defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32C3) + WiFi.setTxPower(WIFI_POWER_8_5dBm); + #endif + WiFi.mode(WIFI_STA); + } else + if (settings.accessPointEnabled && settings.stationEnabled) { + VF("MSG: WiFi, starting Soft AP for SSID "); V(settings.ap.ssid); V(" PWD "); V(settings.ap.pwd); V(" CH "); VL(settings.ap.channel); + WiFi.softAP(settings.ap.ssid, settings.ap.pwd, settings.ap.channel); + VF("MSG: WiFi, starting Station for SSID "); V(sta->ssid); V(" PWD "); VL(staPwd->password); + WiFi.begin(sta->ssid, staPwd->password); + #if defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32C3) + WiFi.setTxPower(WIFI_POWER_8_5dBm); + #endif + WiFi.mode(WIFI_AP_STA); + } + + delay(100); + + if (settings.stationEnabled && !sta->dhcpEnabled) WiFi.config(sta_ip, sta_gw, sta_sn); + if (settings.accessPointEnabled) WiFi.softAPConfig(ap_ip, ap_gw, ap_sn); + + // wait for connection + if (settings.stationEnabled) { + for (int i = 0; i < 8; i++) if (WiFi.status() != WL_CONNECTED) delay(1000); else break; + } + + if (settings.stationEnabled && WiFi.status() != WL_CONNECTED) { + + // if connection fails fall back to access-point mode + if (settings.stationApFallback && !settings.accessPointEnabled) { + VLF("MSG: WiFi, starting station failed"); + WiFi.disconnect(); + delay(3000); + VLF("MSG: WiFi, switching to SoftAP mode"); + settings.stationEnabled = false; + settings.accessPointEnabled = true; + goto TryAgain; + } + + // no fallback but the AP is still enabled + if (settings.accessPointEnabled) { + active = true; + VLF("MSG: WiFi, started AP but station failed"); + } else { + // the station failed to connect and the AP isn't enabled + DLF("WRN: WiFi, starting station failed"); + WiFi.disconnect(); + } + } else { + active = true; + VLF("MSG: WiFi, started"); + + #if MDNS_SERVER == ON && !defined(ESP8266) + sstrcpy(name, MDNS_NAME); + strtohostname2(name); + if (MDNS.begin(name)) { VF("MSG: WiFi, mDNS started for "); VL(name); } else { DF("WRN: WiFi, mDNS start FAILED for "); DL(name); } + #endif + + if (staNameLookup && strlen(wifiManager.sta->host) > 0) { + IPAddress ip; + char name[32] = ""; + sstrcpy(name, wifiManager.sta->host); + strtohostname(name); + + if (WiFi.hostByName(name, ip)) { + VF("MSG: WiFi, host name "); V(name); VF(" DNS resolved to "); VL(ip.toString().c_str()); + ip4toip4(wifiManager.sta->target, ip); + } else { + VF("MSG: WiFi, host name "); V(name); VLF(" DNS resolution failed!"); + #if MDNS_CLIENT == ON && !defined(ESP8266) + strtohostname2(name); + ip = MDNS.queryHost(name); + if (validip4(ip)) { + ip4toip4(wifiManager.sta->target, ip); + VF("MSG: WiFi, host name "); V(name); VF(" mDNS resolved to "); VL(ip.toString().c_str()); + } else { + VF("MSG: WiFi, host name "); V(name); VLF(" mDNS resolution failed!"); + } + #endif + } + } + + #if STA_AUTO_RECONNECT == true + if (settings.stationEnabled) { + VF("MSG: WiFi, start connection check task (rate 8s priority 7)... "); + if (tasks.add(8000, 0, true, 7, reconnectStationWrapper, "WifiChk")) { VLF("success"); } else { VLF("FAILED!"); } + } + #endif + } + } + + return active; +} + +#if STA_AUTO_RECONNECT == true + void WifiManager::reconnectStation() { + if (WiFi.status() != WL_CONNECTED) { + VLF("MSG: WiFi, attempting reconnect"); + WiFi.disconnect(); + WiFi.reconnect(); + } + } +#endif + +void WifiManager::setStation(int number) { + if (number >= 1 && number <= WifiStationCount) stationNumber = number; + sta = &station[stationNumber - 1]; + staPwd = &stationPassword[stationNumber - 1]; +} + +void WifiManager::disconnect() { + WiFi.disconnect(); + WiFi.softAPdisconnect(true); + active = false; + VLF("MSG: WiFi, disconnected"); +} + +void WifiManager::readSettings() { + if (settingsReady) return; + + #ifdef NV_WIFI_SETTINGS + VLF("MSG: WifiManager, reading settings from NV"); + + if (!nv().kv().getOrInit("WIFI_SETTINGS", settings)) { DLF("WRN: Nv, init failed for WIFI_SETTINGS"); } + + for (uint8_t i = 1; i <= WifiStationCount; i++) { + char keyStr[24]; + snprintf(keyStr, sizeof(keyStr), "WIFI_STATION%u", i); + if (!nv().kv().getOrInit(keyStr, station[i - 1])) { DF("WRN: Nv, init failed for "); DL(keyStr); } + snprintf(keyStr, sizeof(keyStr), "WIFI_STATION%u_PWD", i); + if (!nv().kv().getOrInit(keyStr, stationPassword[i - 1])) { DF("WRN: Nv, init failed for "); DL(keyStr); } + } + #endif + + #if DEBUG != OFF + IPAddress ap_ip = IPAddress(settings.ap.ip); + IPAddress ap_gw = IPAddress(settings.ap.gw); + IPAddress ap_sn = IPAddress(settings.ap.sn); + + VF("MSG: WiFi, Master Pwd = "); VL(settings.masterPassword); + + VF("MSG: WiFi, AP Enable = "); VL(settings.accessPointEnabled); + VF("MSG: WiFi, AP Fallback = "); VL(settings.stationApFallback); + + VF("MSG: WiFi, AP SSID = "); VL(settings.ap.ssid); + VF("MSG: WiFi, AP PWD = "); VL(settings.ap.pwd); + VF("MSG: WiFi, AP CH = "); VL(settings.ap.channel); + VF("MSG: WiFi, AP IP = "); VL(ap_ip.toString()); + VF("MSG: WiFi, AP GATEWAY = "); VL(ap_gw.toString()); + VF("MSG: WiFi, AP SN = "); VL(ap_sn.toString()); + + int currentStationNumber = stationNumber; + + VF("MSG: WiFi, Sta Enable = "); VL(settings.stationEnabled); + + VF("MSG: WiFi, Sta Select = "); VL(stationNumber); + + for (int i = 1; i <= WifiStationCount; i++) { + setStation(i); + + IPAddress sta_ip = IPAddress(sta->ip); UNUSED(sta_ip); + IPAddress sta_gw = IPAddress(sta->gw); UNUSED(sta_gw); + IPAddress sta_sn = IPAddress(sta->sn); UNUSED(sta_sn); + IPAddress target = IPAddress(sta->target); UNUSED(target); + + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" SSID = "); VL(sta->ssid); + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" PWD = "); VL(staPwd->password); + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" DHCP En = "); VL(sta->dhcpEnabled); + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" IP = "); VL(sta_ip.toString()); + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" GATEWAY = "); VL(sta_gw.toString()); + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" SN = "); VL(sta_sn.toString()); + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" NAME = "); VL(sta->host); + VF("MSG: WiFi, Sta"); V(stationNumber); VF(" TARGET = "); VL(target.toString()); + } + stationNumber = currentStationNumber; + #endif + + settingsReady = true; +} + +void WifiManager::writeSettings() { + if (!settingsReady) return; + + #ifdef NV_WIFI_SETTINGS + VLF("MSG: WifiManager, writing settings to NV"); + nv().kv().put("WIFI_SETTINGS", settings); + + for (uint8_t i = 1; i <= WifiStationCount; i++) { + char keyStr[24]; + snprintf(keyStr, sizeof(keyStr), "WIFI_STATION%u", i); + nv().kv().put(keyStr, station[i - 1]); + snprintf(keyStr, sizeof(keyStr), "WIFI_STATION%u_PWD", i); + nv().kv().put(keyStr, stationPassword[i - 1]); + } + #endif +} + +WifiManager wifiManager; + +#endif diff --git a/src/lib/wifi/WifiManager.defaults.h b/src/lib/wifi/WifiManager.defaults.h new file mode 100755 index 0000000..9f7332f --- /dev/null +++ b/src/lib/wifi/WifiManager.defaults.h @@ -0,0 +1,337 @@ +#pragma once + +// ------------------------------------------------------------------------------------------------- +// general ip settings + +#ifndef HOST_NAME +#define HOST_NAME "Unknown" +#endif + +// default WIFI disabled +#ifndef OPERATIONAL_MODE +#define OPERATIONAL_MODE OFF +#endif + +// optional web-server +#ifndef WEB_SERVER +#define WEB_SERVER OFF // ON for website at port 80 +#endif + +// optional LX200 command server IP channels 9996 to 9999 (listens to clients) used by SWS +// OFF or STANDARD (port 9999), or PERSISTENT (ports 9996 to 9998), or BOTH +#ifndef COMMAND_SERVER +#define COMMAND_SERVER OFF +#endif + +// optional Arduino Serial class work-alike IP channels 9996 to 9999 as a server (listens to clients) +// OFF or STANDARD (port 9999), or PERSISTENT (ports 9996 to 9998), or BOTH +#ifndef SERIAL_SERVER +#define SERIAL_SERVER OFF // SERIAL_SIP, SERIAL_PIP1, etc. +#endif + +// optional Arduino Serial class work-alike IP channel (ports 9996 to 9998) as a client (connects to a server) +#ifndef SERIAL_CLIENT +#define SERIAL_CLIENT OFF // ON to enable SERIAL_IP +#endif + +// mDNS defaults +#ifndef MDNS_SERVER +#define MDNS_SERVER OFF +#endif + +#ifndef MDNS_CLIENT +#define MDNS_CLIENT OFF +#endif + +#ifndef MDNS_NAME +#define MDNS_NAME HOST_NAME +#endif + +// ------------------------------------------------------------------------------------------------- +// wifi manager settings + +#ifndef PASSWORD_DEFAULT +#define PASSWORD_DEFAULT "password" // default pwd for wifi settings +#endif + +#ifndef AP_ENABLED +#define AP_ENABLED false // to disable the soft AP +#endif + +#ifndef AP_SSID +#define AP_SSID HOST_NAME // Wifi Access Point SSID +#endif + +#ifndef AP_PASSWORD +#define AP_PASSWORD "password" // Wifi Access Point password +#endif + +#ifndef AP_CHANNEL +#define AP_CHANNEL 7 // Wifi Access Point channel +#endif + +#ifndef AP_IP_ADDR +#define AP_IP_ADDR {192,168,0,1} // Wifi Access Point IP Address +#endif + +#ifndef AP_GW_ADDR +#define AP_GW_ADDR {192,168,0,1} // Wifi Access Point GATEWAY Address +#endif + +#ifndef AP_SN_MASK +#define AP_SN_MASK {255,255,255,0} // Wifi Access Point SUBNET Mask +#endif + +// station + +#ifndef STA_AP_FALLBACK +#define STA_AP_FALLBACK false // activate SoftAP if station fails to connect +#endif + +#ifndef STA_ENABLED +#define STA_ENABLED false // normally not enabled +#endif + +#ifndef STA_AUTO_RECONNECT +#define STA_AUTO_RECONNECT false // normally not enabled +#endif + +#ifndef STA_SSID +#define STA_SSID "Home" // Wifi Station SSID to connnect to +#endif + +#ifndef STA_PASSWORD +#define STA_PASSWORD "password" // Wifi Station mode password +#endif + +#ifndef STA_DHCP_ENABLED +#define STA_DHCP_ENABLED false // true to use LAN DHCP addresses +#endif +#define STA_DHCP STA_DHCP_ENABLED + +#ifndef STA_IP_ADDR +#define STA_IP_ADDR {192,168,0,2} // Wifi Station IP Address +#endif + +#ifndef STA_GW_ADDR +#define STA_GW_ADDR {192,168,0,1} // Wifi Station GATEWAY Address +#endif + +#ifndef STA_SN_MASK +#define STA_SN_MASK {255,255,255,0} // Wifi Station SUBNET Mask +#endif + +#ifndef STA_HOST_NAME +#define STA_HOST_NAME "Home" // Host name to connect to (OnStep for example) +#endif + +#ifndef STA_TARGET_IP_ADDR +#define STA_TARGET_IP_ADDR {192,168,0,1} // IP Address to connect to +#endif +#ifndef STA_TARGET +#define STA_TARGET STA_TARGET_IP_ADDR +#endif + +// station 1 + +#ifndef STA1_HOST_NAME +#define STA1_HOST_NAME STA_HOST_NAME +#endif + +#ifndef STA1_SSID +#define STA1_SSID STA_SSID +#endif + +#ifndef STA1_PASSWORD +#define STA1_PASSWORD STA_PASSWORD +#endif + +#ifndef STA1_DHCP_ENABLED +#define STA1_DHCP_ENABLED STA_DHCP +#endif + +#ifndef STA1_TARGET_IP_ADDR +#define STA1_TARGET_IP_ADDR STA_TARGET +#endif + +#ifndef STA1_IP_ADDR +#define STA1_IP_ADDR STA_IP_ADDR +#endif + +#ifndef STA1_GW_ADDR +#define STA1_GW_ADDR STA_GW_ADDR +#endif + +#ifndef STA1_SN_MASK +#define STA1_SN_MASK STA_SN_MASK +#endif + +// station 2 + +#ifndef STA2_SSID +#define STA2_SSID "" +#endif + +#ifndef STA2_PASSWORD +#define STA2_PASSWORD "" +#endif + +#ifndef STA2_DHCP_ENABLED +#define STA2_DHCP_ENABLED false +#endif + +#ifndef STA2_IP_ADDR +#define STA2_IP_ADDR {192,168,0,2} +#endif + +#ifndef STA2_GW_ADDR +#define STA2_GW_ADDR {192,168,0,1} +#endif + +#ifndef STA2_SN_MASK +#define STA2_SN_MASK {255,255,255,0} +#endif + +#ifndef STA2_HOST_NAME +#define STA2_HOST_NAME "" +#endif + +#ifndef STA2_TARGET_IP_ADDR +#define STA2_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 3 + +#ifndef STA3_SSID +#define STA3_SSID "" +#endif + +#ifndef STA3_PASSWORD +#define STA3_PASSWORD "" +#endif + +#ifndef STA3_DHCP_ENABLED +#define STA3_DHCP_ENABLED false +#endif + +#ifndef STA3_IP_ADDR +#define STA3_IP_ADDR {192,168,0,2} +#endif + +#ifndef STA3_GW_ADDR +#define STA3_GW_ADDR {192,168,0,1} +#endif + +#ifndef STA3_SN_MASK +#define STA3_SN_MASK {255,255,255,0} +#endif + +#ifndef STA3_HOST_NAME +#define STA3_HOST_NAME "" +#endif + +#ifndef STA3_TARGET_IP_ADDR +#define STA3_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 4 + +#ifndef STA4_SSID +#define STA4_SSID "" +#endif + +#ifndef STA4_PASSWORD +#define STA4_PASSWORD "" +#endif + +#ifndef STA4_DHCP_ENABLED +#define STA4_DHCP_ENABLED false +#endif + +#ifndef STA4_IP_ADDR +#define STA4_IP_ADDR {192,168,0,2} +#endif + +#ifndef STA4_GW_ADDR +#define STA4_GW_ADDR {192,168,0,1} +#endif + +#ifndef STA4_SN_MASK +#define STA4_SN_MASK {255,255,255,0} +#endif + +#ifndef STA4_HOST_NAME +#define STA4_HOST_NAME "" +#endif + +#ifndef STA4_TARGET_IP_ADDR +#define STA4_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 5 + +#ifndef STA5_SSID +#define STA5_SSID "" +#endif + +#ifndef STA5_PASSWORD +#define STA5_PASSWORD "" +#endif + +#ifndef STA5_DHCP_ENABLED +#define STA5_DHCP_ENABLED false +#endif + +#ifndef STA5_IP_ADDR +#define STA5_IP_ADDR {192,168,0,2} +#endif + +#ifndef STA5_GW_ADDR +#define STA5_GW_ADDR {192,168,0,1} +#endif + +#ifndef STA5_SN_MASK +#define STA5_SN_MASK {255,255,255,0} +#endif + +#ifndef STA5_HOST_NAME +#define STA5_HOST_NAME "" +#endif + +#ifndef STA5_TARGET_IP_ADDR +#define STA5_TARGET_IP_ADDR {192,168,0,1} +#endif + +// station 6 + +#ifndef STA6_SSID +#define STA6_SSID "" +#endif + +#ifndef STA6_PASSWORD +#define STA6_PASSWORD "" +#endif + +#ifndef STA6_DHCP_ENABLED +#define STA6_DHCP_ENABLED false +#endif + +#ifndef STA6_IP_ADDR +#define STA6_IP_ADDR {192,168,0,2} +#endif + +#ifndef STA6_GW_ADDR +#define STA6_GW_ADDR {192,168,0,1} +#endif + +#ifndef STA6_SN_MASK +#define STA6_SN_MASK {255,255,255,0} +#endif + +#ifndef STA6_HOST_NAME +#define STA6_HOST_NAME "" +#endif + +#ifndef STA6_TARGET_IP_ADDR +#define STA6_TARGET_IP_ADDR {192,168,0,1} +#endif diff --git a/src/lib/wifi/WifiManager.h b/src/lib/wifi/WifiManager.h new file mode 100755 index 0000000..6eaccae --- /dev/null +++ b/src/lib/wifi/WifiManager.h @@ -0,0 +1,174 @@ +// wifi manager, used by the webserver and wifi serial IP +#pragma once + +#include "../../Common.h" + +#ifndef OPERATIONAL_MODE +#define OPERATIONAL_MODE OFF +#endif + +#if OPERATIONAL_MODE == WIFI + +#include "WifiManager.defaults.h" + +#if defined(ESP32) + #include + #include + #include + #if MDNS_SERVER == ON || MDNS_CLIENT == ON + #include + #endif +#elif defined(ESP8266) + #include + #include + #include +#else + #error "Configuration (Config.h): No Wifi support is present for this device" +#endif + +#ifndef WifiStationCount + // number of wifi stations supported, between 1 and 6 + #define WifiStationCount 1 +#endif + +#pragma pack(1) + +#define WifiStationPasswordSize 64 +typedef struct StationPassword { + char password[64]; +} StationPassword; + +#define WifiStationSize 82 +typedef struct StationSettings { + char ssid[33]; + bool dhcpEnabled; + uint8_t ip[4]; + uint8_t gw[4]; + uint8_t sn[4]; + char host[32]; + uint8_t target[4]; +} StationSettings; + +typedef struct AccessPointSettings { + char ssid[32]; + char pwd[32]; + unsigned char channel; + uint8_t ip[4]; + uint8_t gw[4]; + uint8_t sn[4]; +} AccessPointSettings; + +#define WifiSettingsSize 112 +typedef struct WifiSettings { + char masterPassword[32]; + + bool accessPointEnabled; + AccessPointSettings ap; + + bool stationEnabled; + bool stationApFallback; +} WifiSettings; + +#pragma pack() + +class WifiManager { + public: + // starts WiFi + bool init(); + + // stops WiFi + void disconnect(); + + #if STA_AUTO_RECONNECT == true + void reconnectStation(); + #endif + + // set the currently active station + // \param number from 1 to WifiStationCount + void setStation(int number); + + // read settings from NV + void readSettings(); + + // write settings to NV + void writeSettings(); + + // currently selected station + StationSettings *sta; + + // currently selected station password + StationPassword *staPwd; + + // currently selected station number + int stationNumber = 1; + + // optional station host name lookup overrides target IP + bool staNameLookup = false; + + // true if the WiFi AP or Station is active + bool active = false; + + WifiSettings settings = { + PASSWORD_DEFAULT, + + AP_ENABLED, + { + AP_SSID, AP_PASSWORD, AP_CHANNEL, + AP_IP_ADDR, AP_GW_ADDR, AP_SN_MASK + }, + + STA_ENABLED, + STA_AP_FALLBACK, + }; + + private: + bool settingsReady = false; + + StationPassword stationPassword[WifiStationCount] = + { + #if WifiStationCount > 0 + {STA1_PASSWORD}, + #endif + #if WifiStationCount > 1 + {STA2_PASSWORD}, + #endif + #if WifiStationCount > 2 + {STA3_PASSWORD}, + #endif + #if WifiStationCount > 3 + {STA4_PASSWORD}, + #endif + #if WifiStationCount > 4 + {STA5_PASSWORD}, + #endif + #if WifiStationCount > 5 + {STA6_PASSWORD}, + #endif + }; + + StationSettings station[WifiStationCount] = + { + #if WifiStationCount > 0 + {STA1_SSID, STA1_DHCP_ENABLED, STA1_IP_ADDR, STA1_GW_ADDR, STA1_SN_MASK, STA1_HOST_NAME, STA1_TARGET_IP_ADDR}, + #endif + #if WifiStationCount > 1 + {STA2_SSID, STA2_DHCP_ENABLED, STA2_IP_ADDR, STA2_GW_ADDR, STA2_SN_MASK, STA2_HOST_NAME, STA2_TARGET_IP_ADDR}, + #endif + #if WifiStationCount > 2 + {STA3_SSID, STA3_DHCP_ENABLED, STA3_IP_ADDR, STA3_GW_ADDR, STA3_SN_MASK, STA3_HOST_NAME, STA3_TARGET_IP_ADDR}, + #endif + #if WifiStationCount > 3 + {STA4_SSID, STA4_DHCP_ENABLED, STA4_IP_ADDR, STA4_GW_ADDR, STA4_SN_MASK, STA4_HOST_NAME, STA4_TARGET_IP_ADDR}, + #endif + #if WifiStationCount > 4 + {STA5_SSID, STA5_DHCP_ENABLED, STA5_IP_ADDR, STA5_GW_ADDR, STA5_SN_MASK, STA5_HOST_NAME, STA5_TARGET_IP_ADDR}, + #endif + #if WifiStationCount > 5 + {STA6_SSID, STA6_DHCP_ENABLED, STA6_IP_ADDR, STA6_GW_ADDR, STA6_SN_MASK, STA6_HOST_NAME, STA6_TARGET_IP_ADDR}, + #endif + }; +}; + +extern WifiManager wifiManager; + +#endif diff --git a/src/lib/wifi/cmdServer/CmdServer.cpp b/src/lib/wifi/cmdServer/CmdServer.cpp new file mode 100755 index 0000000..1ac067a --- /dev/null +++ b/src/lib/wifi/cmdServer/CmdServer.cpp @@ -0,0 +1,81 @@ +// wifi IP command server +#include "CmdServer.h" + +#if OPERATIONAL_MODE == WIFI && COMMAND_SERVER != OFF + + #include "../../tasks/OnTask.h" + #include "../../../libApp/cmd/Cmd.h" + + CmdServer::CmdServer(uint32_t port, long clientTimeoutMs, bool persist) { + cmdSvr = nullptr; + cmdBuffer[0] = 0; + cmdBufferPos = 0; + clientEndTimeMs = 0; + this->clientTimeoutMs = clientTimeoutMs; + this->persist = persist; + this->port = port; + } + + void CmdServer::begin() { + cmdSvr = new WiFiServer(port); + cmdSvr->begin(); + cmdSvr->setNoDelay(true); + } + + void CmdServer::handleClient() { + const unsigned long now = millis(); + + // drop stale client socket (disconnected or timed out/hung) + if (cmdSvrClient && + (!cmdSvrClient.connected() || (long)(clientEndTimeMs - now) < 0)) { + cmdSvrClient.stop(); + cmdBuffer[0] = 0; + cmdBufferPos = 0; + } + + // new client + if (!cmdSvrClient && cmdSvr->hasClient()) { + cmdSvrClient = cmdSvr->available(); + if (cmdSvrClient) { + clientEndTimeMs = now + clientTimeoutMs; + cmdBuffer[0] = 0; + cmdBufferPos = 0; + } + } + + // check clients for data, if found get the command, pass to OnStep and pickup the response, then return the response to client + while (cmdSvrClient && cmdSvrClient.connected() && cmdSvrClient.available() > 0) { + // still active? push back disconnect + if (persist) clientEndTimeMs = millis() + clientTimeoutMs; + + // get the data + int c = cmdSvrClient.read(); + if (c < 0 || c > 255) break; + + // insert into the command buffer + cmdBuffer[cmdBufferPos] = (char)c; + cmdBufferPos++; + if (cmdBufferPos > 39) cmdBufferPos = 39; + cmdBuffer[cmdBufferPos] = 0; + + // send cmd and pickup the response + if (c == '#' || (strlen(cmdBuffer) == 1 && c == (char)6)) { + char result[40] = ""; + + onStep.processCommand(cmdBuffer, result, cmdTimeout); + + // pickup response + if (strlen(result) > 0) { + if (cmdSvrClient && cmdSvrClient.connected()) { + cmdSvrClient.print(result); // there was a delay(2); after this + } + } + + // reset command buffer + cmdBuffer[0] = 0; + cmdBufferPos = 0; + } else tasks.yield(); + } + } + +#endif diff --git a/src/lib/wifi/cmdServer/CmdServer.h b/src/lib/wifi/cmdServer/CmdServer.h new file mode 100755 index 0000000..563edcf --- /dev/null +++ b/src/lib/wifi/cmdServer/CmdServer.h @@ -0,0 +1,28 @@ +// wifi IP command server +#pragma once + +#include "../../../Common.h" +#include "../WifiManager.h" + +#if OPERATIONAL_MODE == WIFI && COMMAND_SERVER != OFF + + class CmdServer { + public: + CmdServer(uint32_t port, long clientTimeoutMs, bool persist = false); + void begin(); + void handleClient(); + + private: + WiFiServer *cmdSvr; + WiFiClient cmdSvrClient; + + char cmdBuffer[40]; + int cmdBufferPos; + + unsigned long clientTimeoutMs; + unsigned long clientEndTimeMs; + bool persist; + long port; + }; + +#endif diff --git a/src/lib/wifi/webServer/WebServer.cpp b/src/lib/wifi/webServer/WebServer.cpp new file mode 100755 index 0000000..6ff041c --- /dev/null +++ b/src/lib/wifi/webServer/WebServer.cpp @@ -0,0 +1,14 @@ +// ----------------------------------------------------------------------------------- +// Web server + +#include "WebServer.h" + +#if OPERATIONAL_MODE == WIFI && WEB_SERVER == ON + + #if defined(ESP32) + WebServer www(80); + #elif defined(ESP8266) + ESP8266WebServer www(80); + #endif + +#endif diff --git a/src/lib/wifi/webServer/WebServer.h b/src/lib/wifi/webServer/WebServer.h new file mode 100755 index 0000000..d5a95a6 --- /dev/null +++ b/src/lib/wifi/webServer/WebServer.h @@ -0,0 +1,26 @@ +// ----------------------------------------------------------------------------------- +// Web server +#pragma once + +#include "../../../Common.h" +#include "../WifiManager.h" + +#if OPERATIONAL_MODE == WIFI && WEB_SERVER == ON + + #if defined(ESP32) + #include + #elif defined(ESP8266) + #include + #else + #error "Configuration (Config.h): No Wifi support is present for this device" + #endif + + #if defined(ESP32) + extern WebServer www; + #define sendContentAndClear(x) sendContent(x); x = ""; + #elif defined(ESP8266) + extern ESP8266WebServer www; + #define sendContentAndClear(x) sendContent(x); x = ""; delay(1); + #endif + +#endif diff --git a/src/libApp/commands/CommandBroker.cpp b/src/libApp/commands/CommandBroker.cpp new file mode 100755 index 0000000..862214c --- /dev/null +++ b/src/libApp/commands/CommandBroker.cpp @@ -0,0 +1,197 @@ +// ----------------------------------------------------------------------------------- +// Local command broker + +#include "CommandBroker.h" + +#if defined(SERIAL_LOCAL_MODE) && SERIAL_LOCAL_MODE == ON + +#include "../../lib/serial/Serial_Local.h" +#include "../../lib/tasks/OnTask.h" + +void commandBrokerWrapper() { commandBroker.poll(); } + +bool CommandBroker::init() { + if (initialized) return true; + + #ifdef ESP32 + mutex = xSemaphoreCreateMutex(); + if (mutex == NULL) return false; + #endif + + for (uint8_t i = 0; i < COMMAND_BROKER_SLOTS; i++) { + requests[i].state = CB_FREE; + requests[i].generation = 0; + } + + VF("MSG: System, start command broker task (rate 3ms priority 5)... "); + if (tasks.add(3, 0, true, 5, commandBrokerWrapper, "CmdBrkr")) { + initialized = true; + VLF("success"); + return true; + } else { + VLF("FAILED!"); + return false; + } +} + +uint8_t CommandBroker::request(const char *command, unsigned long timeoutMs) { + return enqueue(command, timeoutMs, true); +} + +bool CommandBroker::send(const char *command) { + return enqueue(command, 0, false) != 0; +} + +uint8_t CommandBroker::enqueue(const char *command, unsigned long timeoutMs, bool replyExpected) { + if (!initialized || command == NULL || command[0] == 0) return 0; + + lock(); + for (uint8_t i = 0; i < COMMAND_BROKER_SLOTS; i++) { + if (requests[i].state == CB_FREE) { + sstrcpy(requests[i].command, command); + requests[i].reply[0] = 0; + requests[i].timeoutMs = timeoutMs; + requests[i].deadline = 0; + requests[i].replyExpected = replyExpected; + requests[i].generation++; + if (requests[i].generation == 0) requests[i].generation = 1; + requests[i].state = CB_PENDING; + uint8_t handle = makeHandle(i); + unlock(); + return handle; + } + } + unlock(); + return 0; +} + +CommandBrokerStatus CommandBroker::status(uint8_t handle) { + if (!initialized) return CB_FREE; + + int8_t slot = handleToSlot(handle); + if (slot < 0) return CB_FREE; + + lock(); + CommandBrokerStatus state = handleValid(handle, slot) ? requests[slot].state : CB_FREE; + unlock(); + return state; +} + +CommandBrokerStatus CommandBroker::result(uint8_t &handle, char *reply, size_t replySize) { + if (reply != NULL && replySize > 0) reply[0] = 0; + if (reply == NULL || replySize == 0) { handle = 0; return CB_FREE; } + if (!initialized) { handle = 0; return CB_FREE; } + + int8_t slot = handleToSlot(handle); + if (slot < 0) { handle = 0; return CB_FREE; } + + lock(); + CommandBrokerStatus state = handleValid(handle, slot) ? requests[slot].state : CB_FREE; + if (state == CB_DONE) { + sstrcpyex(reply, requests[slot].reply, replySize); + requests[slot].state = CB_FREE; + handle = 0; + } else if (state == CB_TIMEOUT) { + requests[slot].state = CB_FREE; + handle = 0; + } else if (state == CB_FREE) { + handle = 0; + } + unlock(); + return state; +} + +void CommandBroker::release(uint8_t handle) { + if (!initialized) return; + + int8_t slot = handleToSlot(handle); + if (slot < 0) return; + + lock(); + if (handleValid(handle, slot) && requests[slot].state != CB_SENT) requests[slot].state = CB_FREE; + unlock(); +} + +void CommandBroker::poll() { + if (!initialized) return; + + if (active < 0) { + lock(); + for (uint8_t i = 0; i < COMMAND_BROKER_SLOTS; i++) { + if (requests[i].state == CB_PENDING) { + if (requests[i].replyExpected) requests[i].deadline = millis() + requests[i].timeoutMs; + requests[i].state = CB_SENT; + active = i; + break; + } + } + unlock(); + + if (active >= 0) { + SERIAL_LOCAL.receive(); + SERIAL_LOCAL.transmit(requests[active].command); + if (!requests[active].replyExpected) { + lock(); + requests[active].state = CB_FREE; + active = -1; + unlock(); + } + } + } + + if (active < 0) return; + + if (SERIAL_LOCAL.receiveAvailable()) { + char *reply = SERIAL_LOCAL.receive(); + lock(); + sstrcpy(requests[active].reply, reply); + requests[active].state = CB_DONE; + active = -1; + unlock(); + } else { + lock(); + bool timedOut = requests[active].timeoutMs == 0 || (long)(millis() - requests[active].deadline) >= 0; + if (timedOut) { + requests[active].reply[0] = 0; + requests[active].state = CB_TIMEOUT; + active = -1; + } + unlock(); + } +} + +uint8_t CommandBroker::makeHandle(uint8_t slot) { + return ((requests[slot].generation & 0x0F) << 4) | ((slot + 1) & 0x0F); +} + +int8_t CommandBroker::handleToSlot(uint8_t handle) { + if (handle == 0) return -1; + + uint8_t slot = (handle & 0x0F) - 1; + if (slot >= COMMAND_BROKER_SLOTS) return -1; + return slot; +} + +bool CommandBroker::handleValid(uint8_t handle, uint8_t slot) { + return (requests[slot].generation & 0x0F) == (handle >> 4) && requests[slot].state != CB_FREE; +} + +void CommandBroker::lock() { + #ifdef ESP32 + xSemaphoreTake(mutex, portMAX_DELAY); + #else + noInterrupts(); + #endif +} + +void CommandBroker::unlock() { + #ifdef ESP32 + xSemaphoreGive(mutex); + #else + interrupts(); + #endif +} + +CommandBroker commandBroker; + +#endif diff --git a/src/libApp/commands/CommandBroker.h b/src/libApp/commands/CommandBroker.h new file mode 100755 index 0000000..abebf57 --- /dev/null +++ b/src/libApp/commands/CommandBroker.h @@ -0,0 +1,84 @@ +// ----------------------------------------------------------------------------------- +// Local command broker +#pragma once + +#include "../../Common.h" + +#if defined(SERIAL_LOCAL_MODE) && SERIAL_LOCAL_MODE == ON + +#ifndef COMMAND_BROKER_SLOTS + #define COMMAND_BROKER_SLOTS 8 +#endif + +#ifndef COMMAND_BROKER_COMMAND_LEN + #define COMMAND_BROKER_COMMAND_LEN 40 +#endif + +#ifndef COMMAND_BROKER_REPLY_LEN + #define COMMAND_BROKER_REPLY_LEN 80 +#endif + +#ifndef COMMAND_BROKER_TIMEOUT_MS + #define COMMAND_BROKER_TIMEOUT_MS 100 +#endif + +#if COMMAND_BROKER_SLOTS > 15 + #error "COMMAND_BROKER_SLOTS must be 15 or less." +#endif + +enum CommandBrokerStatus : uint8_t { + CB_FREE, + CB_PENDING, + CB_SENT, + CB_DONE, + CB_TIMEOUT +}; + +class CommandBroker { + public: + bool init(); + + // Queue commands here when a caller expects a reply. Do not mix brokered + // request/reply traffic with direct SERIAL_LOCAL.receive() calls. + uint8_t request(const char *command, unsigned long timeoutMs = COMMAND_BROKER_TIMEOUT_MS); + // Queue commands here only when no reply will be consumed. + bool send(const char *command); + + CommandBrokerStatus status(uint8_t handle); + inline bool ready(uint8_t handle) { return status(handle) == CB_DONE; } + inline bool timeout(uint8_t handle) { return status(handle) == CB_TIMEOUT; } + CommandBrokerStatus result(uint8_t &handle, char *reply, size_t replySize); + void release(uint8_t handle); + + void poll(); + + private: + struct Request { + CommandBrokerStatus state = CB_FREE; + char command[COMMAND_BROKER_COMMAND_LEN] = ""; + char reply[COMMAND_BROKER_REPLY_LEN] = ""; + unsigned long timeoutMs = COMMAND_BROKER_TIMEOUT_MS; + unsigned long deadline = 0; + bool replyExpected = true; + uint8_t generation = 0; + }; + + Request requests[COMMAND_BROKER_SLOTS]; + int8_t active = -1; + bool initialized = false; + + uint8_t enqueue(const char *command, unsigned long timeoutMs, bool replyExpected); + uint8_t makeHandle(uint8_t slot); + int8_t handleToSlot(uint8_t handle); + bool handleValid(uint8_t handle, uint8_t slot); + void lock(); + void unlock(); + + #ifdef ESP32 + SemaphoreHandle_t mutex = NULL; + #endif +}; + +extern CommandBroker commandBroker; + +#endif diff --git a/src/libApp/commands/ProcessCmds.cpp b/src/libApp/commands/ProcessCmds.cpp new file mode 100755 index 0000000..3cc25ba --- /dev/null +++ b/src/libApp/commands/ProcessCmds.cpp @@ -0,0 +1,315 @@ +// ----------------------------------------------------------------------------------- +// Command processing + +#include "../../Common.h" +#include "../../lib/tasks/OnTask.h" +#include "../../lib/convert/Convert.h" +#include "ProcessCmds.h" + +#include "../../telescope/Telescope.h" + +#ifdef MOUNT_PRESENT + #if ST4_INTERFACE == ON && ST4_HAND_CONTROL == ON + #include "../../lib/serial/Serial_ST4_Master.h" + #endif + #include "../../lib/serial/Serial_Local.h" +#endif +#ifdef SERIAL_LOCAL + #include "CommandBroker.h" +#endif + +#if DEBUG != OFF + #define L_CE_NONE "no errors" + #define L_CE_1 "no error true" + #define L_CE_0 "no error false/fail" + #define L_CE_CMD_UNKNOWN "command unknown" + #define L_CE_REPLY_UNKNOWN "invalid reply" + #define L_CE_PARAM_RANGE "parameter out of range" + #define L_CE_PARAM_FORM "bad parameter format" + #define L_CE_ALIGN_FAIL "align failed" + #define L_CE_ALIGN_NOT_ACTIVE "align not active" + #define L_CE_NOT_PARKED_OR_AT_HOME "not parked or at home" + #define L_CE_PARKED "already parked" + #define L_CE_PARK_FAILED "park failed" + #define L_CE_NOT_PARKED "not parked" + #define L_CE_NO_PARK_POSITION_SET "no park position set" + #define L_CE_SLEW_FAIL "goto failed" + #define L_CE_LIBRARY_FULL "library full" + #define L_CE_SLEW_ERR_BELOW_HORIZON "goto below horizon" + #define L_CE_SLEW_ERR_ABOVE_OVERHEAD "goto above overhead" + #define L_CE_SLEW_ERR_IN_STANDBY "slew in standby" + #define L_CE_SLEW_ERR_IN_PARK "slew in park" + #define L_CE_SLEW_ERR_SLEW "already in goto" + #define L_CE_SLEW_ERR_OUTSIDE_LIMITS "outside limits" + #define L_CE_SLEW_ERR_HARDWARE_FAULT "hardware fault" + #define L_CE_MOUNT_IN_MOTION "mount in motion" + #define L_CE_SLEW_ERR_UNSPECIFIED "other" + #define L_CE_UNK "unknown" + + static const char commandErrorStr[26][25] = { + L_CE_NONE, L_CE_1, L_CE_0, L_CE_CMD_UNKNOWN, L_CE_REPLY_UNKNOWN, L_CE_PARAM_RANGE, + L_CE_PARAM_FORM, L_CE_ALIGN_FAIL, L_CE_ALIGN_NOT_ACTIVE, L_CE_NOT_PARKED_OR_AT_HOME, + L_CE_PARKED, L_CE_PARK_FAILED, L_CE_NOT_PARKED, L_CE_NO_PARK_POSITION_SET, L_CE_SLEW_FAIL, + L_CE_LIBRARY_FULL, L_CE_SLEW_ERR_BELOW_HORIZON, L_CE_SLEW_ERR_ABOVE_OVERHEAD, + L_CE_SLEW_ERR_IN_STANDBY, L_CE_SLEW_ERR_IN_PARK, L_CE_SLEW_ERR_SLEW, L_CE_SLEW_ERR_OUTSIDE_LIMITS, + L_CE_SLEW_ERR_HARDWARE_FAULT, L_CE_MOUNT_IN_MOTION, L_CE_SLEW_ERR_UNSPECIFIED, L_CE_UNK + }; +#endif + +// command processors +#ifdef SERIAL_A + CommandProcessor processCommandsA(SERIAL_A_BAUD_DEFAULT,'A'); + void processCmdsA() { processCommandsA.poll(); } +#endif +#ifdef SERIAL_B + CommandProcessor processCommandsB(SERIAL_B_BAUD_DEFAULT,'B'); + void processCmdsB() { processCommandsB.poll(); } +#endif +#ifdef SERIAL_C + CommandProcessor processCommandsC(SERIAL_C_BAUD_DEFAULT,'C'); + void processCmdsC() { processCommandsC.poll(); } +#endif +#ifdef SERIAL_D + CommandProcessor processCommandsD(SERIAL_D_BAUD_DEFAULT,'D'); + void processCmdsD() { processCommandsD.poll(); } +#endif +#ifdef SERIAL_ST4 + CommandProcessor processCommandsST4(9600,'S'); + void processCmdsST4() { processCommandsST4.poll(); } +#endif +#if SERIAL_BT_MODE == SLAVE + CommandProcessor processCommandsBT(9600,'T'); + void processCmdsBT() { processCommandsBT.poll(); } +#endif +#ifdef SERIAL_PIP1 + CommandProcessor processCommandsPIP1(9600,'1'); + void processCmdsPIP1() { ::yield(); processCommandsPIP1.poll(); } +#endif +#ifdef SERIAL_PIP2 + CommandProcessor processCommandsPIP2(9600,'2'); + void processCmdsPIP2() { ::yield(); processCommandsPIP2.poll(); } +#endif +#ifdef SERIAL_PIP3 + CommandProcessor processCommandsPIP3(9600,'3'); + void processCmdsPIP3() { ::yield(); processCommandsPIP3.poll(); } +#endif +#ifdef SERIAL_SIP + CommandProcessor processCommandsIP(9600,'I'); + void processCmdsIP() { ::yield(); processCommandsIP.poll(); } +#endif +#ifdef SERIAL_LOCAL + CommandProcessor processCommandsLocal(9600,'L'); + void processCmdsLocal() { processCommandsLocal.poll(); } +#endif + +CommandProcessor::CommandProcessor(long baud, char channel) { + this->channel = channel; + serialBaud = baud; +} + +CommandProcessor::~CommandProcessor() { + SerialPort.end(); +} + +void CommandProcessor::poll() { + if (!serialReady) { delay(200); SerialPort.begin(serialBaud); serialReady = true; } + + unsigned long tout = micros() + 500; + while (SerialPort.available()) { char c = SerialPort.read(); buffer.add(c); if (buffer.ready() || (long)(micros() - tout) > 0) break; } + + if (buffer.ready()) { + char reply[80] = ""; + constexpr size_t replyCapacity = sizeof(reply); + bool numericReply = true; + bool suppressFrame = false; + + commandError = command(reply, buffer.getCmd(), buffer.getParameter(), &suppressFrame, &numericReply); + + if (numericReply) { + if (commandError != CE_NONE && commandError != CE_1) strcpy(reply,"0"); else strcpy(reply,"1"); + suppressFrame = true; + } + if (strlen(reply) > 0 || buffer.checksum) { + if (buffer.checksum) { + sstrreserveex(reply, 4, replyCapacity); + appendChecksum(reply, replyCapacity); + sstrcatcex(reply, buffer.getSeq()[0], replyCapacity); + suppressFrame = false; + } + if (!suppressFrame) { + sstrreserveex(reply, 1, replyCapacity); + sstrcatcex(reply, '#', replyCapacity); + } + SerialPort.write(reply); + } + + // debug, log errors and/or commands + #ifdef DEBUG_ECHO_COMMANDS_CH + if (DEBUG_ECHO_COMMANDS_CH == channel) { + #endif + #if DEBUG_ECHO_COMMANDS != OFF + if (DEBUG_ECHO_COMMANDS == ON || commandError > CE_0) { + DF("MSG: cmd"); D(channel); D(" = "); D(buffer.getCmd()); D(buffer.getParameter()); DF(", reply = "); D(reply); + } + #endif + if (commandError != CE_NULL) { + lastCommandError = commandError; + #if DEBUG_ECHO_COMMANDS != OFF + if (commandError > CE_0) { DF(", Error "); D(commandErrorStr[commandError]); } + #endif + } + #if DEBUG_ECHO_COMMANDS != OFF + if (DEBUG_ECHO_COMMANDS == ON || commandError > CE_0) { DL(""); } + #endif + #ifdef DEBUG_ECHO_COMMANDS_CH + } + #endif + + buffer.flush(); + } +} + +CommandError CommandProcessor::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply) { + commandError = CE_NONE; + + // handle telescope commands + if (telescope.command(reply, command, parameter, suppressFrame, numericReply, &commandError)) return commandError; + + // silent bool "errors" allow processing commands more than once + if (commandError == CE_0 || commandError == CE_1) return commandError; + + // (char)6 - Special + if (command[0] == (char)6) { + if (command[1] == '0') { + reply[0] = command[1]; + strcpy(reply,"CK_FAIL"); // last cmd checksum failed + } else { + reply[0] = command[1]; + reply[1] = 0; // Equatorial or Horizon mode, A or P + *suppressFrame = true; + } + *numericReply = false; + return commandError; + } else + + // :SB[n]# Set Baud Rate where n is an ASCII digit (1..9) with the following interpertation + // B=460.8K, A=230.4K, 0=115.2K, 1=56.7K, 2=38.4K, 3=28.8K, 4=19.2K, 5=14.4K, 6=9600, 7=4800, 8=2400, 9=1200 + // Returns: 1 (at the current baud rate and then changes to the new rate for further communication) + if (command[0] == 'S' && command[1] == 'B') { + int rate = parameter[0] - '0'; + if (parameter[0] == 'A') { + SerialPort.print("1"); + tasks.yield(50); + SerialPort.begin(230400); + *numericReply = false; + } else + if (parameter[0] == 'B') { + SerialPort.print("1"); + tasks.yield(50); + SerialPort.begin(460800); + *numericReply = false; + } else + if (rate >= 0 && rate <= 9) { + const static long baud[10] = {115200, 56700, 38400, 28800, 19200, 14400, 9600, 4800, 2400, 1200}; + SerialPort.print("1"); + tasks.yield(50); + SerialPort.begin(baud[rate]); + *numericReply = false; + } else commandError = CE_PARAM_RANGE; + return commandError; + } else + + // :GE# Get last command error numeric code + // Returns: CC# + if (command[0] == 'G' && command[1] == 'E' && parameter[0] == 0) { + sprintf(reply, "%02d", lastCommandError); + *numericReply = false; + return commandError; + } else + + return CE_CMD_UNKNOWN; +} + +void CommandProcessor::appendChecksum(char *s, size_t capacity) { + char HEXS[3] = ""; + uint8_t cks = 0; for (unsigned int cksCount0 = 0; cksCount0 < strlen(s); cksCount0++) { cks += s[cksCount0]; } + snprintf(HEXS, sizeof(HEXS), "%02X", cks); + sstrcatex(s, HEXS, capacity); +} + +void commandChannelInit() { + // Command processing + // add tasks to process commands + // period ms (0=idle), duration ms (0=forever), repeat, priority (highest 0..7 lowest), task_handle + uint8_t handle; + #ifdef HAL_SLOW_PROCESSOR + long comPollRate = 5000; + #else + long comPollRate = 2500; + #endif + #ifdef SERIAL_A + VF("MSG: System, start command channel A task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsA, "SysCmdA"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_B + VF("MSG: System, start command channel B task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsB, "SysCmdB"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_C + VF("MSG: System, start command channel C task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsC, "SysCmdC"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_D + VF("MSG: System, start command channel D task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsD, "SysCmdD"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_ST4 + VF("MSG: System, start command channel ST4 task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsST4, "SysCmdS"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate*4); + #endif + #if SERIAL_BT_MODE == SLAVE + VF("MSG: System, start command channel BT task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsBT, "SysCmdT"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_PIP1 + VF("MSG: System, start command channel PIP1 task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsPIP1, "SysCmd1"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_PIP2 + VF("MSG: System, start command channel PIP2 task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsPIP2, "SysCmd2"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_PIP3 + VF("MSG: System, start command channel PIP3 task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsPIP3, "SysCmd3"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_SIP + VF("MSG: System, start command channel IP task (priority 5)... "); + handle = tasks.add(0, 0, true, 5, processCmdsIP, "SysCmdI"); + if (handle) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handle, comPollRate); + #endif + #ifdef SERIAL_LOCAL + VF("MSG: System, start command channel Local task (priority 5)... "); + if (tasks.add(3, 0, true, 5, processCmdsLocal, "SysCmdL")) { VLF("success"); } else { VLF("FAILED!"); } + commandBroker.init(); + #endif +} diff --git a/src/libApp/commands/ProcessCmds.h b/src/libApp/commands/ProcessCmds.h new file mode 100755 index 0000000..cc22b2a --- /dev/null +++ b/src/libApp/commands/ProcessCmds.h @@ -0,0 +1,36 @@ +// ----------------------------------------------------------------------------------- +// Command processing +#pragma once + +#include +#include "../../lib/commands/BufferCmds.h" +#include "../../lib/commands/SerialWrapper.h" +#include "../../lib/commands/CommandErrors.h" + +class CommandProcessor { + public: + // start and stop the serial port for the associated command channel + CommandProcessor(long baud, char channel); + ~CommandProcessor(); + + // check for incomming commands and send responses + void poll(); + + // pass along commands as required for processing + CommandError command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply); + + private: + void logErrors(char *cmd, char *param, char *reply, CommandError e); + void appendChecksum(char *s, size_t capacity); + + CommandError commandError = CE_NONE; + CommandError lastCommandError = CE_NONE; + bool serialReady = false; + long serialBaud = 9600; + char channel = '?'; + + Buffer buffer; + SerialWrapper SerialPort; +}; + +extern void commandChannelInit(); diff --git a/src/libApp/temperature/Ds1820.cpp b/src/libApp/temperature/Ds1820.cpp new file mode 100755 index 0000000..06234b5 --- /dev/null +++ b/src/libApp/temperature/Ds1820.cpp @@ -0,0 +1,150 @@ +// ----------------------------------------------------------------------------------- +// Dallas/Maxim 1-Wire DS1820 device support + +#include "Ds1820.h" + +#ifdef DS1820_DEVICES_PRESENT + +#include "../../lib/tasks/OnTask.h" + +#include "../../lib/1wire/1Wire.h" +#include // my Dallas Temperature library https://github.com/hjd1964/Arduino-DS1820-Temperature-Library +DallasTemperature DS18X20(&oneWire); + +#include "../weather/Weather.h" + +void ds1820Wrapper() { temperature.poll(); } + +// scan for DS18B20 devices on the 1-wire bus and prepare for operation +bool Ds1820::init() { + static bool initialized = false; + if (initialized) return found; + + // clear then pre-load any user defined DS1820 addresses + deviceCount = 0; + for (int i = 0; i < 9; i++) { + // check that it's not OFF and also not an (auto assigned) DS1820, so must be an DS1820 address + if (device[i] != (uint64_t)OFF && device[i] != DS1820) { + for (int j = 0; j < 8; j++) address[i][j] = (device[i] >> (7 - j)*8) & 0xff; + deviceCount++; + } else for (int j = 0; j < 8; j++) address[i][j] = 0; + } + + // scan the 1-wire bus and record the devices found + oneWire.reset_search(); + + // only search out DS1820's IF none are explicitly specified + bool search = deviceCount == 0; + #if DEBUG == VERBOSE + bool detected = false; + #endif + + VLF("*********************************************"); + VLF("MSG: Dallas/Maxim 1-wire DS1820 device s/n's:"); + + int index = 0; + uint8_t addressfound[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + while (oneWire.search(addressfound)) { + if (oneWire.crc8(addressfound, 7) == addressfound[7]) { + if (addressfound[0] == 0x10 || addressfound[0] == 0x28) { + if (search) { + if (index <= 8) { while (device[index] != DS1820 && index <= 8) index++; } + if (index <= 8) { for (int j = 0; j < 8; j++) { address[index][j] = addressfound[j]; deviceCount++; } } + index++; + } + + #if DEBUG == VERBOSE + if (addressfound[0] == 0x10) { VF("DS18S20: 0x"); } else { VF("DS18B20: 0x"); } + for (int i = 0; i < 8; i++) { + if (addressfound[i] < 16) { V("0"); } + if (DEBUG != OFF) SERIAL_DEBUG.print(addressfound[i], HEX); + } + if (search) { + if (index == 1) { VLF(" auto-assigned to FOCUSER_TEMPERATURE"); } else + if (index <= 9) { VF(" auto-assigned to FEATURE"); V(index - 1); VLF("_TEMP"); } else { VLF(" not assigned"); } + } else { VLF(" auto-assign disabled"); } + detected = true; + #endif + } + } + } + + #if DEBUG == VERBOSE + if (!detected) { VLF("No DS1820 devices found"); } + #endif + + VLF("*********************************************"); + + DS18X20.setWaitForConversion(false); + if (deviceCount > 0) { + found = true; + VF("MSG: Temperature, start DS1820 monitor task (rate 250ms priority 7)... "); + if (tasks.add(250, 0, true, 7, ds1820Wrapper, "ds1820")) { VLF("success"); } else { VLF("FAILED!"); } + } else found = false; + + initialized = true; + return found; +} + +// read devices +void Ds1820::poll() { + static int index = -1; + static unsigned long requestTime = 0; + + if (found) { + if (index < 0) { requestTime = millis(); if (DS18X20.requestTemperatures(true)) index++; } + if (millis() - requestTime < 200U) return; + + if (device[index] != (uint64_t)OFF) { + // loop to read the temperature + // tasks.yield() during the 1-wire command sequence is ok since: + // 1. only higher priority level tasks are allowed to run during a yield + // 2. all 1-wire task polling is run at the lowest priority level + float rawTemperature = NAN; + for (int i = 0; i < 22; i++) { + rawTemperature = DS18X20.getTempC(address[index], true); + if (polling(rawTemperature)) tasks.yield(2); else break; + } + + float temperature = validated(rawTemperature); + if (!isnan(temperature)) { + if (isnan(averageTemperature[index])) averageTemperature[index] = temperature; + averageTemperature[index] = (averageTemperature[index]*9.0F + temperature)/10.0F; + goodUntil[index] = millis() + 30000U; + } else { + // we must get a reading at least once every 30 seconds otherwise flag the failure with a NAN + if ((long)(millis() - goodUntil[index]) > 0) averageTemperature[index] = NAN; + } + } + } + index++; + if (index > 8) index = -1; +} + +// nine temperature sensors are supported, this gets the averaged temperature +// in deg. C otherwise it falls back to the weather sensor temperature +// index 0 is the ambient temperature, 1 through 8 are point temperatures #1, #2, etc. +// returns NAN if no temperature source is available or if a communications failure +// results in no valid readings for > 30 seconds +float Ds1820::getChannel(int index) { + if (found && index >= 0 && index <= 7) { + if (device[index] == (uint64_t)OFF) averageTemperature[index] = weather.getTemperature(); + return averageTemperature[index]; + } else return NAN; +} + +// checks for polling status code from DS1820 library +bool Ds1820::polling(float f) { + return (fabs(f - DEVICE_POLLING_C) < 0.001F); +} + +// checks for validated status code from DS1820 library +float Ds1820::validated(float f) { + if (fabs(f - DEVICE_DISCONNECTED_C) < 0.001F) return NAN; + if (f < -100 || f > 70) return NAN; + return f; +} + +Ds1820 temperature; + +#endif diff --git a/src/libApp/temperature/Ds1820.h b/src/libApp/temperature/Ds1820.h new file mode 100755 index 0000000..8763213 --- /dev/null +++ b/src/libApp/temperature/Ds1820.h @@ -0,0 +1,42 @@ +// ----------------------------------------------------------------------------------- +// Dallas/Maxim 1-Wire DS1820 device support +#pragma once + +#include "../../Common.h" + +#ifdef DS1820_DEVICES_PRESENT + +class Ds1820 { + public: + // scan for DS18B20 devices on the 1-wire bus and prepare for operation + bool init(); + + // read devices + void poll(); + + // nine temperature sensors are supported, this gets the averaged + // temperature in deg. C otherwise it falls back to the weather sensor temperature + // index 0 is the ambient temperature, 1 through 8 are point temperatures #1, #2, etc. + // returns NAN if no temperature source is available or if a communications failure + // results in no valid readings for > 30 seconds + float getChannel(int index); + + private: + // checks for polling status code from DS1820 library + bool polling(float f); + + // checks for validated status code from DS1820 library + float validated(float f); + + bool found = false; + uint8_t deviceCount = 0; + uint8_t address[9][8]; + uint64_t device[9] = { (uint64_t)FOCUSER_TEMPERATURE, (uint64_t)FEATURE1_TEMP, (uint64_t)FEATURE2_TEMP, (uint64_t)FEATURE3_TEMP, (uint64_t)FEATURE4_TEMP, (uint64_t)FEATURE5_TEMP, (uint64_t)FEATURE6_TEMP, (uint64_t)FEATURE7_TEMP, (uint64_t)FEATURE8_TEMP }; + + float averageTemperature[9] = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN }; + unsigned long goodUntil[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; +}; + +extern Ds1820 temperature; + +#endif diff --git a/src/libApp/temperature/Temperature.cpp b/src/libApp/temperature/Temperature.cpp new file mode 100755 index 0000000..c0871b0 --- /dev/null +++ b/src/libApp/temperature/Temperature.cpp @@ -0,0 +1,21 @@ +// ----------------------------------------------------------------------------------- +// Spot temperature measurements + +#include "Temperature.h" + +#ifdef TEMPERATURE_PRESENT + +#include "../weather/Weather.h" + +bool Temperature::init() { + return false; +} + +float Temperature::getChannel(int index) { + UNUSED(index); + return weather.getTemperature(); +} + +Temperature temperature; + +#endif diff --git a/src/libApp/temperature/Temperature.h b/src/libApp/temperature/Temperature.h new file mode 100755 index 0000000..bca5cb8 --- /dev/null +++ b/src/libApp/temperature/Temperature.h @@ -0,0 +1,27 @@ +// ----------------------------------------------------------------------------------- +// Spot temperature measurements +#pragma once + +#include "../../Common.h" + +#include "Ds1820.h" +#ifndef DS1820_DEVICES_PRESENT + +#include "Thermistor.h" +#ifndef THERMISTOR_DEVICES_PRESENT + +#define TEMPERATURE_PRESENT + +class Temperature { + public: + bool init(); + + float getChannel(int index); + + private: +}; + +extern Temperature temperature; + +#endif +#endif diff --git a/src/libApp/temperature/Thermistor.cpp b/src/libApp/temperature/Thermistor.cpp new file mode 100755 index 0000000..4964850 --- /dev/null +++ b/src/libApp/temperature/Thermistor.cpp @@ -0,0 +1,135 @@ +// ----------------------------------------------------------------------------------- +// Thermistor device support + +#include "Thermistor.h" + +#ifdef THERMISTOR_DEVICES_PRESENT + +#include "../../lib/tasks/OnTask.h" + +#include "../weather/Weather.h" + +#ifndef ANALOG_READ_RANGE + #define ANALOG_READ_RANGE 1023 +#endif + +void thermistorWrapper() { temperature.poll(); } + +// prepare for operation +bool Thermistor::init() { + static bool initialized = false; + if (initialized) return found; + + deviceCount = 1; + + if (deviceCount > 0) { + found = true; + VF("MSG: Temperature, start Thermistor monitor task (rate 500ms priority 6)... "); + if (tasks.add(500, 0, true, 6, thermistorWrapper, "therm")) { VLF("success"); } else { VLF("FAILED!"); } + } else found = false; + + found = true; + initialized = true; + + return found; +} + +// read devices, designed for a 0.5s polling interval +void Thermistor::poll() { + static int index = 0; + + if (found) { + int thermistorType = -1; + if (device[index] == THERMISTOR1) thermistorType = 0; else + if (device[index] == THERMISTOR2) thermistorType = 1; + + if (thermistorType >= 0 && devicePin[index] != OFF) { + + #ifdef ESP32 + float voltage = analogReadMilliVolts(devicePin[index])/1000.0F; + #else + float voltage = (analogRead(devicePin[index])/(float)ANALOG_READ_RANGE)*HAL_VCC; + #endif + + // calculate the device resistance + float resistance = NAN; + + const float eps = 0.001F; // 1 mV + if (voltage > eps && voltage < (HAL_VCC - eps)) { + + // handle special case where we add a resistor (say 10k) parallel to the thermistor (say 10k 3950) + #if defined(THERMISTOR_RPARALLEL_NEW) + float Req = settings[thermistorType].rSeries*(voltage/(HAL_VCC - voltage)); + float inv = (1.0F/Req) - (1.0F/THERMISTOR_RPARALLEL_NEW); + resistance = (inv > 0.0F) ? (1.0F/inv) : NAN; + + #elif defined(THERMISTOR_RPARALLEL) + float RtLow = settings[thermistorType].rNom/30.0F; + float RtHigh = settings[thermistorType].rNom*30.0F; + float Rpr = 1.0F/THERMISTOR_RPARALLEL; + bool vOutLowChanged = true; + bool vOutHighChanged = true; + float vOutLow, vOutHigh; + for (int i = 0; i < 100; i++) { + if (vOutLowChanged) { + resistance = 1.0F/(Rpr + 1.0F/RtLow); + vOutLow = HAL_VCC * (resistance/(settings[thermistorType].rSeries + resistance)); + vOutLowChanged = false; + } + if (vOutHighChanged) { + resistance = 1.0F/(Rpr + 1.0F/RtHigh); + vOutHigh = HAL_VCC * (resistance/(settings[thermistorType].rSeries + resistance)); + vOutHighChanged = false; + } + if (voltage < vOutLow) { RtLow /= 1.5F; vOutLowChanged = true; } else + if (voltage > vOutHigh) { RtHigh *= 1.5F; vOutHighChanged = true; } else + if (voltage > (vOutLow + vOutHigh)/2.0F) { RtLow = (RtLow*4.0F + RtHigh)/5.0F; vOutLowChanged = true; } else + if (voltage < (vOutLow + vOutHigh)/2.0F) { RtHigh = (RtLow + RtHigh*4.0F)/5.0F; vOutHighChanged = true; }; + } + resistance = 1.0F/((1.0F/resistance) - Rpr); + #else + // standard thermistor with pullup (say 10k 3950) + resistance = settings[thermistorType].rSeries*(voltage/(HAL_VCC - voltage)); + #endif + } + + // convert to temperature in degrees C + float f = log(resistance/settings[thermistorType].rNom)/settings[thermistorType].beta; + f += 1.0F/(settings[thermistorType].tNom + 273.15F); + float temperature = 1.0F/f - 273.15F; + + // constrain to a reasonable range, outside of this something is definately wrong + if (temperature < THERMISTOR_TEMPERATURE_MINIMUM || temperature > THERMISTOR_TEMPERATURE_MAXIMUM) temperature = NAN; + + // do a running average on the temperature + if (!isnan(temperature)) { + if (isnan(averageTemperature[index])) averageTemperature[index] = temperature; + averageTemperature[index] = (averageTemperature[index]*19.0F + temperature)/20.0F; + goodUntil[index] = millis() + 30000U; + } else { + // we must get a reading at least once every 30 seconds otherwise flag the failure with a NAN + if ((long)(millis() - goodUntil[index]) > 0) averageTemperature[index] = NAN; + } + + } + } + + index++; + if (index > 8) index = 0; +} + +// nine temperature sensors are supported, this gets the averaged temperature +// in deg. C otherwise it falls back to the weather sensor temperature +// index 0 is the ambient temperature, 1 through 8 are point temperatures #1, #2, etc. +// returns NAN if no temperature source is available or if a communications failure +// results in no valid readings for > 30 seconds +float Thermistor::getChannel(int index) { + if (found && index >= 0 && index <= 7) { + if (device[index] == (uint64_t)OFF) averageTemperature[index] = weather.getTemperature(); + return averageTemperature[index]; + } else return NAN; +} + +Thermistor temperature; + +#endif diff --git a/src/libApp/temperature/Thermistor.h b/src/libApp/temperature/Thermistor.h new file mode 100755 index 0000000..ce3f66c --- /dev/null +++ b/src/libApp/temperature/Thermistor.h @@ -0,0 +1,56 @@ +// ----------------------------------------------------------------------------------- +// Thermistor device support +#pragma once + +#include "../../Common.h" + +#ifdef THERMISTOR_DEVICES_PRESENT + +#ifndef THERMISTOR_TEMPERATURE_MINIMUM + #define THERMISTOR_TEMPERATURE_MINIMUM -30.0F +#endif + +#ifndef THERMISTOR_TEMPERATURE_MAXIMUM + #define THERMISTOR_TEMPERATURE_MAXIMUM 60.0F +#endif + +typedef struct ThermistorSettings { + float tNom; + float rNom; + float beta; + float rSeries; +} ThermistorSettings; + +class Thermistor { + public: + // prepare for operation + bool init(); + + // read devices, designed for a 0.1s polling interval + void poll(); + + // nine temperature sensors are supported, this gets the averaged + // temperature in deg. C otherwise it falls back to the weather sensor temperature + // index 0 is the ambient temperature, 1 through 8 are point temperatures #1, #2, etc. + // returns NAN if no temperature source is available or if a communications failure + // results in no valid readings for > 30 seconds + float getChannel(int index); + + private: + bool found = false; + uint8_t deviceCount = 0; + uint64_t device[9] = { (uint64_t)FOCUSER_TEMPERATURE, (uint64_t)FEATURE1_TEMP, (uint64_t)FEATURE2_TEMP, (uint64_t)FEATURE3_TEMP, (uint64_t)FEATURE4_TEMP, (uint64_t)FEATURE5_TEMP, (uint64_t)FEATURE6_TEMP, (uint64_t)FEATURE7_TEMP, (uint64_t)FEATURE8_TEMP }; + int16_t devicePin[9] = { FOCUSER_TEMPERATURE_PIN, FEATURE1_TEMPERATURE_PIN, FEATURE2_TEMPERATURE_PIN, FEATURE3_TEMPERATURE_PIN, FEATURE4_TEMPERATURE_PIN, FEATURE5_TEMPERATURE_PIN, FEATURE6_TEMPERATURE_PIN, FEATURE7_TEMPERATURE_PIN, FEATURE8_TEMPERATURE_PIN }; + + float averageTemperature[9] = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN }; + unsigned long goodUntil[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + ThermistorSettings settings[2] = { + { THERMISTOR1_TNOM, THERMISTOR1_RNOM, THERMISTOR1_BETA, THERMISTOR1_RSERIES }, + { THERMISTOR2_TNOM, THERMISTOR2_RNOM, THERMISTOR2_BETA, THERMISTOR2_RSERIES } + }; +}; + +extern Thermistor temperature; + +#endif diff --git a/src/libApp/weather/Weather.cpp b/src/libApp/weather/Weather.cpp new file mode 100755 index 0000000..6bdf5f0 --- /dev/null +++ b/src/libApp/weather/Weather.cpp @@ -0,0 +1,204 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// Weather related functions + +#include "Weather.h" + +#include "../../lib/tasks/OnTask.h" + +extern bool xBusy; + +#if WEATHER == BME280 || WEATHER == BME280_0x76 || WEATHER == BME280_SPI + #include // https://github.com/adafruit/Adafruit_BME280_Library + // and https://github.com/adafruit/Adafruit_Sensor + #if WEATHER == BME280 + #define BME_ADDRESS 0x77 + Adafruit_BME280 bmx; + #elif WEATHER == BME280_0x76 + #define BME_ADDRESS 0x76 + Adafruit_BME280 bmx; + #elif WEATHER == BME280_SPI && defined(SSPI_SHARED) + Adafruit_BME280 bmx(BMx280_CS_PIN, SSPI_MOSI, SSPI_MISO, SSPI_SCK); // software SPI + #elif WEATHER == BME280_SPI + Adafruit_BME280 bmx(BMx280_CS_PIN); // hardware SPI + #endif +#endif + +// BMP280 is the BME280 without humidity +#if WEATHER == BMP280 || WEATHER == BMP280_0x76 || WEATHER == BMP280_SPI + #include // https://github.com/adafruit/Adafruit_BMP280_Library + // and https://github.com/adafruit/Adafruit_Sensor + #if WEATHER == BMP280 + #define BMP_ADDRESS 0x77 + Adafruit_BMP280 bmx(&HAL_WIRE); + #elif WEATHER == BMP280_0x76 + #define BMP_ADDRESS 0x76 + Adafruit_BMP280 bmx(&HAL_WIRE); + #elif WEATHER == BMP280_SPI + Adafruit_BMP280 bmx(BMx280_CS_PIN); // hardware SPI + #endif +#endif + +void weatherPollWrapper() { weather.poll(); } + +bool Weather::init() { + #if WEATHER != OFF + success = false; + #if WEATHER == BME280 || WEATHER == BME280_0x76 + if (bmx.begin(BME_ADDRESS, &HAL_WIRE)) { + bmx.setSampling(Adafruit_BME280::MODE_FORCED, Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::FILTER_OFF); + weatherSensor = WS_BME280; success = true; + } else { DF("WRN: Weather.init(), BME280 (I2C 0x"); if (DEBUG != OFF) SERIAL_DEBUG.print(BME_ADDRESS, HEX); DLF(") not found"); } + #elif WEATHER == BMP280 || WEATHER == BMP280_0x76 + if (bmx.begin(BMP_ADDRESS)) { + bmx.setSampling(Adafruit_BMP280::MODE_FORCED, Adafruit_BMP280::SAMPLING_X1, Adafruit_BMP280::SAMPLING_X1, Adafruit_BMP280::FILTER_OFF); + weatherSensor = WS_BMP280; success = true; + } else { DF("WRN: Weather.init(), BMP280 (I2C 0x"); if (DEBUG != OFF) SERIAL_DEBUG.print(BMP_ADDRESS, HEX); DLF(") not found"); } + #elif WEATHER == BME280_SPI + if (bmx.begin()) { + bmx.setSampling(Adafruit_BME280::MODE_FORCED, Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::FILTER_OFF); + weatherSensor = WS_BME280; success = true; + } else { DLF("WRN: Weather.init(), BME280 (SPI) not found"); } + #elif WEATHER == BMP280_SPI + if (bmx.begin()) { + bmx.setSampling(Adafruit_BMP280::MODE_FORCED, Adafruit_BMP280::SAMPLING_X1, Adafruit_BMP280::SAMPLING_X1, Adafruit_BMP280::FILTER_OFF); + weatherSensor = WS_BMP280; success = true; + } else { DLF("WRN: Weather.init(), BMP280 (SPI) not found"); } + #else + #error "Configuration: Setting WEATHER unknown sensor value!" + #endif + + // follow any I2C device in-library init with a reset of the I2C bus speed + #if WEATHER == BME280 || WEATHER == BME280_0x76 || WEATHER == BMP280_0x76 || WEATHER == BMP280 + #ifdef HAL_WIRE_RESET_AFTER_CONNECT + HAL_WIRE.end(); + HAL_WIRE.begin(); + HAL_WIRE_SET_CLOCK(); + #endif + #endif + + if (success) { + VF("MSG: Weather, start weather monitor task (rate 5000ms priority 7)... "); + if (tasks.add(1000, 0, true, 7, weatherPollWrapper, "WeaPoll")) { VLF("success"); } else { VLF("FAILED!"); } + } + #else + success = true; + #endif + return success; +} + +// poll the weather sensor once every 5 seconds +void Weather::poll() { + #if WEATHER != OFF + if (success && !xBusy) { + static int phase = 0; + switch (++phase) { + case 1: + temperature = bmx.readTemperature(); + if (!isnan(temperature) && (temperature < -60.0F || temperature > 60.0F)) temperature = NAN; + return; + break; + case 2: + pressure = bmx.readPressure()/100.0F; + if (!isnan(pressure) && (pressure < 100.0F || pressure > 1100.0F)) pressure = NAN; + return; + break; + case 3: + #if WEATHER == BME280 || WEATHER == BME280_0x76 || WEATHER == BME280_SPI + humidity = bmx.readHumidity(); + if (!isnan(humidity) && (humidity < 0.0F || humidity > 100.0F)) humidity = NAN; + #endif + phase = 0; + break; + } + bmx.takeForcedMeasurement(); + + // if any measurements are anomalous assume all are invalid + #if WEATHER == BME280 || WEATHER == BME280_0x76 || WEATHER == BME280_SPI + if (isnan(temperature) || isnan(pressure) || isnan(humidity)) { + temperature = NAN; + pressure = NAN; + humidity = NAN; + } + #else + if (isnan(temperature) || isnan(pressure)) { + temperature = NAN; + pressure = NAN; + } + #endif + + // apply a 10 sample rolling average to the ambient temperature + static uint8_t nanCount = 0; + static bool firstSample = true; + if (firstSample) { + firstSample = false; + averageTemperature = temperature; + } + + if (!isnan(temperature)) { + nanCount = 0; + averageTemperature = (averageTemperature*9.0F + temperature)/10.0F; + } else { + if (nanCount < 15) { + nanCount++; + DLF("WRN: Weather.poll(), ambient temp. invalid"); + } else { + averageTemperature = NAN; + firstSample = true; + DLF("WRN: Weather.poll(), ambient temp. reset"); + } + } + } + #if WEATHER_SUPRESS_ERRORS == OFF + else { temperature = NAN; pressure = NAN; humidity = NAN; } + #endif + #endif +} + +// get temperature in deg. C +float Weather::getTemperature() { + return averageTemperature; +} + +// set temperature in deg. C +bool Weather::setTemperature(float t) { + if (weatherSensor == WS_NONE) { + if (t >= -60.0F && t <= 60.0F) { temperature = t; averageTemperature = t; } else return false; + } + return true; +} + +// get barometric pressure in hPa/mb +float Weather::getPressure() { + return pressure; +} + +// set barometric pressure in hPa/mb +bool Weather::setPressure(float p) { + if (weatherSensor == WS_NONE) { + if (p >= 100.0F && p <= 1100.0F) pressure = p; else return false; + } + return true; +} + +// get relative humidity in % +float Weather::getHumidity() { + return humidity; +} + +// set relative humidity in % +bool Weather::setHumidity(float h) { + if (weatherSensor == WS_NONE || weatherSensor == WS_BMP280) { + if (h >= 0.0F && h <= 100.0F) humidity = h; else return false; + } + return true; +} + +// get dew point in deg. C +// accurate to +/- 1 deg. C for RH above 50% +float Weather::getDewPoint() { + return averageTemperature - ((100.0F - humidity)/5.0F); + // a more accurate formula? + // return 243.04*(log(humidity/100.0) + ((17.625*averageTemperature)/(243.04 + averageTemperature)))/(17.625 - log(humidity/100.0) - ((17.625*averageTemperature)/(243.04 + averageTemperature))); +} + +Weather weather; diff --git a/src/libApp/weather/Weather.h b/src/libApp/weather/Weather.h new file mode 100755 index 0000000..1633db5 --- /dev/null +++ b/src/libApp/weather/Weather.h @@ -0,0 +1,49 @@ +// ----------------------------------------------------------------------------------------------------------------------------- +// Weather related functions +#pragma once + +#include "../../Common.h" + +enum WeatherSensor: uint8_t {WS_NONE, WS_BMP280, WS_BME280}; + +class Weather { + public: + bool init(); + + // designed for a 2s polling interval + void poll(); + + // get temperature in deg. C + float getTemperature(); + + // set temperature in deg. C + bool setTemperature(float t); + + // get barometric pressure in hPa/mb + float getPressure(); + + // get barometric pressure in hPa/mb + bool setPressure(float p); + + // get relative humidity in % + float getHumidity(); + + // set relative humidity in % + bool setHumidity(float h); + + // get dew point in deg. C + // accurate to +/- 1 deg. C for RH above 50% + float getDewPoint(); + + private: + WeatherSensor weatherSensor = WS_NONE; + + bool success = false; + + float temperature = NAN; + float averageTemperature = NAN; + float pressure = NAN; + float humidity = NAN; +}; + +extern Weather weather; diff --git a/src/pinmaps/Models.h b/src/pinmaps/Models.h new file mode 100755 index 0000000..e9512ad --- /dev/null +++ b/src/pinmaps/Models.h @@ -0,0 +1,101 @@ +// ------------------------------------------------------------------------------------------------- +// Loads pinmap model for current configuration +#pragma once + +#if PINMAP == MiniPCB + #define PINMAP_STR "MiniPCB v1" + #include "Pins.MiniPCB.h" +#endif +#if PINMAP == MiniPCB13 + #define PINMAP_STR "MiniPCB v1.3" + #include "Pins.MiniPCB.h" +#endif +#if PINMAP == MiniPCB2 + #define PINMAP_STR "MiniPCB v2" + #include "Pins.MiniPCB.h" +#endif +#if PINMAP == MaxPCB + #define PINMAP_STR "MaxPCB v1" + #include "Pins.MaxPCB.h" +#endif +#if PINMAP == MaxPCB2 + #define PINMAP_STR "MaxPCB v2" + #include "Pins.MaxPCB.h" +#endif +#if PINMAP == MaxPCB3 + #ifdef ARDUINO_TEENSY41 + #define PINMAP_STR "MaxPCB v3.6" + #include "Pins.MaxPCB36.h" + #else + #define PINMAP_STR "MaxPCB v3" + #include "Pins.MaxPCB3.h" + #endif +#endif +#if PINMAP == MaxPCB4 + #define PINMAP_STR "MaxPCB v4" + #include "Pins.MaxPCB4.h" +#endif +#if PINMAP == MaxESP3 + #define PINMAP_STR "MaxESP v3" + #include "Pins.MaxESP3.h" +#endif +#if PINMAP == MaxESP4 + #define PINMAP_STR "MaxESP v4" + #include "Pins.MaxESP4.h" +#endif +#if PINMAP == CNC3 + #define PINMAP_STR "CNC v3" + #include "Pins.CNC3.h" +#endif +#if PINMAP == MicroScope // Experimental and may be removed at any point! USE AY YOUR OWN RISK!!! + #define PINMAP_STR "MicroScope v0.2" + #include "Pins.MicroScope.h" +#endif +#if PINMAP == STM32Blue + #define PINMAP_STR "STM32 Bluepill" + #include "Pins.STM32B.h" +#endif +#if PINMAP == FYSETC_E4 + #define PINMAP_STR "FYSETC E4" + #include "Pins.FYSETC_E4.h" +#endif +#if PINMAP == FYSETC_S6 + #define PINMAP_STR "FYSETC S6" + #include "Pins.FYSETC_S6.h" +#endif +#if PINMAP == FYSETC_S6_2 + #define PINMAP_STR "FYSETC S6 v2" + #include "Pins.FYSETC_S6.h" +#endif +#if PINMAP == BTT_SKR_PRO + #define PINMAP_STR "BTT SKR PRO v1.2" + #include "Pins.SKR_PRO.h" +#endif +#if PINMAP == BTT_OCTOPUS_PRO + #define PINMAP_STR "BTT OCTOPUS PRO v1.1" + #include "Pins.OctopusPro.h" +#endif +#if PINMAP == JTWSTM + #define PINMAP_STR "JTWSTM v2.1" + #include "Pins.JTWPCB2.h" +#endif +#if PINMAP == MANTICORE + #define PINMAP_STR "JTW MANTICORE v1.0" + #include "Pins.Manticore.h" +#endif +#if PINMAP == MaxSTM3 || PINMAP == MaxSTM3I + #define PINMAP_STR "MaxSTM v3" + #include "Pins.MaxSTM.h" +#endif +#if PINMAP == SAL_XB1 + #define PINMAP_STR "SAL-XB1" + #include "Pins.SAL_XB1.h" +#endif +#ifndef PINMAP_STR + #define PINMAP_STR "Unknown" +#endif + +// all unassigned pins OFF +#include "Pins.defaults.h" + +#include "Validate.h" \ No newline at end of file diff --git a/src/pinmaps/Pins.CNC3.h b/src/pinmaps/Pins.CNC3.h new file mode 100755 index 0000000..b12a100 --- /dev/null +++ b/src/pinmaps/Pins.CNC3.h @@ -0,0 +1,130 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for CNC Shield Version 3 (with WeMos D1 R32) +#pragma once + +// Note: SPI for TMC drivers: GPIO32 (MOSI), GPIO33 (SCK), GPIO15 (CS Axis1) +// on WeMos D1 R32 PCB are somewhat difficult to wire into, but optional +// GPIO18 (CS Axis2) is on top + +#if defined(ESP32) + +// Serial0: RX Pin GPIO3, TX Pin GPIO1 (to USB serial adapter) +// Serial1: RX1 Pin GPIO10, TX1 Pin GPIO9 (on SPI Flash pins, must be moved to be used) +// Serial2: RX2 Pin GPIO13, TX2 Pin GPIO5 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 + #define SERIAL_B_RX 13 + #define SERIAL_B_TX 5 +#endif + +// Uses default ESP32 I2C GPIO21 (SDA) and GPIO22 (SCL) + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX2_PIN 18 // ESP8266 RST control, or SPI MISO Axis1&2 +#define AUX3_PIN 21 // Home SW for Axis1, or I2C SDA {assuming default SCL/SDA pins are present} +#define AUX4_PIN 22 // Home SW for Axis2, or I2C SCL +#define AUX7_PIN 39 // Limit SW, PPS, etc. +#define AUX8_PIN 23 // 1-Wire, Status LED1, Status LED2, Reticle LED, etc. + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX8_PIN // Default Pin for 1-wire bus +#endif +#define ADDON_GPIO0_PIN 27 // ESP8266 GPIO0 (Dir2) +#define ADDON_RESET_PIN AUX2_PIN // ESP8266 RST + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 36 // [input only 36] (also analog A0) +#endif + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif +#define MOUNT_LED_PIN STATUS_LED_PIN // Default LED Cathode (-) +#define RETICLE_LED_PIN STATUS_LED_PIN // Default LED Cathode (-) + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN 2 // [must be low at boot 2] +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN AUX7_PIN // PPS time source, GPS for example +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN +#endif + +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#define SHARED_ENABLE_PIN 12 // Hint that the enable pins are shared + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN SHARED // [must be low at boot 12] +#define AXIS1_M0_PIN 32 // SPI MOSI +#define AXIS1_M1_PIN 33 // SPI SCK +#define AXIS1_M2_PIN 15 // SPI CS (UART TX) +#define AXIS1_M3_PIN AUX2_PIN // SPI MISO (UART RX) +#define AXIS1_STEP_PIN 26 +#define AXIS1_DIR_PIN 16 +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN SHARED +#define AXIS2_M0_PIN 32 // SPI MOSI +#define AXIS2_M1_PIN 33 // SPI SCK +#define AXIS2_M2_PIN 0 // [must be high at boot 0] SPI CS (UART TX) +#define AXIS2_M3_PIN AUX2_PIN // SPI MISO (UART RX) +#define AXIS2_STEP_PIN 25 +#define AXIS2_DIR_PIN 27 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN SHARED +#define AXIS3_M0_PIN OFF // SPI MOSI +#define AXIS3_M1_PIN OFF // SPI SCK +#define AXIS3_M2_PIN OFF // SPI CS (UART TX) +#define AXIS3_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS3_STEP_PIN 19 +#define AXIS3_DIR_PIN 14 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN SHARED +#define AXIS4_M0_PIN OFF // SPI MOSI +#define AXIS4_M1_PIN OFF // SPI SCK +#define AXIS4_M2_PIN OFF // SPI CS (UART TX) +#define AXIS4_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS4_STEP_PIN 17 +#define AXIS4_DIR_PIN 14 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN SHARED +#define AXIS5_M0_PIN OFF // SPI MOSI +#define AXIS5_M1_PIN OFF // SPI SCK +#define AXIS5_M2_PIN OFF // SPI CS (UART TX) +#define AXIS5_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS5_STEP_PIN 19 +#define AXIS5_DIR_PIN 14 + +// ST4 interface +#define ST4_RA_W_PIN 34 // [input only 34] ST4 RA- West +#define ST4_DEC_S_PIN 18 // ST4 DE- South +#define ST4_DEC_N_PIN 4 // ST4 DE+ North +#define ST4_RA_E_PIN 35 // [input only 35] ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.FYSETC_E4.h b/src/pinmaps/Pins.FYSETC_E4.h new file mode 100755 index 0000000..8f0c315 --- /dev/null +++ b/src/pinmaps/Pins.FYSETC_E4.h @@ -0,0 +1,182 @@ +// ------------------------------------------------------------------------------------------------- +// Pinmap for FYSETC E4 Version 1.0 (ESP32) +#pragma once + +#if defined(ESP32) + +// Serial0: RX Pin GPIO3, TX Pin GPIO1 (to USB serial adapter) +// Serial1: RX1 Pin GPIO10, TX1 Pin GPIO9 (on SPI Flash pins, must be moved to be used) +// Serial2: RX2 Pin GPIO16, TX2 Pin GPIO17 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif + +// Uses default ESP32 I2C GPIO21 (SDA) and GPIO22 (SCL) + +// Use the following settings for the 4x TMC UART drivers +#if defined(STEP_DIR_TMC_UART_PRESENT) || defined(SERVO_TMC2209_PRESENT) + #define SERIAL_TMC_HARDWARE_UART + #define SERIAL_TMC Serial1 // Use a single hardware serial port to up to four drivers + #define SERIAL_TMC_BAUD 460800 // Baud rate + #if SERIAL_A_BAUD_DEFAULT == OFF + // if SERIAL_A is OFF map the hardware serial UART to the Serial0 pins + // you will need to remove jumpers to allow USB to work for an E4 fimware update + #define SERIAL_TMC_RX 3 // Recieving data + #define SERIAL_TMC_TX 1 // Transmit data + #define SPARE_RX_PIN OFF // Set _RX above to 0 (GPIO0) and use 3 here + #else + #ifndef SERIAL_TMC_RX + #define SERIAL_TMC_RX 0 // Recieving data (GPIO0 unused except for flashing) + #endif + #define SERIAL_TMC_TX 15 // Transmit data (Z-MIN) + #define SPARE_RX_PIN OFF // Not supported in this case + #endif + // map the driver addresses so axis Axis1(0) is X, Axis2(1) is Y, Axis3(2) is Z, Axis4(3) is E0... + // Axis1(0) is 1, Axis2(1) is 3, Axis3(2) is 0, Axis4(3) is 2, Axis5(4) is 0 + #define SERIAL_TMC_ADDRESS_MAP(x) (((x)==0)?1 : (((x)==1)?3 : (((x)==2)?0 : ((x)==3)?2 : 0))) +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX3_PIN 34 // [input only 34] Home SW for Axis1 (X_MIN) +#define AUX4_PIN 35 // [input only 35] Home SW for Axis2 (Y_MIN) +#define AUX5_PIN 2 // [must be low at boot 2] Dew heater, etc. (HEAT_E0) +#define AUX6_PIN 4 // Dew heater, etc. (HEAT_BED) +#define AUX7_PIN SPARE_RX_PIN // Option for 1-Wire, etc. +#define AUX8_PIN 13 // Status LED or Buzzer, Dew heater, etc. (FAN_E0) + +// Thermistor (temperature) sensor inputs have built-in 4.7K Ohm pullups and a 10uF cap for noise supression +#define TEMP0_PIN 36 // [input only 36] PEC (TE) +#define TEMP1_PIN 39 // [input only 39] Limit (TB) + +#ifndef FOCUSER_TEMPERATURE_PIN + #define FOCUSER_TEMPERATURE_PIN TEMP0_PIN +#endif +#ifndef FEATURE1_TEMPERATURE_PIN + #define FEATURE1_TEMPERATURE_PIN TEMP0_PIN +#endif +#ifndef FEATURE2_TEMPERATURE_PIN + #define FEATURE2_TEMPERATURE_PIN TEMP1_PIN +#endif + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX7_PIN // Default Pin for OneWire bus +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 36 // [input only 36] PEC Sense, analog or digital (TE) +#endif + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif +#define MOUNT_LED_PIN STATUS_LED_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN STATUS_LED_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN AUX8_PIN // Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN OFF +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN 39 // [input only 39] (TB) +#endif + +#define SHARED_ENABLE_PIN 25 // Hint that the enable pins are shared + +// Axis1 RA/Azm step/dir driver (X-AXIS) +#define AXIS1_ENABLE_PIN SHARED +#define AXIS1_M0_PIN OFF // SPI MOSI +#define AXIS1_M1_PIN OFF // SPI SCK +#define AXIS1_M2_PIN OFF // SPI CS (UART TX) +#define AXIS1_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS1_STEP_PIN 27 +#define AXIS1_DIR_PIN 26 +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif +#ifndef AXIS1_SERVO_PH1_PIN + #define AXIS1_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS1_SERVO_PH2_PIN + #define AXIS1_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS1_ENCODER_A_PIN + #define AXIS1_ENCODER_A_PIN OFF +#endif +#ifndef AXIS1_ENCODER_B_PIN + #define AXIS1_ENCODER_B_PIN OFF +#endif + +// Axis2 Dec/Alt step/dir driver (Y-AXIS) +#define AXIS2_ENABLE_PIN SHARED +#define AXIS2_M0_PIN OFF // SPI MOSI +#define AXIS2_M1_PIN OFF // SPI SCK +#define AXIS2_M2_PIN OFF // SPI CS (UART TX) +#define AXIS2_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS2_STEP_PIN 33 +#define AXIS2_DIR_PIN 32 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif +#ifndef AXIS2_SERVO_PH1_PIN + #define AXIS2_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS2_SERVO_PH2_PIN + #define AXIS2_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS2_ENCODER_PH1_PIN + #define AXIS2_ENCODER_A_PIN OFF +#endif +#ifndef AXIS2_ENCODER_B_PIN + #define AXIS2_ENCODER_B_PIN OFF +#endif + +// For rotator stepper driver (Z-AXIS) +#define AXIS3_ENABLE_PIN SHARED +#define AXIS3_M0_PIN OFF // SPI MOSI +#define AXIS3_M1_PIN OFF // SPI SCK +#define AXIS3_M2_PIN OFF // SPI CS (UART TX) +#define AXIS3_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS3_STEP_PIN 14 +#define AXIS3_DIR_PIN 12 // [must be low at boot 12] + +// For focuser1 stepper driver (E0-AXIS) +#define AXIS4_ENABLE_PIN SHARED +#define AXIS4_M0_PIN OFF // SPI MOSI +#define AXIS4_M1_PIN OFF // SPI SCK +#define AXIS4_M2_PIN OFF // SPI CS (UART TX) +#define AXIS4_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS4_STEP_PIN 16 +#define AXIS4_DIR_PIN 17 + +// For focuser2 stepper driver (Z-AXIS) +#define AXIS5_ENABLE_PIN SHARED +#define AXIS5_M0_PIN OFF // SPI MOSI +#define AXIS5_M1_PIN OFF // SPI SCK +#define AXIS5_M2_PIN OFF // SPI CS (UART TX) +#define AXIS5_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS5_STEP_PIN 14 +#define AXIS5_DIR_PIN 12 // [must be low at boot 12] + +// ST4 interface +#define ST4_RA_W_PIN OFF // ST4 RA- West +#define ST4_DEC_S_PIN OFF // ST4 DE- South +#define ST4_DEC_N_PIN OFF // ST4 DE+ North +#define ST4_RA_E_PIN OFF // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.FYSETC_S6.h b/src/pinmaps/Pins.FYSETC_S6.h new file mode 100755 index 0000000..066b094 --- /dev/null +++ b/src/pinmaps/Pins.FYSETC_S6.h @@ -0,0 +1,204 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for FYSETC S6 +#pragma once + +#if defined(STM32F446xx) + +// TX2/RX2 (PA2/PA3) is on the Y+ and Z+ end stops and is reserved for GPS (etc, no command channel is associated with this port) + +// Schematic isn't entirely clear, best guess: +// Serial1 RX1 Pin PB7, TX1 Pin PB6 +// Serial2 RX2 Pin PA3, TX2 Pin PA2 +// Serial3 RX3 Pin PC11, TX3 Pin PC10 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial1 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C Serial3 +#endif + +// Auto assign the Serial2 port pins for GPS +#if SERIAL_GPS_BAUD != OFF + #ifndef SERIAL_GPS + #define SERIAL_GPS SoftSerial + #define SERIAL_GPS_RX PA3 + #define SERIAL_GPS_TX PA2 + #endif +#endif + +// Thermistor (temperature) sensor inputs have built-in 4.7K Ohm pullups and a 10uF cap for noise supression +#define TEMP0_PIN PC0 // Temp0 (on TE0, THERMO0) +#define TEMP1_PIN PC1 // Temp1 (on TE1, THERMO1) +#define TEMP2_PIN PC2 // Temp2 (on TE2, THERMO2) +#define TEMP3_PIN PC3 // Temp3 (on TB , THERMO3) + +#ifndef FOCUSER_TEMPERATURE_PIN + #define FOCUSER_TEMPERATURE_PIN TEMP0_PIN +#endif +#ifndef FEATURE1_TEMPERATURE_PIN + #define FEATURE1_TEMPERATURE_PIN TEMP1_PIN +#endif +#ifndef FEATURE2_TEMPERATURE_PIN + #define FEATURE2_TEMPERATURE_PIN TEMP2_PIN +#endif +#ifndef FEATURE3_TEMPERATURE_PIN + #define FEATURE3_TEMPERATURE_PIN TEMP3_PIN +#endif + +// Fans (From Marlin) we use for Auxiliary Features (switches etc.) Probably with a little crafty wiring these can be 3V3 or 5V. +#define FAN0_PIN PB0 // Fan0 (on FAN0) +#define FAN1_PIN PB1 // Fan1 (on FAN1) +#define FAN2_PIN PB2 // Fan2 (on FAN2) + +// Heaters (From Marlin) we use for Auxiliary Features (switches, dew heaters, etc.) +#define HEATER0_PIN PB3 // Heater0 (on E0-OUT, E0-Heater) +#define HEATER1_PIN PB4 // Heater1 (on E1-OUT, E1-Heater) +#define HEATER2_PIN PB15 // Heater2 (on E2-OUT, E2-Heater) +#define HEATER3_PIN PC8 // Heater3 (on BED-OUT, Heated-Bed) + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +// I defined 7 Aux pins so they match up with the first 7 Auxiliary Feature slots avaliable in OnStep +// Aux1-3 can be used for pretty much anything +// Aux4-7 are more for dew-heaters +#define AUX1_PIN FAN0_PIN +#define AUX2_PIN FAN1_PIN +#define AUX3_PIN FAN2_PIN +#define AUX4_PIN HEATER0_PIN +#define AUX5_PIN HEATER1_PIN +#define AUX6_PIN HEATER2_PIN +#define AUX7_PIN HEATER3_PIN + +// Misc. pins +#ifndef DS3234_CS_PIN + #define DS3234_CS_PIN PA4 // Default CS Pin for DS3234 on SPI (on EXP2 shared with the ESP8266 RST pin) +#endif +#ifndef BMx280_CS_PIN + #define BMx280_CS_PIN PC7 // Default CS Pin for BME280/BMP280 on SPI (on EXP2 shared with LED2/Reticle) +#endif +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN PE0 // Default Pin for OneWire bus (on E2-MOT PD-EN, right hand pin) +#endif +#define ADDON_GPIO0_PIN PD0 // ESP8266 GPIO0 (on EXP1) +#define ADDON_RESET_PIN PA4 // ESP8266 RST (on EXP2 shared with the DS3234 CS pin) + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN +#define PEC_SENSE_PIN PA1 // PEC Sense, analog or digital (on X+ so it can have 3v3 or 5v on adjacent pin) +#endif + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN PC6 // Drain (on EXP2) One could perhaps move these to the RGB leds, there's a header but no +5V present on it. +#endif +#ifndef MOUNT_LED_PIN + #define MOUNT_LED_PIN PC6 // Drain (on EXP2 shared with Reticle/BME280_CS) +#endif +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN PC7 // Drain (on EXP2 shared with LED2/BME280_CS) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN PC9 // Tone (on BEEP/EXP1) +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN PB10 // PPS time source, GPS for example (on EXP2) +#endif + +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN PA0 // Limit switch sense (on Z-) +#endif + +// Soft SPI bus to stepper drivers +#if PINMAP == FYSETC_S6_2 + #define SS_MOSI PE14 + #define SS_SCK PE12 + #define SS_MISO PE13 +#else + #define SS_MOSI PA7 + #define SS_SCK PA5 + #define SS_MISO PA6 +#endif + +// Axis1 RA/Azm step/dir driver +#if PINMAP == FYSETC_S6_2 + #define AXIS1_ENABLE_PIN PE9 +#else + #define AXIS1_ENABLE_PIN PE12 +#endif +#define AXIS1_M0_PIN SS_MOSI // SPI MOSI +#define AXIS1_M1_PIN SS_SCK // SPI SCK +#define AXIS1_M2_PIN PE7 // SPI CS (UART TX) +#define AXIS1_M3_PIN SS_MISO // SPI MISO (UART RX) +#define AXIS1_STEP_PIN PE11 +#define AXIS1_DIR_PIN PE10 +#define AXIS1_FAULT_PIN AXIS1_M3_PIN +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN PB14 // (on X-) +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN PD9 +#define AXIS2_M0_PIN SS_MOSI // SPI MOSI +#define AXIS2_M1_PIN SS_SCK // SPI SCK +#define AXIS2_M2_PIN PE15 // SPI CS (UART TX) +#define AXIS2_M3_PIN SS_MISO // SPI MISO (UART RX) +#define AXIS2_STEP_PIN PD8 +#define AXIS2_DIR_PIN PB12 +#define AXIS2_FAULT_PIN AXIS2_M3_PIN +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN PB13 // (on Y-) +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN PD15 +#define AXIS3_M0_PIN SS_MOSI // SPI MOSI +#define AXIS3_M1_PIN SS_SCK // SPI SCK +#define AXIS3_M2_PIN PD10 // SPI CS (UART TX) +#define AXIS3_M3_PIN SS_MISO // SPI MISO (UART RX) +#define AXIS3_STEP_PIN PD14 +#define AXIS3_DIR_PIN PD13 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN PD4 +#define AXIS4_M0_PIN SS_MOSI // SPI MOSI +#define AXIS4_M1_PIN SS_SCK // SPI SCK +#define AXIS4_M2_PIN PD7 // SPI CS (UART TX) +#define AXIS4_M3_PIN SS_MISO // SPI MISO (UART RX) +#define AXIS4_STEP_PIN PD5 +#define AXIS4_DIR_PIN PD6 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN PE5 +#define AXIS5_M0_PIN SS_MOSI // SPI MOSI +#define AXIS5_M1_PIN SS_SCK // SPI SCK +#define AXIS5_M2_PIN PC14 // SPI CS (UART TX) +#define AXIS5_M3_PIN SS_MISO // SPI MISO (UART RX) +#define AXIS5_STEP_PIN PE6 +#define AXIS5_DIR_PIN PC13 + +// For focuser3 stepper driver +#define AXIS6_ENABLE_PIN PE3 +#define AXIS6_M0_PIN SS_MOSI // SPI MOSI +#define AXIS6_M1_PIN SS_SCK // SPI SCK +#define AXIS6_M2_PIN PC15 // SPI CS (UART TX) +#define AXIS6_M3_PIN SS_MISO // SPI MISO (UART RX) +#define AXIS6_STEP_PIN PE2 +#define AXIS6_DIR_PIN PE4 + +// ST4 interface +#define ST4_RA_W_PIN PA8 // ST4 RA- West (on EXP1) +#define ST4_DEC_S_PIN PD2 // ST4 DE- South (on EXP1) +#define ST4_DEC_N_PIN PC12 // ST4 DE+ North (on EXP1) +#define ST4_RA_E_PIN PD1 // ST4 RA+ East (on EXP1) + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.JTWPCB2.h b/src/pinmaps/Pins.JTWPCB2.h new file mode 100755 index 0000000..97a4d0b --- /dev/null +++ b/src/pinmaps/Pins.JTWPCB2.h @@ -0,0 +1,66 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep JTW PCB rev 2.1 STM32F411CE PCB (or the Blackpill https://github.com/WeActTC/MiniF4-STM32F4x1) +#pragma once + +#if defined(STM32F411xE) || defined(STM32F401xC) + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A HardSerial + #define SERIAL_A_RX PA10 + #define SERIAL_A_TX PA9 +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B HardSerial + #define SERIAL_B_RX PA3 + #define SERIAL_B_TX PA2 +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN PB12 // Status LED +#define AUX2_PIN PC14 // PPS +#define AUX5_PIN PA9 // TX1 +#define AUX6_PIN PA10 // RX1 + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#define STATUS_BUZZER_PIN PC15 // Tone + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN AUX2_PIN // PPS time source, GPS for example (MaxSTM version 3.61 and later) +#endif + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN PB13 +#define AXIS1_M0_PIN PA7 // SPI MOSI +#define AXIS1_M1_PIN PA5 // SPI SCK +#define AXIS1_M2_PIN PA1 // SPI CS (UART TX) +#define AXIS1_M3_PIN PA6 // SPI MISO (UART RX) +#define AXIS1_STEP_PIN PB10 +#define AXIS1_DIR_PIN PB2 + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN PB14 +#define AXIS2_M0_PIN PA7 // SPI MOSI +#define AXIS2_M1_PIN PA5 // SPI SCK +#define AXIS2_M2_PIN PA0 // SPI CS (UART TX) +#define AXIS2_M3_PIN PA6 // SPI MISO (UART RX) +#define AXIS2_STEP_PIN PA4 +#define AXIS2_DIR_PIN PB0 + +// ST4 interface +#define ST4_RA_W_PIN PA15 // ST4 RA- West +#define ST4_DEC_S_PIN PB3 // ST4 DE- South +#define ST4_DEC_N_PIN PB4 // ST4 DE+ North +#define ST4_RA_E_PIN PB5 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.Manticore.h b/src/pinmaps/Pins.Manticore.h new file mode 100755 index 0000000..71705bf --- /dev/null +++ b/src/pinmaps/Pins.Manticore.h @@ -0,0 +1,137 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for JTW Manticore (ESP32S) +#pragma once + +#if defined(ESP32) + +// let any special processing the pinmap needs happen +#define PIN_INIT() { \ + pinMode(33, INPUT_PULLDOWN); \ + delay(100); \ + if (digitalRead(33) != HIGH) { \ + do { \ + SERIAL_DEBUG.begin(SERIAL_DEBUG_BAUD); \ + SERIAL_DEBUG.println("ERR: OnStepX firmware detected the SWS MCU; you must upload the SWS firmware here!"); \ + SERIAL_DEBUG.println(analogRead(33)); \ + delay(2000); \ + } while (true); \ + } \ + pinMode(33, INPUT); \ +} + +// Serial0: RX Pin GPIO3, TX Pin GPIO1 (to USB serial adapter) +// Serial1: RX1 Pin GPIO17, TX1 Pin GPIO16 (for TMC2209 UART COMMS) +// Serial2: RX2 Pin GPIO39, TX2 Pin GPIO18 (to secondary MCU) + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 + #define SERIAL_B_RX 39 + #define SERIAL_B_TX 18 +#endif + +#ifndef SERIAL_GPS + #define SERIAL_GPS SoftSerial + #define SERIAL_GPS_RX 19 + #define SERIAL_GPS_TX 2 +#endif + +// Use the following settings for any TMC UART driver (TMC2209) that may be present +#if defined(STEP_DIR_TMC_UART_PRESENT) || defined(SERVO_TMC2209_PRESENT) + #define SERIAL_TMC_HARDWARE_UART + #define SERIAL_TMC Serial1 // Use a single hardware serial port to up to four drivers + #define SERIAL_TMC_BAUD 460800 // Baud rate + #define SERIAL_TMC_RX 17 // Recieving data + #define SERIAL_TMC_TX 16 // Transmit data + #define SERIAL_TMC_ADDRESS_MAP(x) ((x==4)?2 : x) // Axis1(0) is 0, Axis2(1) is 1, Axis3(2) is 2, Axis4(3) is 3, Axis5(4) is 2 +#endif + +// Uses default ESP32 I2C GPIO21 (SDA) and GPIO22 (SCL) + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX3_PIN OFF // Home SW for Axis1 +#define AUX4_PIN OFF // Home SW for Axis2 + +#define ADDON_SELECT_PIN 15 + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN OFF // Default Pin for OneWire bus +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 36 // PEC Sense, analog (A0) or digital +#endif + +// The status LED w/ 4.7k resistor in series to limit the current to the LED +#ifdef STATUS_LED_ON_STATE + #undef STATUS_LED_ON_STATE +#endif +#define STATUS_LED_ON_STATE HIGH +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN 2 // Default LED Anode (+) +#endif +#define MOUNT_LED_PIN STATUS_LED_PIN // Default LED Anode (+) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN OFF // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN 2 // Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN 23 // PPS time source, GPS for example +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN GPIO_PIN(4) +#endif + +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#define SHARED_ENABLE_PIN 5 // Hint that the enable pins are shared + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN SHARED // Enable pin control +#define AXIS1_M0_PIN OFF // hardwired for TMC UART address 0 +#define AXIS1_M1_PIN OFF // hardwired for TMC UART address 0 +#define AXIS1_M2_PIN OFF // UART TX +#define AXIS1_M3_PIN OFF // UART RX +#define AXIS1_STEP_PIN 4 +#define AXIS1_DIR_PIN 0 // [must be high at boot 0] +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN GPIO_PIN(0) +#endif +#define AXIS1_ENCODER_A_PIN 14 +#define AXIS1_ENCODER_B_PIN 27 + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN SHARED +#define AXIS2_M0_PIN OFF // hardwired for TMC UART address 1 +#define AXIS2_M1_PIN OFF // hardwired for TMC UART address 1 +#define AXIS2_M2_PIN OFF // N/C +#define AXIS2_M3_PIN OFF // UART RX +#define AXIS2_STEP_PIN 12 // [low at boot] +#define AXIS2_DIR_PIN 13 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN GPIO_PIN(1) +#endif +#define AXIS2_ENCODER_A_PIN 26 +#define AXIS2_ENCODER_B_PIN 25 + +// ST4 interface +#define ST4_RA_W_PIN 34 // [input only 34] ST4 RA- West +#define ST4_DEC_S_PIN 32 // ST4 DE- South +#define ST4_DEC_N_PIN 33 // ST4 DE+ North +#define ST4_RA_E_PIN 35 // [input only 35] ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MaxESP3.h b/src/pinmaps/Pins.MaxESP3.h new file mode 100755 index 0000000..84f9f20 --- /dev/null +++ b/src/pinmaps/Pins.MaxESP3.h @@ -0,0 +1,145 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MaxESP Version 3.x (ESP32S) +#pragma once + +#if defined(ESP32) + +// Serial0: RX Pin GPIO3, TX Pin GPIO1 (to USB serial adapter) +// Serial1: RX1 Pin GPIO10, TX1 Pin GPIO9 (on SPI Flash pins, must be moved to be used) +// Serial2: RX2 Pin GPIO16, TX2 Pin GPIO17 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 +#endif + +// Use the following settings for any TMC UART driver (TMC2209) that may be present +#if defined(STEP_DIR_TMC_UART_PRESENT) || defined(SERVO_TMC2209_PRESENT) + #if defined(SERIAL_TMC_HARDWARE_UART) + #define SERIAL_TMC Serial1 // Use a single hardware serial port to up to four drivers + #define SERIAL_TMC_BAUD 460800 // Baud rate + #define SERIAL_TMC_RX 39 // Recieving data + #define SERIAL_TMC_TX 23 // Transmit data + #define SERIAL_TMC_ADDRESS_MAP(x) ((x==4)?2 : x) // Axis1(0) is 0, Axis2(1) is 1, Axis3(2) is 2, Axis4(3) is 3, Axis5(4) is 2 + #endif +#endif + +// Uses default ESP32 I2C GPIO21 (SDA) and GPIO22 (SCL) + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX2_PIN 4 // ESP8266 RST control, or MISO for Axis1&2, or Axis4 EN support +#define AUX3_PIN 21 // Home SW for Axis1, or I2C SDA +#define AUX4_PIN 22 // Home SW for Axis2, or I2C SCL +#define AUX7_PIN 39 // Limit SW, PPS, etc. +#define AUX8_PIN 25 // 1-Wire, Status LED, Reticle LED, Tone, etc. + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX8_PIN // Default Pin for OneWire bus +#endif +#define ADDON_GPIO0_PIN 26 // ESP8266 GPIO0 (Dir2) +#ifndef ADDON_RESET_PIN + #define ADDON_RESET_PIN AUX2_PIN // ESP8266 RST +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 36 // [input only 36] PEC Sense, analog (A0) or digital (GPIO36) +#endif + +// The status LED is a two wire jumper with a 2k resistor in series to limit the current to the LED +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif +#define MOUNT_LED_PIN STATUS_LED_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN STATUS_LED_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN AUX8_PIN // Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN AUX7_PIN // PPS time source, GPS for example +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN +#endif + +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#define SHARED_ENABLE_PIN 12 // Hint that the enable pins are shared + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN SHARED // [must be low at boot 12] +#define AXIS1_M0_PIN 13 // SPI MOSI +#define AXIS1_M1_PIN 14 // SPI SCK +#define AXIS1_M2_PIN 23 // SPI CS (UART TX) +#if AXIS4_POWER_DOWN != ON + #define AXIS1_M3_PIN AUX2_PIN // SPI MISO (UART RX) +#endif +#define AXIS1_STEP_PIN 18 +#define AXIS1_DIR_PIN 0 // [must be high at boot 0] +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN SHARED +#define AXIS2_M0_PIN 13 // SPI MOSI +#define AXIS2_M1_PIN 14 // SPI SCK +#define AXIS2_M2_PIN 5 // SPI CS (UART TX) +#if AXIS4_POWER_DOWN != ON + #define AXIS2_M3_PIN AUX2_PIN // SPI MISO (UART RX) +#endif +#define AXIS2_STEP_PIN 27 +#define AXIS2_DIR_PIN 26 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN OFF // No enable pin control (always enabled) +#define AXIS3_M0_PIN OFF // SPI MOSI +#define AXIS3_M1_PIN OFF // SPI SCK +#define AXIS3_M2_PIN OFF // SPI CS (UART TX) +#define AXIS3_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS3_STEP_PIN 2 // [must be low at boot 2] +#define AXIS3_DIR_PIN 15 + +// For focuser1 stepper driver +#if !defined(SERIAL_TMC_HARDWARE_UART) && AXIS4_POWER_DOWN == ON + #define AXIS4_ENABLE_PIN AUX2_PIN +#endif +#define AXIS4_M0_PIN OFF // SPI MOSI +#define AXIS4_M1_PIN OFF // SPI SCK +#define AXIS4_M2_PIN OFF // SPI CS (UART TX) +#define AXIS4_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS4_STEP_PIN 19 +#define AXIS4_DIR_PIN 15 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN OFF // No enable pin control (always enabled) +#define AXIS5_M0_PIN OFF // SPI MOSI +#define AXIS5_M1_PIN OFF // SPI SCK +#define AXIS5_M2_PIN OFF // SPI CS (UART TX) +#define AXIS5_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS5_STEP_PIN 2 +#define AXIS5_DIR_PIN 15 + +// ST4 interface +#define ST4_RA_W_PIN 34 // [input only 34] ST4 RA- West +#define ST4_DEC_S_PIN 32 // ST4 DE- South +#define ST4_DEC_N_PIN 33 // ST4 DE+ North +#define ST4_RA_E_PIN 35 // [input only 35] ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MaxESP4.h b/src/pinmaps/Pins.MaxESP4.h new file mode 100755 index 0000000..90b477c --- /dev/null +++ b/src/pinmaps/Pins.MaxESP4.h @@ -0,0 +1,151 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MaxESP4 (ESP32S) +#pragma once + +#if defined(ESP32) + +// Serial0: RX Pin GPIO3, TX Pin GPIO1 (to USB serial adapter) +// Serial1: RX1 Pin GPIO10, TX1 Pin GPIO9 (on SPI Flash pins, must be moved to be used) +// Serial2: RX2 Pin GPIO16, TX2 Pin GPIO17 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 +#endif + +// Use the following settings for any TMC UART driver (TMC2209) that may be present +#if defined(STEP_DIR_TMC_UART_PRESENT) || defined(SERVO_TMC2209_PRESENT) + #define SERIAL_TMC_HARDWARE_UART + #define SERIAL_TMC Serial1 // Use a single hardware serial port to up to four drivers + #define SERIAL_TMC_BAUD 460800 // Baud rate + #define SERIAL_TMC_RX 39 // Recieving data + #ifndef SERIAL_TMC_TX + #define SERIAL_TMC_TX 4 // Transmit data + #endif + #define SERIAL_TMC_ADDRESS_MAP(x) ((x==4)?2 : x) // Axis1(0) is 0, Axis2(1) is 1, Axis3(2) is 2, Axis4(3) is 3, Axis5(4) is 2 +#endif + +// Uses default ESP32 I2C GPIO21 (SDA) and GPIO22 (SCL) + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX2_PIN 39 // M3 pin TMC UART RX +#define AUX3_PIN 14 // Home SW for Axis1 +#define AUX4_PIN 13 // Home SW for Axis2 +#define AUX5_PIN 1 // TX0 main USB, etc. +#define AUX6_PIN 3 // RX0 main USB, etc. +#define AUX7_PIN 23 // Limit SW, PPS, etc. +#define AUX8_PIN 25 // 1-Wire, Status LED, Reticle LED, Tone, etc. + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX8_PIN // Default Pin for OneWire bus +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 36 // [input only 36] PEC Sense, analog (A0) or digital (GPIO36) +#endif + +// The status LED w/ 4.7k resistor in series to limit the current to the LED +#ifdef STATUS_LED_ON_STATE + #undef STATUS_LED_ON_STATE +#endif +#define STATUS_LED_ON_STATE HIGH +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN 12 // Default LED Anode (+) +#endif +#ifdef MOUNT_LED_ON_STATE + #undef MOUNT_LED_ON_STATE +#endif +#define MOUNT_LED_ON_STATE HIGH +#define MOUNT_LED_PIN STATUS_LED_PIN // Default LED Anode (+) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN 12 // [must be low at boot 12] Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN AUX7_PIN // PPS time source, GPS for example +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN +#endif + +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#ifndef SHARED_ENABLE_PIN + #define SHARED_ENABLE_PIN 5 // Hint that the enable pins are shared +#endif + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN SHARED // Enable pin control +#define AXIS1_M0_PIN OFF // hardwired for TMC UART address 0 +#define AXIS1_M1_PIN OFF // hardwired for TMC UART address 0 +#define AXIS1_M2_PIN OFF // UART TX +#define AXIS1_M3_PIN OFF // UART RX +#define AXIS1_STEP_PIN 18 +#define AXIS1_DIR_PIN 0 // [must be high at boot 0] +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN SHARED +#define AXIS2_M0_PIN OFF // hardwired for TMC UART address 1 +#define AXIS2_M1_PIN OFF // hardwired for TMC UART address 1 +#define AXIS2_M2_PIN OFF // N/C +#define AXIS2_M3_PIN OFF // UART RX +#define AXIS2_STEP_PIN 27 +#define AXIS2_DIR_PIN 26 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif +#ifndef AXIS2_ENCODER_B_PIN + #define AXIS2_ENCODER_B_PIN AUX2_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN SHARED +#define AXIS3_M0_PIN OFF // hardwired for TMC UART address 2 +#define AXIS3_M1_PIN OFF // hardwired for TMC UART address 2 +#define AXIS3_M2_PIN OFF // N/C +#define AXIS3_M3_PIN OFF // UART RX +#define AXIS3_STEP_PIN 2 // [must be low at boot 2] +#define AXIS3_DIR_PIN 15 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN SHARED +#define AXIS4_M0_PIN OFF // hardwired for TMC UART address 3 +#define AXIS4_M1_PIN OFF // hardwired for TMC UART address 3 +#define AXIS4_M2_PIN OFF // N/C +#define AXIS4_M3_PIN OFF // UART RX +#define AXIS4_STEP_PIN 19 +#define AXIS4_DIR_PIN 15 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN SHARED +#define AXIS5_M0_PIN OFF // hardwired for TMC UART address 2 +#define AXIS5_M1_PIN OFF // hardwired for TMC UART address 2 +#define AXIS5_M2_PIN OFF // N/C +#define AXIS5_M3_PIN OFF // UART RX +#define AXIS5_STEP_PIN 2 +#define AXIS5_DIR_PIN 15 + +// ST4 interface +#define ST4_RA_W_PIN 34 // [input only 34] ST4 RA- West +#define ST4_DEC_S_PIN 32 // ST4 DE- South +#define ST4_DEC_N_PIN 33 // ST4 DE+ North +#define ST4_RA_E_PIN 35 // [input only 35] ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MaxPCB.h b/src/pinmaps/Pins.MaxPCB.h new file mode 100755 index 0000000..18735fb --- /dev/null +++ b/src/pinmaps/Pins.MaxPCB.h @@ -0,0 +1,138 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MaxPCB (Teensy3.5/3.6) +#pragma once + +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + +// Serial1: RX1 Pin 0, TX1 Pin 1 +// Serial4: RX4 Pin 31, TX4 Pin 32 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial1 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C Serial4 +#endif +#if defined(USB_DUAL_SERIAL) || defined(USB_TRIPLE_SERIAL) + #define SERIAL_D SerialUSB1 + #define SERIAL_D_BAUD_DEFAULT 9600 +#endif +#if defined(USB_TRIPLE_SERIAL) + #define SERIAL_E SerialUSB2 + #define SERIAL_E_BAUD_DEFAULT 9600 +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN 19 // Status LED +#define AUX1_PIN 18 // ESP8266 GPIO0, SPI MISO/Fault +#define AUX2_PIN 5 // ESP8266 RST, SPI MISO/Fault +#define AUX3_PIN 36 // Home SW +#define AUX4_PIN 39 // OneWire, Home SW +#define AUX5_PIN DAC_PIN(A21) // AXIS3_EN_PIN; true analog output +#define AUX6_PIN DAC_PIN(A22) // AXIS4_EN_PIN; true analog output +#define AUX7_PIN 4 // Limit SW +#define AUX8_PIN 22 // Reticle LED + +// Misc. pins +#ifndef DS3234_CS_PIN + #define DS3234_CS_PIN 10 // Default CS Pin for DS3234 on SPI +#endif +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX4_PIN // Default Pin for OneWire bus (note: this pin has a 0.1uF capacitor that must be removed for OneWire to function) +#endif +#define ADDON_GPIO0_PIN AUX1_PIN // ESP8266 GPIO0 or SPI MISO/Fault +#define ADDON_RESET_PIN AUX2_PIN // ESP8266 RST or SPI MISO/Fault + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 23 // PEC Sense, analog or digital +#endif + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#define STATUS_BUZZER_PIN 29 // Tone + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN 28 // PPS time source, GPS for example +#endif + +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN // The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#endif + +// hint that the driver mode pins are dedicated (not shared SPI bus except possibly MISO) +#define DEDICATED_MODE_PINS + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN 14 +#define AXIS1_M0_PIN 15 // SPI MOSI +#define AXIS1_M1_PIN 16 // SPI SCK +#define AXIS1_M2_PIN 17 // SPI CS (UART TX) +#define AXIS1_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS1_STEP_PIN 20 +#define AXIS1_DIR_PIN 21 +#define AXIS1_FAULT_PIN AXIS1_M3_PIN +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN 9 +#define AXIS2_M0_PIN 8 // SPI MOSI +#define AXIS2_M1_PIN 7 // SPI SCK +#define AXIS2_M2_PIN 6 // SPI CS (UART TX) +#define AXIS2_M3_PIN AUX2_PIN // SPI MISO (UART RX) +#define AXIS2_STEP_PIN 3 +#define AXIS2_DIR_PIN 2 +#define AXIS2_FAULT_PIN AXIS2_M3_PIN +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN AUX5_PIN +#define AXIS3_M0_PIN OFF // SPI MOSI +#define AXIS3_M1_PIN OFF // SPI SCK +#define AXIS3_M2_PIN OFF // SPI CS +#define AXIS3_M3_PIN OFF // SPI MISO +#define AXIS3_STEP_PIN 30 +#define AXIS3_DIR_PIN 33 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN AUX6_PIN +#define AXIS4_M0_PIN OFF // SPI MOSI +#define AXIS4_M1_PIN OFF // SPI SCK +#define AXIS4_M2_PIN OFF // SPI CS +#define AXIS4_M3_PIN OFF // SPI MISO +#define AXIS4_STEP_PIN 34 +#define AXIS4_DIR_PIN 35 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN AUX5_PIN +#define AXIS5_M0_PIN OFF // SPI MOSI +#define AXIS5_M1_PIN OFF // SPI SCK +#define AXIS5_M2_PIN OFF // SPI CS +#define AXIS5_M3_PIN OFF // SPI MISO +#define AXIS5_STEP_PIN 30 +#define AXIS5_DIR_PIN 33 + +// ST4 interface +#define ST4_RA_W_PIN 24 // ST4 RA- West +#define ST4_DEC_S_PIN 25 // ST4 DE- South +#define ST4_DEC_N_PIN 26 // ST4 DE+ North +#define ST4_RA_E_PIN 27 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MaxPCB3.h b/src/pinmaps/Pins.MaxPCB3.h new file mode 100755 index 0000000..4772285 --- /dev/null +++ b/src/pinmaps/Pins.MaxPCB3.h @@ -0,0 +1,134 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MaxPCB 3.0 and 3.5 (Teensy3.5/3.6) +#pragma once + +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + +// Serial1: RX1 Pin 0, TX1 Pin 1 +// Serial4: RX4 Pin 31, TX4 Pin 32 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial1 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C Serial4 +#endif +#if defined(USB_DUAL_SERIAL) || defined(USB_TRIPLE_SERIAL) + #define SERIAL_D SerialUSB1 + #define SERIAL_D_BAUD_DEFAULT 9600 +#endif +#if defined(USB_TRIPLE_SERIAL) + #define SERIAL_E SerialUSB2 + #define SERIAL_E_BAUD_DEFAULT 9600 +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN 19 // Status LED +#define AUX1_PIN 18 // ESP8266 GPIO0, SPI MISO/Fault +#define AUX2_PIN 5 // ESP8266 RST, SPI MISO/Fault +#define AUX3_PIN 36 // Home SW +#define AUX4_PIN 39 // OneWire, Home SW +#define AUX5_PIN DAC_PIN(A21) // AXIS3_EN_PIN; true analog output +#define AUX6_PIN DAC_PIN(A22) // AXIS4_EN_PIN; true analog output +#define AUX7_PIN 4 // Limit SW +#define AUX8_PIN 22 // Reticle LED, 1-Wire alternate + +// Misc. pins +#ifndef DS3234_CS_PIN + #define DS3234_CS_PIN 10 // Default CS Pin for DS3234 on SPI +#endif +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX4_PIN // Default Pin for OneWire bus (note: this pin has a 0.1uF capacitor that must be removed for OneWire to function) +#endif +#define ADDON_GPIO0_PIN 2 // ESP8266 GPIO0 (shared with AXIS2_DIR_PIN) +#define ADDON_RESET_PIN AUX2_PIN // ESP8266 RST or Fault + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#define PEC_SENSE_PIN 23 // PEC Sense, analog or digital + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#define STATUS_BUZZER_PIN 29 // Tone + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN 28 // PPS time source, GPS for example +#endif + +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN // The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#endif + +// hint that the driver mode pins are dedicated (not shared SPI bus except possibly MISO) +#define DEDICATED_MODE_PINS + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN 14 +#define AXIS1_M0_PIN 15 // SPI MOSI +#define AXIS1_M1_PIN 16 // SPI SCK +#define AXIS1_M2_PIN 17 // SPI CS +#define AXIS1_M3_PIN AUX1_PIN // SPI MISO +#define AXIS1_STEP_PIN 20 +#define AXIS1_DIR_PIN 21 +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN 9 +#define AXIS2_M0_PIN 15 // SPI MOSI +#define AXIS2_M1_PIN 16 // SPI SCK +#define AXIS2_M2_PIN 6 // SPI CS +#define AXIS2_M3_PIN AUX1_PIN // SPI MISO +#define AXIS2_STEP_PIN 3 +#define AXIS2_DIR_PIN 2 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN AUX5_PIN +#define AXIS3_M0_PIN 15 // SPI MOSI +#define AXIS3_M1_PIN 16 // SPI SCK +#define AXIS3_M2_PIN 7 // SPI CS +#define AXIS3_M3_PIN AUX1_PIN // SPI MISO +#define AXIS3_STEP_PIN 30 +#define AXIS3_DIR_PIN 33 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN AUX6_PIN +#define AXIS4_M0_PIN 15 // SPI MOSI +#define AXIS4_M1_PIN 16 // SPI SCK +#define AXIS4_M2_PIN 8 // SPI CS +#define AXIS4_M3_PIN AUX1_PIN // SPI MISO +#define AXIS4_STEP_PIN 34 +#define AXIS4_DIR_PIN 35 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN AUX5_PIN +#define AXIS5_M0_PIN 15 // SPI MOSI +#define AXIS5_M1_PIN 16 // SPI SCK +#define AXIS5_M2_PIN 7 // SPI CS +#define AXIS5_M3_PIN AUX1_PIN // SPI MISO +#define AXIS5_STEP_PIN 30 +#define AXIS5_DIR_PIN 33 + +// ST4 interface +#define ST4_RA_W_PIN 24 // ST4 RA- West +#define ST4_DEC_S_PIN 25 // ST4 DE- South +#define ST4_DEC_N_PIN 26 // ST4 DE+ North +#define ST4_RA_E_PIN 27 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MaxPCB36.h b/src/pinmaps/Pins.MaxPCB36.h new file mode 100755 index 0000000..1a00dd2 --- /dev/null +++ b/src/pinmaps/Pins.MaxPCB36.h @@ -0,0 +1,130 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MaxPCB 3.6 Teensy4.1 PCB +#pragma once + +#if defined(ARDUINO_TEENSY41) + +// Serial1: RX1 Pin 0, TX1 Pin 1 +// Serial2: RX2 Pin 7, TX2 Pin 8 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C Serial1 +#endif +#if defined(USB_DUAL_SERIAL) || defined(USB_TRIPLE_SERIAL) + #define SERIAL_D SerialUSB1 + #define SERIAL_D_BAUD_DEFAULT 9600 +#endif +#if defined(USB_TRIPLE_SERIAL) + #define SERIAL_E SerialUSB2 + #define SERIAL_E_BAUD_DEFAULT 9600 +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN 23 // Status LED +#define AUX1_PIN 37 // TMC SPI MISO/Fault +#define AUX2_PIN 20 // PPS +#define AUX3_PIN 21 // Home SW +#define AUX4_PIN 22 // 1-Wire, Home SW +#define AUX5_PIN 1 // TX1 +#define AUX6_PIN 0 // RX1 +#define AUX7_PIN 9 // Limit SW +#define AUX8_PIN 10 // Reticle LED, 1-Wire alternate + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX4_PIN // Default Pin for OneWire bus (note: this pin has a 0.1uF capacitor that must be removed for OneWire to function) +#endif +#define ADDON_GPIO0_PIN OFF // ESP8266 GPIO0 (shared with AXIS2_DIR_PIN) +#define ADDON_RESET_PIN OFF // ESP8266 RST + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#define PEC_SENSE_PIN 2 // PEC Sense, analog or digital + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#define STATUS_BUZZER_PIN 17 // Tone + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN AUX2_PIN // PPS time source, GPS for example (MaxSTM version 3.61 and later) +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN +#endif + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN 33 +#define AXIS1_M0_PIN 34 // SPI MOSI +#define AXIS1_M1_PIN 35 // SPI SCK +#define AXIS1_M2_PIN 36 // SPI CS (UART TX) +#define AXIS1_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS1_STEP_PIN 38 +#define AXIS1_DIR_PIN 39 +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN 40 +#define AXIS2_M0_PIN 41 // SPI MOSI +#define AXIS2_M1_PIN 13 // SPI SCK +#define AXIS2_M2_PIN 14 // SPI CS (UART TX) +#define AXIS2_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS2_STEP_PIN 15 +#define AXIS2_DIR_PIN 16 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN 11 +#define AXIS3_M0_PIN 12 // SPI MOSI +#define AXIS3_M1_PIN 24 // SPI SCK +#define AXIS3_M2_PIN 25 // SPI CS (UART TX) +#define AXIS3_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS3_STEP_PIN 26 +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#define AXIS3_DIR_PIN 27 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN 28 +#define AXIS4_M0_PIN 29 // SPI MOSI +#define AXIS4_M1_PIN 30 // SPI SCK +#define AXIS4_M2_PIN 31 // SPI CS (UART TX) +#define AXIS4_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS4_STEP_PIN 32 +#define AXIS4_DIR_PIN 27 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN 11 +#define AXIS5_M0_PIN 12 // SPI MOSI +#define AXIS5_M1_PIN 24 // SPI SCK +#define AXIS5_M2_PIN 25 // SPI CS (UART TX) +#define AXIS5_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS5_STEP_PIN 26 +#define AXIS5_DIR_PIN 27 + +// ST4 interface +#define ST4_RA_W_PIN 3 // ST4 RA- West +#define ST4_DEC_S_PIN 4 // ST4 DE- South +#define ST4_DEC_N_PIN 5 // ST4 DE+ North +#define ST4_RA_E_PIN 6 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MaxPCB4.h b/src/pinmaps/Pins.MaxPCB4.h new file mode 100755 index 0000000..1d54483 --- /dev/null +++ b/src/pinmaps/Pins.MaxPCB4.h @@ -0,0 +1,145 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MaxPCB4 Teensy4.1 PCB +#pragma once + +#if defined(ARDUINO_TEENSY41) + +// Serial1: RX1 Pin 0, TX1 Pin 1 +// Serial2: RX2 Pin 7, TX2 Pin 8 +// Serial8: RX8 Pin 34, TX8 Pin 35 (TMC2209 UART) + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C Serial1 +#endif +#if defined(USB_DUAL_SERIAL) || defined(USB_TRIPLE_SERIAL) + #define SERIAL_D SerialUSB1 + #define SERIAL_D_BAUD_DEFAULT 9600 +#endif +#if defined(USB_TRIPLE_SERIAL) + #define SERIAL_E SerialUSB2 + #define SERIAL_E_BAUD_DEFAULT 9600 +#endif + +// Use the following settings for the 4x TMC UART drivers +#if defined(STEP_DIR_TMC_UART_PRESENT) || defined(SERVO_TMC2209_PRESENT) + #define SERIAL_TMC_HARDWARE_UART + #define DRIVER_TMC_STEPPER + #define SERIAL_TMC Serial8 // Use a single hardware serial port to up to four drivers + #define SERIAL_TMC_BAUD 460800 // Baud rate + #define SERIAL_TMC_ADDRESS_MAP(x) ((x==4)?2 : x) // Axis1(0) is 0, Axis2(1) is 1, Axis3(2) is 2, Axis4(3) is 3, Axis5(4) is 2 +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN 23 // Status LED +#define AUX1_PIN 34 // TMC SPI MISO or UART RX (Serial8) +#define AUX2_PIN 20 // PPS +#define AUX3_PIN 21 // Home SW +#define AUX4_PIN 22 // Home SW +#define AUX5_PIN 1 // 1-Wire, TX1 +#define AUX6_PIN 0 // RX1 +#define AUX7_PIN 9 // Limit SW +#define AUX8_PIN 10 // Reticle LED, 1-Wire alternate + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX5_PIN // Default Pin for 1-Wire bus +#endif +#define ADDON_GPIO0_PIN OFF // ESP8266 GPIO0 (shared with AXIS2_DIR_PIN) +#define ADDON_RESET_PIN OFF // ESP8266 RST + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#define PEC_SENSE_PIN 2 // PEC Sense, analog or digital + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN 17 // Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN AUX2_PIN // PPS time source, GPS for example (MaxSTM version 3.61 and later) +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN +#endif + +// hint that the driver mode pins are dedicated (not shared SPI bus except possibly MISO) +#define DEDICATED_MODE_PINS + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN 33 +#define AXIS1_M0_PIN 37 // SPI MOSI +#define AXIS1_M1_PIN 36 // SPI SCK +#define AXIS1_M2_PIN 35 // SPI CS (UART TX) +#define AXIS1_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS1_STEP_PIN 38 +#define AXIS1_DIR_PIN 39 +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN 40 +#define AXIS2_M0_PIN 41 // SPI MOSI +#define AXIS2_M1_PIN 13 // SPI SCK +#define AXIS2_M2_PIN 14 // SPI CS (UART TX) +#define AXIS2_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS2_STEP_PIN 15 +#define AXIS2_DIR_PIN 16 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN 11 +#define AXIS3_M0_PIN 12 // SPI MOSI +#define AXIS3_M1_PIN 24 // SPI SCK +#define AXIS3_M2_PIN 25 // SPI CS (UART TX) +#define AXIS3_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS3_STEP_PIN 26 +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#define AXIS3_DIR_PIN 27 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN 28 +#define AXIS4_M0_PIN 29 // SPI MOSI +#define AXIS4_M1_PIN 30 // SPI SCK +#define AXIS4_M2_PIN 31 // SPI CS (UART TX) +#define AXIS4_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS4_STEP_PIN 32 +#define AXIS4_DIR_PIN 27 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN 11 +#define AXIS5_M0_PIN 12 // SPI MOSI +#define AXIS5_M1_PIN 24 // SPI SCK +#define AXIS5_M2_PIN 25 // SPI CS (UART TX) +#define AXIS5_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS5_STEP_PIN 26 +#define AXIS5_DIR_PIN 27 + +// ST4 interface +#define ST4_RA_W_PIN 3 // ST4 RA- West +#define ST4_DEC_S_PIN 4 // ST4 DE- South +#define ST4_DEC_N_PIN 5 // ST4 DE+ North +#define ST4_RA_E_PIN 6 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MaxSTM.h b/src/pinmaps/Pins.MaxSTM.h new file mode 100755 index 0000000..8a08ea5 --- /dev/null +++ b/src/pinmaps/Pins.MaxSTM.h @@ -0,0 +1,148 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MaxSTM STM32F411CE/STM32F401CE PCB (or the Blackpill https://github.com/WeActTC/MiniF4-STM32F4x1) +#pragma once + +#if defined(STM32F411xE) || defined(STM32F401xC) + +// For an 8KB EEPROM on the MaxSTM3.6I +#if PINMAP == MaxSTM3I + #define NV_M24C32 +#endif + +// Serial ports (see Pins.defaults.h for SERIAL_A) +// Serial1 RX1 Pin PA10, TX1 Pin PA9 +// Serial2 RX2 Pin PA3 , TX2 Pin PA2 +// Serial6 RX6 Pin PA12, TX6 Pin PA11 (used for USB virtual serial) + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B HardSerial + #define SERIAL_B_RX PA3 + #define SERIAL_B_TX PA2 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C HardSerial + #define SERIAL_C_RX PA10 + #define SERIAL_C_TX PA9 +#endif + +// Auto assign the Serial1 port pins for GPS +#if SERIAL_GPS_BAUD != OFF + #ifndef SERIAL_GPS + #define SERIAL_GPS HardSerial + #define SERIAL_GPS_RX PA10 + #define SERIAL_GPS_TX PA9 + #endif +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN PB12 // Status LED +#define AUX1_PIN PA6 // TMC SPI MISO/Fault +#define AUX2_PIN PA13 // PPS +#define AUX3_PIN PB13 // Home SW +#define AUX4_PIN PB14 // 1-Wire, Home SW +#define AUX5_PIN PA9 // TX1 +#define AUX6_PIN PA10 // RX1 +#define AUX7_PIN PB15 // Limit SW +#define AUX8_PIN PA8 // Reticle LED, 1-Wire alternate + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX4_PIN // Default Pin for OneWire bus (note: this pin has a 0.1uF capacitor that must be removed for OneWire to function) +#endif +#define ADDON_GPIO0_PIN OFF // ESP8266 GPIO0 (shared with AXIS2_DIR_PIN) +#define ADDON_RESET_PIN OFF // ESP8266 RST + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN PB1 // PEC Sense, analog or digital +#endif + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#define STATUS_BUZZER_PIN PA14 // Tone + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #ifdef MAXSTM_AUX0_PPS + #define PPS_SENSE_PIN AUX0_PIN // PPS time source, GPS for example (MaxSTM version 3.6) + #else + #define PPS_SENSE_PIN AUX2_PIN // PPS time source, GPS for example (MaxSTM version 3.61 and later) + #endif +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN +#endif + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN OFF +#define AXIS1_M0_PIN PA7 // SPI MOSI +#define AXIS1_M1_PIN PA5 // SPI SCK +#define AXIS1_M2_PIN PA1 // SPI CS (UART TX) +#define AXIS1_M3_PIN PA6 // SPI MISO (UART RX) +#define AXIS1_STEP_PIN PB10 +#define AXIS1_DIR_PIN PB2 +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN OFF +#define AXIS2_M0_PIN PA7 // SPI MOSI +#define AXIS2_M1_PIN PA5 // SPI SCK +#define AXIS2_M2_PIN PA0 // SPI CS (UART TX) +#define AXIS2_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS2_STEP_PIN PA4 +#define AXIS2_DIR_PIN PB0 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#define AXIS3_ENABLE_PIN OFF +#define AXIS3_M0_PIN PA7 // SPI MOSI +#define AXIS3_M1_PIN PA5 // SPI SCK +#define AXIS3_M2_PIN PC15 // SPI CS (UART TX) +#define AXIS3_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS3_STEP_PIN PB8 +#define AXIS3_DIR_PIN PC13 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN OFF +#define AXIS4_M0_PIN PA7 // SPI MOSI +#define AXIS4_M1_PIN PA5 // SPI SCK +#define AXIS4_M2_PIN PC14 // SPI CS (UART TX) +#define AXIS4_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS4_STEP_PIN PB9 +#define AXIS4_DIR_PIN PC13 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN OFF +#define AXIS5_M0_PIN PA7 // SPI MOSI +#define AXIS5_M1_PIN PA5 // SPI SCK +#define AXIS5_M2_PIN PC15 // SPI CS (UART TX) +#define AXIS5_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS5_STEP_PIN PB8 +#define AXIS5_DIR_PIN PC13 + +// ST4 interface +#define ST4_RA_W_PIN PA15 // ST4 RA- West +#define ST4_DEC_S_PIN PB3 // ST4 DE- South +#define ST4_DEC_N_PIN PB4 // ST4 DE+ North +#define ST4_RA_E_PIN PB5 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MicroScope.h b/src/pinmaps/Pins.MicroScope.h new file mode 100755 index 0000000..1f71e74 --- /dev/null +++ b/src/pinmaps/Pins.MicroScope.h @@ -0,0 +1,119 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for Microscope PCB Version 0.2 (ESP32S, experimental and may be removed at any point!) + +// !!! USE AT YOUR OWN RISK !!! + +#pragma once + +#if defined(ESP32) + +// Serial0: RX Pin GPIO3, TX Pin GPIO1 (to USB serial adapter) +// Serial1: RX1 Pin GPIO10, TX1 Pin GPIO9 (on SPI Flash pins, must be moved to be used) +// Serial2: RX2 Pin GPIO16, TX2 Pin GPIO17 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 +#endif + +// Uses default ESP32 I2C GPIO21 (SDA) and GPIO22 (SCL) + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN 26 // Default Pin for OneWire bus +#endif +#define ADDON_GPIO0_PIN OFF // 26 ESP8266 GPIO0 (Dir2) +#ifndef ADDON_RESET_PIN + #define ADDON_RESET_PIN OFF // 4 ESP8266 RST +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 36 // [input only 36] PEC Sense, analog (A0) or digital (GPIO36) +#endif + +// The status LED is a two wire jumper with a 2k resistor in series to limit the current to the LED +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN 25 // 25 Default LED Cathode (-) +#endif +#define MOUNT_LED_PIN 25 // 25 Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN 25 // 25 Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN OFF // 25 Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN 39 // 37 PPS time source, GPS for example +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN OFF // 37 +#endif + +#define SHARED_ENABLE_PIN GPIO_PIN(13) // Hint that the enable pins are shared + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN SHARED // [must be low at boot 12] +#define AXIS1_M0_PIN 23 // 13 SPI MOSI +#define AXIS1_M1_PIN 18 // 14 SPI SCK +#define AXIS1_M2_PIN 25 // 23 SPI CS (UART TX) +#define AXIS1_M3_PIN 19 // 4 SPI MISO (UART RX) +#define AXIS1_STEP_PIN 2 // 18 +#define AXIS1_DIR_PIN GPIO_PIN(15) // [must be high at boot 0] +#define AXIS1_SENSE_HOME_PIN GPIO_PIN(1) // 21 + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN SHARED +#define AXIS2_M0_PIN 23 // 13 SPI MOSI +#define AXIS2_M1_PIN 18 // 14 SPI SCK +#define AXIS2_M2_PIN 14 // 5 SPI CS (UART TX) +#define AXIS2_M3_PIN 19 // 4 SPI MISO (UART RX) +#define AXIS2_STEP_PIN 12 // 27 +#define AXIS2_DIR_PIN GPIO_PIN(16) // 26 +#define AXIS2_SENSE_HOME_PIN GPIO_PIN(0) // 22 + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN SHARED // No enable pin control (always enabled) +#define AXIS3_M0_PIN 23 // SPI MOSI +#define AXIS3_M1_PIN 18 // SPI SCK +#define AXIS3_M2_PIN 4 // SPI CS (UART TX) +#define AXIS3_M3_PIN 19 // SPI MISO (UART RX) +#define AXIS3_STEP_PIN 15 // [must be low at boot 2] +#define AXIS3_DIR_PIN GPIO_PIN(17) // 15 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN SHARED // 4s +#define AXIS4_M0_PIN 23 // SPI MOSI +#define AXIS4_M1_PIN 18 // SPI SCK +#define AXIS4_M2_PIN OFF // SPI CS (UART TX) +#define AXIS4_M3_PIN 19 // SPI MISO (UART RX) +#define AXIS4_STEP_PIN 5 // 19 +#define AXIS4_DIR_PIN GPIO_PIN(14) // 15 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN SHARED // No enable pin control (always enabled) +#define AXIS5_M0_PIN 23 // SPI MOSI +#define AXIS5_M1_PIN 18 // SPI SCK +#define AXIS5_M2_PIN 4 // SPI CS (UART TX) +#define AXIS5_M3_PIN 19 // SPI MISO (UART RX) +#define AXIS5_STEP_PIN 15 // 2 +#define AXIS5_DIR_PIN GPIO_PIN(17) // 15 + +// ST4 interface +#define ST4_RA_W_PIN 33 // [input only 34] ST4 RA- West +#define ST4_DEC_S_PIN 32 // [i/o 32] ST4 DE- South +#define ST4_DEC_N_PIN 35 // [i/o 33] ST4 DE+ North +#define ST4_RA_E_PIN 34 // [input only 35] ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.MiniPCB.h b/src/pinmaps/Pins.MiniPCB.h new file mode 100755 index 0000000..a573994 --- /dev/null +++ b/src/pinmaps/Pins.MiniPCB.h @@ -0,0 +1,143 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep MiniPCB (Teensy3.0, 3.1, 3.2, and 4.0) +#pragma once + +#if defined(__MK20DX256__) || defined(_mk20dx128_h_) || defined(__MK20DX128__) || defined(__IMXRT1052__) || defined(__IMXRT1062__) + +// Serial ports (see Pins.defaults.h for SERIAL_A) +// Serial1: RX1 Pin 0, TX1 Pin 1 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial1 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #error "Configuration (Config.h): SerialC isn't supported, disable this option." +#endif +#if defined(USB_DUAL_SERIAL) || defined(USB_TRIPLE_SERIAL) + #define SERIAL_D SerialUSB1 + #define SERIAL_D_BAUD_DEFAULT 9600 +#endif +#if defined(USB_TRIPLE_SERIAL) + #define SERIAL_E SerialUSB2 + #define SERIAL_E_BAUD_DEFAULT 9600 +#endif + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN 19 // Status LED +#define AUX1_PIN 18 // ESP8266 GPIO0, SPI MISO/Fault +#define AUX2_PIN 5 // ESP8266 RST, SPI MISO/Fault +#define AUX3_PIN 4 // Limit SW, Home SW +#define AUX4_PIN 22 // Reticle LED, Home SW +#if !defined(_mk20dx128_h_) && !defined(__MK20DX128__) && !defined(__IMXRT1052__) && !defined(__IMXRT1062__) + #define AUX5_PIN DAC_PIN(A14) // true analog output +#endif + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN 24 // Default Pin for OneWire bus +#endif +#if PINMAP == MiniPCB13 + #define ADDON_GPIO0_PIN 2 // ESP8266 GPIO0 (Dir2) + #define ADDON_RESET_PIN AUX2_PIN // ESP8266 RST +#else + #define ADDON_GPIO0_PIN AUX1_PIN // ESP8266 GPIO0 or SPI MISO/Fault + #define ADDON_RESET_PIN AUX2_PIN // ESP8266 RST or SPI MISO/Fault +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#define PEC_SENSE_PIN 23 // PEC Sense, analog or digital + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX4_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN 29 // Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN 28 // PPS time source, GPS for example +#endif + +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX3_PIN // The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#endif + +// hint that the driver mode pins are dedicated (not shared SPI bus except possibly MISO) +#define DEDICATED_MODE_PINS + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN 14 +#define AXIS1_M0_PIN 15 // SPI MOSI +#define AXIS1_M1_PIN 16 // SPI SCK +#define AXIS1_M2_PIN 17 // SPI CS (UART TX) +#define AXIS1_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS1_STEP_PIN 20 +#define AXIS1_DIR_PIN 21 +#define AXIS1_FAULT_PIN AXIS1_M3_PIN // SPI MISO +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN 9 +#define AXIS2_M0_PIN 8 // SPI MOSI +#define AXIS2_M1_PIN 7 // SPI SCK +#define AXIS2_M2_PIN 6 // SPI CS (UART TX) +#if PINMAP == MiniPCB13 + #define AXIS2_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#else + #define AXIS2_M3_PIN AUX2_PIN // SPI MISO (UART RX) +#endif +#define AXIS2_STEP_PIN 3 +#define AXIS2_DIR_PIN 2 +#define AXIS2_FAULT_PIN AXIS2_M3_PIN +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN OFF +#define AXIS3_M0_PIN OFF // SPI MOSI +#define AXIS3_M1_PIN OFF // SPI SCK +#define AXIS3_M2_PIN OFF // SPI CS (UART TX) +#define AXIS3_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS3_STEP_PIN 30 +#define AXIS3_DIR_PIN 33 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN OFF +#define AXIS4_M0_PIN OFF // SPI MOSI +#define AXIS4_M1_PIN OFF // SPI SCK +#define AXIS4_M2_PIN OFF // SPI CS (UART TX) +#define AXIS4_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS4_STEP_PIN 31 +#define AXIS4_DIR_PIN 32 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN OFF +#define AXIS5_M0_PIN OFF // SPI MOSI +#define AXIS5_M1_PIN OFF // SPI SCK +#define AXIS5_M2_PIN OFF // SPI CS (UART TX) +#define AXIS5_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS5_STEP_PIN 30 +#define AXIS5_DIR_PIN 33 + +// ST4 interface +#define ST4_RA_W_PIN 10 // ST4 RA- West +#define ST4_DEC_S_PIN 11 // ST4 DE- South +#define ST4_DEC_N_PIN 12 // ST4 DE+ North +#define ST4_RA_E_PIN 13 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.OctopusPro.h b/src/pinmaps/Pins.OctopusPro.h new file mode 100755 index 0000000..4d9283d --- /dev/null +++ b/src/pinmaps/Pins.OctopusPro.h @@ -0,0 +1,182 @@ +// ------------------------------------------------------------------------------------------------- +// Pinmap for the BTT Octopus Pro Version 1.1 (STM32H723) +#pragma once + +#if defined(STM32H723xx) + +// Serial: Built in USB (not TTL) +// Serial2: RX2 Pin PD6, TX2 Pin PD5 +// Serial3: RX3 Pin PD9, TX3 Pin PD8 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B HardSerial + #define SERIAL_B_RX PD9 + #define SERIAL_B_TX PD8 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C HardSerial + #define SERIAL_C_RX PD6 + #define SERIAL_C_TX PD5 +#endif + +// Connector Labeled I2C uses PB8 (SCL) and PB9 (SDA) which are forced in the HAL +#define I2C_SCL_PIN PB8 +#define I2C_SDA_PIN PB9 + +// Use software serial for GPS + +// The multi-purpose pins (automatically associated with Auxiliary Features) +#define AUX1_PIN PA0 // Connector Labeled HE0 +#define AUX2_PIN PA3 // Connector Labeled HE1 +#define AUX3_PIN PB0 // Connector Labeled HE2 +#define AUX4_PIN PB11 // Connector Labeled HE3 +#define AUX5_PIN PA1 // Connector Labeled BED-OUT +#define AUX6_PIN PA8 // Connector Labeled FAN0 +#define AUX7_PIN PE5 // Connector Labeled FAN1 +#define AUX8_PIN PD12 // Connector Labeled FAN2 + +// Thermistor (temperature) sensor inputs have built-in 4.7K Ohm pullups and a 10uF cap for noise supression +#ifndef FEATURE1_TEMPERATURE_PIN + #define FEATURE1_TEMPERATURE_PIN PF4 // Connector Labeled T0 +#endif +#ifndef FEATURE2_TEMPERATURE_PIN + #define FEATURE2_TEMPERATURE_PIN PF5 // Connector Labeled T1 +#endif +#ifndef FEATURE3_TEMPERATURE_PIN + #define FEATURE3_TEMPERATURE_PIN PF6 // Connector Labeled T2 +#endif +#ifndef FEATURE4_TEMPERATURE_PIN + #define FEATURE4_TEMPERATURE_PIN PF7 // Connector Labeled T3 +#endif +#ifndef FEATURE5_TEMPERATURE_PIN + #define FEATURE5_TEMPERATURE_PIN PF3 // Connector Labeled THB +#endif +#ifndef FOCUSER_TEMPERATURE_PIN + #define FOCUSER_TEMPERATURE_PIN PF7 // Connector Labeled T3 +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN PB7 // Connector labeled Probe (Analog or Digital) +#endif + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN PD13 // Connector Labeled FAN3 +#endif +#ifndef MOUNT_LED_PIN + #define MOUNT_LED_PIN PD14 // Connector Labeled FAN4 +#endif +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN PB2 // Connector Labeled EXP2 +#endif + +#ifdef STATUS_LED_ON_STATE +#undef STATUS_LED_ON_STATE +#endif +#define STATUS_LED_ON_STATE HIGH +#ifdef MOUNT_LED_ON_STATE +#undef MOUNT_LED_ON_STATE +#endif +#define MOUNT_LED_ON_STATE HIGH + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN PD15 // Connector Labeled FAN5 +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN PC15 // Connector Labeled EXP2 +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN PG6 // Limit Connector +#endif + +// Axis1 RA/Azm step/dir driver (Labeled S1) +#define AXIS1_ENABLE_PIN PF14 +#define AXIS1_M0_PIN PA7 // SPI MOSI +#define AXIS1_M1_PIN PA5 // SPI SCK +#define AXIS1_M2_PIN PC4 // SPI CS +#define AXIS1_M3_PIN PA6 // SPI MISO +#define AXIS1_STEP_PIN PF13 +#define AXIS1_DIR_PIN PF12 + +// Axis2 RA/Azm step/dir driver (Labeled S2) +#define AXIS2_ENABLE_PIN PF15 +#define AXIS2_M0_PIN PA7 // SPI MOSI +#define AXIS2_M1_PIN PA5 // SPI SCK +#define AXIS2_M2_PIN PD11 // SPI CS +#define AXIS2_M3_PIN PA6 // SPI MISO +#define AXIS2_STEP_PIN PG0 +#define AXIS2_DIR_PIN PG1 + +// Axis3 RA/Azm step/dir driver (Labeled S3) +#define AXIS3_ENABLE_PIN PG5 +#define AXIS3_M0_PIN PA7 // SPI MOSI +#define AXIS3_M1_PIN PA5 // SPI SCK +#define AXIS3_M2_PIN PC6 // SPI CS +#define AXIS3_M3_PIN PA6 // SPI MISO +#define AXIS3_STEP_PIN PF11 +#define AXIS3_DIR_PIN PG3 + +// Axis4 RA/Azm step/dir driver (Labeled S4) +#define AXIS4_ENABLE_PIN PA2 +#define AXIS4_M0_PIN PA7 // SPI MOSI +#define AXIS4_M1_PIN PA5 // SPI SCK +#define AXIS4_M2_PIN PC7 // SPI CS +#define AXIS4_M3_PIN PA6 // SPI MISO +#define AXIS4_STEP_PIN PG4 +#define AXIS4_DIR_PIN PC1 + +// Axis5 RA/Azm step/dir driver (Labeled S5) +#define AXIS5_ENABLE_PIN PG2 +#define AXIS5_M0_PIN PA7 // SPI MOSI +#define AXIS5_M1_PIN PA5 // SPI SCK +#define AXIS5_M2_PIN PF2 // SPI CS +#define AXIS5_M3_PIN PA6 // SPI MISO +#define AXIS5_STEP_PIN PF9 +#define AXIS5_DIR_PIN PF10 + +// Axis6 RA/Azm step/dir driver (Labeled S6) +#define AXIS6_ENABLE_PIN PF1 +#define AXIS6_M0_PIN PA7 // SPI MOSI +#define AXIS6_M1_PIN PA5 // SPI SCK +#define AXIS6_M2_PIN PE4 // SPI CS +#define AXIS6_M3_PIN PA6 // SPI MISO +#define AXIS6_STEP_PIN PC13 +#define AXIS6_DIR_PIN PF0 + +// Axis7 RA/Azm step/dir driver (Labeled S7) +#define AXIS7_ENABLE_PIN PD4 +#define AXIS7_M0_PIN PA7 // SPI MOSI +#define AXIS7_M1_PIN PC6 // SPI SCK +#define AXIS7_M2_PIN PE1 // SPI CS +#define AXIS7_M3_PIN PA6 // SPI MISO +#define AXIS7_STEP_PIN PE2 +#define AXIS7_DIR_PIN PE3 + +// Axis8 RA/Azm step/dir driver (Labeled S8) +#define AXIS8_ENABLE_PIN PE0 +#define AXIS8_M0_PIN PA7 // SPI MOSI +#define AXIS8_M1_PIN PA5 // SPI SCK +#define AXIS8_M2_PIN PD3 // SPI CS +#define AXIS8_M3_PIN PA6 // SPI MISO +#define AXIS8_STEP_PIN PE6 +#define AXIS8_DIR_PIN PA14 + +// ST4 interface (Connector Labeled EXP1 which has Gnd and +5V so near ideal) +#define ST4_RA_W_PIN PE14 // ST4 RA- West +#define ST4_DEC_S_PIN PE15 // ST4 DE- South +#define ST4_DEC_N_PIN PE12 // ST4 DE+ North +#define ST4_RA_E_PIN PE13 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.SAL_XB1.h b/src/pinmaps/Pins.SAL_XB1.h new file mode 100755 index 0000000..4b2e129 --- /dev/null +++ b/src/pinmaps/Pins.SAL_XB1.h @@ -0,0 +1,149 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for SAL_XB1 (ESP32S) +#pragma once + +#if defined(ESP32) + +// Serial0: RX Pin GPIO3, TX Pin GPIO1 (to USB serial adapter) +// Serial1: RX1 Pin GPIO10, TX1 Pin GPIO9 (on SPI Flash pins, must be moved to be used) +// Serial2: RX2 Pin GPIO16, TX2 Pin GPIO17 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B Serial2 +#endif + +// Use the following settings for any TMC UART driver (TMC2209) that may be present +#if defined(STEP_DIR_TMC_UART_PRESENT) || defined(SERVO_TMC2209_PRESENT) + #define SERIAL_TMC_HARDWARE_UART + #define SERIAL_TMC Serial1 // Use a single hardware serial port to up to four drivers + #define SERIAL_TMC_BAUD 460800 // Baud rate + #define SERIAL_TMC_RX 39 // Recieving data + #define SERIAL_TMC_TX 4 // Transmit data + #define SERIAL_TMC_ADDRESS_MAP(x) ((x==4)?2 : x) // Axis1(0) is 0, Axis2(1) is 1, Axis3(2) is 2, Axis4(3) is 3, Axis5(4) is 2 +#endif + +// Specify the ESP32 I2C pins +#define I2C_SDA_PIN 21 +#define I2C_SCL_PIN 22 + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX2_PIN 39 // M3 pin TMC UART RX +#define AUX3_PIN 14 // Home SW for Axis1 +#define AUX4_PIN 13 // Home SW for Axis2 +#define AUX5_PIN 1 // TX0 main USB, etc. +#define AUX6_PIN 3 // RX0 main USB, etc. +#define AUX7_PIN 23 // Limit SW, PPS, etc. +#define AUX8_PIN 25 // 1-Wire, Status LED, Reticle LED, Tone, etc. + +// Misc. pins +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX8_PIN // Default Pin for OneWire bus +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN 36 // [input only 36] PEC Sense, analog (A0) or digital (GPIO36) +#endif + +// The status LED w/ 4.7k resistor in series to limit the current to the LED +#ifdef STATUS_LED_ON_STATE + #undef STATUS_LED_ON_STATE +#endif +#define STATUS_LED_ON_STATE HIGH +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN 12 // Default LED Anode (+) +#endif +#ifdef MOUNT_LED_ON_STATE + #undef MOUNT_LED_ON_STATE +#endif +#define MOUNT_LED_ON_STATE HIGH +#define MOUNT_LED_PIN STATUS_LED_PIN // Default LED Anode (+) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX8_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN 12 // [must be low at boot 12] Tone +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN AUX7_PIN // PPS time source, GPS for example +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN AUX7_PIN +#endif + +#define SHARED_DIRECTION_PINS // Hint that the direction pins are shared +#define SHARED_ENABLE_PIN 5 // Hint that the enable pins are shared + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN SHARED // Enable pin control +#define AXIS1_M0_PIN OFF // hardwired for TMC UART address 0 +#define AXIS1_M1_PIN OFF // hardwired for TMC UART address 0 +#define AXIS1_M2_PIN OFF // UART TX +#define AXIS1_M3_PIN OFF // UART RX +#define AXIS1_STEP_PIN 18 +#define AXIS1_DIR_PIN 19 // +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN SHARED +#define AXIS2_M0_PIN OFF // hardwired for TMC UART address 1 +#define AXIS2_M1_PIN OFF // hardwired for TMC UART address 1 +#define AXIS2_M2_PIN OFF // N/C +#define AXIS2_M3_PIN OFF // UART RX +#define AXIS2_STEP_PIN 27 +#define AXIS2_DIR_PIN 26 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif +#ifndef AXIS2_ENCODER_B_PIN + #define AXIS2_ENCODER_B_PIN AUX2_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN SHARED +#define AXIS3_M0_PIN OFF // hardwired for TMC UART address 2 +#define AXIS3_M1_PIN OFF // hardwired for TMC UART address 2 +#define AXIS3_M2_PIN OFF // N/C +#define AXIS3_M3_PIN OFF // UART RX +#define AXIS3_STEP_PIN 2 // [must be low at boot 2] +#define AXIS3_DIR_PIN 15 + +// For focuser1 stepper driver +#define AXIS4_ENABLE_PIN SHARED +#define AXIS4_M0_PIN OFF // hardwired for TMC UART address 3 +#define AXIS4_M1_PIN OFF // hardwired for TMC UART address 3 +#define AXIS4_M2_PIN OFF // N/C +#define AXIS4_M3_PIN OFF // UART RX +#define AXIS4_STEP_PIN 19 +#define AXIS4_DIR_PIN 15 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN SHARED +#define AXIS5_M0_PIN OFF // hardwired for TMC UART address 2 +#define AXIS5_M1_PIN OFF // hardwired for TMC UART address 2 +#define AXIS5_M2_PIN OFF // N/C +#define AXIS5_M3_PIN OFF // UART RX +#define AXIS5_STEP_PIN 2 +#define AXIS5_DIR_PIN 15 + +// ST4 interface +#define ST4_RA_W_PIN 34 // [input only 34] ST4 RA- West +#define ST4_DEC_S_PIN 32 // ST4 DE- South +#define ST4_DEC_N_PIN 33 // ST4 DE+ North +#define ST4_RA_E_PIN 35 // [input only 35] ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif \ No newline at end of file diff --git a/src/pinmaps/Pins.SKR_PRO.h b/src/pinmaps/Pins.SKR_PRO.h new file mode 100755 index 0000000..4e68ee4 --- /dev/null +++ b/src/pinmaps/Pins.SKR_PRO.h @@ -0,0 +1,234 @@ +// ------------------------------------------------------------------------------------------------- +// Pinmap for the BTT SKR PRO Version 1.2 (STM32F407) +#pragma once + +#if defined(STM32F407xx) + +// Serial: Built in USB (not TTL) +// Serial1: RX1 Pin PA10, TX1 Pin PA9 (Connector Labeled TFT) +// Serial3: RX3 Pin PD9, TX3 Pin PD8 (Connector Labeled SERIAL3) +// Serial6: RX6 Pin PC7, TX6 Pin PC6 (Connector Labeled ESP-01) + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B HardSerial + #define SERIAL_B_RX PC7 + #define SERIAL_B_TX PC6 +#endif +#if SERIAL_C_BAUD_DEFAULT != OFF + #define SERIAL_C HardSerial + #define SERIAL_C_RX PA10 + #define SERIAL_C_TX PA9 + // #define SERIAL_C Serial1 +#endif + +// Connector Labeled I2C uses PB6 (SCL) and PB7 (SDA) which are forced in the HAL + +// Auto assign the Serial3 port pins for GPS +#if SERIAL_GPS_BAUD != OFF + #ifndef SERIAL_GPS + #define SERIAL_GPS HardSerial + #define SERIAL_GPS_RX PD9 + #define SERIAL_GPS_TX PD8 + #endif +#endif + +// allow RX from TMC UART drivers +#ifndef SERIAL_TMC_RX_DISABLE + #define SERIAL_TMC_RX_DISABLE false +#endif + +// Allow resetting ESP8266 into firmware upload mode +#ifndef ADDON_GPIO0_PIN + #define ADDON_GPIO0_PIN PF14 // ESP8266 GPIO0 +#endif +#ifndef ADDON_RESET_PIN + #define ADDON_RESET_PIN PG1 // ESP8266 RST +#endif + +#define HEATER0_PIN PB1 // Connector Labeled HEAT0 +#define HEATER1_PIN PD14 // Connector Labeled HEAT1 +#define HEATER2_PIN PB0 // Connector Labeled HEAT2 +#define HEATER3_PIN PD12 // Connector Labeled BED +#define HEATER4_PIN PC8 // Connector Labeled FAN0 + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX1_PIN HEATER0_PIN +#define AUX2_PIN HEATER1_PIN +#define AUX3_PIN HEATER2_PIN +#define AUX4_PIN HEATER3_PIN +#define AUX5_PIN HEATER4_PIN + +// Thermistor (temperature) sensor inputs have built-in 4.7K Ohm pullups and a 10uF cap for noise supression +#define TEMP0_PIN PF3 // Connector Labeled T0 +#define TEMP1_PIN PF4 // Connector Labeled T1 +#define TEMP2_PIN PF5 // Connector Labeled T2 +#define TEMP3_PIN PF6 // Connector Labeled T3 + +#ifndef FEATURE1_TEMPERATURE_PIN + #define FEATURE1_TEMPERATURE_PIN TEMP0_PIN +#endif +#ifndef FEATURE2_TEMPERATURE_PIN + #define FEATURE2_TEMPERATURE_PIN TEMP1_PIN +#endif +#ifndef FEATURE3_TEMPERATURE_PIN + #define FEATURE3_TEMPERATURE_PIN TEMP2_PIN +#endif +#ifndef FOCUSER_TEMPERATURE_PIN + #define FOCUSER_TEMPERATURE_PIN TEMP3_PIN +#endif + +// The PEC index sense is a logic level input, resets the PEC index on rising edge then waits for 60 seconds before allowing another reset +#ifndef PEC_SENSE_PIN + #define PEC_SENSE_PIN PF12 // Connector labeled EXP2 (Analog or Digital) +#endif + +// The status LED is a two wire jumper with a 10k resistor in series to limit the current to the LED +#ifndef STATUS_LED_PIN + #define STATUS_LED_PIN PE6 // Connector Labeled FAN2 (has an LED built in too) +#endif +#define MOUNT_LED_PIN PE6 // Connector Labeled FAN2 (shared) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN PE5 // Connector Labeled FAN1 (has an LED built in too) TIM9 +#endif + +#ifdef STATUS_LED_ON_STATE +#undef STATUS_LED_ON_STATE +#endif +#define STATUS_LED_ON_STATE HIGH +#ifdef MOUNT_LED_ON_STATE +#undef MOUNT_LED_ON_STATE +#endif +#define MOUNT_LED_ON_STATE HIGH + +// For a piezo buzzer +#ifndef STATUS_BUZZER_PIN + #define STATUS_BUZZER_PIN PF11 // Connector Labeled EXP2 +#endif + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN PF13 // Connector Labeled EXP2 +#endif + +// The limit switch sense is a logic level input normally pull high (2k resistor,) shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN PB10 // Limit Connector Labeled E0 +#endif + +// Axis1 RA/Azm step/dir driver (Labeled X) +#define AXIS1_ENABLE_PIN PF2 +#define AXIS1_M0_PIN PC12 // SPI MOSI +#define AXIS1_M1_PIN PC10 // SPI SCK +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC_HARDWARE_UART) + #define AXIS1_M2_PIN PE4 // TX + #define AXIS1_M3_PIN PC13 // RX +#else + #define AXIS1_M2_PIN PA15 // SPI CS + #define AXIS1_M3_PIN PC11 // SPI MISO +#endif +#define AXIS1_STEP_PIN PE9 // TIM1 +#define AXIS1_DIR_PIN PF1 +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN PE10 // Home SW for Axis1 (Limit Connector Labeled E1) +#endif + +// Axis2 Dec/Alt step/dir driver (Labeled Y) +#define AXIS2_ENABLE_PIN PD7 +#define AXIS2_M0_PIN PC12 +#define AXIS2_M1_PIN PC10 +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC_HARDWARE_UART) + #define AXIS2_M2_PIN PE2 // TX + #define AXIS2_M3_PIN PE3 // RX +#else + #define AXIS2_M2_PIN PB8 // SPI CS + #define AXIS2_M3_PIN PC11 +#endif +#define AXIS2_STEP_PIN PE11 // TIM1 +#define AXIS2_DIR_PIN PE8 +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN PG5 // Home SW for Axis1 (Limit Connector Labeled E2) +#endif + +// For rotator stepper driver (Labeled Z) +#define AXIS3_ENABLE_PIN PC0 +#define AXIS3_M0_PIN PC12 +#define AXIS3_M1_PIN PC10 +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC_HARDWARE_UART) + #define AXIS3_M2_PIN PE0 // TX + #define AXIS3_M3_PIN PE1 // RX +#else + #define AXIS3_M2_PIN PB9 // SPI CS + #define AXIS3_M3_PIN PC11 +#endif +#define AXIS3_STEP_PIN PE13 +#define AXIS3_DIR_PIN PC2 + +// For focuser1 stepper driver (Labeled E0) +#define AXIS4_ENABLE_PIN PC3 +#define AXIS4_M0_PIN PC12 +#define AXIS4_M1_PIN PC10 +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC_HARDWARE_UART) + #define AXIS4_M2_PIN PD2 // TX + #define AXIS4_M3_PIN PD4 // RX +#else + #define AXIS4_M2_PIN PB3 // SPI CS + #define AXIS4_M3_PIN PC11 +#endif +#define AXIS4_STEP_PIN PE14 +#define AXIS4_DIR_PIN PA0 + +// For focuser2 stepper driver (Labeled E1) +#define AXIS5_ENABLE_PIN PA3 +#define AXIS5_M0_PIN PC12 +#define AXIS5_M1_PIN PC10 +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC_HARDWARE_UART) + #define AXIS5_M2_PIN PD0 // TX + #define AXIS5_M3_PIN PD1 // RX +#else + #define AXIS5_M2_PIN PG15 // SPI CS + #define AXIS5_M3_PIN PC11 +#endif +#define AXIS5_STEP_PIN PD15 +#define AXIS5_DIR_PIN PE7 + +// For focuser3 stepper driver (Labeled E2) +#define AXIS6_ENABLE_PIN PF0 +#define AXIS6_M0_PIN PC12 +#define AXIS6_M1_PIN PC10 +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC_HARDWARE_UART) + #define AXIS6_M2_PIN PD5 // TX + #define AXIS6_M3_PIN PD6 // RX +#else + #define AXIS6_M2_PIN PG12 // SPI CS + #define AXIS6_M3_PIN PC11 +#endif +#define AXIS6_STEP_PIN PD13 +#define AXIS6_DIR_PIN PG9 + +// For focuser4 stepper driver (Labeled Z) +#define AXIS7_ENABLE_PIN PC0 +#define AXIS7_M0_PIN PC12 +#define AXIS7_M1_PIN PC10 +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC_HARDWARE_UART) + #define AXIS7_M2_PIN PE0 // TX + #define AXIS7_M3_PIN PE1 // RX +#else + #define AXIS7_M2_PIN PB9 // SPI CS + #define AXIS7_M3_PIN PC11 +#endif +#define AXIS7_STEP_PIN PE13 +#define AXIS7_DIR_PIN PC2 + +// ST4 interface (Connector Labeled EXTENSION-2 which has Gnd and +5V so near ideal) +#define ST4_RA_W_PIN PD0 // ST4 RA- West +#define ST4_DEC_S_PIN PD2 // ST4 DE- South +#define ST4_DEC_N_PIN PD5 // ST4 DE+ North +#define ST4_RA_E_PIN PE0 // ST4 RA+ East + +#else +#error "Wrong processor for this configuration!" + +#endif diff --git a/src/pinmaps/Pins.STM32B.h b/src/pinmaps/Pins.STM32B.h new file mode 100755 index 0000000..faecf38 --- /dev/null +++ b/src/pinmaps/Pins.STM32B.h @@ -0,0 +1,146 @@ +// ------------------------------------------------------------------------------------------------- +// Pin map for OnStep on STM32 +#pragma once + +// This pin map is for the STM32F103CB (or C8 with 128KB) +// and STM32F303xC "Blue Pill" boards... +// They run at 72MHz, with 48K of RAM, and 256K of flash +// +// More info, schematic at: +// http://wiki.stm32duino.com/index.php?title=Blue_Pill +// +// Cost on eBay and AliExpress is less than US $2.50 + +#if defined(STM32F103xB) || defined(STM32F303xC) + +// Serial ports +// Serial1 RX1 Pin PA10, TX1 Pin PA9 (to CP2102 USB serial adapter) +// Serial2 RX2 Pin PA3 , TX2 Pin PA2 (pins are used for other purposes) +// Serial3 RX3 Pin PB11, TX3 Pin PB10 + +#if SERIAL_A_BAUD_DEFAULT != OFF + #define SERIAL_A Serial1 +#endif +#if SERIAL_B_BAUD_DEFAULT != OFF + #define SERIAL_B HardSerial + #define SERIAL_B_RX PB11 + #define SERIAL_B_TX PB10 +#endif + +// === Pins for DS3231 RTC/EEPROM +// The STM32 has no built in EEPROM. Therefore, we use a DS3231 RTC module +// which has an EEPROM chip on the board. HAL takes care of its address and size: +// +// STM32 pin PB6 -> SCL on DS3231 +// STM32 pin PB7 -> SDA on DS3231 + +#if PINMAP == STM32Blue + +// The multi-purpose pins (Aux3..Aux8 can be analog pwm/dac if supported) +#define AUX0_PIN PC13 // Status LED +#define AUX1_PIN PB14 // ESP8266 GPIO0, SPI MISO/Fault +#define AUX2_PIN PA1 // ESP8266 RST, SPI MISO/Fault +#define AUX3_PIN PB8 // Reticle, Home SW +#define AUX4_PIN PA13 // OneWire, Home SW + +#ifndef ONE_WIRE_PIN + #define ONE_WIRE_PIN AUX4_PIN // Default Pin for OneWire bus +#endif + +// For ESP8266 control +#define ADDON_GPIO0_PIN AUX1_PIN // ESP8266 GPIO0 +#define ADDON_RESET_PIN AUX2_PIN // ESP8266 RST + +// The PEC index sense is a logic level input, resets the PEC index on rising +// edge then waits for 60 seconds before allowing another reset +#define PEC_SENSE_PIN PC14 // PEC Sense + +// This is the built in LED for the Black Pill board. There is a pin +// available from it too, in case you want to power another LED with a wire +#define STATUS_LED_PIN AUX0_PIN // Default LED Cathode (-) +#define MOUNT_LED_PIN AUX0_PIN // Default LED Cathode (-) +#ifndef RETICLE_LED_PIN + #define RETICLE_LED_PIN AUX3_PIN // Default LED Cathode (-) +#endif + +// For a piezo buzzer +#define STATUS_BUZZER_PIN PB9 // Tone + +// The PPS pin is a 3.3V logic input, OnStep measures time between rising edges and +// adjusts the internal sidereal clock frequency +#ifndef PPS_SENSE_PIN + #define PPS_SENSE_PIN PB5 // Pulse Per Second time source, e.g. GPS, RTC +#endif + +// The limit switch sense is a logic level input which uses the internal pull up, +// shorted to ground it stops gotos/tracking +#ifndef LIMIT_SENSE_PIN + #define LIMIT_SENSE_PIN PA14 +#endif + +// Axis1 RA/Azm step/dir driver +#define AXIS1_ENABLE_PIN PA12 +#define AXIS1_M0_PIN PA11 // SPI MOSI +#define AXIS1_M1_PIN PA8 // SPI CLK +#define AXIS1_M2_PIN PB15 // SPI CS (UART TX) +#define AXIS1_M3_PIN AUX1_PIN // SPI MISO (UART RX) +#define AXIS1_STEP_PIN PB13 +#define AXIS1_DIR_PIN PB12 +#define AXIS1_FAULT_PIN AXIS1_M3_PIN +#ifndef AXIS1_SENSE_HOME_PIN + #define AXIS1_SENSE_HOME_PIN AUX3_PIN +#endif + +// Axis2 Dec/Alt step/dir driver +#define AXIS2_ENABLE_PIN PA5 +#define AXIS2_M0_PIN PA4 // SPI MOSI +#define AXIS2_M1_PIN PA3 // SPI CLK +#define AXIS2_M2_PIN PA2 // SPI CS (UART TX) +#define AXIS2_M3_PIN AUX2_PIN // SPI MISO (UART RX) +#define AXIS2_STEP_PIN PA0 +#define AXIS2_DIR_PIN PC15 +#define AXIS2_FAULT_PIN AXIS2_M3_PIN +#ifndef AXIS2_SENSE_HOME_PIN + #define AXIS2_SENSE_HOME_PIN AUX4_PIN +#endif + +// For rotator stepper driver +#define AXIS3_ENABLE_PIN PB4 +#define AXIS3_M0_PIN OFF // SPI MOSI +#define AXIS3_M1_PIN OFF // SPI SCK +#define AXIS3_M2_PIN OFF // SPI CS (UART TX) +#define AXIS3_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS3_STEP_PIN PB3 +#define AXIS3_DIR_PIN PA15 + +// Pins to focuser1 stepper driver +#define AXIS4_ENABLE_PIN PB4 +#define AXIS4_M0_PIN OFF // SPI MOSI +#define AXIS4_M1_PIN OFF // SPI SCK +#define AXIS4_M2_PIN OFF // SPI CS (UART TX) +#define AXIS4_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS4_STEP_PIN PB3 +#define AXIS4_DIR_PIN PA15 + +// For focuser2 stepper driver +#define AXIS5_ENABLE_PIN OFF +#define AXIS5_M0_PIN OFF // SPI MOSI +#define AXIS5_M1_PIN OFF // SPI SCK +#define AXIS5_M2_PIN OFF // SPI CS (UART TX) +#define AXIS5_M3_PIN OFF // SPI MISO (UART RX) +#define AXIS5_STEP_PIN OFF +#define AXIS5_DIR_PIN OFF + +// ST4 interface +#define ST4_DEC_N_PIN PA7 // ST4 DE+ North +#define ST4_DEC_S_PIN PA6 // ST4 DE- South +#define ST4_RA_W_PIN PB1 // ST4 RA- West +#define ST4_RA_E_PIN PB0 // ST4 RA+ East + +#else + #error "Unknown STM32 Board. This pinmap is only for Blue and Black Pill variants" +#endif + +#else + #error "Wrong processor for this configuration!" +#endif diff --git a/src/pinmaps/Pins.defaults.h b/src/pinmaps/Pins.defaults.h new file mode 100755 index 0000000..c42f0f6 --- /dev/null +++ b/src/pinmaps/Pins.defaults.h @@ -0,0 +1,685 @@ +// ------------------------------------------------------------------------------------------------- +// Null pin map, assigns OFF to all values not already assigned +#pragma once + +// -------------------------------------------------------------------------------------------------------- +// Serial, SPI, and I2C interface pins + +// usually the default serial port +#if SERIAL_A_BAUD_DEFAULT != OFF +#ifndef SERIAL_A +#define SERIAL_A Serial +#endif +#endif + +// automatic setup of serial passthrough +#if SERIAL_B_ESP_FLASHING == ON +#define SERIAL_PASSTHROUGH SERIAL_B +#define SERIAL_PASSTHROUGH_BAUD_DEFAULT SERIAL_B_BAUD_DEFAULT +#ifdef SERIAL_B_RX +#define SERIAL_PASSTHROUGH_RX SERIAL_B_RX +#endif +#ifdef SERIAL_B_TX +#define SERIAL_PASSTHROUGH_TX SERIAL_B_TX +#endif +#ifdef SERIAL_B_RXTX_SET +#define SERIAL_PASSTHROUGH_RXTX_SET SERIAL_B_RXTX_SET +#endif +#endif + +// default settings for any TMC UART drivers that may be present +#if defined(STEP_DIR_TMC_UART_PRESENT) +#ifndef SERIAL_TMC +#define SERIAL_TMC SoftSerial // Use software serial with TX on M3 (CS) of each axis +#endif +#ifndef SERIAL_TMC_RX_DISABLE +#define SERIAL_TMC_RX_DISABLE true // Recieving data often doesn't work with software serial +#endif +#ifndef SERIAL_TMC_BAUD +#define SERIAL_TMC_BAUD 115200 // Baud rate +#endif +#ifndef SERIAL_TMC_ADDRESS_MAP +#define SERIAL_TMC_ADDRESS_MAP(x) (0) // driver addresses are 0 for all axes +#endif +#ifndef SERIAL_TMC_INVERT +#define SERIAL_TMC_INVERT OFF // invert the serial interface signal logic +#endif +#endif + +// pin# for controlling the reset of W5500 so it comes up properly +#ifndef ETHERNET_RESET_PIN +#define ETHERNET_RESET_PIN OFF +#endif + +// default settings for I2C +#ifndef I2C_SDA_PIN +#define I2C_SDA_PIN OFF +#endif + +#ifndef I2C_SCL_PIN +#define I2C_SCL_PIN OFF +#endif + +#ifdef ESP32 + #if (defined(I2C_SCL_PIN) && I2C_SCL_PIN != OFF) && (defined(I2C_SDA_PIN) && I2C_SDA_PIN != OFF) + #define WIRE_INIT() HAL_WIRE.begin(I2C_SDA_PIN, I2C_SCL_PIN); HAL_WIRE_SET_CLOCK() + #else + #define WIRE_INIT() HAL_WIRE.begin(); HAL_WIRE_SET_CLOCK() + #endif +#else + #if (defined(I2C_SCL_PIN) && I2C_SCL_PIN != OFF) && (defined(I2C_SDA_PIN) && I2C_SDA_PIN != OFF) + #define WIRE_INIT() HAL_WIRE.setSDA(I2C_SDA_PIN); HAL_WIRE.setSCL(I2C_SCL_PIN); HAL_WIRE_SET_CLOCK() + #else + #define WIRE_INIT() HAL_WIRE_SET_CLOCK() + #endif +#endif + +// -------------------------------------------------------------------------------------------------------- +// GPIO SSR74HC595 pins +#ifndef GPIO_SSR74HC595_LATCH_PIN +#define GPIO_SSR74HC595_LATCH_PIN OFF // If used, only pins 0 to 31 are supported on the ESP32 +#endif + +#ifndef GPIO_SSR74HC595_CLOCK_PIN +#define GPIO_SSR74HC595_CLOCK_PIN OFF // If used, only pins 0 to 31 are supported on the ESP32 +#endif + +#ifndef GPIO_SSR74HC595_DATA_PIN +#define GPIO_SSR74HC595_DATA_PIN OFF // If used, only pins 0 to 31 are supported on the ESP32 +#endif + +#ifndef GPIO_SSR74HC595_COUNT +#define GPIO_SSR74HC595_COUNT 8 // 8, 16, 24, or 32 (for 1, 2, 3, or 4 74HC595's) +#endif + +// -------------------------------------------------------------------------------------------------------- +// define standard pins as inactive +#ifndef AUX0_PIN +#define AUX0_PIN OFF +#endif + +#ifndef AUX1_PIN +#define AUX1_PIN OFF +#endif + +#ifndef AUX2_PIN +#define AUX2_PIN OFF +#endif + +#ifndef AUX3_PIN +#define AUX3_PIN OFF +#endif + +#ifndef AUX4_PIN +#define AUX4_PIN OFF +#endif + +#ifndef AUX5_PIN +#define AUX5_PIN OFF +#endif + +#ifndef AUX6_PIN +#define AUX6_PIN OFF +#endif + +#ifndef AUX7_PIN +#define AUX7_PIN OFF +#endif + +#ifndef AUX8_PIN +#define AUX8_PIN OFF +#endif + +#ifndef ONE_WIRE_PIN +#define ONE_WIRE_PIN OFF +#endif + +#ifndef ADDON_TRIGR_PIN +#define ADDON_TRIGR_PIN OFF +#endif + +#ifndef ADDON_SELECT_PIN +#define ADDON_SELECT_PIN OFF +#endif + +#ifndef ADDON_GPIO0_PIN +#define ADDON_GPIO0_PIN OFF +#endif + +#ifndef ADDON_RESET_PIN +#define ADDON_RESET_PIN OFF +#endif + +#ifndef PARK_SENSE_PIN +#define PARK_SENSE_PIN OFF +#endif + +#ifndef PARK_SIGNAL_PIN +#define PARK_SIGNAL_PIN OFF +#endif + +#ifndef PARK_STATUS_PIN +#define PARK_STATUS_PIN OFF +#endif + +#ifndef PEC_SENSE_PIN +#define PEC_SENSE_PIN OFF +#endif + +#ifndef STATUS_LED_PIN +#define STATUS_LED_PIN OFF +#endif + +#ifndef MOUNT_LED_PIN +#define MOUNT_LED_PIN OFF +#endif + +#ifndef RETICLE_LED_PIN +#define RETICLE_LED_PIN OFF +#endif + +#ifndef STATUS_BUZZER_PIN +#define STATUS_BUZZER_PIN OFF +#endif + +#ifndef PPS_SENSE_PIN +#define PPS_SENSE_PIN OFF +#endif + +#ifndef LIMIT_SENSE_PIN +#define LIMIT_SENSE_PIN OFF +#endif + +#ifndef ST4_RA_W_PIN +#define ST4_RA_W_PIN OFF +#endif +#ifndef ST4_DEC_S_PIN +#define ST4_DEC_S_PIN OFF +#endif +#ifndef ST4_DEC_N_PIN +#define ST4_DEC_N_PIN OFF +#endif +#ifndef ST4_RA_E_PIN +#define ST4_RA_E_PIN OFF +#endif + +#ifndef AXIS1_ENABLE_PIN +#define AXIS1_ENABLE_PIN OFF +#endif +#ifndef AXIS1_M0_PIN +#define AXIS1_M0_PIN OFF +#endif +#ifndef AXIS1_M1_PIN +#define AXIS1_M1_PIN OFF +#endif +#ifndef AXIS1_M2_PIN +#define AXIS1_M2_PIN OFF +#endif +#ifndef AXIS1_M3_PIN +#define AXIS1_M3_PIN OFF +#endif +#ifndef AXIS1_STEP_PIN +#define AXIS1_STEP_PIN OFF +#endif +#ifndef AXIS1_DIR_PIN +#define AXIS1_DIR_PIN OFF +#endif +#ifndef AXIS1_DECAY_PIN +#define AXIS1_DECAY_PIN OFF +#endif +#ifndef AXIS1_SERVO_PH1_PIN +#define AXIS1_SERVO_PH1_PIN AXIS1_DIR_PIN +#endif +#ifndef AXIS1_SERVO_PH2_PIN +#define AXIS1_SERVO_PH2_PIN AXIS1_STEP_PIN +#endif +#ifndef AXIS1_FAULT_PIN +#define AXIS1_FAULT_PIN OFF +#endif +#ifndef AXIS1_SENSE_HOME_PIN +#define AXIS1_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS1_SENSE_LIMIT_MIN_PIN +#define AXIS1_SENSE_LIMIT_MIN_PIN LIMIT_SENSE_PIN +#endif +#ifndef AXIS1_SENSE_LIMIT_MAX_PIN +#define AXIS1_SENSE_LIMIT_MAX_PIN LIMIT_SENSE_PIN +#endif + +#ifndef AXIS2_ENABLE_PIN +#define AXIS2_ENABLE_PIN OFF +#endif +#ifndef AXIS2_M0_PIN +#define AXIS2_M0_PIN OFF +#endif +#ifndef AXIS2_M1_PIN +#define AXIS2_M1_PIN OFF +#endif +#ifndef AXIS2_M2_PIN +#define AXIS2_M2_PIN OFF +#endif +#ifndef AXIS2_M3_PIN +#define AXIS2_M3_PIN OFF +#endif +#ifndef AXIS2_STEP_PIN +#define AXIS2_STEP_PIN OFF +#endif +#ifndef AXIS2_DIR_PIN +#define AXIS2_DIR_PIN OFF +#endif +#ifndef AXIS2_DECAY_PIN +#define AXIS2_DECAY_PIN OFF +#endif +#ifndef AXIS2_SERVO_PH1_PIN +#define AXIS2_SERVO_PH1_PIN AXIS2_DIR_PIN +#endif +#ifndef AXIS2_SERVO_PH2_PIN +#define AXIS2_SERVO_PH2_PIN AXIS2_STEP_PIN +#endif +#ifndef AXIS2_FAULT_PIN +#define AXIS2_FAULT_PIN OFF +#endif +#ifndef AXIS2_SENSE_HOME_PIN +#define AXIS2_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS2_SENSE_LIMIT_MIN_PIN +#define AXIS2_SENSE_LIMIT_MIN_PIN LIMIT_SENSE_PIN +#endif +#ifndef AXIS2_SENSE_LIMIT_MAX_PIN +#define AXIS2_SENSE_LIMIT_MAX_PIN LIMIT_SENSE_PIN +#endif + +#ifndef AXIS3_ENABLE_PIN +#define AXIS3_ENABLE_PIN OFF +#endif +#ifndef AXIS3_M0_PIN +#define AXIS3_M0_PIN OFF +#endif +#ifndef AXIS3_M1_PIN +#define AXIS3_M1_PIN OFF +#endif +#ifndef AXIS3_M2_PIN +#define AXIS3_M2_PIN OFF +#endif +#ifndef AXIS3_M3_PIN +#define AXIS3_M3_PIN OFF +#endif +#ifndef AXIS3_STEP_PIN +#define AXIS3_STEP_PIN OFF +#endif +#ifndef AXIS3_DIR_PIN +#define AXIS3_DIR_PIN OFF +#endif +#ifndef AXIS3_DECAY_PIN +#define AXIS3_DECAY_PIN OFF +#endif +#ifndef AXIS3_SERVO_PH1_PIN +#define AXIS3_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS3_SERVO_PH2_PIN +#define AXIS3_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS3_ENCODER_A_PIN +#define AXIS3_ENCODER_A_PIN OFF +#endif +#ifndef AXIS3_ENCODER_B_PIN +#define AXIS3_ENCODER_B_PIN OFF +#endif +#ifndef AXIS3_FAULT_PIN +#define AXIS3_FAULT_PIN OFF +#endif +#ifndef AXIS3_SENSE_HOME_PIN +#define AXIS3_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS3_SENSE_LIMIT_MIN_PIN +#define AXIS3_SENSE_LIMIT_MIN_PIN OFF +#endif +#ifndef AXIS3_SENSE_LIMIT_MAX_PIN +#define AXIS3_SENSE_LIMIT_MAX_PIN OFF +#endif + +#ifndef AXIS4_ENABLE_PIN +#define AXIS4_ENABLE_PIN OFF +#endif +#ifndef AXIS4_M0_PIN +#define AXIS4_M0_PIN OFF +#endif +#ifndef AXIS4_M1_PIN +#define AXIS4_M1_PIN OFF +#endif +#ifndef AXIS4_M2_PIN +#define AXIS4_M2_PIN OFF +#endif +#ifndef AXIS4_M3_PIN +#define AXIS4_M3_PIN OFF +#endif +#ifndef AXIS4_STEP_PIN +#define AXIS4_STEP_PIN OFF +#endif +#ifndef AXIS4_DIR_PIN +#define AXIS4_DIR_PIN OFF +#endif +#ifndef AXIS4_DECAY_PIN +#define AXIS4_DECAY_PIN OFF +#endif +#ifndef AXIS4_SERVO_PH1_PIN +#define AXIS4_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS4_SERVO_PH2_PIN +#define AXIS4_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS4_ENCODER_A_PIN +#define AXIS4_ENCODER_A_PIN OFF +#endif +#ifndef AXIS4_ENCODER_B_PIN +#define AXIS4_ENCODER_B_PIN OFF +#endif +#ifndef AXIS4_FAULT_PIN +#define AXIS4_FAULT_PIN OFF +#endif +#ifndef AXIS4_SENSE_HOME_PIN +#define AXIS4_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS4_SENSE_LIMIT_MIN_PIN +#define AXIS4_SENSE_LIMIT_MIN_PIN OFF +#endif +#ifndef AXIS4_SENSE_LIMIT_MAX_PIN +#define AXIS4_SENSE_LIMIT_MAX_PIN OFF +#endif + +#ifndef AXIS1_ENCODER_A_PIN +#define AXIS1_ENCODER_A_PIN AXIS3_STEP_PIN +#endif +#ifndef AXIS1_ENCODER_B_PIN +#define AXIS1_ENCODER_B_PIN AXIS3_DIR_PIN +#endif +#ifndef AXIS2_ENCODER_A_PIN +#define AXIS2_ENCODER_A_PIN AXIS4_STEP_PIN +#endif +#ifndef AXIS2_ENCODER_B_PIN + #if AXIS4_DIR_PIN != AXIS3_DIR_PIN || AXIS4_DIR_PIN == OFF + #define AXIS2_ENCODER_B_PIN AXIS4_DIR_PIN + #else + #define AXIS2_ENCODER_B_PIN AXIS2_M3_PIN + #endif +#endif + +#ifndef AXIS5_ENABLE_PIN +#define AXIS5_ENABLE_PIN OFF +#endif +#ifndef AXIS5_M0_PIN +#define AXIS5_M0_PIN OFF +#endif +#ifndef AXIS5_M1_PIN +#define AXIS5_M1_PIN OFF +#endif +#ifndef AXIS5_M2_PIN +#define AXIS5_M2_PIN OFF +#endif +#ifndef AXIS5_M3_PIN +#define AXIS5_M3_PIN OFF +#endif +#ifndef AXIS5_STEP_PIN +#define AXIS5_STEP_PIN OFF +#endif +#ifndef AXIS5_DIR_PIN +#define AXIS5_DIR_PIN OFF +#endif +#ifndef AXIS5_DECAY_PIN +#define AXIS5_DECAY_PIN OFF +#endif +#ifndef AXIS5_SERVO_PH1_PIN +#define AXIS5_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS5_SERVO_PH2_PIN +#define AXIS5_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS5_ENCODER_A_PIN +#define AXIS5_ENCODER_A_PIN OFF +#endif +#ifndef AXIS5_ENCODER_B_PIN +#define AXIS5_ENCODER_B_PIN OFF +#endif +#ifndef AXIS5_FAULT_PIN +#define AXIS5_FAULT_PIN OFF +#endif +#ifndef AXIS5_SENSE_HOME_PIN +#define AXIS5_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS5_SENSE_LIMIT_MIN_PIN +#define AXIS5_SENSE_LIMIT_MIN_PIN OFF +#endif +#ifndef AXIS5_SENSE_LIMIT_MAX_PIN +#define AXIS5_SENSE_LIMIT_MAX_PIN OFF +#endif + +#ifndef AXIS6_ENABLE_PIN +#define AXIS6_ENABLE_PIN OFF +#endif +#ifndef AXIS6_M0_PIN +#define AXIS6_M0_PIN OFF +#endif +#ifndef AXIS6_M1_PIN +#define AXIS6_M1_PIN OFF +#endif +#ifndef AXIS6_M2_PIN +#define AXIS6_M2_PIN OFF +#endif +#ifndef AXIS6_M3_PIN +#define AXIS6_M3_PIN OFF +#endif +#ifndef AXIS6_STEP_PIN +#define AXIS6_STEP_PIN OFF +#endif +#ifndef AXIS6_DIR_PIN +#define AXIS6_DIR_PIN OFF +#endif +#ifndef AXIS6_DECAY_PIN +#define AXIS6_DECAY_PIN OFF +#endif +#ifndef AXIS6_SERVO_PH1_PIN +#define AXIS6_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS6_SERVO_PH2_PIN +#define AXIS6_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS6_ENCODER_A_PIN +#define AXIS6_ENCODER_A_PIN OFF +#endif +#ifndef AXIS6_ENCODER_B_PIN +#define AXIS6_ENCODER_B_PIN OFF +#endif +#ifndef AXIS6_FAULT_PIN +#define AXIS6_FAULT_PIN OFF +#endif +#ifndef AXIS6_SENSE_HOME_PIN +#define AXIS6_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS6_SENSE_LIMIT_MIN_PIN +#define AXIS6_SENSE_LIMIT_MIN_PIN OFF +#endif +#ifndef AXIS6_SENSE_LIMIT_MAX_PIN +#define AXIS6_SENSE_LIMIT_MAX_PIN OFF +#endif + +#ifndef AXIS7_ENABLE_PIN +#define AXIS7_ENABLE_PIN OFF +#endif +#ifndef AXIS7_M0_PIN +#define AXIS7_M0_PIN OFF +#endif +#ifndef AXIS7_M1_PIN +#define AXIS7_M1_PIN OFF +#endif +#ifndef AXIS7_M2_PIN +#define AXIS7_M2_PIN OFF +#endif +#ifndef AXIS7_M3_PIN +#define AXIS7_M3_PIN OFF +#endif +#ifndef AXIS7_STEP_PIN +#define AXIS7_STEP_PIN OFF +#endif +#ifndef AXIS7_DIR_PIN +#define AXIS7_DIR_PIN OFF +#endif +#ifndef AXIS7_DECAY_PIN +#define AXIS7_DECAY_PIN OFF +#endif +#ifndef AXIS7_SERVO_PH1_PIN +#define AXIS7_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS7_SERVO_PH2_PIN +#define AXIS7_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS7_ENCODER_A_PIN +#define AXIS7_ENCODER_A_PIN OFF +#endif +#ifndef AXIS7_ENCODER_B_PIN +#define AXIS7_ENCODER_B_PIN OFF +#endif +#ifndef AXIS7_FAULT_PIN +#define AXIS7_FAULT_PIN OFF +#endif +#ifndef AXIS7_SENSE_HOME_PIN +#define AXIS7_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS7_SENSE_LIMIT_MIN_PIN +#define AXIS7_SENSE_LIMIT_MIN_PIN OFF +#endif +#ifndef AXIS7_SENSE_LIMIT_MAX_PIN +#define AXIS7_SENSE_LIMIT_MAX_PIN OFF +#endif + +#ifndef AXIS8_ENABLE_PIN +#define AXIS8_ENABLE_PIN OFF +#endif +#ifndef AXIS8_M0_PIN +#define AXIS8_M0_PIN OFF +#endif +#ifndef AXIS8_M1_PIN +#define AXIS8_M1_PIN OFF +#endif +#ifndef AXIS8_M2_PIN +#define AXIS8_M2_PIN OFF +#endif +#ifndef AXIS8_M3_PIN +#define AXIS8_M3_PIN OFF +#endif +#ifndef AXIS8_STEP_PIN +#define AXIS8_STEP_PIN OFF +#endif +#ifndef AXIS8_DIR_PIN +#define AXIS8_DIR_PIN OFF +#endif +#ifndef AXIS8_DECAY_PIN +#define AXIS8_DECAY_PIN OFF +#endif +#ifndef AXIS8_SERVO_PH1_PIN +#define AXIS8_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS8_SERVO_PH2_PIN +#define AXIS8_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS8_ENCODER_A_PIN +#define AXIS8_ENCODER_A_PIN OFF +#endif +#ifndef AXIS8_ENCODER_B_PIN +#define AXIS8_ENCODER_B_PIN OFF +#endif +#ifndef AXIS8_FAULT_PIN +#define AXIS8_FAULT_PIN OFF +#endif +#ifndef AXIS8_SENSE_HOME_PIN +#define AXIS8_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS8_SENSE_LIMIT_MIN_PIN +#define AXIS8_SENSE_LIMIT_MIN_PIN OFF +#endif +#ifndef AXIS8_SENSE_LIMIT_MAX_PIN +#define AXIS8_SENSE_LIMIT_MAX_PIN OFF +#endif + +#ifndef AXIS9_ENABLE_PIN +#define AXIS9_ENABLE_PIN OFF +#endif +#ifndef AXIS9_M0_PIN +#define AXIS9_M0_PIN OFF +#endif +#ifndef AXIS9_M1_PIN +#define AXIS9_M1_PIN OFF +#endif +#ifndef AXIS9_M2_PIN +#define AXIS9_M2_PIN OFF +#endif +#ifndef AXIS9_M3_PIN +#define AXIS9_M3_PIN OFF +#endif +#ifndef AXIS9_STEP_PIN +#define AXIS9_STEP_PIN OFF +#endif +#ifndef AXIS9_DIR_PIN +#define AXIS9_DIR_PIN OFF +#endif +#ifndef AXIS9_DECAY_PIN +#define AXIS9_DECAY_PIN OFF +#endif +#ifndef AXIS9_SERVO_PH1_PIN +#define AXIS9_SERVO_PH1_PIN OFF +#endif +#ifndef AXIS9_SERVO_PH2_PIN +#define AXIS9_SERVO_PH2_PIN OFF +#endif +#ifndef AXIS9_ENCODER_A_PIN +#define AXIS9_ENCODER_A_PIN OFF +#endif +#ifndef AXIS9_ENCODER_B_PIN +#define AXIS9_ENCODER_B_PIN OFF +#endif +#ifndef AXIS9_FAULT_PIN +#define AXIS9_FAULT_PIN OFF +#endif +#ifndef AXIS9_SENSE_HOME_PIN +#define AXIS9_SENSE_HOME_PIN OFF +#endif +#ifndef AXIS9_SENSE_LIMIT_MIN_PIN +#define AXIS9_SENSE_LIMIT_MIN_PIN OFF +#endif +#ifndef AXIS9_SENSE_LIMIT_MAX_PIN +#define AXIS9_SENSE_LIMIT_MAX_PIN OFF +#endif + +#ifndef FOCUSER_TEMPERATURE_PIN +#define FOCUSER_TEMPERATURE_PIN OFF // analog pin +#endif +#ifndef FOCUSER_BUTTON_SENSE_IN_PIN +#define FOCUSER_BUTTON_SENSE_IN_PIN OFF +#endif +#ifndef FOCUSER_BUTTON_SENSE_OUT_PIN +#define FOCUSER_BUTTON_SENSE_OUT_PIN OFF +#endif + +#ifndef FEATURE1_TEMPERATURE_PIN +#define FEATURE1_TEMPERATURE_PIN OFF // analog pin +#endif +#ifndef FEATURE2_TEMPERATURE_PIN +#define FEATURE2_TEMPERATURE_PIN OFF +#endif +#ifndef FEATURE3_TEMPERATURE_PIN +#define FEATURE3_TEMPERATURE_PIN OFF +#endif +#ifndef FEATURE4_TEMPERATURE_PIN +#define FEATURE4_TEMPERATURE_PIN OFF +#endif +#ifndef FEATURE5_TEMPERATURE_PIN +#define FEATURE5_TEMPERATURE_PIN OFF +#endif +#ifndef FEATURE6_TEMPERATURE_PIN +#define FEATURE6_TEMPERATURE_PIN OFF +#endif +#ifndef FEATURE7_TEMPERATURE_PIN +#define FEATURE7_TEMPERATURE_PIN OFF +#endif +#ifndef FEATURE8_TEMPERATURE_PIN +#define FEATURE8_TEMPERATURE_PIN OFF +#endif diff --git a/src/pinmaps/Validate.h b/src/pinmaps/Validate.h new file mode 100755 index 0000000..d6e3c7d --- /dev/null +++ b/src/pinmaps/Validate.h @@ -0,0 +1,276 @@ +// ------------------------------------------------------------------------------------------------- +// Validates pinmap model for current configuration +#pragma once + +// -------------------------------------------------------------------------------------- + +#if !defined(SERIAL_A) && SERIAL_A_BAUD_DEFAULT != OFF + #error "Configuration (Config.h): SERIAL_A isn't defined, SERIAL_A_BAUD_DEFAULT should be OFF." +#endif + +#if !defined(SERIAL_B) && SERIAL_B_BAUD_DEFAULT != OFF + #error "Configuration (Config.h): SERIAL_B isn't defined, SERIAL_B_BAUD_DEFAULT should be OFF." +#endif + +#if !defined(SERIAL_C) && SERIAL_C_BAUD_DEFAULT != OFF + #error "Configuration (Config.h): SERIAL_C isn't defined, SERIAL_C_BAUD_DEFAULT should be OFF." +#endif + +#if !defined(SERIAL_D) && SERIAL_D_BAUD_DEFAULT != OFF + #error "Configuration (Config.h): SERIAL_D isn't defined, SERIAL_D_BAUD_DEFAULT should be OFF." +#endif + +#if !defined(SERIAL_E) && SERIAL_E_BAUD_DEFAULT != OFF + #error "Configuration (Config.h): SERIAL_E isn't defined, SERIAL_E_BAUD_DEFAULT should be OFF." +#endif + +// can we flash an ESP8266? +#if SERIAL_B_ESP_FLASHING == ON && (!defined(ADDON_GPIO0_PIN) || !defined(ADDON_RESET_PIN)) + #error "Configuration (Config.h): SERIAL_B_ESP_FLASHING not supported for this PINMAP" +#endif + +// make sure a serial interface is available for any TMC UART driver +#if defined(STEP_DIR_TMC_UART_PRESENT) && !defined(SERIAL_TMC) + #error "Configuration (Config.h): SERIAL_TMC is required for TMC UART drivers" +#endif + +// powering down Dec supported only if EN is available and not shared with Axis1 +#if AXIS2_POWER_DOWN == ON && (AXIS2_EN_PIN == OFF || AXIS2_EN_PIN == SHARED) + #error "Configuration (Config.h): AXIS2_POWER_DOWN not supported on this PINMAP" +#endif + +// TIME_LOCATION_SOURCE DS3234 SPI allowed? +#if TIME_LOCATION_SOURCE == DS3234 && !defined(DS3234_CS_PIN) + #if PINMAP == Classic + #error "Configuration (Config.h): DS3234 TIME_LOCATION_SOURCE using SPI is not supported, use PINMAP ClassicShield or add '#define ST4_ALTERNATE_PINS_ON' to move the ST4 port pins" + #else + #error "Configuration (Config.h): DS3234 TIME_LOCATION_SOURCE using SPI is not supported for this PINMAP" + #endif +#endif + +// focuser/rotators invalid combination? +#if ROTATOR == ON && FOCUSER1 == ON + #if AXIS3_STEP_PIN == AXIS4_STEP_PIN + #error "Configuration (Config.h): AXIS3 and AXIS4 step/dir interface is shared, so enabling both is not supported on this PINMAP" + #endif +#endif + +#if ROTATOR == ON && FOCUSER2 == ON + #if AXIS3_STEP_PIN == AXIS5_STEP_PIN + #error "Configuration (Config.h): AXIS3 and AXIS5 step/dir interface is shared, so enabling both is not supported on this PINMAP" + #endif +#endif + +// -------------------------------------------------------------------------------------- +// check AUX0 pin assignments +#if AUX0_PIN != OFF + #if AUX0_PIN == MOUNT_LED_PIN && STATUS_MOUNT_LED != OFF + #ifdef AUX0_PIN_IN_USE + #error "Configuration (Config.h): STATUS_MOUNT_LED enabled but AUX0_PIN is already in use, choose one feature on AUX0_PIN" + #endif + #define AUX0_PIN_IN_USE + #endif +#endif + +// check AUX1 pin assignments +#if AUX1_PIN != OFF + #if AUX1_PIN == ADDON_GPIO0_PIN && SERIAL_B_ESP_FLASHING != OFF + #ifdef AUX1_PIN_IN_USE + #error "Configuration (Config.h): SERIAL_B_ESP_FLASHING enabled but AUX1_PIN is already in use, choose one feature on AUX1_PIN" + #endif + #define AUX1_PIN_IN_USE + #endif + #if (AUX1_PIN == AXIS1_M3_PIN && AXIS1_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS2_M3_PIN && AXIS2_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS3_M3_PIN && AXIS3_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS4_M3_PIN && AXIS4_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS5_M3_PIN && AXIS5_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS6_M3_PIN && AXIS6_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS7_M3_PIN && AXIS7_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS8_M3_PIN && AXIS8_DRIVER_STATUS != OFF) || \ + (AUX1_PIN == AXIS9_M3_PIN && AXIS9_DRIVER_STATUS != OFF) + #ifdef AUX1_PIN_IN_USE + #error "Configuration (Config.h): AXISn_DRIVER_STATUS enabled but AUX1_PIN is already in use, choose one feature on AUX1_PIN" + #endif + #define AUX1_PIN_IN_USE + #endif +#endif + +// check AUX2 pin assignments +#if AUX2_PIN != OFF + #if AUX2_PIN == ADDON_RESET_PIN && SERIAL_B_ESP_FLASHING != OFF + #ifdef AUX2_PIN_IN_USE + #error "Configuration (Config.h): SERIAL_B_ESP_FLASHING enabled but AUX2_PIN is already in use, choose one feature on AUX2_PIN" + #endif + #define AUX1_PIN_IN_USE + #endif + #if (AUX2_PIN == AXIS1_M3_PIN && AXIS1_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS2_M3_PIN && AXIS2_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS3_M3_PIN && AXIS3_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS4_M3_PIN && AXIS4_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS5_M3_PIN && AXIS5_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS6_M3_PIN && AXIS6_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS7_M3_PIN && AXIS7_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS8_M3_PIN && AXIS8_DRIVER_STATUS != OFF) || \ + (AUX2_PIN == AXIS9_M3_PIN && AXIS9_DRIVER_STATUS != OFF) + #ifdef AUX2_PIN_IN_USE + #error "Configuration (Config.h): AXISn_DRIVER_STATUS enabled but AUX2_PIN is already in use, choose one feature on AUX2_PIN" + #endif + #define AUX2_PIN_IN_USE + #endif +#endif + +// check AUX3 pin assignments +#if AUX3_PIN != OFF + #if (AUX3_PIN) == (AXIS1_SENSE_HOME_PIN) && (AXIS1_SENSE_HOME) != OFF + #ifdef AUX3_PIN_IN_USE + #error "Configuration (Config.h): AXIS1_SENSE_HOME enabled but AUX3_PIN is already in use, choose one feature on AUX3_PIN" + #endif + #define AUX3_PIN_IN_USE + #endif + #if (AUX3_PIN) == (LIMIT_SENSE_PIN) && (LIMIT_SENSE) != OFF + #ifdef AUX3_PIN_IN_USE + #error "Configuration (Config.h): LIMIT_SENSE enabled but AUX3_PIN is already in use, choose one feature on AUX3_PIN" + #endif + #define AUX3_PIN_IN_USE + #endif + #if AUX3_PIN == RETICLE_LED_PIN && RETICLE_LED_DEFAULT != OFF + #if AUX3_PIN == STATUS_LED_PIN && STATUS_LED != OFF + #error "Configuration (Config.h): RETICLE_LED_DEFAULT and STATUS_LED can't both be enabled on the same AUX3_PIN" + #endif + #ifdef AUX3_PIN_IN_USE + #error "Configuration (Config.h): RETICLE_LED_DEFAULT enabled but AUX3_PIN is already in use, choose one feature on AUX3_PIN" + #endif + #define AUX3_PIN_IN_USE + #endif +#endif + +// check AUX4 pin assignments +#if AUX4_PIN != OFF + #if (AUX4_PIN) == (AXIS2_SENSE_HOME_PIN) && (AXIS2_SENSE_HOME) != OFF + #ifdef AUX4_PIN_IN_USE + #error "Configuration (Config.h): AXIS2_SENSE_HOME enabled but AUX4_PIN is already in use, choose one feature on AUX4_PIN" + #endif + #define AUX4_PIN_IN_USE + #endif + #if AUX4_PIN == RETICLE_LED_PIN && RETICLE_LED_DEFAULT != OFF + #ifdef AUX4_PIN_IN_USE + #error "Configuration (Config.h): RETICLE_LED_DEFAULT enabled but AUX4_PIN is already in use, choose one feature on AUX4_PIN" + #endif + #define AUX4_PIN_IN_USE + #endif +#endif + +// check AUX5 pin assignments +// not checked, for Axis3 enable or Serial + +// check AUX6 pin assignments +// not checked, for Axis4 enable or Serial + +// check AUX7 pin assignments +#if AUX7_PIN != OFF + #if (AUX7_PIN) == (LIMIT_SENSE_PIN) && (LIMIT_SENSE) != OFF + #ifdef AUX7_PIN_IN_USE + #error "Configuration (Config.h): LIMIT_SENSE enabled but AUX7_PIN is already in use, choose one feature on AUX7_PIN" + #endif + #define AUX7_PIN_IN_USE + #endif + #if (AUX7_PIN) == (PPS_SENSE_PIN) && (TIME_LOCATION_PPS_SENSE) != OFF + #ifdef AUX7_PIN_IN_USE + #error "Configuration (Config.h): TIME_LOCATION_PPS_SENSE enabled but AUX7_PIN is already in use, choose one feature on AUX7_PIN" + #endif + #define AUX7_PIN_IN_USE + #endif +#endif + +// check AUX8 pin assignments +#if AUX8_PIN != OFF + #if AUX8_PIN == MOUNT_LED_PIN && STATUS_MOUNT_LED != OFF + #ifdef AUX8_PIN_IN_USE + #error "Configuration (Config.h): STATUS_MOUNT_LED enabled but AUX8_PIN is already in use, choose one feature on AUX8_PIN" + #endif + #define AUX8_PIN_IN_USE + #endif + #if AUX8_PIN == RETICLE_LED_PIN && RETICLE_LED_DEFAULT != OFF + #if AUX8_PIN == STATUS_LED_PIN && STATUS_LED != OFF + #error "Configuration (Config.h): RETICLE_LED_DEFAULT and STATUS_LED can't both be enabled on the same AUX8_PIN" + #endif + #ifdef AUX8_PIN_IN_USE + #error "Configuration (Config.h): RETICLE_LED_DEFAULT enabled but AUX8_PIN is already in use, choose one feature on AUX8_PIN" + #endif + #define AUX8_PIN_IN_USE + #endif + #if AUX8_PIN == STATUS_BUZZER_PIN && STATUS_BUZZER != OFF + #ifdef AUX8_PIN_IN_USE + #error "Configuration (Config.h): STATUS_BUZZER enabled but AUX8_PIN is already in use, choose one feature on AUX8_PIN" + #endif + #define AUX8_PIN_IN_USE + #endif +#endif + +// -------------------------------------------------------------------------------------- +// check/assign AUX pins to auxiliary features +#if FEATURE1_PURPOSE != OFF && FEATURE1_PIN == AUX && AUX1_PIN != OFF + #ifdef AUX1_PIN_IN_USE + #error "Configuration (Config.h): FEATURE1_PIN AUX enabled but AUX1_PIN is already in use, choose one feature on AUX1_PIN" + #endif + #undef FEATURE1_PIN + #define FEATURE1_PIN AUX1_PIN + #define AUX1_PIN_IN_USE +#endif +#if FEATURE2_PURPOSE != OFF && FEATURE2_PIN == AUX && AUX2_PIN != OFF + #ifdef AUX2_PIN_IN_USE + #error "Configuration (Config.h): FEATURE2_PIN AUX enabled but AUX2_PIN is already in use, choose one feature on AUX2_PIN" + #endif + #undef FEATURE2_PIN + #define FEATURE2_PIN AUX2_PIN + #define AUX2_PIN_IN_USE +#endif +#if FEATURE3_PURPOSE != OFF && FEATURE3_PIN == AUX && AUX3_PIN != OFF + #ifdef AUX3_PIN_IN_USE + #error "Configuration (Config.h): FEATURE3_PIN AUX enabled but AUX3_PIN is already in use, choose one feature on AUX3_PIN" + #endif + #undef FEATURE3_PIN + #define FEATURE3_PIN AUX3_PIN + #define AUX3_PIN_IN_USE +#endif +#if FEATURE4_PURPOSE != OFF && FEATURE4_PIN == AUX && AUX4_PIN != OFF + #ifdef AUX4_PIN_IN_USE + #error "Configuration (Config.h): FEATURE4_PIN AUX enabled but AUX4_PIN is already in use, choose one feature on AUX4_PIN" + #endif + #undef FEATURE4_PIN + #define FEATURE4_PIN AUX4_PIN + #define AUX4_PIN_IN_USE +#endif +#if FEATURE5_PURPOSE != OFF && FEATURE5_PIN == AUX && AUX5_PIN != OFF + #ifdef AUX5_PIN_IN_USE + #error "Configuration (Config.h): FEATURE5_PIN AUX enabled but AUX5_PIN is already in use, choose one feature on AUX5_PIN" + #endif + #undef FEATURE5_PIN + #define FEATURE5_PIN AUX5_PIN + #define AUX5_PIN_IN_USE +#endif +#if FEATURE6_PURPOSE != OFF && FEATURE6_PIN == AUX && AUX6_PIN != OFF + #ifdef AUX6_PIN_IN_USE + #error "Configuration (Config.h): FEATURE6_PIN AUX enabled but AUX6_PIN is already in use, choose one feature on AUX6_PIN" + #endif + #undef FEATURE6_PIN + #define FEATURE6_PIN AUX6_PIN + #define AUX6_PIN_IN_USE +#endif +#if FEATURE7_PURPOSE != OFF && FEATURE7_PIN == AUX && AUX7_PIN != OFF + #ifdef AUX7_PIN_IN_USE + #error "Configuration (Config.h): FEATURE7_PIN AUX enabled but AUX7_PIN is already in use, choose one feature on AUX7_PIN" + #endif + #undef FEATURE7_PIN + #define FEATURE7_PIN AUX7_PIN + #define AUX7_PIN_IN_USE +#endif +#if FEATURE8_PURPOSE != OFF && FEATURE8_PIN == AUX && AUX8_PIN != OFF + #ifdef AUX8_PIN_IN_USE + #error "Configuration (Config.h): FEATURE8_PIN AUX enabled but AUX8_PIN is already in use, choose one feature on AUX8_PIN" + #endif + #undef FEATURE8_PIN + #define FEATURE8_PIN AUX8_PIN + #define AUX8_PIN_IN_USE +#endif diff --git a/src/telescope/Telescope.command.cpp b/src/telescope/Telescope.command.cpp new file mode 100755 index 0000000..00ce48d --- /dev/null +++ b/src/telescope/Telescope.command.cpp @@ -0,0 +1,285 @@ +//-------------------------------------------------------------------------------------------------- +// OnStepX telescope control + +#include "../Common.h" + +#include "Telescope.h" + +#include "../lib/tasks/OnTask.h" +#include "../lib/gpioEx/GpioEx.h" +#include "../lib/nv/Nv.h" + +#include "../lib/analog/Analog.h" +#include "../lib/convert/Convert.h" + +#include "../libApp/commands/ProcessCmds.h" +#include "../libApp/weather/Weather.h" + +#include "addonFlasher/AddonFlasher.h" +#include "mount/Mount.h" +#include "mount/goto/Goto.h" +#include "mount/guide/Guide.h" +#include "mount/home/Home.h" +#include "mount/library/Library.h" +#include "mount/limits/Limits.h" +#include "mount/park/Park.h" +#include "mount/pec/Pec.h" +#include "mount/site/Site.h" +#include "mount/status/Status.h" +#include "rotator/Rotator.h" +#include "focuser/Focuser.h" +#include "auxiliary/Features.h" +#include "../plugins/Plugins.config.h" + +// possible reset function for this MCU +#ifdef HAL_RESET_FUNC + HAL_RESET_FUNC; +#endif + +bool Telescope::command(char reply[], char command[], char parameter[], bool *suppressFrame, bool *numericReply, CommandError *commandError) { + + #if PLUGIN1 != OFF && PLUGIN1_COMMAND_PROCESSING == ON + if (PLUGIN1.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + #if PLUGIN2 != OFF && PLUGIN2_COMMAND_PROCESSING == ON + if (PLUGIN2.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + #if PLUGIN3 != OFF && PLUGIN3_COMMAND_PROCESSING == ON + if (PLUGIN3.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + #if PLUGIN4 != OFF && PLUGIN4_COMMAND_PROCESSING == ON + if (PLUGIN4.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + #if PLUGIN5 != OFF && PLUGIN5_COMMAND_PROCESSING == ON + if (PLUGIN5.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + #if PLUGIN6 != OFF && PLUGIN6_COMMAND_PROCESSING == ON + if (PLUGIN6.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + #if PLUGIN7 != OFF && PLUGIN7_COMMAND_PROCESSING == ON + if (PLUGIN7.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + #if PLUGIN8 != OFF && PLUGIN8_COMMAND_PROCESSING == ON + if (PLUGIN8.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + + #ifdef MOUNT_PRESENT + if (mount.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (guide.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #if GPIO_DEVICE != OFF + if (gpio.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + if (mountStatus.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (goTo.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (park.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (library.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (site.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (limits.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (home.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (pec.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (axis1.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + if (axis2.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + + #if defined(ROTATOR_PRESENT) || defined(ROTATOR_CLIENT_PRESENT) + if (rotator.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + + #if defined(FOCUSER_PRESENT) || defined(FOCUSER_CLIENT_PRESENT) + if (focuser.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + + #if defined(FEATURES_PRESENT) || defined(FEATURES_CLIENT_PRESENT) + if (features.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + #endif + + // B - Reticle/Accessory Control + // :B+# Increase reticle Brightness + // Returns: Nothing + // :B-# Decrease Reticle Brightness + // Returns: Nothing + if (command[0] == 'B' && (command[1] == '+' || command[1] == '-') && parameter[0] == 0) { + #if RETICLE_LED_DEFAULT >= 0 && RETICLE_LED_PIN != OFF + int scale; + if (reticleBrightness > 255 - 8) scale = 1; else + if (reticleBrightness > 255 - 32) scale = 4; else + if (reticleBrightness > 255 - 64) scale = 12; else + if (reticleBrightness > 255 - 128) scale = 32; else scale = 64; + if (command[1] == '-') reticleBrightness += scale; + if (reticleBrightness > 255) reticleBrightness = 255; + if (command[1] == '+') reticleBrightness -= scale; + if (reticleBrightness < 0) reticleBrightness = 0; + + float duty = (float)reticleBrightness*(1.0F/255.0F); + analog.write(RETICLE_LED_PIN, RETICLE_LED_INVERT == ON ? duty : 1.0F - duty); + + #if RETICLE_LED_MEMORY == ON + nv().kv().put("TELESCOPE_SETTINGS", reticleBrightness); + #endif + #endif + *numericReply = false; + } else + + // E - Enter special mode + if (command[0] == 'E') { + // :EC[s]# Echo string [s] as a debug message + // Return: Nothing + if (command[1] == 'C') { + // spaces are encoded as '_' + for (unsigned int i = 0; i < strlen(parameter); i++) if (parameter[i] == '_') parameter[i] = ' '; + // prefix with "REM> " + if (strstr(parameter, "ERR:") == parameter || strstr(parameter, "WRN:") == parameter || strstr(parameter, "MSG:") == parameter) { D("REM> "); } + // a newline is encoded as '&' in the last char of message + int l = strlen(parameter); + if (l > 0 && parameter[l - 1] == '&') { parameter[l - 1] = 0; DL(parameter); } else { D(parameter); } + *numericReply = false; + } else + + // :ERESET# Reset the MCU + // Returns: Nothing + if (command[1] == 'R' && parameter[0] == 'E' && parameter[1] == 'S' && parameter[2] == 'E' && parameter[3] == 'T' && parameter[4] == 0) { + + #ifdef HAL_RESET + VLF("MSG: Mount, resetting OnStep..."); + if (nv().device().hasCommit()) { nv().device().commit(); } + const uint32_t startMs = millis(); + const uint32_t timeoutMs = 30000; + while (!nv().device().commitDone() && (uint32_t)(millis() - startMs) < timeoutMs) { tasks.yield(1000); } + tasks.yield(1000); + HAL_RESET(); + #endif + + *numericReply = false; + } else + + // :ENVRESET# Wipe flash + if (command[1] == 'N' && parameter[0] == 'V' && parameter[1] == 'R' && parameter[2] == 'E' && parameter[3] == 'S' && parameter[4] == 'E' && parameter[5] == 'T' && parameter[6] == 0) { + nv().volume().invalidateNextMount(); + strcpy(reply, "NV memory will be cleared on the next boot."); + *numericReply = false; + } else + + // :ESPFLASH# ESP8266 device flash mode + // Return: 1 on completion (after up to one minute from start of command.) + #if SERIAL_B_ESP_FLASHING == ON + if (command[1] == 'S' && parameter[0] == 'P' && parameter[1] == 'F' && parameter[2] == 'L' && parameter[3] == 'A' && parameter[4] == 'S' && parameter[5] == 'H' && parameter[6] == 0) { + SERIAL_A.println("The SWS microcontroller will now be placed in flash upload mode (at 115200 Baud.)"); + SERIAL_A.println("Arduino's 'Tools -> Upload Speed' should be set to 115200 Baud."); + SERIAL_A.println("Waiting for data, you have one minute to start the upload."); + tasks.yield(1000); + addonFlasher.go(); + SERIAL_A.println("SWS microcontroller reset and in run mode, resuming operation..."); + tasks.yield(1000); + } else + #elif ADDON_SELECT_PIN != OFF + if (command[1] == 'S' && parameter[0] == 'P' && parameter[1] == 'F' && parameter[2] == 'L' && parameter[3] == 'A' && parameter[4] == 'S' && parameter[5] == 'H' && parameter[6] == 0) { + SERIAL_A.println("The SWS microcontroller will now be connected to USB for flashing firmware."); + SERIAL_A.println("Once done uploading power cycle or reboot to restore normal operation."); + tasks.yield(1000); + pinModeEx(ADDON_SELECT_PIN, OUTPUT); + digitalWriteEx(ADDON_SELECT_PIN, LOW); + } else + #endif + *commandError = CE_CMD_UNKNOWN; + } else + + if (command[0] == 'G') { + // :GVD# Get OnStepX Firmware Date + // Returns: MTH DD YYYY# + // :GVM# General Message + // Returns: s# (where s is a string up to 16 chars) + // :GVN# Get Firmware Number + // Returns: M.mp# + // :GVP# Get Product Name + // Returns: s# + // :GVT# Get Firmware Time + // Returns: HH:MM:SS# + // :GVC# Get Firmware Config Name + // Returns: s# + // :GVH# Get Firmware Hardware + // Returns: s# + if (command[1] == 'V' && parameter[1] == 0) { + if (parameter[0] == 'D') strcpy(reply, firmware.date); else + if (parameter[0] == 'M') sprintf(reply, "%s %i.%02i%s", firmware.name, firmware.version.major, firmware.version.minor, firmware.version.patch); else + if (parameter[0] == 'N') sprintf(reply, "%i.%02i%s", firmware.version.major, firmware.version.minor, firmware.version.patch); else + if (parameter[0] == 'P') strcpy(reply, firmware.name); else + if (parameter[0] == 'T') strcpy(reply, firmware.time); else + if (parameter[0] == 'C') { strncpy(reply, PRODUCT_DESCRIPTION, 40); } else + if (parameter[0] == 'H') strcpy(reply, PINMAP_STR); else *commandError = CE_CMD_UNKNOWN; + *numericReply = false; + } else + + if (command[1] == 'X' && parameter[2] == 0) { + if (parameter[0] == '9') { + // :GX9A# temperature in deg. C + // Returns: +/-n.n + if (parameter[1] == 'A') { + sprintF(reply, "%3.1f", weather.getTemperature()); + *numericReply = false; + } else + + // :GX9B# pressure in mb + // Returns: +/-n.n + if (parameter[1] == 'B') { + sprintF(reply, "%3.1f", weather.getPressure()); + *numericReply = false; + } else + + // :GX9C# relative humidity in % + // Returns: +/-n.n + if (parameter[1] == 'C') { + sprintF(reply, "%3.1f", weather.getHumidity()); + *numericReply = false; + } else + + // :GX9E# dew point in deg. C + // Returns: +/-n.n + if (parameter[1] == 'E') { + sprintF(reply, "%3.1f", weather.getDewPoint()); + *numericReply = false; + } else + + // :GX9F# MCU temperature in deg. C + // Returns: +/-n.n + if (parameter[1] == 'F') { + if (!isnan(telescope.mcuTemperature)) { + sprintF(reply, "%1.0f", telescope.mcuTemperature); + *numericReply = false; + } else { + *numericReply = true; + *commandError = CE_0; + } + } else return false; + } else return false; + + } else return false; + } else + + if (command[0] == 'S' && command[1] == 'X' && parameter[2] == ',') { + if (parameter[0] == '9') { + char *conv_end; + float f = strtod(¶meter[3], &conv_end); + if (¶meter[3] == conv_end) f = NAN; + // :SX9A,[sn.n]# + // Set temperature in deg. C + // Return: 0 failure, 1 success + if (parameter[1] == 'A') { + if (!weather.setTemperature(f)) *commandError = CE_PARAM_RANGE; + } else + // :SX9B,[n.n]# + // Set pressure in mb + // Return: 0 failure, 1 success + if (parameter[1] == 'B') { + if (!weather.setPressure(f)) *commandError = CE_PARAM_RANGE; + } else + // :SX9C,[n.n]# + // relative humidity in % + // Return: 0 failure, 1 success + if (parameter[1] == 'C') { + if (!weather.setHumidity(f)) *commandError = CE_PARAM_RANGE; + } else return false; + } else return false; + } else return false; + + return true; +} diff --git a/src/telescope/Telescope.cpp b/src/telescope/Telescope.cpp new file mode 100755 index 0000000..1bcfafa --- /dev/null +++ b/src/telescope/Telescope.cpp @@ -0,0 +1,334 @@ +//-------------------------------------------------------------------------------------------------- +// OnStepX telescope control + +#include "../Common.h" + +#include "../lib/tasks/OnTask.h" +#include "../lib/gpioEx/GpioEx.h" +#include "../lib/nv/Nv.h" +#include "../lib/analog/Analog.h" +#include "../lib/convert/Convert.h" +#include "../lib/canPlus/CanPlus.h" + +#include "../libApp/commands/ProcessCmds.h" +#include "../libApp/weather/Weather.h" +#include "../libApp/temperature/Temperature.h" + +#include "Telescope.h" + +#include "addonFlasher/AddonFlasher.h" +#include "mount/Mount.h" +#include "mount/goto/Goto.h" +#include "mount/guide/Guide.h" +#include "mount/home/Home.h" +#include "mount/library/Library.h" +#include "mount/limits/Limits.h" +#include "mount/park/Park.h" +#include "mount/pec/Pec.h" +#include "mount/site/Site.h" +#include "mount/status/Status.h" +#include "rotator/Rotator.h" +#include "focuser/Focuser.h" +#include "auxiliary/Features.h" + +bool xBusy = false; +InitError initError; + +void mcuTempWrapper() { telescope.mcuTemperature = (telescope.mcuTemperature*9.0F + HAL_TEMP())/10.0F; } + +#if STATUS_LED != OFF && STATUS_LED_PIN != OFF + void statusFlash() { + static uint8_t cycle = 0; + if (cycle++ > 16) cycle = 0; + + #if GPIO_DEVICE != OFF + if (gpio.lateInitError) initError.gpio = true; + #endif + + // show only the most severe error (in order) + uint8_t flashes = 0; + if (initError.nv) flashes = 1; else + if (initError.value) flashes = 2; else + if (initError.gpio) flashes = 3; else + if (initError.driver) flashes = 4; else + if (initError.tls) flashes = 5; else + if (initError.weather) flashes = 6; + + int pin = STATUS_LED_PIN; + + // everything is ok, turn on LED and exit + if (flashes == 0) { digitalWriteEx(pin, STATUS_LED_ON_STATE); return; } + + // flash the LED if there's an error + if (cycle%2 == 0) { digitalWriteEx(pin, !STATUS_LED_ON_STATE); } else { if (cycle/2 < flashes) digitalWriteEx(pin, STATUS_LED_ON_STATE); } + } +#endif + +Telescope::Telescope() { + // early init for stepper drivers + #ifdef AXIS1_STEP_DIR_PRESENT + if (AXIS1_STEP_PIN >= 0 && AXIS1_STEP_PIN <= 255) pinMode(AXIS1_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS2_STEP_DIR_PRESENT + if (AXIS2_STEP_PIN >= 0 && AXIS2_STEP_PIN <= 255) pinMode(AXIS2_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS3_STEP_DIR_PRESENT + if (AXIS3_STEP_PIN >= 0 && AXIS3_STEP_PIN <= 255) pinMode(AXIS3_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS4_STEP_DIR_PRESENT + if (AXIS4_STEP_PIN >= 0 && AXIS4_STEP_PIN <= 255) pinMode(AXIS4_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS5_STEP_DIR_PRESENT + if (AXIS5_STEP_PIN >= 0 && AXIS5_STEP_PIN <= 255) pinMode(AXIS5_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS6_STEP_DIR_PRESENT + if (AXIS6_STEP_PIN >= 0 && AXIS6_STEP_PIN <= 255) pinMode(AXIS6_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS7_STEP_DIR_PRESENT + if (AXIS7_STEP_PIN >= 0 && AXIS7_STEP_PIN <= 255) pinMode(AXIS7_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS8_STEP_DIR_PRESENT + if (AXIS8_STEP_PIN >= 0 && AXIS8_STEP_PIN <= 255) pinMode(AXIS8_STEP_PIN, OUTPUT); + #endif + #ifdef AXIS9_STEP_DIR_PRESENT + if (AXIS9_STEP_PIN >= 0 && AXIS9_STEP_PIN <= 255) pinMode(AXIS9_STEP_PIN, OUTPUT); + #endif + + if (AXIS1_ENABLE_PIN >= 0 && AXIS1_ENABLE_PIN <= 255) { pinMode(AXIS1_ENABLE_PIN, OUTPUT); digitalWrite(AXIS1_ENABLE_PIN, !AXIS1_ENABLE_STATE); } + if (AXIS2_ENABLE_PIN >= 0 && AXIS2_ENABLE_PIN <= 255) { pinMode(AXIS2_ENABLE_PIN, OUTPUT); digitalWrite(AXIS2_ENABLE_PIN, !AXIS2_ENABLE_STATE); } + if (AXIS3_ENABLE_PIN >= 0 && AXIS3_ENABLE_PIN <= 255) { pinMode(AXIS3_ENABLE_PIN, OUTPUT); digitalWrite(AXIS3_ENABLE_PIN, !AXIS3_ENABLE_STATE); } + if (AXIS4_ENABLE_PIN >= 0 && AXIS4_ENABLE_PIN <= 255) { pinMode(AXIS4_ENABLE_PIN, OUTPUT); digitalWrite(AXIS4_ENABLE_PIN, !AXIS4_ENABLE_STATE); } + if (AXIS5_ENABLE_PIN >= 0 && AXIS5_ENABLE_PIN <= 255) { pinMode(AXIS5_ENABLE_PIN, OUTPUT); digitalWrite(AXIS5_ENABLE_PIN, !AXIS5_ENABLE_STATE); } + if (AXIS6_ENABLE_PIN >= 0 && AXIS6_ENABLE_PIN <= 255) { pinMode(AXIS6_ENABLE_PIN, OUTPUT); digitalWrite(AXIS6_ENABLE_PIN, !AXIS6_ENABLE_STATE); } + if (AXIS7_ENABLE_PIN >= 0 && AXIS7_ENABLE_PIN <= 255) { pinMode(AXIS7_ENABLE_PIN, OUTPUT); digitalWrite(AXIS7_ENABLE_PIN, !AXIS7_ENABLE_STATE); } + if (AXIS8_ENABLE_PIN >= 0 && AXIS8_ENABLE_PIN <= 255) { pinMode(AXIS8_ENABLE_PIN, OUTPUT); digitalWrite(AXIS8_ENABLE_PIN, !AXIS8_ENABLE_STATE); } + if (AXIS9_ENABLE_PIN >= 0 && AXIS9_ENABLE_PIN <= 255) { pinMode(AXIS9_ENABLE_PIN, OUTPUT); digitalWrite(AXIS9_ENABLE_PIN, !AXIS9_ENABLE_STATE); } + #ifdef SHARED_ENABLE_PIN + if (SHARED_ENABLE_PIN >= 0 && SHARED_ENABLE_PIN <= 255) { pinMode(SHARED_ENABLE_PIN, OUTPUT); digitalWrite(SHARED_ENABLE_PIN, !SHARED_ENABLE_STATE); } + #endif + #ifdef SHARED_ENABLE_PIN2 + if (SHARED_ENABLE_PIN2 >= 0 && SHARED_ENABLE_PIN2 <= 255) { pinMode(SHARED_ENABLE_PIN2, OUTPUT); digitalWrite(SHARED_ENABLE_PIN2, !SHARED2_ENABLE_STATE); } + #endif + #ifdef SHARED_ENABLE_PIN3 + if (SHARED_ENABLE_PIN3 >= 0 && SHARED_ENABLE_PIN3 <= 255) { pinMode(SHARED_ENABLE_PIN3, OUTPUT); digitalWrite(SHARED_ENABLE_PIN3, !SHARED3_ENABLE_STATE); } + #endif +} + +void Telescope::init(const char *fwName, int fwMajor, int fwMinor, const char *fwPatch, int fwConfig) { + sstrcpy(firmware.name, fwName); + firmware.version.major = fwMajor; + firmware.version.minor = fwMinor; + sstrcpy(firmware.version.patch, fwPatch); + firmware.version.config = fwConfig; + sstrcpy(firmware.date, __DATE__); + sstrcpy(firmware.time, __TIME__); + + // -------------------------------------------------------------------------------------------------- + // start the NV/EEPROM subsystem for settings storage + initError.nv = false; + initError.value = false; + bool success = nv().init(); + NvVolume &nvVolume = nv().volume(); + + success = success && (nvVolume.mount("OnStepX", NV_VOLUME_SIGNATURE) == NvVolume::Status::Ok); + VF("MSG: Nv, volume "); + if (success) { VLF("'OnStepX' mounted"); } else { VLF("invalid/unformatted"); } + + if (!success) { + + // automatic kv partition sizing + uint32_t vSize = nvVolume.byteCount() - 32; + uint32_t kvSize = vSize; + if (vSize > 1039) { + + // start the volume format + success = nvVolume.formatBegin("OnStepX", NV_VOLUME_SIGNATURE) == NvVolume::Status::Ok; + if (success) { VF("MSG: Nv, volume 'OnStepX' format started ("); V(vSize); VLF(" bytes)"); } + + // first add the KV partition + if (vSize < 1536) kvSize = 1040; // 1080 class (no PEC/Lib) + else if (vSize < 3072) kvSize = 1536; // 2KB class (~0.5K for Lib) + else if (vSize < 6144) kvSize = 2048; // 4KB class (~2K for PEC/Lib) + else if (vSize < 16384) kvSize = 3072; // 8KB class (~5K for PEC/Lib) + else kvSize = 4080; // and up + + success = success && nvVolume.formatAddPartition("KV", kvSize); + if (success) { VF("MSG: Nv, volume format added 'KV' partition ("); V(kvSize); VLF(" bytes)"); } + + // next any PEC partition + uint32_t pecSize = 0; + #if AXIS1_PEC == ON + pecSize = ((PEC_BUFFER_SIZE_LIMIT) >> 4) << 4; + if (kvSize + pecSize > vSize) { pecSize = 0; } + + if (pecSize > 15) { + success = success && nvVolume.formatAddPartition("PEC", pecSize); + if (success) { VF("MSG: Nv, volume format added 'PEC' partition ("); V(pecSize); VLF(" bytes)"); } + } + #endif + + // next any Library partition + uint32_t libSize = vSize - pecSize - kvSize; + if (libSize >= 16) { + success = success && nvVolume.formatAddPartition("LIBRARY", 0); + if (success) { VF("MSG: Nv, volume format added 'LIBRARY' partition ("); V(libSize); VLF(" bytes)"); } + } + + // finally commit the volume format + success = success && (nvVolume.formatCommit() == NvVolume::Status::Ok); + if (success) { VLF("MSG: Nv, volume format done"); } + + // try to mount the volume again + success = success && (nvVolume.mount("OnStepX", NV_VOLUME_SIGNATURE) == NvVolume::Status::Ok); + if (success) { VLF("MSG: Nv, volume 'OnStepX' mounted"); } else { DLF("WRN: Nv, volume 'OnStepX' mount FAILED!"); } + + } else { + DLF("WRN: Nv, volume compatible storage device NOT FOUND!"); + success = false; + } + } + + // Bind global KV instance to the KV partition index + success = success && (nv().kv().init(nvVolume, "KV") == KvPartition::Status::Ok); + if (success) { VLF("MSG: Nv, partition 'KV' mounted"); } else { DLF("WRN: Nv, partition 'KV' mount FAILED!"); } + + if (!success) { + VLF("WRN: Nv, init FAILED!"); + initError.nv = true; + } + nv().kv().resetInitErrorFlag(); + // -------------------------------------------------------------------------------------------------- + + #if RETICLE_LED_DEFAULT >= 0 && RETICLE_LED_PIN != OFF + analog.pwmInit(RETICLE_LED_DEFAULT); + #endif + + #ifdef USES_HW_SPI + SPI.begin(); + #endif + + #if CAN_PLUS != OFF + canPlus.init(); + #endif + + #if GPIO_DEVICE != OFF + if (!gpio.init()) initError.gpio = true; + #endif + + #ifdef SHARED_ENABLE_PIN + pinModeEx(SHARED_ENABLE_PIN, OUTPUT); + digitalWriteEx(SHARED_ENABLE_PIN, !SHARED_ENABLE_STATE); + #endif + #ifdef SHARED_ENABLE_PIN2 + pinModeEx(SHARED_ENABLE_PIN2, OUTPUT); + digitalWriteEx(SHARED_ENABLE_PIN2, !SHARED2_ENABLE_STATE); + #endif + #ifdef SHARED_ENABLE_PIN3 + pinModeEx(SHARED_ENABLE_PIN3, OUTPUT); + digitalWriteEx(SHARED_ENABLE_PIN3, !SHARED3_ENABLE_STATE); + #endif + + #if SERIAL_B_ESP_FLASHING == ON + addonFlasher.init(); + #endif + + mcuTemperature = HAL_TEMP(); + if (!isnan(mcuTemperature)) { + VF("MSG: System, start MCU temperature monitor task (rate 500ms priority 7)... "); + if (tasks.add(500, 0, true, 6, mcuTempWrapper, "SysTemp")) { VLF("success"); } else { VLF("FAILED!"); } + } + weather.init(); + temperature.init(); + + #if OPERATIONAL_MODE == WIFI && WEB_SERVER == ON + wifiManager.init(); + #endif + + #ifdef MOUNT_PRESENT + #if defined(ESP32) && STATUS_BUZZER >= 0 + // hack to trigger one-time ESP32 code debug message to get it out of the way early + tone(STATUS_BUZZER_PIN, STATUS_BUZZER, 1); + #endif + mount.init(); + mountStatus.init(); + #endif + + #if defined(ROTATOR_PRESENT) || defined(ROTATOR_CLIENT_PRESENT) + rotator.init(); + #endif + + #if defined(FOCUSER_PRESENT) || defined(FOCUSER_CLIENT_PRESENT) + focuser.init(); + #endif + + #if defined(FEATURES_PRESENT) || defined(FEATURES_CLIENT_PRESENT) + features.init(); + #endif + + delay(1000); + + #ifdef SHARED_ENABLE_PIN + digitalWriteEx(SHARED_ENABLE_PIN, SHARED_ENABLE_STATE); + #endif + #ifdef SHARED_ENABLE_PIN2 + digitalWriteEx(SHARED_ENABLE_PIN2, SHARED2_ENABLE_STATE); + #endif + #ifdef SHARED_ENABLE_PIN3 + digitalWriteEx(SHARED_ENABLE_PIN3, SHARED3_ENABLE_STATE); + #endif + + #ifdef MOUNT_PRESENT + mount.begin(); + #endif + + #if defined(ROTATOR_PRESENT) || defined(ROTATOR_CLIENT_PRESENT) + rotator.begin(); + #endif + + #if defined(FOCUSER_PRESENT) || defined(FOCUSER_CLIENT_PRESENT) + focuser.begin(); + #endif + + #if defined(FEATURES_PRESENT) || defined(FEATURES_CLIENT_PRESENT) + features.begin(); + #endif + + #if RETICLE_LED_DEFAULT != OFF && RETICLE_LED_PIN != OFF + #if RETICLE_LED_MEMORY == ON + if (!nv().kv().getOrInit("TELESCOPE_SETTINGS", reticleBrightness)) { DLF("WRN: Nv, init failed for TELESCOPE_SETTINGS"); } + reticleBrightness = constrain(reticleBrightness, 0, 255); + #endif + + pinMode(RETICLE_LED_PIN, OUTPUT); + + float duty = (float)reticleBrightness*(1.0F/255.0F); + analog.write(RETICLE_LED_PIN, RETICLE_LED_INVERT == ON ? duty : 1.0F - duty); + #endif + + // bring up status LED and flash error codes + #if STATUS_LED != OFF && STATUS_LED_PIN != OFF + int pin = STATUS_LED_PIN; + pinModeEx(pin, OUTPUT); + VF("MSG: Telescope, start status LED task (rate 500ms priority 4)... "); + if (tasks.add(500, 0, true, 4, statusFlash, "StaLed")) { VLF("success"); } else { VLF("FAILED!"); } + #endif + + // -------------------------------------------------------------------------------------------------- + // init is done let the user see what's in the KV + #if DEBUG != OFF + KvPartition::Stats stats; + if (success && nv().kv().stats(stats) == KvPartition::Status::Ok) { + VF("MSG: Nv, partition 'KV' data blocks used = "); + V(stats.dataBlocksTotal - stats.dataBlocksFree); VF(" (of "); V(stats.dataBlocksTotal); VF(")"); + VF(" key slots used = "); + V(stats.slotsTotal - stats.slotsFree); VF(" (of "); V(stats.slotsTotal); VLF(")"); + } + #endif + + // and capture any errors + if (nv().kv().getInitErrorFlag()) initError.nv = true; + // -------------------------------------------------------------------------------------------------- + +} + +Telescope telescope; diff --git a/src/telescope/Telescope.h b/src/telescope/Telescope.h new file mode 100755 index 0000000..baea0ed --- /dev/null +++ b/src/telescope/Telescope.h @@ -0,0 +1,68 @@ +//-------------------------------------------------------------------------------------------------- +// OnStepX telescope control +#pragma once + +#include "../Common.h" +#include "../libApp/commands/ProcessCmds.h" + +#ifndef ANALOG_WRITE_RANGE + #define ANALOG_WRITE_RANGE 255 +#endif + +enum ParkState: uint8_t {PS_UNPARKED, PS_PARKING, PS_PARKED, PS_PARK_FAILED, PS_UNPARKING}; + +typedef struct InitError { + uint8_t nv:1; // NV data size/structure error (disables writes to NV) + uint8_t value:1; // invalid value error + uint8_t driver:1; // the stepper driver wasn't detected properly (TMC SPI drivers only) + uint8_t weather:1; // the weather source (BME280, etc.) was not found + uint8_t tls:1; // the time location source (DS3231, GPS, etc.) was not found + uint8_t gpio:1; // the GPIO device was not found +} InitError; + +extern InitError initError; + +typedef struct Version { + uint8_t major; + uint8_t minor; + char patch[2]; + uint8_t config; +} Version; + +typedef struct Firmware { + char name[8]; + Version version; + char date[20]; + char time[20]; +} Firmware; + +class Telescope { + public: + Telescope(); + + // setup the location, time keeping, and coordinate converson + void init(const char *fwName, int fwMajor, int fwMinor, const char *fwPatch, int fwConfig); + + // update the location for time keeping and coordinate conversion + void updateSite(); + + // handle commands + bool command(char reply[], char command[], char parameter[], bool *suppressFrame, bool *numericReply, CommandError *commandError); + + void statusInit(); + + bool ready = false; + + float mcuTemperature; + + private: + Firmware firmware; + int16_t reticleBrightness = RETICLE_LED_DEFAULT; +}; + +// true during timing sensitive operations (for disabling I2C etc.) +extern bool xBusy; + +#define analog8BitToAnalogRange(v) ((v)*ANALOG_WRITE_RANGE/255) + +extern Telescope telescope; diff --git a/src/telescope/addonFlasher/AddonFlasher.cpp b/src/telescope/addonFlasher/AddonFlasher.cpp new file mode 100755 index 0000000..dd67170 --- /dev/null +++ b/src/telescope/addonFlasher/AddonFlasher.cpp @@ -0,0 +1,135 @@ +// Flashes an esp8266 based SWS on the SERIAL_PASSTHROUGH interface by passing data to/from SERIAL_A + +#include "AddonFlasher.h" + +#include "../../lib/tasks/OnTask.h" +#include "../../lib/gpioEx/GpioEx.h" + +#if SERIAL_PASSTHROUGH == HardSerial +#define SERIAL_PASSTHROUGH_RXTX_SET +extern HardwareSerial HWSerialB; +#undef SERIAL_PASSTHROUGH +#define SERIAL_PASSTHROUGH HWSerialB +#endif + +// ADDON_TRIGR_PIN HIGH for run and LOW for trigger serial passthrough mode +// ADDON_RESET_PIN HIGH for run and LOW for reset +// ADDON_GPIO0_PIN HIGH for run and LOW for firmware upload + +#if defined(SERIAL_PASSTHROUGH) + + #if ADDON_TRIGR_PIN != OFF + void addonFlasherWrapper() { addonFlasher.poll(); } + #endif + + void AddonFlasher::init() { + VF("MSG: AddonFlasher, init gpio0="); V(ADDON_GPIO0_PIN); VF(", reset="); VL(ADDON_RESET_PIN); + pinModeEx(ADDON_GPIO0_PIN, OUTPUT); + pinModeEx(ADDON_RESET_PIN, OUTPUT); + + run(); + + #if ADDON_TRIGR_PIN != OFF + pinModeEx(ADDON_TRIGR_PIN, INPUT); + VF("MSG: AddonFlasher, trigger pin monitor (rate 100ms priority 5)... "); + if (tasks.add(100, 0, true, 5, addonFlasherWrapper, "adonTgr")) { VLF("success"); } else { VLF("FAILED!"); } + #endif + } + + void AddonFlasher::go(bool timeout) { + // program mode and 115200 serial comms + flash(); + + VLF("MSG: AddonFlasher, activating serial passthrough..."); + + // so we have a total of 1.5 minutes to start the upload + unsigned long lastRead = millis() + 85000U; + while (true) { + // read from port 1, send to port 0: + if (SERIAL_PASSTHROUGH.available()) { + int inByte = SERIAL_PASSTHROUGH.read(); + delayMicroseconds(5); + SERIAL_A.write(inByte); + delayMicroseconds(5); + } + + // read from port 0, send to port 1: + if (SERIAL_A.available()) { + int inByte = SERIAL_A.read(); + delayMicroseconds(5); + SERIAL_PASSTHROUGH.write(inByte); + delayMicroseconds(5); + if ((long)(millis() - lastRead > 0)) lastRead = millis(); + } + //tasks.yield(); + + // wait 30 seconds w/no traffic before resuming normal operation + if (timeout && (millis() - lastRead > 30000U)) break; + } + VLF("MSG: AddonFlasher, serial passthrough deactivated"); + + // back to run mode and normal serial comms + run(true); + } + + void AddonFlasher::run(bool setSerial) { + VLF("MSG: AddonFlasher, setting addon run mode"); + + // enter run mode + digitalWriteEx(ADDON_GPIO0_PIN, HIGH); + reset(); + + if (setSerial) { + #if defined(SERIAL_PASSTHROUGH_RX) && defined(SERIAL_PASSTHROUGH_TX) && !defined(SERIAL_PASSTHROUGH_RXTX_SET) + SERIAL_PASSTHROUGH.begin(SERIAL_PASSTHROUGH_BAUD_DEFAULT, SERIAL_8N1, SERIAL_PASSTHROUGH_RX, SERIAL_PASSTHROUGH_TX); + #else + SERIAL_PASSTHROUGH.begin(SERIAL_PASSTHROUGH_BAUD_DEFAULT); + #endif + + #if defined(SERIAL_A_RX) && defined(SERIAL_A_TX) && !defined(SERIAL_A_RXTX_SET) + SERIAL_A.begin(SERIAL_A_BAUD_DEFAULT, SERIAL_8N1, SERIAL_A_RX, SERIAL_A_TX); + #else + SERIAL_A.begin(SERIAL_A_BAUD_DEFAULT); + #endif + tasks.yield(1000); + } + } + + void AddonFlasher::flash() { + VLF("MSG: AddonFlasher, setting addon program mode"); + + #if defined(SERIAL_A_RX) && defined(SERIAL_A_TX) && !defined(SERIAL_A_RXTX_SET) + SERIAL_A.begin(115200, SERIAL_8N1, SERIAL_A_RX, SERIAL_A_TX); + #else + SERIAL_A.begin(115200); + #endif + + #if defined(SERIAL_PASSTHROUGH_RX) && defined(SERIAL_PASSTHROUGH_TX) && !defined(SERIAL_PASSTHROUGH_RXTX_SET) + SERIAL_PASSTHROUGH.begin(115200, SERIAL_8N1, SERIAL_PASSTHROUGH_RX, SERIAL_PASSTHROUGH_TX); + #else + SERIAL_PASSTHROUGH.begin(115200); + #endif + tasks.yield(1000); + + // enter program mode + digitalWriteEx(ADDON_GPIO0_PIN, LOW); + reset(); + } + + void AddonFlasher::reset() { + // reset LOW (active) HIGH (inactive) + tasks.yield(200); + digitalWriteEx(ADDON_RESET_PIN, LOW); + tasks.yield(200); + digitalWriteEx(ADDON_RESET_PIN, HIGH); + tasks.yield(200); + } + + void AddonFlasher::poll() { + #if ADDON_TRIGR_PIN == ON + if (ADDON_TRIGR_PIN != OFF && digitalRead(ADDON_TRIGR_PIN) == LOW) go(false); + #endif + } + + AddonFlasher addonFlasher; +#endif diff --git a/src/telescope/addonFlasher/AddonFlasher.h b/src/telescope/addonFlasher/AddonFlasher.h new file mode 100755 index 0000000..97c8f69 --- /dev/null +++ b/src/telescope/addonFlasher/AddonFlasher.h @@ -0,0 +1,29 @@ +// Flashes an esp8266 based SWS on the SERIAL_B interface by passing data to/from SERIAL_A +#pragma once +#include "../../Common.h" + +#if defined(SERIAL_PASSTHROUGH) + class AddonFlasher { + public: + // startup the addon flasher, monitors the ADDON_TRIGR_PIN if present + void init(); + + // flashes the addon + void go(bool timeout = true); + + // set the addon for run mode + void run(bool setSerial = false); + + // set the addon for flash mode + void flash(); + + // reset the addon + void reset(); + + // monitors for flash button press + void poll(); + private: + }; + + extern AddonFlasher addonFlasher; +#endif diff --git a/src/telescope/auxiliary/Features.h b/src/telescope/auxiliary/Features.h new file mode 100755 index 0000000..c652775 --- /dev/null +++ b/src/telescope/auxiliary/Features.h @@ -0,0 +1,21 @@ +//-------------------------------------------------------------------------------------------------- +// local and remote Auxiliary Features control switchboard +// ----------------------------------------------------------------------------------- +#pragma once + +#include "../../Common.h" + +#include "FeaturesBase.h" + +// ----------------------------------------------------------------------------- +// Include the chosen implementation +// - CLIENT: Features is a proxy class +// - otherwise (local present): Features is the local hardware class +// and REMOTE (server) support is compiled in as additional code paths. +// ----------------------------------------------------------------------------- +#if defined(FEATURES_CAN_CLIENT_PRESENT) + #include "remote/Features.h" // defines class Features (proxy) +#elif defined(FEATURES_PRESENT) + #include "local/Features.h" // defines class Features (local hardware) + // conditionally includes/declares server hooks when FEATURES_CAN_SERVER_PRESENT is defined +#endif diff --git a/src/telescope/auxiliary/FeaturesBase.h b/src/telescope/auxiliary/FeaturesBase.h new file mode 100755 index 0000000..26df391 --- /dev/null +++ b/src/telescope/auxiliary/FeaturesBase.h @@ -0,0 +1,108 @@ +// ----------------------------------------------------------------------------- +// CAN Auxiliary Features switchboard +// ----------------------------------------------------------------------------- +#pragma once + +#include "../../Common.h" + +// ----------------------------------------------------------------------------- +// Where the base CAN ID's start for this CANbus transport +// ----------------------------------------------------------------------------- + +#ifndef CAN_FEATURES_REQ_ID + #define CAN_FEATURES_REQ_ID (0x0F + 0x00) +#endif + +#ifndef CAN_FEATURES_RSP_ID + #define CAN_FEATURES_RSP_ID (CAN_FEATURES_REQ_ID + 0x10) +#endif + +#ifndef CAN_FEATURES_HB_ID_BASE + #define CAN_FEATURES_HB_ID_BASE (CAN_FEATURES_REQ_ID + 0x20) +#endif + +// ----------------------------------------------------------------------------- +// CAN_FEATURES role selection +// OFF : no CAN auxiliary feature support +// ON : telescope forwards auxiliary feature commands over CAN (client/proxy) +// REMOTE : remote node answers auxiliary feature commands over CAN (server/local FEATURES) +// ----------------------------------------------------------------------------- + +#ifndef CAN_FEATURES + #define CAN_FEATURES OFF +#endif + +// ----------------------------------------------------------------------------- +// Validate CAN requirements +// ----------------------------------------------------------------------------- +#if (CAN_FEATURES != OFF) + #if !defined(CAN_PLUS) || (CAN_PLUS == OFF) + #error "Features: CAN_FEATURES requires CAN_PLUS to be enabled." + #endif +#endif + +// ----------------------------------------------------------------------------- +// Derive presence macros used by Telescope.cpp command router +// ----------------------------------------------------------------------------- +#if (CAN_FEATURES == ON) + // Local FEATURES_PRESENT must NOT exist in this build; commands are proxied. + #define FEATURES_CLIENT_PRESENT // remote auxiliary features may be present + #define FEATURES_CAN_CLIENT_PRESENT // client implementation using CAN +#elif (CAN_FEATURES == REMOTE) + // Must have a local Features implementation to serve anything. + #define FEATURES_CAN_SERVER_PRESENT // server implementation (compiled into local Features) +#endif + +// ----------------------------------------------------------------------------- +// Enforce “only one auxiliary-features provider” contract +// ----------------------------------------------------------------------------- + +// CLIENT build must NOT also have local FEATURES_PRESENT. +#if defined(FEATURES_PRESENT) && defined(FEATURES_CLIENT_PRESENT) + #error "Config: both local FEATURES_PRESENT and CAN_FEATURES ON are enabled. Choose one." +#endif + +// REMOTE build MUST have local FEATURES_PRESENT (otherwise it can't serve anything). +#if !defined(FEATURES_PRESENT) && defined(FEATURES_CAN_SERVER_PRESENT) + #error "Config: CAN_FEATURES REMOTE requires local FEATURES_PRESENT." +#endif + +// ----------------------------------------------------------------------------- +// CANbus command opcode set (5-bit). Per-ID namespace, so Features "owns" these. +// These ops map to the LX200-ish AF commands shown in Features.command.cpp. +// ----------------------------------------------------------------------------- + +// Suggested v1 set (keep lean; expand as needed): +#define FEAT_OP_GET_ACTIVE_Y0 0x01 // :GXY0# -> RX: bitmap of active features (8 bits) +#define FEAT_OP_SET_VALUE_Xn 0x02 // :SXXn,* -> RX: status only (single frame) +#define FEAT_OP_GET_INFO_Yn 0x1E // :GXYn# -> RX: purpose + short name (2-frame) +#define FEAT_OP_GET_VALUE_Xn 0x1F // :GXXn# -> RX: per-purpose packed values (2-frame) + +// ----------------------------------------------------------------------------- +// Power telemetry packing (appended to :GXXn# replies when POWER_MONITOR_PRESENT) +// ----------------------------------------------------------------------------- +// Encoding matches the LX200 :GXXn# ASCII additions from Features::strCatPower(): +// ,,, +// where / are formatted as %1.1f or "NAN" and is 5 chars: +// P (present) or !, then C/v/V/T each replaced by ! on fault. +// +// On-wire, we use signed int16 fixed-point with 0.1 units (LE), and a flags bitfield. +// +// Voltage/current encoding: +// enc = round(value * 10) +// enc = FEAT_POWER_NAN_I16 (0x8000) means NAN +// +// Flags bitfield (fault bits are 1 when faulted): +// bit0: present (P) +// bit1: over-current fault (C -> !) +// bit2: under-voltage fault (v -> !) +// bit3: over-voltage fault (V -> !) +// bit4: over-temperature fault (T -> !) +#define FEAT_POWER_NAN_I16 0x8000 +#define FEAT_POWER_FLAGS_PRESENT 0x01 +#define FEAT_POWER_FAULT_OC 0x02 +#define FEAT_POWER_FAULT_UV 0x04 +#define FEAT_POWER_FAULT_OV 0x08 +#define FEAT_POWER_FAULT_OT 0x10 + +// (Reserve additional opcodes here for future AF expansions.) diff --git a/src/telescope/auxiliary/dewHeater/DewHeater.cpp b/src/telescope/auxiliary/dewHeater/DewHeater.cpp new file mode 100755 index 0000000..58e4d58 --- /dev/null +++ b/src/telescope/auxiliary/dewHeater/DewHeater.cpp @@ -0,0 +1,92 @@ +// ----------------------------------------------------------------------------------- +// Dew Heater control + +#include "DewHeater.h" + +#ifdef FEATURES_PRESENT + +#include "../../../lib/nv/Nv.h" + +void DewHeater::init(uint8_t index) { + if (index > 7) return; + this->index = index; + + char keyStr[26]; + snprintf(keyStr, sizeof(keyStr), "FEATURE%u_DEW_SETTINGS", index); + nvKey = nv().kv().computeKey(keyStr); + + // write the default settings to NV + if (!nv().kv().getOrInit(nvKey, settings)) { DF("WRN: Nv, init failed for "); VL(keyStr); } + settings.zero = constrain(settings.zero, -5, 20); + settings.span = constrain(settings.span, settings.zero, 20); + + if (settings.zero >= settings.span) { + if (settings.span > -5.0) settings.zero = settings.span - 0.1; else settings.span = settings.zero + 0.1; + DLF("ERR: dewHeater::init(), NV zero >= span (corrected)"); + } +} + +void DewHeater::poll(float deltaAboveDewPointC) { + if (isnan(deltaAboveDewPointC)) { heaterOn = false; return; } + + if (!enabled) return; + + int switchTimeMs = 0; + switchTimeMs = map(lroundf(deltaAboveDewPointC*10.0F), lroundf(settings.zero*10.0F), lroundf(settings.span*10.0F), DEW_HEATER_PULSE_WIDTH_MS, 0); + switchTimeMs = constrain(switchTimeMs, 0, DEW_HEATER_PULSE_WIDTH_MS); + #ifdef DEW_HEATER_MAX_POWER + switchTimeMs = lroundf(switchTimeMs*(DEW_HEATER_MAX_POWER/100.0)); + #endif + + currentTime = millis(); + if ((long)(currentTime - (lastHeaterCycle + DEW_HEATER_PULSE_WIDTH_MS)) > 0) { + lastHeaterCycle = currentTime; + } else + + if (!heaterOn && (long)(currentTime - (lastHeaterCycle + switchTimeMs)) <= 0) { + heaterOn = true; + } else + + if (heaterOn && (long)(currentTime - (lastHeaterCycle + switchTimeMs)) > 0) { + heaterOn = false; + } +} + +float DewHeater::getZero() { + return settings.zero; +} + +void DewHeater::setZero(float t) { + if (t >= -5.0 && t <= 20.0) { + settings.zero = t; + if (settings.zero >= settings.span) settings.zero = settings.span - 0.1; + nv().kv().put(nvKey, settings); + } +} + +float DewHeater::getSpan() { + return settings.span; +} + +void DewHeater::setSpan(float t) { + if (t >= -5.0 && t <= 20.0) { + settings.span = t; + if (settings.span <= settings.zero) settings.span = settings.zero + 0.1; + nv().kv().put(nvKey, settings); + } +} + +bool DewHeater::isEnabled() { + return enabled; +} + +void DewHeater::enable(bool state) { + heaterOn = false; + enabled = state; +} + +bool DewHeater::isOn() { + return heaterOn; +} + +#endif diff --git a/src/telescope/auxiliary/dewHeater/DewHeater.h b/src/telescope/auxiliary/dewHeater/DewHeater.h new file mode 100755 index 0000000..2dcfcf7 --- /dev/null +++ b/src/telescope/auxiliary/dewHeater/DewHeater.h @@ -0,0 +1,55 @@ +// ----------------------------------------------------------------------------------- +// Dew Heater control +#pragma once + +#include "../../../Common.h" + +#if FEATURE1_PURPOSE != OFF || FEATURE2_PURPOSE != OFF || FEATURE3_PURPOSE != OFF || FEATURE4_PURPOSE != OFF || FEATURE5_PURPOSE != OFF || FEATURE6_PURPOSE != OFF || FEATURE7_PURPOSE != OFF || FEATURE8_PURPOSE != OFF + #ifndef FEATURES_PRESENT + #define FEATURES_PRESENT + #endif +#endif + +#ifdef FEATURES_PRESENT + +#ifndef DEW_HEATER_PULSE_WIDTH_MS + #define DEW_HEATER_PULSE_WIDTH_MS 2000 +#endif + +typedef struct FeatureDewSettings { + float zero; + float span; +} FeatureDewSettings; + +class DewHeater { + public: + void init(uint8_t index); + + void poll(float deltaAboveDewPointC); + + float getZero(); + void setZero(float t); + + float getSpan(); + void setSpan(float t); + + bool isEnabled(); + void enable(bool state); + + bool isOn(); + + private: + unsigned long lastHeaterCycle = 0; + unsigned long currentTime = 0; + + bool heaterOn = false; + bool enabled = false; + + FeatureDewSettings settings = {-5.0F, 15.0F}; + + uint8_t index = 0; + + uint32_t nvKey; +}; + +#endif \ No newline at end of file diff --git a/src/telescope/auxiliary/intervalometer/Intervalometer.cpp b/src/telescope/auxiliary/intervalometer/Intervalometer.cpp new file mode 100755 index 0000000..be3f5e0 --- /dev/null +++ b/src/telescope/auxiliary/intervalometer/Intervalometer.cpp @@ -0,0 +1,107 @@ +// ----------------------------------------------------------------------------------- +// Intervalometer control + +#include "Intervalometer.h" + +#ifdef FEATURES_PRESENT + +#include "../../../lib/nv/Nv.h" + +void Intervalometer::init(uint8_t index) { + if (index > 7) return; + this->index = index; + + char keyStr[26]; + snprintf(keyStr, sizeof(keyStr), "FEATURE%u_INTV_SETTINGS", index); + nvKey = nv().kv().computeKey(keyStr); + + if (!nv().kv().getOrInit(nvKey, settings)) { DF("WRN: Nv, init failed for "); VL(keyStr); } + settings.expCount = constrain(settings.expCount, 0, 255); + settings.expDelay = constrain(settings.expDelay, 1, 3600); + settings.expTime = constrain(settings.expTime, 0, 3600); +} + +void Intervalometer::poll() { + if (!enabled) return; + + if (pressed == P_EXP_START) { + // count and stop when done + if (thisCount == 0) { pressed = P_STANDBY; enabled = false; return; } + thisCount--; + + // start a new exposure + pressed = P_EXP_DONE; + expDone = millis() + (unsigned long)(settings.expTime*1000.0); // set exposure time in ms + } else + + // wait until exposure is done + if (pressed == P_EXP_DONE && (long)(millis() - expDone) > 0) { + // finish an exposure + pressed = P_WAIT; + waitDone = millis() + (unsigned long)(settings.expDelay*1000.0); // set wait time in ms + } else + + // wait until pause between exposures is done + if (pressed == P_WAIT && (long)(millis() - waitDone) > 0) { + // start next count + pressed = P_EXP_START; + } +} + +float Intervalometer::getExposure() { + return settings.expTime; +} + +void Intervalometer::setExposure(float t) { + if (pressed == P_STANDBY && t >= 0 && t <= 3600) { + settings.expTime = t; + nv().kv().put(nvKey, settings); + } +} + +float Intervalometer::getDelay() { + return settings.expDelay; +} + +void Intervalometer::setDelay(float t) { + if (pressed == P_STANDBY && t >= 1 && t <= 3600) { + settings.expDelay = t; + nv().kv().put(nvKey, settings); + } +} + +float Intervalometer::getCurrentCount() { + return thisCount; +} + +float Intervalometer::getCount() { + return settings.expCount; +} + +void Intervalometer::setCount(float count) { + long c = lroundf(count); + if (pressed == P_STANDBY && c >= 0 && c <= 255) { + settings.expCount = c; + } +} + +bool Intervalometer::isEnabled() { + return enabled; +} + +void Intervalometer::enable(bool state) { + enabled = state; + if (enabled) { + thisCount = settings.expCount; + pressed = P_EXP_START; + } else { + thisCount = 0; + pressed = P_STANDBY; + } +} + +bool Intervalometer::isOn() { + return pressed == P_EXP_DONE; +} + +#endif diff --git a/src/telescope/auxiliary/intervalometer/Intervalometer.h b/src/telescope/auxiliary/intervalometer/Intervalometer.h new file mode 100755 index 0000000..6f42841 --- /dev/null +++ b/src/telescope/auxiliary/intervalometer/Intervalometer.h @@ -0,0 +1,58 @@ +// ----------------------------------------------------------------------------------- +// Intervalometer control +#pragma once + +#include "../../../Common.h" + +#if FEATURE1_PURPOSE != OFF || FEATURE2_PURPOSE != OFF || FEATURE3_PURPOSE != OFF || FEATURE4_PURPOSE != OFF || FEATURE5_PURPOSE != OFF || FEATURE6_PURPOSE != OFF || FEATURE7_PURPOSE != OFF || FEATURE8_PURPOSE != OFF + #ifndef FEATURES_PRESENT + #define FEATURES_PRESENT + #endif +#endif + +#ifdef FEATURES_PRESENT + +typedef struct FeatureIntervalometerSettings { + float expTime; + float expDelay; + uint8_t expCount; +} FeatureIntervalometerSettings; + +class Intervalometer { + public: + void init(uint8_t index); + + void poll(); + + float getExposure(); + void setExposure(float t); + + float getDelay(); + void setDelay(float t); + + float getCurrentCount(); + float getCount(); + void setCount(float c); + + bool isEnabled(); + void enable(bool state); + + bool isOn(); + + private: + enum Pressed { P_STANDBY, P_EXP_START, P_EXP_START_PHASE2, P_EXP_DONE, P_EXP_DONE_PHASE2, P_WAIT }; + + Pressed pressed = P_STANDBY; + bool enabled = false; + + FeatureIntervalometerSettings settings = {0.0F, 0.0F, 0}; + uint8_t thisCount = 0; + + unsigned long expDone = 0; + unsigned long waitDone = 0; + + uint8_t index = 0; + + int16_t nvKey; +}; +#endif diff --git a/src/telescope/auxiliary/local/Features.can.command.cpp b/src/telescope/auxiliary/local/Features.can.command.cpp new file mode 100755 index 0000000..027476f --- /dev/null +++ b/src/telescope/auxiliary/local/Features.can.command.cpp @@ -0,0 +1,256 @@ +//-------------------------------------------------------------------------------------------------- +// local auxiliary features control, using the CANbus interface (CAN server) + +#include "Features.h" + +#if defined(FEATURES_CAN_SERVER_PRESENT) + +#include "../FeaturesBase.h" +#include "../../../lib/convert/Convert.h" +#include "../../../libApp/weather/Weather.h" +#include "../../../telescope/Telescope.h" + +// Dew heater: -5.0..+20.0C encoded as 0..250 in 0.1C steps (one byte). +static inline uint8_t packDew01C(float v) { + if (v < -5.0f) v = -5.0f; + if (v > 20.0f) v = 20.0f; + long enc = lroundf((v + 5.0f) * 10.0f); + if (enc < 0) enc = 0; + if (enc > 250) enc = 250; + return (uint8_t)enc; +} + +static inline float unpackDew01C(uint8_t enc) { + return ((float)enc / 10.0f) - 5.0f; +} + +void Features::processCommand() { + + bool handled = true; + bool numericReply = true; + bool suppressFrame = false; + CommandError commandError = CE_NONE; + + switch (opCode()) { + + // ------------------------------------------------------------ + // :GXY0# -> bitmap of active features (8 bits) + // Payload: [u8 bitmap] + // ------------------------------------------------------------ + case FEAT_OP_GET_ACTIVE_Y0: { + uint8_t bitmap = 0; + for (uint8_t i = 0; i < 8; i++) { + if (device[i].purpose != OFF) bitmap |= (uint8_t)(1U << i); + } + writeU8(bitmap); + numericReply = false; + } break; + + // ------------------------------------------------------------ + // :GXYn# (wire: idx 0..7) -> purpose + short name + // Payload: [u8 purpose][name bytes... up to 10] + // (No terminator; client adds it.) + // ------------------------------------------------------------ + case FEAT_OP_GET_INFO_Yn: { + uint8_t idx = 0; + if (!readU8(idx)) { commandError = CE_PARAM_FORM; break; } + if (idx > 7) { commandError = CE_PARAM_RANGE; break; } + + const int pRaw = device[idx].purpose; + if (pRaw == OFF) { commandError = CE_0; break; } + + // Match Features.command.cpp: report MOMENTARY/COVER as SWITCH in purpose field. + int p = pRaw; + if (p == MOMENTARY_SWITCH || p == COVER_SWITCH) p = SWITCH; + + writeU8((uint8_t)p); + + char name10[10]; + memset(name10, 0, sizeof(name10)); + sstrcpy(name10, device[idx].name); + + const uint8_t n = (uint8_t)strnlen(name10, 10); + if (n > 0) writeBytes((const uint8_t*)name10, n); + + // payload <= 11 bytes -> fits dual (12 max) + numericReply = false; + } break; + + // ------------------------------------------------------------ + // :GXXn# (wire: idx 0..7) -> per-purpose packed values + // Payload always begins with [u8 purpose] + // Then purpose-specific bytes: + // SWITCH/ANALOG: [u8 value] + // DEW_HEATER: [u8 en][u8 zEnc][u8 sEnc][u8 dEnc(0.5C int8 as u8)] + // INTERVALOMETER: [u8 cur][u8 expB][u8 delB][u8 count] + // Optional (when POWER_MONITOR_PRESENT): append 5 bytes: + // [i16 volts_q0.1][i16 amps_q0.1][u8 flags] + // Total stays <= 12. + // ------------------------------------------------------------ + case FEAT_OP_GET_VALUE_Xn: { + uint8_t idx = 0; + if (!readU8(idx)) { commandError = CE_PARAM_FORM; break; } + if (idx > 7) { commandError = CE_PARAM_RANGE; break; } + + const int p = device[idx].purpose; + if (p == OFF) { commandError = CE_CMD_UNKNOWN; break; } + + writeU8((uint8_t)p); + + if (p == SWITCH || p == MOMENTARY_SWITCH || p == COVER_SWITCH) { + writeU8((uint8_t)device[idx].value); + } else + + if (p == ANALOG_OUTPUT) { + writeU8((uint8_t)device[idx].value); + } else + + if (p == DEW_HEATER) { + const uint8_t en = (uint8_t)(device[idx].dewHeater->isEnabled() ? 1 : 0); + const uint8_t zEnc = packDew01C(device[idx].dewHeater->getZero()); + const uint8_t sEnc = packDew01C(device[idx].dewHeater->getSpan()); + const float delta = (float)(temperature.getChannel((int)idx + 1) - weather.getDewPoint()); + + writeU8(en); + writeU8(zEnc); + writeU8(sEnc); + writeFixedI16(delta, 10); + } else + + if (p == INTERVALOMETER) { + const uint8_t cur = (uint8_t)device[idx].intervalometer->getCurrentCount(); + const uint8_t expB = (uint8_t)convert.packSeconds(device[idx].intervalometer->getExposure()); + const uint8_t delB = (uint8_t)convert.packSeconds(device[idx].intervalometer->getDelay()); + const uint8_t count = (uint8_t)device[idx].intervalometer->getCount(); + + writeU8(cur); + writeU8(expB); + writeU8(delB); + writeU8(count); + } else { + // Unknown purpose: return stored value byte + writeU8((uint8_t)device[idx].value); + } + + // Power telemetry: always append when compiled in so client sees remaining()==5. + #ifdef POWER_MONITOR_PRESENT + uint8_t flags = 0; + + const bool present = powerMonitor.hasChannel((int)idx); + if (present) flags |= FEAT_POWER_FLAGS_PRESENT; + + if (powerMonitor.errOverCurrent((int)idx)) flags |= FEAT_POWER_FAULT_OC; + if (powerMonitor.errUnderVoltage((int)idx)) flags |= FEAT_POWER_FAULT_UV; + if (powerMonitor.errOverVoltage((int)idx)) flags |= FEAT_POWER_FAULT_OV; + if (powerMonitor.errOverTemperature((int)idx)) flags |= FEAT_POWER_FAULT_OT; + + float volts = NAN; + float amps = NAN; + if (powerMonitor.hasVoltage((int)idx)) volts = powerMonitor.getVoltage((int)idx); + if (powerMonitor.hasCurrent((int)idx)) amps = powerMonitor.getCurrent((int)idx); + + // Exactly 5 bytes appended + writeFixedI16(volts, 10); + writeFixedI16(amps, 10); + writeU8(flags); + #endif + + numericReply = false; + } break; + + // ------------------------------------------------------------ + // :SXXn,... (wire: [idx1 1..8][sub][u8 payload]) + // Response is numeric-only (numericReply=true). Success = CE_NONE. + // ------------------------------------------------------------ + case FEAT_OP_SET_VALUE_Xn: { + uint8_t idx1 = 0; + uint8_t sub = 0; + if (!readU8(idx1) || !readU8(sub)) { commandError = CE_PARAM_FORM; break; } + if (idx1 < 1 || idx1 > 8) { commandError = CE_PARAM_RANGE; break; } + + const uint8_t idx = (uint8_t)(idx1 - 1); + const int p = device[idx].purpose; + if (p == OFF) { commandError = CE_CMD_UNKNOWN; break; } + + if (sub == (uint8_t)'V') { + uint8_t v = 0; + if (!readU8(v)) { commandError = CE_PARAM_FORM; break; } + device[idx].value = (long)v; + + if (p == SWITCH || p == MOMENTARY_SWITCH || p == COVER_SWITCH) { + if (v > 1) { commandError = CE_PARAM_RANGE; break; } + + #ifdef COVER_SWITCH_SERVO_PRESENT + if (p == COVER_SWITCH) { + if (v == 0) cover[idx].target = COVER_SWITCH_SERVO_OPEN_DEG; + else cover[idx].target = COVER_SWITCH_SERVO_CLOSED_DEG; + } else + #endif + { + digitalWriteEx(device[idx].pin, (v != 0) == device[idx].active); + if (p == MOMENTARY_SWITCH && device[idx].value) momentarySwitchTime[idx] = 50; + } + } else + + if (p == ANALOG_OUTPUT) { + analogWriteEx(device[idx].pin, analog8BitToAnalogRange(v)); + } else + + if (p == DEW_HEATER) { + if (v > 1) { commandError = CE_PARAM_RANGE; break; } + device[idx].dewHeater->enable(v != 0); + } else + + if (p == INTERVALOMETER) { + if (v > 1) { commandError = CE_PARAM_RANGE; break; } + device[idx].intervalometer->enable(v != 0); + } + + } else + + if (sub == (uint8_t)'Z' || sub == (uint8_t)'S') { + uint8_t enc = 0; + if (!readU8(enc)) { commandError = CE_PARAM_FORM; break; } + if (p != DEW_HEATER) { commandError = CE_CMD_UNKNOWN; break; } + + const float f = unpackDew01C(enc); + if (sub == (uint8_t)'Z') device[idx].dewHeater->setZero(f); + else device[idx].dewHeater->setSpan(f); + + } else + + if (sub == (uint8_t)'E' || sub == (uint8_t)'D') { + uint8_t tb = 0; + if (!readU8(tb)) { commandError = CE_PARAM_FORM; break; } + if (p != INTERVALOMETER) { commandError = CE_CMD_UNKNOWN; break; } + + const float sec = convert.unpackSeconds(tb); + if (sub == (uint8_t)'E') device[idx].intervalometer->setExposure(sec); + else device[idx].intervalometer->setDelay(sec); + + } else + + if (sub == (uint8_t)'C') { + uint8_t c = 0; + if (!readU8(c)) { commandError = CE_PARAM_FORM; break; } + if (p != INTERVALOMETER) { commandError = CE_CMD_UNKNOWN; break; } + + device[idx].intervalometer->setCount((float)c); + + } else { + commandError = CE_PARAM_FORM; + } + + // numericReply stays true + } break; + + default: + handled = false; + commandError = CE_CMD_UNKNOWN; + break; + } + + sendResponse(handled, suppressFrame, numericReply, commandError); +} + +#endif diff --git a/src/telescope/auxiliary/local/Features.command.cpp b/src/telescope/auxiliary/local/Features.command.cpp new file mode 100755 index 0000000..e6bdde4 --- /dev/null +++ b/src/telescope/auxiliary/local/Features.command.cpp @@ -0,0 +1,218 @@ +// ----------------------------------------------------------------------------------- +// Auxiliary Features + +#include "Features.h" + +#ifdef FEATURES_PRESENT + +#include "../../../lib/convert/Convert.h" +#include "../../../libApp/weather/Weather.h" +#include "../../../telescope/Telescope.h" + +void Features::strCatPower(char *reply, int index) { + #ifdef POWER_MONITOR_PRESENT + if (index < 0 || index > 7) return; + + char s[40]; + if (powerMonitor.hasVoltage(index)) sprintF(s, ";%1.1f", powerMonitor.getVoltage(index)); else strcpy(s, ";NAN"); + strcat(reply, s); + if (powerMonitor.hasCurrent(index)) sprintF(s, ",%1.1f", powerMonitor.getCurrent(index)); else strcpy(s, ",NAN"); + strcat(reply, s); + if (!powerMonitor.hasChannel(index)) strcat(reply, ",P"); else strcat(reply, ",!"); + if (powerMonitor.errOverCurrent(index)) strcat(reply, "C"); else strcat(reply, "!"); + if (powerMonitor.errUnderVoltage(index)) strcat(reply, "U"); else strcat(reply, "!"); + if (powerMonitor.errOverVoltage(index)) strcat(reply, "V"); else strcat(reply, "!"); + if (powerMonitor.errOverTemperature(index)) strcat(reply, "T"); else strcat(reply, "!"); + #else + UNUSED(reply); + UNUSED(index); + #endif +} + +// by default reply[80] == "", suppressFrame == false, numericReply == true, and commandError == CE_NONE +// return true if the command has been completely handled and no further command() will be called, or false if not +// for commands that are handled repeatedly commandError might contain CE_NONE or CE_1 to indicate success +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +bool Features::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + if (!ready) return false; + + // get auXiliary feature + if (command[0] == 'G' && command[1] == 'X' && parameter[2] == 0) { + // :GXX[n]# + if (parameter[0] == 'X') { + int i = parameter[1] - '1'; + if (i < 0 || i > 7) { *commandError = CE_PARAM_FORM; return true; } + + char s[255]; + if (device[i].purpose == SWITCH || device[i].purpose == MOMENTARY_SWITCH || device[i].purpose == COVER_SWITCH) { + snprintf(s, sizeof(s), "%d", device[i].value); + strcat(reply, s); + strCatPower(reply, i); + } else + + if (device[i].purpose == ANALOG_OUTPUT) { + snprintf(s, sizeof(s), "%d", device[i].value); + strcat(reply, s); + strCatPower(reply, i); + } else + + if (device[i].purpose == DEW_HEATER) { + snprintf(s, sizeof(s), "%d", (int)device[i].dewHeater->isEnabled()); + strcat(reply, s); + strcat(reply, ","); + + sprintF(s, "%3.1f", device[i].dewHeater->getZero()); + strcat(reply, s); + strcat(reply, ","); + + sprintF(s, "%3.1f", device[i].dewHeater->getSpan()); + strcat(reply, s); + strcat(reply, ","); + + float deltaT = temperature.getChannel(i + 1) - weather.getDewPoint(); + if (isnan(deltaT)) strcpy(s,"NAN"); else sprintF(s, "%3.1f", deltaT); + strcat(reply, s); + + strCatPower(reply, i); + } else + + if (device[i].purpose == INTERVALOMETER) { + snprintf(s, sizeof(s), "%d", (int)device[i].intervalometer->getCurrentCount()); + strcat(reply, s); + strcat(reply, ","); + + float v; + int d; + v = device[i].intervalometer->getExposure(); + if (v < 1.0) d = 3; else if (v < 10.0) d = 2; else if (v < 30.0) d = 1; else d = 0; + dtostrf(v, 0, d, s); + strcat(reply, s); + strcat(reply, ","); + + v = device[i].intervalometer->getDelay(); + if (v < 10.0) d = 2; else if (v < 30.0) d = 1; else d = 0; + dtostrf(v, 0, d, s); + strcat(reply, s); + strcat(reply, ","); + + snprintf(s, sizeof(s), "%d", (int)device[i].intervalometer->getCount()); + strcat(reply, s); + } else { *commandError = CE_CMD_UNKNOWN; return true; } + + *numericReply = false; + } else + + // :GXY[n]# + // where [n] = 1..8 to get auXiliary feature name and purpose + // :GXY0# + // return active auXiliary features + if (parameter[0] == 'Y') { + int i = parameter[1] - '1'; + + if (i == -1) { + for (int j = 0; j < 8; j++) { + if (device[j].purpose == OFF) reply[j] = '0'; else reply[j] = '1'; + reply[j + 1] = 0; + } + *numericReply = false; + return true; + } + + if (i < 0 || i > 7) { *commandError = CE_PARAM_FORM; return true; } + if (device[i].purpose == OFF) { *commandError = CE_0; return true; } + + char s[255]; + sstrcpy(s, device[i].name); + if (strlen(s) > 10) s[10] = 0; + strcpy(reply, s); + strcat(reply, ","); + + int p = device[i].purpose; + if (p == MOMENTARY_SWITCH || p == COVER_SWITCH) p = SWITCH; + snprintf(s, sizeof(s), "%d", p); + strcat(reply, s); + + *numericReply = false; + } else return false; + + } else + + // set auXiliary feature + if (command[0] == 'S' && command[1] == 'X' && parameter[2] == ',') { + // :SXX[n],V[Z][S][v]# + // for example :SXX1,V1# :SXX1,Z0.5# + if (parameter[0] == 'X') { + int i = parameter[1] - '1'; + if (i < 0 || i > 7) { *commandError = CE_PARAM_FORM; return true; } + if (device[i].purpose == OFF) { *commandError = CE_CMD_UNKNOWN; return true; } + + char* conv_end; + float f = strtof(¶meter[4], &conv_end); + if (¶meter[4] == conv_end) { *commandError = CE_PARAM_FORM; return true; } + long v = lroundf(f); + + if (parameter[3] == 'V' && v >= 0 && v <= 255) device[i].value = v; + + if (device[i].purpose == SWITCH || device[i].purpose == MOMENTARY_SWITCH || device[i].purpose == COVER_SWITCH) { + if (parameter[3] == 'V') { + if (v >= 0 && v <= 1) { // value 0..1 for enabled or not + #ifdef COVER_SWITCH_SERVO_PRESENT + if (device[i].purpose == COVER_SWITCH) { + if (v == 0) cover[i].target = COVER_SWITCH_SERVO_OPEN_DEG; else + if (v == 1) cover[i].target = COVER_SWITCH_SERVO_CLOSED_DEG; + } else + #endif + + digitalWriteEx(device[i].pin, v == device[i].active); + if (device[i].purpose == MOMENTARY_SWITCH && device[i].value) momentarySwitchTime[i] = 50; + + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + if (device[i].purpose == ANALOG_OUTPUT) { + if (parameter[3] == 'V') { // value 0..255 for 0..100% power + if (v >= 0 && v <= 255) { + analogWriteEx(device[i].pin, analog8BitToAnalogRange(v)); + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + if (device[i].purpose == DEW_HEATER) { + if (parameter[3] == 'V') { // value 0..1 for enabled or not + if (v >= 0 && v <= 1) device[i].dewHeater->enable(v); else *commandError = CE_PARAM_RANGE; + } else + + if (parameter[3] == 'Z') { // zero + if (f >= -5.0F && f <= 20.0F) device[i].dewHeater->setZero(f); else *commandError = CE_PARAM_RANGE; + } else + + if (parameter[3] == 'S') { // span + if (f >= -5.0F && f <= 20.0F) device[i].dewHeater->setSpan(f); else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + if (device[i].purpose == INTERVALOMETER) { + if (parameter[3] == 'V') { // value 0..1 for enabled or not + if (v >= 0 && v <= 1) device[i].intervalometer->enable(v); else *commandError = CE_PARAM_RANGE; + } else + + if (parameter[3] == 'E') { // exposure length + if (f >= 0.0F && f <= 3600.0F) device[i].intervalometer->setExposure(f); else *commandError = CE_PARAM_RANGE; + } else + + if (parameter[3] == 'D') { // delay + if (f >= 1.0F && f <= 3600.0F) device[i].intervalometer->setDelay(f); else *commandError = CE_PARAM_RANGE; + } else + + if (parameter[3] == 'C') { // count + if (f >= 0.0F && f <= 255.0F) device[i].intervalometer->setCount(f); else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } + } else return false; + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/auxiliary/local/Features.cpp b/src/telescope/auxiliary/local/Features.cpp new file mode 100755 index 0000000..e9aef60 --- /dev/null +++ b/src/telescope/auxiliary/local/Features.cpp @@ -0,0 +1,166 @@ +// ----------------------------------------------------------------------------------- +// Auxiliary Features + +#include "Features.h" + +#ifdef FEATURES_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../../libApp/weather/Weather.h" +#include "../../../telescope/Telescope.h" + +void featuresPollWrapper() { features.poll(); } + +#if defined(FEATURES_CAN_SERVER_PRESENT) +// 1 Hz heartbeat TX for this AF node +static void auxHeartbeatWrapper() { + if (!canPlus.ready) return; + const uint16_t hbId = (uint16_t)(CAN_FEATURES_HB_ID_BASE); + const uint8_t b = 0; // payload reserved; currently unused + canPlus.writePacket((int)hbId, &b, 1); +} +#endif + +bool Features::init() { + + #if defined(FEATURES_CAN_SERVER_PRESENT) + if (!CanTransportServer::init(true, 2)) return false; + #endif + + #ifdef POWER_MONITOR_PRESENT + if (!powerMonitor.init()) return false; + #endif + + for (uint8_t i = 0; i < 8; i++) { + if (device[i].pin == AUX) device[i].pin = auxPins[i]; + + if (device[i].value == ON) device[i].value = 1; else + if (device[i].value < 0 || device[i].value > 255) device[i].value = 0; + + if (device[i].memory == ON) { + if (device[i].purpose == SWITCH || + device[i].purpose == ANALOG_OUTPUT || + device[i].purpose == DEW_HEATER) { + + char keyStr[26]; + snprintf(keyStr, sizeof(keyStr), "FEATURE%u_VALUE", i); + nvKey[i] = nv().kv().computeKey(keyStr); + + if (!nv().kv().getOrInit(nvKey[i], device[i].value)) { DF("WRN: Nv, init failed for "); VL(keyStr); } + } + } + + if (device[i].purpose == SWITCH || device[i].purpose == MOMENTARY_SWITCH || device[i].purpose == HIDDEN_SWITCH) { + pinModeEx(device[i].pin, OUTPUT); + digitalWriteEx(device[i].pin, device[i].value == device[i].active); + if (device[i].purpose == MOMENTARY_SWITCH && device[i].value) momentarySwitchTime[i] = 50; + if (device[i].purpose == HIDDEN_SWITCH) device[i].purpose = OFF; + } else + + #ifdef COVER_SWITCH_SERVO_PRESENT + if (device[i].purpose == COVER_SWITCH) { + cover[i].servo = new Servo; + #ifdef ESP32 + cover[i].servo->setPeriodHertz(COVER_SWITCH_SERVO_PERIOD_HZ); + #endif + cover[i].servo->attach(device[i].pin, COVER_SWITCH_SERVO_MIN, COVER_SWITCH_SERVO_MAX); + cover[i].servo->write(COVER_SWITCH_SERVO_CLOSED_DEG); + cover[i].position = COVER_SWITCH_SERVO_CLOSED_DEG; + } else + #endif + + if (device[i].purpose == ANALOG_OUTPUT) { + pinModeEx(device[i].pin, OUTPUT); + analogWriteEx(device[i].pin, analog8BitToAnalogRange(device[i].value)); + } else + + if (device[i].purpose == DEW_HEATER) { + device[i].dewHeater = new DewHeater; + device[i].dewHeater->init(i); + pinModeEx(device[i].pin, OUTPUT); + device[i].dewHeater->enable(device[i].value); + } else + + if (device[i].purpose == INTERVALOMETER) { + device[i].intervalometer = new Intervalometer; + device[i].intervalometer->init(i); + pinModeEx(device[i].pin, OUTPUT); + digitalWriteEx(device[i].pin, device[i].value == device[i].active); + } + } + + VF("MSG: Auxiliary, start feature monitor task (rate 20ms priority 6)... "); + if (tasks.add(20, 0, true, 6, featuresPollWrapper, "AuxPoll")) { VLF("success"); } else { VLF("FAILED!"); return false; } + + ready = true; + + return true; +} + +void Features::begin() { + if (!ready) return; + + // start heartbeat task + #if defined(FEATURES_CAN_SERVER_PRESENT) + VF("MSG: Auxiliary, starting CAN heartbeat task (rate 1s priority 6)... "); + if (tasks.add(1000, 0, true, 6, auxHeartbeatWrapper, "AuxHB")) { VLF("success"); } else { VLF("FAILED!"); } + #endif +} + +void Features::poll() { + for (int i = 0; i < 8; i++) { + if (device[i].memory == ON) { + if (device[i].value != lastValue[i]) { + if (device[i].purpose == SWITCH || + device[i].purpose == ANALOG_OUTPUT || + device[i].purpose == DEW_HEATER) { + nv().kv().put(nvKey[i], device[i].value); + } + lastValue[i] = device[i].value; + } + } + + if (device[i].purpose == MOMENTARY_SWITCH) { + if (momentarySwitchTime[i] > 0) { + momentarySwitchTime[i]--; + if (momentarySwitchTime[i] == 0) { + device[i].value = 0; + digitalWriteEx(device[i].pin, device[i].value == device[i].active); + } + } + } else + + if (device[i].purpose == DEW_HEATER) { + device[i].dewHeater->poll(temperature.getChannel(i + 1) - weather.getDewPoint()); + digitalWriteEx(device[i].pin, device[i].dewHeater->isOn() == device[i].active); +// if (gpio.failure(i)) device[i].dewHeater->enable(false); + } else + + if (device[i].purpose == INTERVALOMETER) { + device[i].intervalometer->poll(); + digitalWriteEx(device[i].pin, device[i].intervalometer->isOn() == device[i].active); +// if (gpio.failure(i)) device[i].intervalometer->enable(false); + } + } + + #ifdef COVER_SWITCH_SERVO_PRESENT + // default moves at 25 degrees/second + static int toggle = 0; + if (toggle++ % (100/COVER_SWITCH_SERVO_SPEED_PERCENT) == 0) { + for (int i = 0; i < 8; i++) { + if (device[i].purpose == COVER_SWITCH) { + if (cover[i].position > cover[i].target) cover[i].position--; else + if (cover[i].position < cover[i].target) cover[i].position++; + cover[i].servo->write(cover[i].position); + } + } + } + #endif + +} + +Features features; + +#endif diff --git a/src/telescope/auxiliary/local/Features.h b/src/telescope/auxiliary/local/Features.h new file mode 100755 index 0000000..307ae04 --- /dev/null +++ b/src/telescope/auxiliary/local/Features.h @@ -0,0 +1,118 @@ +// ----------------------------------------------------------------------------------- +// Auxiliary Features +#pragma once + +#include "../../../Common.h" + +#include "../FeaturesBase.h" + +#ifdef FEATURES_PRESENT + +#include "../../../lib/gpioEx/GpioEx.h" + +#include "../../../libApp/commands/ProcessCmds.h" +#include "../../../libApp/temperature/Temperature.h" +#include "../dewHeater/DewHeater.h" +#include "../intervalometer/Intervalometer.h" +#include "../powerMonitor/PowerMonitor.h" + +#ifdef COVER_SWITCH_SERVO_PRESENT + #ifdef ESP32 + #include // https://www.arduino.cc/reference/en/libraries/esp32servo/ + #else + #include + #endif + #ifndef COVER_SWITCH_SERVO_PERIOD_HZ + #define COVER_SWITCH_SERVO_PERIOD_HZ 50 + #endif + #ifndef COVER_SWITCH_SERVO_SPEED_PERCENT + #define COVER_SWITCH_SERVO_SPEED_PERCENT 50 + #endif + #ifndef COVER_SWITCH_SERVO_MIN + #define COVER_SWITCH_SERVO_MIN 1000 + #endif + #ifndef COVER_SWITCH_SERVO_MAX + #define COVER_SWITCH_SERVO_MAX 2000 + #endif + #ifndef COVER_SWITCH_SERVO_CLOSED_DEG + #define COVER_SWITCH_SERVO_CLOSED_DEG 0 + #endif + #ifndef COVER_SWITCH_SERVO_OPEN_DEG + #define COVER_SWITCH_SERVO_OPEN_DEG 100 + #endif + + typedef struct DeviceCoverServo { + int target; + int position; + Servo *servo; + } DeviceCoverServo; + +#endif + +typedef struct Device { + const char* name; + int16_t purpose; + const int64_t temp; + int16_t pin; + int16_t value; + const int16_t active; + const int8_t memory; + DewHeater *dewHeater; + Intervalometer *intervalometer; +} Device; + +#ifdef FEATURES_CAN_SERVER_PRESENT +#include "../../../lib/canTransport/CanTransportServer.h" +class Features : public CanTransportServer { +private: + void processCommand() override; +public: + Features() : CanTransportServer((uint16_t)(CAN_FEATURES_REQ_ID), (uint16_t)(CAN_FEATURES_RSP_ID), 0x1E) {} +#else +class Features { +public: +#endif + + // initialize the aux features + bool init(); + void begin(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + void poll(); + + inline void deviceOff(int index) { device[index].value = 0; digitalWriteEx(device[index].pin, LOW); } + + private: + void strCatPower(char *reply, int index); + + bool ready = false; + + int16_t auxPins[8] = { AUX1_PIN, AUX2_PIN, AUX3_PIN, AUX4_PIN, AUX5_PIN, AUX6_PIN, AUX7_PIN, AUX8_PIN }; + Device device[8] = { + { FEATURE1_NAME, FEATURE1_PURPOSE, FEATURE1_TEMP, FEATURE1_PIN, FEATURE1_VALUE_DEFAULT, FEATURE1_ON_STATE, FEATURE1_VALUE_MEMORY, NULL, NULL }, + { FEATURE2_NAME, FEATURE2_PURPOSE, FEATURE2_TEMP, FEATURE2_PIN, FEATURE2_VALUE_DEFAULT, FEATURE2_ON_STATE, FEATURE2_VALUE_MEMORY, NULL, NULL }, + { FEATURE3_NAME, FEATURE3_PURPOSE, FEATURE3_TEMP, FEATURE3_PIN, FEATURE3_VALUE_DEFAULT, FEATURE3_ON_STATE, FEATURE3_VALUE_MEMORY, NULL, NULL }, + { FEATURE4_NAME, FEATURE4_PURPOSE, FEATURE4_TEMP, FEATURE4_PIN, FEATURE4_VALUE_DEFAULT, FEATURE4_ON_STATE, FEATURE4_VALUE_MEMORY, NULL, NULL }, + { FEATURE5_NAME, FEATURE5_PURPOSE, FEATURE5_TEMP, FEATURE5_PIN, FEATURE5_VALUE_DEFAULT, FEATURE5_ON_STATE, FEATURE5_VALUE_MEMORY, NULL, NULL }, + { FEATURE6_NAME, FEATURE6_PURPOSE, FEATURE6_TEMP, FEATURE6_PIN, FEATURE6_VALUE_DEFAULT, FEATURE6_ON_STATE, FEATURE6_VALUE_MEMORY, NULL, NULL }, + { FEATURE7_NAME, FEATURE7_PURPOSE, FEATURE7_TEMP, FEATURE7_PIN, FEATURE7_VALUE_DEFAULT, FEATURE7_ON_STATE, FEATURE7_VALUE_MEMORY, NULL, NULL }, + { FEATURE8_NAME, FEATURE8_PURPOSE, FEATURE8_TEMP, FEATURE8_PIN, FEATURE8_VALUE_DEFAULT, FEATURE8_ON_STATE, FEATURE8_VALUE_MEMORY, NULL, NULL } + }; + #ifdef COVER_SWITCH_SERVO_PRESENT + DeviceCoverServo cover[8] = { + { 0, 0, NULL }, { 0, 0, NULL }, { 0, 0, NULL }, { 0, 0, NULL }, { 0, 0, NULL }, { 0, 0, NULL }, { 0, 0, NULL }, { 0, 0, NULL } + }; + #endif + uint8_t momentarySwitchTime[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + int16_t lastValue[8] = { + FEATURE1_VALUE_DEFAULT, FEATURE2_VALUE_DEFAULT, FEATURE3_VALUE_DEFAULT, FEATURE4_VALUE_DEFAULT, + FEATURE5_VALUE_DEFAULT, FEATURE6_VALUE_DEFAULT, FEATURE7_VALUE_DEFAULT, FEATURE8_VALUE_DEFAULT + }; + + uint16_t nvKey[8]; +}; + +extern Features features; + +#endif diff --git a/src/telescope/auxiliary/powerMonitor/PowerMonitor.cpp b/src/telescope/auxiliary/powerMonitor/PowerMonitor.cpp new file mode 100755 index 0000000..0fbf5e9 --- /dev/null +++ b/src/telescope/auxiliary/powerMonitor/PowerMonitor.cpp @@ -0,0 +1,179 @@ +//-------------------------------------------------------------------------------------------------- +// Power monitor + safety (auxiliary/AF) + +#include "PowerMonitor.h" + +#ifdef POWER_MONITOR_PRESENT + +#include "../../../lib/tasks/OnTask.h" + +// Adjust these two includes to match your tree (these are the typical locations) +#include "../Features.h" +#include "../../Telescope.h" + +static void powerWrapper() { powerMonitor.poll(); } + +bool PowerMonitor::init() { + VLF("MSG: PowerMonitor, starting: power monitor"); + + VF("MSG: PowerMonitor, start voltage/current monitoring task (rate 200ms priority 7)... "); + if (tasks.add(200, 0, true, 7, powerWrapper, "pwrMtr")) { VLF("success"); } else { VLF("FAILED!"); return false; } + + #if defined(FAN_PIN) + fanMode = FM_OFF; + #endif + + return true; +} + +void PowerMonitor::poll() { + float v = 0.0F; + bool disableFeatures = false; + + // clear fault flags; they are re-asserted on each poll if the condition is present + for (int j = 0; j < 8; j++) { + ocFault[j] = false; + uvFault[j] = false; + ovFault[j] = false; + otFault[j] = false; + } + + // ---------------- Voltage sampling ---------------- + #if defined(POWER_MONITOR_VOLTAGE_PRESENT) + const int voltageSensePin[8] = V_SENSE_PINS; + + for (int j = 0; j < 8; j++) { + if (voltageSensePin[j] >= 0) { + if (isnan(averagedVoltage[j])) delay(10); + + v = analog.readV((int16_t)voltageSensePin[j]); + + const float scaledVoltage = (float)(V_SENSE_FORMULA); + + if (isnan(averagedVoltage[j])) averagedVoltage[j] = scaledVoltage; + else averagedVoltage[j] = (averagedVoltage[j] * 6.0F + scaledVoltage) / 7.0F; + + #if defined(V_SENSE_LIMIT_LOW) && defined(V_SENSE_LIMIT_HIGH) + if (j != V_SENSE_LIMIT_EXCLUDE && + (averagedVoltage[j] < V_SENSE_LIMIT_LOW || averagedVoltage[j] > V_SENSE_LIMIT_HIGH)) { + VLF("MSG: PowerMonitor, OV/UV"); + if (averagedVoltage[j] < V_SENSE_LIMIT_LOW) uvFault[j] = true; + if (averagedVoltage[j] > V_SENSE_LIMIT_HIGH) ovFault[j] = true; + disableFeatures = true; + } + #endif + } + } + #endif + + Y; + + // ---------------- Current sampling ---------------- + #if defined(POWER_MONITOR_CURRENT_PRESENT) + + const int currentSensePin[8] = I_SENSE_PINS; + #if defined(I_SENSE_CHANNEL_MAX) && (I_SENSE_COMBINED_MAX) + float currentSenseTotal = 0.0F; + #endif + + for (int j = 0; j < 8; j++) { + if (currentSensePin[j] >= 0) { + if (isnanf(averagedCurrent[j])) delay(10); + + v = analog.readV((int16_t)currentSensePin[j]); + + const float currentI = (float)(I_SENSE_FORMULA); + + if (isnanf(averagedCurrent[j])) averagedCurrent[j] = currentI; + else averagedCurrent[j] = (averagedCurrent[j] * 6.0F + currentI) / 7.0F; + + #if defined(I_SENSE_CHANNEL_MAX) + const float currentSenseMax[8] = I_SENSE_CHANNEL_MAX; + + if (fabsf(averagedCurrent[j]) > currentSenseMax[j]) { + VF("MSG: PowerMonitor, OC channel"); V(j); + VF(" current="); V(averagedCurrent[j]); VLF("A"); + ocFault[j] = true; + features.deviceOff(j); + } + + #if defined(I_SENSE_COMBINED_MAX) + currentSenseTotal += fabsf(averagedCurrent[j]); + if (currentSenseTotal > I_SENSE_COMBINED_MAX) { + VF("MSG: PowerMonitor, OC combined"); + VF(" current="); V(currentSenseTotal); VLF("A"); + for (int k = 0; k < 8; k++) ocFault[k] = true; + disableFeatures = true; + } + #endif + #endif + } + } + #endif + + Y; + + // ---------------- Fan / OT safety ---------------- + #if defined(FAN_PIN) + FanMode lastFanMode = fanMode; + + // telescope.mcuTemperature exists; NAN means unavailable. + if (!isnan(telescope.mcuTemperature) && (int32_t)(millis() - nextFanModeChangeTime) > 0) { + + #if defined(FAN_THRESHOLD_OT) + if (telescope.mcuTemperature > FAN_THRESHOLD_OT) { + VLF("MSG: PowerMonitor, MCU OT"); + for (int k = 0; k < 8; k++) otFault[k] = true; + disableFeatures = true; + } + #endif + + #if defined(FAN_THRESHOLD_HIGH) && defined(FAN_POWER_HIGH) + if (telescope.mcuTemperature > FAN_THRESHOLD_HIGH) { + if (fanMode != FM_HIGH) { + VLF("MSG: PowerMonitor, fan high"); + analog.write((int16_t)FAN_PIN, (float)(FAN_POWER_HIGH/100.0F)); + fanMode = FM_HIGH; + } + } else + #endif + + #if defined(FAN_THRESHOLD_MID) && defined(FAN_POWER_MID) + if (telescope.mcuTemperature > FAN_THRESHOLD_MID) { + if (fanMode != FM_MID) { + VLF("MSG: PowerMonitor, fan mid"); + analog.write((int16_t)FAN_PIN, (float)(FAN_POWER_MID/100.0F)); + fanMode = FM_MID; + } + } else + #endif + + #if defined(FAN_THRESHOLD_LOW) && defined(FAN_POWER_LOW) + if (telescope.mcuTemperature > FAN_THRESHOLD_LOW) { + if (fanMode != FM_LOW) { + VLF("MSG: PowerMonitor, fan low"); + analog.write((int16_t)FAN_PIN, (float)(FAN_POWER_LOW/100.0F)); + fanMode = FM_LOW; + } + } else + #endif + + if (fanMode != FM_OFF) { + VLF("MSG: PowerMonitor, fan off"); + analog.write((int16_t)FAN_PIN, 0.0F); + fanMode = FM_OFF; + } + } + + if (lastFanMode != fanMode) nextFanModeChangeTime = millis() + 30000UL; + #endif + + if (disableFeatures) { + VLF("MSG: PowerMonitor, disable all features"); + for (int j = 0; j < 8; j++) features.deviceOff(j); + } +} + +PowerMonitor powerMonitor; + +#endif diff --git a/src/telescope/auxiliary/powerMonitor/PowerMonitor.h b/src/telescope/auxiliary/powerMonitor/PowerMonitor.h new file mode 100755 index 0000000..fd90b3e --- /dev/null +++ b/src/telescope/auxiliary/powerMonitor/PowerMonitor.h @@ -0,0 +1,59 @@ +//-------------------------------------------------------------------------------------------------- +// Power monitor + safety (auxiliary/AF) +// +// - Samples V/I channels (optional) with smoothing +// - Applies safety shutoffs to Features (per-channel OC, combined OC, OV/UV, MCU OT) +// - Optional fan control based on telescope.mcuTemperature (NAN => unavailable) +// + +#pragma once + +#include "../../../Common.h" + +#ifdef POWER_MONITOR_PRESENT + +#include "../../../lib/analog/Analog.h" +#include + +class Features; +class Telescope; + +class PowerMonitor { +public: + bool init(); + + // telemetry access (NAN if never sampled / pin not present) + float getVoltage(uint8_t ch) const { return (ch < 8) ? averagedVoltage[ch] : NAN; } + float getCurrent(uint8_t ch) const { return (ch < 8) ? averagedCurrent[ch] : NAN; } + + bool hasVoltage(uint8_t ch) const { return (ch < 8) && !isnan(averagedVoltage[ch]); } + bool hasCurrent(uint8_t ch) const { return (ch < 8) && !isnan(averagedCurrent[ch]); } + + // channel exists if any telemetry is available + bool hasChannel(uint8_t ch) const { return hasVoltage(ch) || hasCurrent(ch); } + + // fault flags (best-effort; cleared each poll unless fault re-triggers) + bool errOverCurrent(uint8_t ch) const { return (ch < 8) ? ocFault[ch] : false; } + bool errUnderVoltage(uint8_t ch) const { return (ch < 8) ? uvFault[ch] : false; } + bool errOverVoltage(uint8_t ch) const { return (ch < 8) ? ovFault[ch] : false; } + bool errOverTemperature(uint8_t ch) const { return (ch < 8) ? otFault[ch] : false; } + + void poll(); + +private: + float averagedVoltage[8] = {NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN}; + float averagedCurrent[8] = {NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN}; + + bool ocFault[8] = {false, false, false, false, false, false, false, false}; + bool uvFault[8] = {false, false, false, false, false, false, false, false}; + bool ovFault[8] = {false, false, false, false, false, false, false, false}; + bool otFault[8] = {false, false, false, false, false, false, false, false}; + + enum FanMode : uint8_t { FM_OFF = 0, FM_LOW, FM_MID, FM_HIGH }; + FanMode fanMode = FM_OFF; + uint32_t nextFanModeChangeTime = 0; +}; + +extern PowerMonitor powerMonitor; + +#endif diff --git a/src/telescope/auxiliary/remote/Features.can.decode.cpp b/src/telescope/auxiliary/remote/Features.can.decode.cpp new file mode 100755 index 0000000..7e8d3c3 --- /dev/null +++ b/src/telescope/auxiliary/remote/Features.can.decode.cpp @@ -0,0 +1,173 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope features control, using the CANbus interface (CAN client decode) + +#include "Features.h" + +#if defined(FEATURES_CAN_CLIENT_PRESENT) + +#include "../FeaturesBase.h" +#include "../../../lib/convert/Convert.h" + +// ---------- helpers ---------- + +// Dew heater: -5.0..+20.0C encoded as 0..250 in 0.1C steps (one byte). +static inline float unpackDew01C(uint8_t enc) { + return ((float)enc / 10.0f) - 5.0f; +} + +static inline void fmtSeconds(char *out, float v, bool isExposure) { + // Match the formatting intent from Features.command.cpp: + // exposure: <1 => 3dp, <10 => 2dp, <30 => 1dp, else 0dp + // delay: <10 => 2dp, <30 => 1dp, else 0dp + int d = 0; + if (isExposure) { + if (v < 1.0f) d = 3; + else if (v < 10.0f) d = 2; + else if (v < 30.0f) d = 1; + else d = 0; + } else { + if (v < 10.0f) d = 2; + else if (v < 30.0f) d = 1; + else d = 0; + } + dtostrf(v, 0, d, out); +} + +bool Features::decodeResponse(char *reply) { + if (!reply) return false; + reply[0] = 0; + + switch (opCode()) { + + // :GXY0# -> "01010101" (8 chars, no terminator transmitted on CAN; we add it locally) + case FEAT_OP_GET_ACTIVE_Y0: { + uint8_t bitmap = 0; + if (!readU8(bitmap)) return false; + + for (uint8_t i = 0; i < 8; i++) { reply[i] = (bitmap & (1U << i)) ? '1' : '0'; } + reply[8] = 0; + } break; + + // :GXYn# -> "NAME,PURPOSE" (NAME up to 10 chars; PURPOSE numeric) + // Wire: + // frame0 data: [purpose][name0..name4] + // frame1 data: [name5..name10] (up to 6 bytes) + case FEAT_OP_GET_INFO_Yn: { + uint8_t purpose = 0; + if (!readU8(purpose)) return false; + + char name[14]; + int len = readRemainingBytes((uint8_t*)name); + name[len] = 0; + + sprintf(reply, "%s,%u", name, (unsigned)purpose); + } break; + + // :GXXn# -> per-purpose value reply, rendered as original comma-separated ASCII, + // with optional power monitor telemetry appended (matches Features::strCatPower()). + // + // Wire (frame0 data is always 6 bytes; purpose is included): + // frame0 data: [purpose][value bytes...] + // frame1 data: optional [intervalometerCount] + optional [V i16 0.1][I i16 0.1][flags] + // + // Voltage/current fixed-point: enc = round(x * 10), int16 LE; 0x8000 => NAN. + // Flags: FEAT_POWER_FLAGS_PRESENT + FEAT_POWER_FAULT_* (see FeaturesBase.h). + case FEAT_OP_GET_VALUE_Xn: { + uint8_t purpose = 0; + if (!readU8(purpose)) return false; + + // -------- value decode -------- + if (purpose == SWITCH || purpose == MOMENTARY_SWITCH || purpose == COVER_SWITCH) { + uint8_t v = 0; + if (!readU8(v)) return false; + sprintf(reply, "%u", (unsigned)v); + } else + + if (purpose == ANALOG_OUTPUT) { + uint8_t v = 0; + if (!readU8(v)) return false; + sprintf(reply, "%u", (unsigned)v); + } else + + if (purpose == DEW_HEATER) { + uint8_t en = 0; + uint8_t zEnc = 0; + uint8_t sEnc = 0; + float delta = 0; + if (!readU8(en) || !readU8(zEnc) || !readU8(sEnc) || !readFixedI16(delta, 10)) return false; + + const float zero = unpackDew01C(zEnc); + const float span = unpackDew01C(sEnc); + + char s1[16], s2[16], s3[16]; + dtostrf(zero, 0, 1, s1); + dtostrf(span, 0, 1, s2); + if (isnan(delta)) strcpy(s3, "NAN"); else dtostrf(delta, 0, 1, s3); + sprintf(reply, "%u,%s,%s,%s", (unsigned)en, s1, s2, s3); + } else + + if (purpose == INTERVALOMETER) { + uint8_t cur = 0; + uint8_t expB = 0; + uint8_t delB = 0; + uint8_t count = 0; + + if (!readU8(cur) || !readU8(expB) || !readU8(delB) || !readU8(count)) return false; + + const float exposure = convert.unpackSeconds(expB); + const float delay = convert.unpackSeconds(delB); + + char sExp[20], sDel[20]; + fmtSeconds(sExp, exposure, true); + fmtSeconds(sDel, delay, false); + sprintf(reply, "%u,%s,%s,%u", (unsigned)cur, sExp, sDel, (unsigned)count); + } else { + // Unknown purpose; return first value byte as integer + uint8_t v = 0; + if (!readU8(v)) return false; + sprintf(reply, "%u", (unsigned)v); + } + + // optional power telemetry append + if (remaining() == 5) { + float volts = NAN; + float amps = NAN; + uint8_t flags = 0; + + if (!readFixedI16(volts, 10) || !readFixedI16(amps, 10) || !readU8(flags)) return false; + + const bool present = ((flags & FEAT_POWER_FLAGS_PRESENT) != 0); + + if (!present) { strcat(reply, ",NAN,NAN,!!!!!"); return true; } + + if (isnan(volts)) { strcat(reply, ",NAN"); } else { char s[24]; sprintF(s, ",%1.1f", volts); strcat(reply, s); } + + if (isnan(amps)) { strcat(reply, ",NAN"); } else { char s[24]; sprintF(s, ",%1.1f", amps); strcat(reply, s); } + + // Flags string: ",P" + (C/v/V/T or !) + char fs[8]; + fs[0] = ','; + fs[1] = 'P'; + fs[2] = (flags & FEAT_POWER_FAULT_OC) ? '!' : 'C'; + fs[3] = (flags & FEAT_POWER_FAULT_UV) ? '!' : 'v'; + fs[4] = (flags & FEAT_POWER_FAULT_OV) ? '!' : 'V'; + fs[5] = (flags & FEAT_POWER_FAULT_OT) ? '!' : 'T'; + fs[6] = 0; + strcat(reply, fs); + } + + } break; + + // SET op is normally numericReply=true (so decodeResponse won't be called), + // but keep it harmless if it is. + case FEAT_OP_SET_VALUE_Xn: + default: + reply[0] = 0; + return true; + break; + } + + return true; +} + +#endif diff --git a/src/telescope/auxiliary/remote/Features.can.encode.cpp b/src/telescope/auxiliary/remote/Features.can.encode.cpp new file mode 100755 index 0000000..3ab5154 --- /dev/null +++ b/src/telescope/auxiliary/remote/Features.can.encode.cpp @@ -0,0 +1,98 @@ +//-------------------------------------------------------------------------------------------------- +// remote auxiliary features control, using the CANbus interface (CAN client encode) + +#include "Features.h" + +#if defined(FEATURES_CAN_CLIENT_PRESENT) + +#include "../FeaturesBase.h" +#include "../../../lib/convert/Convert.h" + +// ---------- helpers ---------- + +// Dew heater: -5.0..+20.0C encoded as 0..250 in 0.1C steps (one byte). +static inline uint8_t packDew01C(float v) { + if (v < -5.0f) v = -5.0f; + if (v > 20.0f) v = 20.0f; + const int enc = (int)lroundf((v + 5.0f) * 10.0f); + if (enc < 0) return 0; + if (enc > 250) return 250; + return (uint8_t)enc; +} + +bool Features::encodeRequest(char *command, char *parameter) { + + if (command[0] == 'G' && command[1] == 'X') { + + // :GXY0# + if (parameter[0] == 'Y' && parameter[1] == '0' && parameter[2] == 0) { + if (!beginNewRequest(FEAT_OP_GET_ACTIVE_Y0)) return false; + } else + + // :GXY[n]# where [n]=1..8 + if (parameter[0] == 'Y' && parameter[2] == 0) { + const char c = parameter[1]; + if (c < '1' || c > '8') return false; + + const uint8_t idx = (uint8_t)(c - '1'); // 0..7 + if (!beginNewRequest(FEAT_OP_GET_INFO_Yn)) return false; + if (!writeU8(idx)) return false; + } else + + // :GXX[n]# where [n]=1..8 + if (parameter[0] == 'X' && parameter[2] == 0) { + const char c = parameter[1]; + if (c < '1' || c > '8') return false; + + const uint8_t idx = (uint8_t)(c - '1'); // 0..7 + if (!beginNewRequest(FEAT_OP_GET_VALUE_Xn)) return false; + if (!writeU8(idx)) return false; + } else return false; + + } else + + if (command[0] == 'S' && command[1] == 'X') { + + // Expected: "Xn,..." (per Features.command.cpp) + // Examples: "X1,V1" "X2,V255" "X1,Z0.5" "X3,E12.3" + if (parameter[0] != 'X' || parameter[2] != ',') return false; + + const uint8_t sub = (uint8_t)parameter[3]; + int idx = parameter[1] - '0'; + if (idx < 1 || idx > 8) return false; + + char* conv_end; + float f = strtof(¶meter[4], &conv_end); + if (¶meter[4] == conv_end) return false; + long i = lroundf(f); + + if (!beginNewRequest(FEAT_OP_SET_VALUE_Xn)) return false; + + if (!writeU8(idx)) return false; + if (!writeU8(sub)) return false; + + // Subcommand packing + if (sub == (uint8_t)'V') { + if (!writeU8(i)) return false; + } else + + if (sub == (uint8_t)'Z' || sub == (uint8_t)'S') { + // Z/S: dew heater params packed as 0.1C steps in one byte + if (!writeU8(packDew01C(f))) return false; + } else + + if (sub == (uint8_t)'E' || sub == (uint8_t)'D') { + if (!writeU8(convert.packSeconds(f))) return false; + } else + + if (sub == (uint8_t)'C') { + // C: intervalometer count (0..255) packed u8 + if (!writeU8(i)) return false; + } else return false; + + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/auxiliary/remote/Features.cpp b/src/telescope/auxiliary/remote/Features.cpp new file mode 100755 index 0000000..042c75a --- /dev/null +++ b/src/telescope/auxiliary/remote/Features.cpp @@ -0,0 +1,65 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope auxiliary features control, using the CANbus interface + +#include "Features.h" + +#ifdef FEATURES_CAN_CLIENT_PRESENT + +// -------------------------------------------------------------------------- +// Heartbeat RX (local node): update last-heard timestamp for features +// -------------------------------------------------------------------------- +static void feaHb(uint8_t data[8], uint8_t len) { (void)data; (void)len; features.heartbeat(); } + +void Features::begin() { + canPlus.callbackRegisterId((int)(CAN_FEATURES_HB_ID_BASE), feaHb); +} + +// by default reply[80] == "", suppressFrame == false, numericReply == true, and commandError == CE_NONE +// return true if the command has been completely handled and no further command() will be called, or false if not +// for commands that are handled repeatedly commandError might contain CE_NONE or CE_1 to indicate success +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +bool Features::command(char *reply, char *command, char *parameter, + bool *suppressFrame, bool *numericReply, CommandError *commandError) { + if (!canPlus.ready) return false; + + // Only handle aux Feature command family: + // :GXXn# :GXYn# :SXXn,*# + if (!((command[0] == 'G' && command[1] == 'X' && (parameter[0] == 'X' || parameter[0] == 'Y')) || + (command[0] == 'S' && command[1] == 'X' && parameter[0] == 'X'))) return false; + + // Presence gate + if (!heartbeatFresh()) return false; + + // Build CAN request (also sets opCode()/tidop internally per your new design) + if (!encodeRequest(command, parameter)) return false; + + // Transport failure => reply unknown + bool handled = false; + if (!transactRequest(handled, *suppressFrame, *numericReply, *commandError)) { *commandError = CE_REPLY_UNKNOWN; return true; } + + // Remote explicitly did not recognize/handle the command + if (!handled) { + *commandError = CE_CMD_UNKNOWN; + *numericReply = true; + *suppressFrame = false; + reply[0] = 0; + return true; + } + + // Numeric reply: nothing to decode; caller will render numeric based on commandError + if (*numericReply) return true; + + // Payload reply: decode into reply buffer; decode failure => reply unknown + if (!decodeResponse(reply)) { + *commandError = CE_REPLY_UNKNOWN; + *numericReply = true; + *suppressFrame = false; + reply[0] = 0; + } + + return true; +} + +Features features; + +#endif diff --git a/src/telescope/auxiliary/remote/Features.h b/src/telescope/auxiliary/remote/Features.h new file mode 100755 index 0000000..2cad691 --- /dev/null +++ b/src/telescope/auxiliary/remote/Features.h @@ -0,0 +1,46 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope auxiliary FEATURES control, using the CANbus interface +#pragma once + +#include "../../../Common.h" + +#include "../FeaturesBase.h" + +#ifdef FEATURES_CAN_CLIENT_PRESENT + +#include "../../../lib/canTransport/CanTransportClient.h" +#include "../../../libApp/commands/ProcessCmds.h" + +class Features : public CanTransportClient { +public: + Features() + : CanTransportClient((uint16_t)(CAN_FEATURES_REQ_ID), (uint16_t)(CAN_FEATURES_RSP_ID), 0x1E) {} + + // initialize the aux features + void begin(); + + bool command(char *reply, char *command, char *parameter, + bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // Heartbeat event + inline void heartbeat() { lastHeartbeatMs = hb_stamp_2ms_odd(); } + +private: + bool encodeRequest(char *command, char *parameter); + bool decodeResponse(char *reply); + + static inline uint32_t hb_stamp_2ms_odd() { return millis() | 1UL; } + + inline bool heartbeatFresh() const { + const uint32_t last = ATOMIC_LOAD(lastHeartbeatMs); + if (last == 0U) return false; + const uint32_t now = hb_stamp_2ms_odd(); + return (uint32_t)(now - last) <= 2000U; + } + + volatile uint32_t lastHeartbeatMs = 0; +}; + +extern Features features; + +#endif diff --git a/src/telescope/focuser/Focuser.h b/src/telescope/focuser/Focuser.h new file mode 100755 index 0000000..d12394d --- /dev/null +++ b/src/telescope/focuser/Focuser.h @@ -0,0 +1,21 @@ +//-------------------------------------------------------------------------------------------------- +// local and remote telescope focuser control switchboard + +#pragma once + +#include "../../Common.h" + +#include "FocuserBase.h" + +// ----------------------------------------------------------------------------- +// Include the chosen implementation +// - CLIENT: Focuser is a proxy class +// - otherwise (local present): Focuser is the local hardware class +// and REMOTE (server) support is compiled in as additional code paths. +// ----------------------------------------------------------------------------- +#if defined(FOCUSER_CAN_CLIENT_PRESENT) + #include "remote/Focuser.h" // defines class Focuser (proxy) +#elif defined(FOCUSER_PRESENT) + #include "local/Focuser.h" // defines class Focuser (local hardware) + // conditionally includes/declares server hooks when FOCUSER_CAN_SERVER_PRESENT is defined +#endif diff --git a/src/telescope/focuser/FocuserBase.h b/src/telescope/focuser/FocuserBase.h new file mode 100755 index 0000000..3fdd032 --- /dev/null +++ b/src/telescope/focuser/FocuserBase.h @@ -0,0 +1,114 @@ +#pragma once + +#include "../../Common.h" + +// ----------------------------------------------------------------------------- +// CAN focuser transport base definitions (shared by local + remote) +// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- +// CAN IDs +// ----------------------------------------------------------------------------- +#ifndef CAN_FOCUSER_REQ_ID + #define CAN_FOCUSER_REQ_ID (0x00 + 4) +#endif + +#ifndef CAN_FOCUSER_RSP_ID + #define CAN_FOCUSER_RSP_ID (CAN_FOCUSER_REQ_ID + 0x10) +#endif + +// Heartbeats are separate IDs (one per focuser 1..6). +#ifndef CAN_FOCUSER_HB_ID_BASE + #define CAN_FOCUSER_HB_ID_BASE (CAN_FOCUSER_REQ_ID + 0x20) +#endif + +// ----------------------------------------------------------------------------- +// CAN_FOCUSER role selection +// OFF : no CAN focuser support +// ON : telescope/mount forwards focuser commands over CAN (client/proxy) +// REMOTE : remote node answers focuser commands over CAN (server/local AXIS4) +// ----------------------------------------------------------------------------- +#ifndef CAN_FOCUSER + #define CAN_FOCUSER OFF +#endif + +#if (CAN_FOCUSER == REMOTE) + #ifndef CAN_FOCUSER_NUMBER + #define CAN_FOCUSER_NUMBER 1 + #endif + #if (CAN_FOCUSER_NUMBER < 1) || (CAN_FOCUSER_NUMBER > 6) + #error "Config: CAN_FOCUSER_NUMBER must be 1..6 when CAN_FOCUSER == REMOTE." + #endif +#endif + +// ----------------------------------------------------------------------------- +// Validate CAN requirements +// ----------------------------------------------------------------------------- +#if (CAN_FOCUSER != OFF) + #if !defined(CAN_PLUS) || (CAN_PLUS == OFF) + #error "Focuser: CAN_FOCUSER requires CAN_PLUS to be enabled." + #endif +#endif + +// ----------------------------------------------------------------------------- +// Derive presence macros used by Telescope.cpp command router +// ----------------------------------------------------------------------------- +#if (CAN_FOCUSER == ON) + #define FOCUSER_CLIENT_PRESENT + #define FOCUSER_CAN_CLIENT_PRESENT +#elif (CAN_FOCUSER == REMOTE) + #define FOCUSER_CAN_SERVER_PRESENT +#endif + +// CLIENT build must NOT also have local Axis4..Axis9 focusers. +#if defined(FOCUSER_PRESENT) && defined(FOCUSER_CLIENT_PRESENT) + #error "Config: both local focusers (FOCUSER_PRESENT) and CAN_FOCUSER ON are enabled. Choose one." +#endif + +// REMOTE build MUST have local Axis4 focuser. +#if (CAN_FOCUSER == REMOTE) && (AXIS4_DRIVER_MODEL == OFF) + #error "Config: CAN_FOCUSER REMOTE requires a local AXIS4 focuser (AXIS4_DRIVER_MODEL != OFF)." +#endif + +// ----------------------------------------------------------------------------- +// Control byte (requestPayload[1]) +// ----------------------------------------------------------------------------- +#define FOC_CTRL_FOCUSER_MASK 0x07 // bits 0..2 store focuser number 1..6 +#define FOC_CTRL_UNIT_MICRONS 0x08 // bit 3: 1=microns, 0=steps + +// ----------------------------------------------------------------------------- +// CAN opcode set (5-bit) +// ----------------------------------------------------------------------------- +#define FOC_OP_GET_STATUS_T 0x00 +#define FOC_OP_GET_MODE_p 0x01 +#define FOC_OP_GET_MIN_I 0x02 +#define FOC_OP_GET_MAX_M 0x03 +#define FOC_OP_GET_TEMPDIFF_e 0x04 +#define FOC_OP_GET_TEMP_t 0x05 +#define FOC_OP_GET_UM_PER_STEP_u 0x06 + +#define FOC_OP_BACKLASH_B 0x07 // get: no args, set: i32LE +#define FOC_OP_PARK_HP 0x08 +#define FOC_OP_UNPARK_HR 0x09 + +#define FOC_OP_TCF_COEF_C 0x0A // get: no args, set: f32LE +#define FOC_OP_TCF_ENABLE_c 0x0B // get: no args, set: u8 (0/1) +#define FOC_OP_TCF_DEADBAND_D 0x0C // get: no args, set: i32LE +#define FOC_OP_DC_POWER_P 0x0D // get: no args, set: u8 (0..100) + +#define FOC_OP_STOP_Q 0x0E +#define FOC_OP_SET_RATE_1_9 0x0F // u8 digit 1..9 +#define FOC_OP_GET_WORKRATE_W 0x10 // i32LE (um/s) + +#define FOC_OP_MOVE_IN_PLUS 0x11 +#define FOC_OP_MOVE_OUT_MINUS 0x12 + +#define FOC_OP_GET_POS_G 0x13 // i32LE position (units) +#define FOC_OP_GOTO_REL_R 0x14 // i32LE delta (units) +#define FOC_OP_GOTO_ABS_S 0x15 // i32LE target (units) + +#define FOC_OP_ZERO_Z 0x16 +#define FOC_OP_SET_HOME_H 0x17 +#define FOC_OP_GOTO_HOME_h 0x18 + +#define FOC_OP_DRIVER_STATUS 0x19 // :GXU[n]# diff --git a/src/telescope/focuser/local/Focuser.axis.prototype.cpp b/src/telescope/focuser/local/Focuser.axis.prototype.cpp new file mode 100755 index 0000000..915f48b --- /dev/null +++ b/src/telescope/focuser/local/Focuser.axis.prototype.cpp @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, axis@ + +#include "Focuser.h" + +// note there is a prototype file that should be copy/pasted to build files +// for axes 4 to 9 by replacing all occurances of '@' with the axis number + +//#if AXIS@_DRIVER_MODEL != OFF +#if false + namespace { + + #if defined(AXIS@_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis@ = {AXIS@_DRIVER_MODEL, AXIS@_DRIVER_STATUS}; + KTechMotor motor_@(@, AXIS@_REVERSE, &DriverSettingsAxis@); + + #elif defined(AXIS@_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis@ = {AXIS@_DRIVER_MODEL, AXIS@_DRIVER_STATUS}; + Mks42DMotor motor_@(@, AXIS@_REVERSE, &DriverSettingsAxis@, AXIS@_STEPS_PER_DEGREE); + + #elif defined(AXIS@_SERVO_PRESENT) + ServoControl servoControlAxis@; + + #if AXIS@_ENCODER == AB + Quadrature encAxis@(@, AXIS@_ENCODER_A_PIN, AXIS@_ENCODER_B_PIN); + #elif AXIS@_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis@(@, AXIS@_ENCODER_A_PIN, AXIS@_ENCODER_B_PIN); + #elif AXIS@_ENCODER == CW_CCW + CwCcw encAxis@(@, AXIS@_ENCODER_A_PIN, AXIS@_ENCODER_B_PIN); + #elif AXIS@_ENCODER == PULSE_DIR + PulseDir encAxis@(@, AXIS@_ENCODER_A_PIN, AXIS@_ENCODER_B_PIN); + #elif AXIS@_ENCODER == PULSE_ONLY + PulseOnly encAxis@(@, AXIS@_ENCODER_A_PIN, &servoControlAxis@.directionHint); + #elif AXIS@_ENCODER == VIRTUAL + VirtualEnc encAxis@(@); + #elif AXIS@_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis@(@); + #endif + + #if AXIS@_SERVO_FEEDBACK == PID + Pid feedbackAxis@(AXIS@_PID_P, AXIS@_PID_I, AXIS@_PID_D); + #elif AXIS@_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis@(AXIS@_PID_P, AXIS@_PID_I, AXIS@_PID_D, AXIS@_PID_P_GOTO, AXIS@_PID_I_GOTO, AXIS@_PID_D_GOTO, AXIS@_PID_SENSITIVITY); + #endif + + #if AXIS@_SERVO_FLTR == KALMAN + KalmanFilter filterAxis@(AXIS@_SERVO_FLTR_MEAS_U, AXIS@_SERVO_FLTR_VARIANCE); + #elif AXIS@_SERVO_FLTR == ROLLING + RollingFilter filterAxis@(AXIS@_SERVO_FLTR_WSIZE); + #elif AXIS@_SERVO_FLTR == OFF + Filter filterAxis@; + #endif + + const ServoPins DriverPinsAxis@ = {AXIS@_SERVO_PH1_PIN, AXIS@_SERVO_PH1_STATE, AXIS@_SERVO_PH2_PIN, AXIS@_SERVO_PH2_STATE, AXIS@_ENABLE_PIN, AXIS@_ENABLE_STATE, AXIS@_M0_PIN, AXIS@_M1_PIN, AXIS@_M2_PIN, AXIS@_M3_PIN, AXIS@_FAULT_PIN}; + const ServoSettings DriverSettingsAxis@ = {AXIS@_DRIVER_MODEL, AXIS@_DRIVER_STATUS, AXIS@_SERVO_ACCELERATION}; + + #if AXIS@_DRIVER_MODEL == SERVO_EE + ServoEE driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_SERVO_DC_PWR_MIN, AXIS@_SERVO_DC_PWR_MAX); + #elif AXIS@_DRIVER_MODEL == SERVO_PE + ServoPE driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_SERVO_DC_PWR_MIN, AXIS@_SERVO_DC_PWR_MAX); + #elif AXIS@_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_SERVO_DC_PWR_MIN, AXIS@_SERVO_DC_PWR_MAX); + #elif AXIS@_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_SERVO_DC_PWR_MIN, AXIS@_SERVO_DC_PWR_MAX); + #elif AXIS@_DRIVER_MODEL == TMC2209 || AXIS@_DRIVER_MODEL == TMC2226 + ServoTmc2209 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_MOTOR_STEPS_PER_MICRON/AXIS@_STEPS_PER_MICRON, AXIS@_DRIVER_MICROSTEPS, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_DECAY, AXIS@_DRIVER_DECAY_GOTO); + #elif AXIS@_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_MOTOR_STEPS_PER_MICRON/AXIS@_STEPS_PER_MICRON, AXIS@_DRIVER_MICROSTEPS, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_DECAY, AXIS@_DRIVER_DECAY_GOTO); + #elif AXIS@_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver@(@, &DriverSettingsAxis@, AXIS@_MOTOR_STEPS_PER_MICRON/AXIS@_STEPS_PER_MICRON); + #endif + + ServoMotor motor_@(@, AXIS@_REVERSE, ((ServoDriver*)&driver@), &filterAxis@, &encAxis@, AXIS@_ENCODER_ORIGIN, AXIS@_ENCODER_REVERSE == ON, &feedbackAxis@, &servoControlAxis@); + + #elif defined(AXIS@_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis@ = {AXIS@_M0_PIN, AXIS@_M1_PIN, AXIS@_M2_PIN, AXIS@_M2_ON_STATE, AXIS@_M3_PIN, AXIS@_DECAY_PIN, AXIS@_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis@ = {AXIS@_DRIVER_MODEL, AXIS@_DRIVER_STATUS, AXIS@_DRIVER_MICROSTEPS, AXIS@_DRIVER_MICROSTEPS_GOTO, AXIS@_DRIVER_DECAY, AXIS@_DRIVER_DECAY_GOTO}; + #if AXIS@_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS@_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@); + #elif AXIS@_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_DRIVER_IHOLD, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_IGOTO, AXIS@_DRIVER_INTPOL); + #elif AXIS@_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_DRIVER_IHOLD, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_IGOTO, AXIS@_DRIVER_INTPOL); + #elif AXIS@_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_DRIVER_IHOLD, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_IGOTO, AXIS@_DRIVER_INTPOL); + #elif AXIS@_DRIVER_MODEL == TMC2209 || AXIS@_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_DRIVER_IHOLD, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_IGOTO, AXIS@_DRIVER_INTPOL); + #elif AXIS@_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_DRIVER_IHOLD, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_IGOTO, AXIS@_DRIVER_INTPOL); + #elif AXIS@_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_DRIVER_IHOLD, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_IGOTO, AXIS@_DRIVER_INTPOL); + #elif AXIS@_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver@(@, &DriverPinsAxis@, &DriverSettingsAxis@, AXIS@_DRIVER_IHOLD, AXIS@_DRIVER_IRUN, AXIS@_DRIVER_IGOTO, AXIS@_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis@ = {AXIS@_STEP_PIN, AXIS@_STEP_STATE, AXIS@_DIR_PIN, AXIS@_ENABLE_PIN, AXIS@_ENABLE_STATE}; + StepDirMotor motor_@(@, AXIS@_REVERSE, &StepDirPinsAxis@, ((StepDirDriver*)&driver@)); + #else + #error "Configuration (Config.h): Focuser has an unrecognized AXIS@_DRIVER_MODEL this should never happen!" + #endif + + const AxisPins PinsAxis@ = {AXIS@_SENSE_LIMIT_MIN_PIN, AXIS@_SENSE_HOME_PIN, AXIS@_SENSE_LIMIT_MAX_PIN, {AXIS@_SENSE_HOME, AXIS@_SENSE_HOME_INIT, AXIS@_SENSE_HOME_DIST_LIMIT*1000.0F, AXIS@_SENSE_LIMIT_MIN, AXIS@_SENSE_LIMIT_MAX, AXIS@_SENSE_LIMIT_INIT}}; + const AxisSettings SettingsAxis@ = {AXIS@_STEPS_PER_MICRON, {AXIS@_LIMIT_MIN*1000.0F, AXIS@_LIMIT_MAX*1000.0F}, AXIS@_BACKLASH_RATE}; + } + + Motor& motor@ = motor_@; + + Axis axis@(@, &PinsAxis@, &SettingsAxis@, AXIS_MEASURE_MICRONS); +#endif diff --git a/src/telescope/focuser/local/Focuser.axis4.cpp b/src/telescope/focuser/local/Focuser.axis4.cpp new file mode 100755 index 0000000..c70544e --- /dev/null +++ b/src/telescope/focuser/local/Focuser.axis4.cpp @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, axis4 + +#include "Focuser.h" + +// note there is a prototype file that should be copy/pasted to build files +// for axes 4 to 9 by replacing all occurances of '@' with the axis number + +#if AXIS4_DRIVER_MODEL != OFF + + namespace { + + #if defined(AXIS4_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis4 = {AXIS4_DRIVER_MODEL, AXIS4_DRIVER_STATUS}; + KTechMotor motor_4(4, AXIS4_REVERSE, &DriverSettingsAxis4); + + #elif defined(AXIS4_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis4 = {AXIS4_DRIVER_MODEL, AXIS4_DRIVER_STATUS}; + Mks42DMotor motor_4(4, AXIS4_REVERSE, &DriverSettingsAxis4, AXIS4_STEPS_PER_DEGREE); + + #elif defined(AXIS4_SERVO_PRESENT) + ServoControl servoControlAxis4; + + #if AXIS4_ENCODER == AB + Quadrature encAxis4(4, AXIS4_ENCODER_A_PIN, AXIS4_ENCODER_B_PIN); + #elif AXIS4_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis4(4, AXIS4_ENCODER_A_PIN, AXIS4_ENCODER_B_PIN); + #elif AXIS4_ENCODER == CW_CCW + CwCcw encAxis4(4, AXIS4_ENCODER_A_PIN, AXIS4_ENCODER_B_PIN); + #elif AXIS4_ENCODER == PULSE_DIR + PulseDir encAxis4(4, AXIS4_ENCODER_A_PIN, AXIS4_ENCODER_B_PIN); + #elif AXIS4_ENCODER == PULSE_ONLY + PulseOnly encAxis4(4, AXIS4_ENCODER_A_PIN, &servoControlAxis4.directionHint); + #elif AXIS4_ENCODER == VIRTUAL + VirtualEnc encAxis4(4); + #elif AXIS4_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis4(4); + #endif + + #if AXIS4_SERVO_FEEDBACK == PID + Pid feedbackAxis4(AXIS4_PID_P, AXIS4_PID_I, AXIS4_PID_D); + #elif AXIS4_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis4(AXIS4_PID_P, AXIS4_PID_I, AXIS4_PID_D, AXIS4_PID_P_GOTO, AXIS4_PID_I_GOTO, AXIS4_PID_D_GOTO, AXIS4_PID_SENSITIVITY); + #endif + + #if AXIS4_SERVO_FLTR == KALMAN + KalmanFilter filterAxis4(AXIS4_SERVO_FLTR_MEAS_U, AXIS4_SERVO_FLTR_VARIANCE); + #elif AXIS4_SERVO_FLTR == ROLLING + RollingFilter filterAxis4(AXIS4_SERVO_FLTR_WSIZE); + #elif AXIS4_SERVO_FLTR == OFF + Filter filterAxis4; + #endif + + const ServoPins DriverPinsAxis4 = {AXIS4_SERVO_PH1_PIN, AXIS4_SERVO_PH1_STATE, AXIS4_SERVO_PH2_PIN, AXIS4_SERVO_PH2_STATE, AXIS4_ENABLE_PIN, AXIS4_ENABLE_STATE, AXIS4_M0_PIN, AXIS4_M1_PIN, AXIS4_M2_PIN, AXIS4_M3_PIN, AXIS4_FAULT_PIN}; + const ServoSettings DriverSettingsAxis4 = {AXIS4_DRIVER_MODEL, AXIS4_DRIVER_STATUS, AXIS4_SERVO_ACCELERATION}; + + #if AXIS4_DRIVER_MODEL == SERVO_EE + ServoEE driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_SERVO_DC_PWR_MIN, AXIS4_SERVO_DC_PWR_MAX); + #elif AXIS4_DRIVER_MODEL == SERVO_PE + ServoPE driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_SERVO_DC_PWR_MIN, AXIS4_SERVO_DC_PWR_MAX); + #elif AXIS4_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_SERVO_DC_PWR_MIN, AXIS4_SERVO_DC_PWR_MAX); + #elif AXIS4_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_SERVO_DC_PWR_MIN, AXIS4_SERVO_DC_PWR_MAX); + #elif AXIS4_DRIVER_MODEL == TMC2209 || AXIS4_DRIVER_MODEL == TMC2226 + ServoTmc2209 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_MOTOR_STEPS_PER_MICRON/AXIS4_STEPS_PER_MICRON, AXIS4_DRIVER_MICROSTEPS, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_DECAY, AXIS4_DRIVER_DECAY_GOTO); + #elif AXIS4_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_MOTOR_STEPS_PER_MICRON/AXIS4_STEPS_PER_MICRON, AXIS4_DRIVER_MICROSTEPS, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_DECAY, AXIS4_DRIVER_DECAY_GOTO); + #elif AXIS4_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver4(4, &DriverSettingsAxis4, AXIS4_MOTOR_STEPS_PER_MICRON/AXIS4_STEPS_PER_MICRON); + #endif + + ServoMotor motor_4(4, AXIS4_REVERSE, ((ServoDriver*)&driver4), &filterAxis4, &encAxis4, AXIS4_ENCODER_ORIGIN, AXIS4_ENCODER_REVERSE == ON, &feedbackAxis4, &servoControlAxis4); + + #elif defined(AXIS4_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis4 = {AXIS4_M0_PIN, AXIS4_M1_PIN, AXIS4_M2_PIN, AXIS4_M2_ON_STATE, AXIS4_M3_PIN, AXIS4_DECAY_PIN, AXIS4_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis4 = {AXIS4_DRIVER_MODEL, AXIS4_DRIVER_STATUS, AXIS4_DRIVER_MICROSTEPS, AXIS4_DRIVER_MICROSTEPS_GOTO, AXIS4_DRIVER_DECAY, AXIS4_DRIVER_DECAY_GOTO}; + #if AXIS4_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS4_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4); + #elif AXIS4_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_DRIVER_IHOLD, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_IGOTO, AXIS4_DRIVER_INTPOL); + #elif AXIS4_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_DRIVER_IHOLD, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_IGOTO, AXIS4_DRIVER_INTPOL); + #elif AXIS4_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_DRIVER_IHOLD, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_IGOTO, AXIS4_DRIVER_INTPOL); + #elif AXIS4_DRIVER_MODEL == TMC2209 || AXIS4_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_DRIVER_IHOLD, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_IGOTO, AXIS4_DRIVER_INTPOL); + #elif AXIS4_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_DRIVER_IHOLD, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_IGOTO, AXIS4_DRIVER_INTPOL); + #elif AXIS4_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_DRIVER_IHOLD, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_IGOTO, AXIS4_DRIVER_INTPOL); + #elif AXIS4_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver4(4, &DriverPinsAxis4, &DriverSettingsAxis4, AXIS4_DRIVER_IHOLD, AXIS4_DRIVER_IRUN, AXIS4_DRIVER_IGOTO, AXIS4_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis4 = {AXIS4_STEP_PIN, AXIS4_STEP_STATE, AXIS4_DIR_PIN, AXIS4_ENABLE_PIN, AXIS4_ENABLE_STATE}; + StepDirMotor motor_4(4, AXIS4_REVERSE, &StepDirPinsAxis4, ((StepDirDriver*)&driver4)); + #else + #error "Configuration (Config.h): Focuser has an unrecognized AXIS4_DRIVER_MODEL this should never happen!" + #endif + + const AxisPins PinsAxis4 = {AXIS4_SENSE_LIMIT_MIN_PIN, AXIS4_SENSE_HOME_PIN, AXIS4_SENSE_LIMIT_MAX_PIN, {AXIS4_SENSE_HOME, AXIS4_SENSE_HOME_INIT, AXIS4_SENSE_HOME_DIST_LIMIT*1000.0F, AXIS4_SENSE_LIMIT_MIN, AXIS4_SENSE_LIMIT_MAX, AXIS4_SENSE_LIMIT_INIT}}; + const AxisSettings SettingsAxis4 = {AXIS4_STEPS_PER_MICRON, {AXIS4_LIMIT_MIN*1000.0F, AXIS4_LIMIT_MAX*1000.0F}, AXIS4_BACKLASH_RATE}; + } + + Motor& motor4 = motor_4; + + Axis axis4(4, &PinsAxis4, &SettingsAxis4, AXIS_MEASURE_MICRONS); +#endif diff --git a/src/telescope/focuser/local/Focuser.axis5.cpp b/src/telescope/focuser/local/Focuser.axis5.cpp new file mode 100755 index 0000000..5802163 --- /dev/null +++ b/src/telescope/focuser/local/Focuser.axis5.cpp @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, axis5 + +#include "Focuser.h" + +// note there is a prototype file that should be copy/pasted to build files +// for axes 4 to 9 by replacing all occurances of '@' with the axis number + +#if AXIS5_DRIVER_MODEL != OFF + + namespace { + + #if defined(AXIS5_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis5 = {AXIS5_DRIVER_MODEL, AXIS5_DRIVER_STATUS}; + KTechMotor motor_5(5, AXIS5_REVERSE, &DriverSettingsAxis5); + + #elif defined(AXIS5_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis5 = {AXIS5_DRIVER_MODEL, AXIS5_DRIVER_STATUS}; + Mks42DMotor motor_5(5, AXIS5_REVERSE, &DriverSettingsAxis5, AXIS5_STEPS_PER_DEGREE); + + #elif defined(AXIS5_SERVO_PRESENT) + ServoControl servoControlAxis5; + + #if AXIS5_ENCODER == AB + Quadrature encAxis5(5, AXIS5_ENCODER_A_PIN, AXIS5_ENCODER_B_PIN); + #elif AXIS5_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis5(5, AXIS5_ENCODER_A_PIN, AXIS5_ENCODER_B_PIN); + #elif AXIS5_ENCODER == CW_CCW + CwCcw encAxis5(5, AXIS5_ENCODER_A_PIN, AXIS5_ENCODER_B_PIN); + #elif AXIS5_ENCODER == PULSE_DIR + PulseDir encAxis5(5, AXIS5_ENCODER_A_PIN, AXIS5_ENCODER_B_PIN); + #elif AXIS5_ENCODER == PULSE_ONLY + PulseOnly encAxis5(5, AXIS5_ENCODER_A_PIN, &servoControlAxis5.directionHint); + #elif AXIS5_ENCODER == VIRTUAL + VirtualEnc encAxis5(5); + #elif AXIS5_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis5(5); + #endif + + #if AXIS5_SERVO_FEEDBACK == PID + Pid feedbackAxis5(AXIS5_PID_P, AXIS5_PID_I, AXIS5_PID_D); + #elif AXIS5_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis5(AXIS5_PID_P, AXIS5_PID_I, AXIS5_PID_D, AXIS5_PID_P_GOTO, AXIS5_PID_I_GOTO, AXIS5_PID_D_GOTO, AXIS5_PID_SENSITIVITY); + #endif + + #if AXIS5_SERVO_FLTR == KALMAN + KalmanFilter filterAxis5(AXIS5_SERVO_FLTR_MEAS_U, AXIS5_SERVO_FLTR_VARIANCE); + #elif AXIS5_SERVO_FLTR == ROLLING + RollingFilter filterAxis5(AXIS5_SERVO_FLTR_WSIZE); + #elif AXIS5_SERVO_FLTR == OFF + Filter filterAxis5; + #endif + + const ServoPins DriverPinsAxis5 = {AXIS5_SERVO_PH1_PIN, AXIS5_SERVO_PH1_STATE, AXIS5_SERVO_PH2_PIN, AXIS5_SERVO_PH2_STATE, AXIS5_ENABLE_PIN, AXIS5_ENABLE_STATE, AXIS5_M0_PIN, AXIS5_M1_PIN, AXIS5_M2_PIN, AXIS5_M3_PIN, AXIS5_FAULT_PIN}; + const ServoSettings DriverSettingsAxis5 = {AXIS5_DRIVER_MODEL, AXIS5_DRIVER_STATUS, AXIS5_SERVO_ACCELERATION}; + + #if AXIS5_DRIVER_MODEL == SERVO_EE + ServoEE driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_SERVO_DC_PWR_MIN, AXIS5_SERVO_DC_PWR_MAX); + #elif AXIS5_DRIVER_MODEL == SERVO_PE + ServoPE driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_SERVO_DC_PWR_MIN, AXIS5_SERVO_DC_PWR_MAX); + #elif AXIS5_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_SERVO_DC_PWR_MIN, AXIS5_SERVO_DC_PWR_MAX); + #elif AXIS5_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_SERVO_DC_PWR_MIN, AXIS5_SERVO_DC_PWR_MAX); + #elif AXIS5_DRIVER_MODEL == TMC2209 || AXIS5_DRIVER_MODEL == TMC2226 + ServoTmc2209 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_MOTOR_STEPS_PER_MICRON/AXIS5_STEPS_PER_MICRON, AXIS5_DRIVER_MICROSTEPS, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_DECAY, AXIS5_DRIVER_DECAY_GOTO); + #elif AXIS5_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_MOTOR_STEPS_PER_MICRON/AXIS5_STEPS_PER_MICRON, AXIS5_DRIVER_MICROSTEPS, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_DECAY, AXIS5_DRIVER_DECAY_GOTO); + #elif AXIS5_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver5(5, &DriverSettingsAxis5, AXIS5_MOTOR_STEPS_PER_MICRON/AXIS5_STEPS_PER_MICRON); + #endif + + ServoMotor motor_5(5, AXIS5_REVERSE, ((ServoDriver*)&driver5), &filterAxis5, &encAxis5, AXIS5_ENCODER_ORIGIN, AXIS5_ENCODER_REVERSE == ON, &feedbackAxis5, &servoControlAxis5); + + #elif defined(AXIS5_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis5 = {AXIS5_M0_PIN, AXIS5_M1_PIN, AXIS5_M2_PIN, AXIS5_M2_ON_STATE, AXIS5_M3_PIN, AXIS5_DECAY_PIN, AXIS5_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis5 = {AXIS5_DRIVER_MODEL, AXIS5_DRIVER_STATUS, AXIS5_DRIVER_MICROSTEPS, AXIS5_DRIVER_MICROSTEPS_GOTO, AXIS5_DRIVER_DECAY, AXIS5_DRIVER_DECAY_GOTO}; + #if AXIS5_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS5_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5); + #elif AXIS5_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_DRIVER_IHOLD, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_IGOTO, AXIS5_DRIVER_INTPOL); + #elif AXIS5_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_DRIVER_IHOLD, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_IGOTO, AXIS5_DRIVER_INTPOL); + #elif AXIS5_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_DRIVER_IHOLD, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_IGOTO, AXIS5_DRIVER_INTPOL); + #elif AXIS5_DRIVER_MODEL == TMC2209 || AXIS5_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_DRIVER_IHOLD, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_IGOTO, AXIS5_DRIVER_INTPOL); + #elif AXIS5_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_DRIVER_IHOLD, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_IGOTO, AXIS5_DRIVER_INTPOL); + #elif AXIS5_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_DRIVER_IHOLD, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_IGOTO, AXIS5_DRIVER_INTPOL); + #elif AXIS5_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver5(5, &DriverPinsAxis5, &DriverSettingsAxis5, AXIS5_DRIVER_IHOLD, AXIS5_DRIVER_IRUN, AXIS5_DRIVER_IGOTO, AXIS5_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis5 = {AXIS5_STEP_PIN, AXIS5_STEP_STATE, AXIS5_DIR_PIN, AXIS5_ENABLE_PIN, AXIS5_ENABLE_STATE}; + StepDirMotor motor_5(5, AXIS5_REVERSE, &StepDirPinsAxis5, ((StepDirDriver*)&driver5)); + #else + #error "Configuration (Config.h): Focuser has an unrecognized AXIS5_DRIVER_MODEL this should never happen!" + #endif + + const AxisPins PinsAxis5 = {AXIS5_SENSE_LIMIT_MIN_PIN, AXIS5_SENSE_HOME_PIN, AXIS5_SENSE_LIMIT_MAX_PIN, {AXIS5_SENSE_HOME, AXIS5_SENSE_HOME_INIT, AXIS5_SENSE_HOME_DIST_LIMIT*1000.0F, AXIS5_SENSE_LIMIT_MIN, AXIS5_SENSE_LIMIT_MAX, AXIS5_SENSE_LIMIT_INIT}}; + const AxisSettings SettingsAxis5 = {AXIS5_STEPS_PER_MICRON, {AXIS5_LIMIT_MIN*1000.0F, AXIS5_LIMIT_MAX*1000.0F}, AXIS5_BACKLASH_RATE}; + } + + Motor& motor5 = motor_5; + + Axis axis5(5, &PinsAxis5, &SettingsAxis5, AXIS_MEASURE_MICRONS); +#endif diff --git a/src/telescope/focuser/local/Focuser.axis6.cpp b/src/telescope/focuser/local/Focuser.axis6.cpp new file mode 100755 index 0000000..d25eb5a --- /dev/null +++ b/src/telescope/focuser/local/Focuser.axis6.cpp @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, axis6 + +#include "Focuser.h" + +// note there is a prototype file that should be copy/pasted to build files +// for axes 4 to 9 by replacing all occurances of '@' with the axis number + +#if AXIS6_DRIVER_MODEL != OFF + + namespace { + + #if defined(AXIS6_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis6 = {AXIS6_DRIVER_MODEL, AXIS6_DRIVER_STATUS}; + KTechMotor motor_6(6, AXIS6_REVERSE, &DriverSettingsAxis6); + + #elif defined(AXIS6_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis6 = {AXIS6_DRIVER_MODEL, AXIS6_DRIVER_STATUS}; + Mks42DMotor motor_6(6, AXIS6_REVERSE, &DriverSettingsAxis6, AXIS6_STEPS_PER_DEGREE); + + #elif defined(AXIS6_SERVO_PRESENT) + ServoControl servoControlAxis6; + + #if AXIS6_ENCODER == AB + Quadrature encAxis6(6, AXIS6_ENCODER_A_PIN, AXIS6_ENCODER_B_PIN); + #elif AXIS6_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis6(6, AXIS6_ENCODER_A_PIN, AXIS6_ENCODER_B_PIN); + #elif AXIS6_ENCODER == CW_CCW + CwCcw encAxis6(6, AXIS6_ENCODER_A_PIN, AXIS6_ENCODER_B_PIN); + #elif AXIS6_ENCODER == PULSE_DIR + PulseDir encAxis6(6, AXIS6_ENCODER_A_PIN, AXIS6_ENCODER_B_PIN); + #elif AXIS6_ENCODER == PULSE_ONLY + PulseOnly encAxis6(6, AXIS6_ENCODER_A_PIN, &servoControlAxis6.directionHint); + #elif AXIS6_ENCODER == VIRTUAL + VirtualEnc encAxis6(6); + #elif AXIS6_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis6(6); + #endif + + #if AXIS6_SERVO_FEEDBACK == PID + Pid feedbackAxis6(AXIS6_PID_P, AXIS6_PID_I, AXIS6_PID_D); + #elif AXIS6_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis6(AXIS6_PID_P, AXIS6_PID_I, AXIS6_PID_D, AXIS6_PID_P_GOTO, AXIS6_PID_I_GOTO, AXIS6_PID_D_GOTO, AXIS6_PID_SENSITIVITY); + #endif + + #if AXIS6_SERVO_FLTR == KALMAN + KalmanFilter filterAxis6(AXIS6_SERVO_FLTR_MEAS_U, AXIS6_SERVO_FLTR_VARIANCE); + #elif AXIS6_SERVO_FLTR == ROLLING + RollingFilter filterAxis6(AXIS6_SERVO_FLTR_WSIZE); + #elif AXIS6_SERVO_FLTR == OFF + Filter filterAxis6; + #endif + + const ServoPins DriverPinsAxis6 = {AXIS6_SERVO_PH1_PIN, AXIS6_SERVO_PH1_STATE, AXIS6_SERVO_PH2_PIN, AXIS6_SERVO_PH2_STATE, AXIS6_ENABLE_PIN, AXIS6_ENABLE_STATE, AXIS6_M0_PIN, AXIS6_M1_PIN, AXIS6_M2_PIN, AXIS6_M3_PIN, AXIS6_FAULT_PIN}; + const ServoSettings DriverSettingsAxis6 = {AXIS6_DRIVER_MODEL, AXIS6_DRIVER_STATUS, AXIS6_SERVO_ACCELERATION}; + + #if AXIS6_DRIVER_MODEL == SERVO_EE + ServoEE driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_SERVO_DC_PWR_MIN, AXIS6_SERVO_DC_PWR_MAX); + #elif AXIS6_DRIVER_MODEL == SERVO_PE + ServoPE driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_SERVO_DC_PWR_MIN, AXIS6_SERVO_DC_PWR_MAX); + #elif AXIS6_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_SERVO_DC_PWR_MIN, AXIS6_SERVO_DC_PWR_MAX); + #elif AXIS6_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_SERVO_DC_PWR_MIN, AXIS6_SERVO_DC_PWR_MAX); + #elif AXIS6_DRIVER_MODEL == TMC2209 || AXIS6_DRIVER_MODEL == TMC2226 + ServoTmc2209 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_MOTOR_STEPS_PER_MICRON/AXIS6_STEPS_PER_MICRON, AXIS6_DRIVER_MICROSTEPS, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_DECAY, AXIS6_DRIVER_DECAY_GOTO); + #elif AXIS6_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_MOTOR_STEPS_PER_MICRON/AXIS6_STEPS_PER_MICRON, AXIS6_DRIVER_MICROSTEPS, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_DECAY, AXIS6_DRIVER_DECAY_GOTO); + #elif AXIS6_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver6(6, &DriverSettingsAxis6, AXIS6_MOTOR_STEPS_PER_MICRON/AXIS6_STEPS_PER_MICRON); + #endif + + ServoMotor motor_6(6, AXIS6_REVERSE, ((ServoDriver*)&driver6), &filterAxis6, &encAxis6, AXIS6_ENCODER_ORIGIN, AXIS6_ENCODER_REVERSE == ON, &feedbackAxis6, &servoControlAxis6); + + #elif defined(AXIS6_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis6 = {AXIS6_M0_PIN, AXIS6_M1_PIN, AXIS6_M2_PIN, AXIS6_M2_ON_STATE, AXIS6_M3_PIN, AXIS6_DECAY_PIN, AXIS6_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis6 = {AXIS6_DRIVER_MODEL, AXIS6_DRIVER_STATUS, AXIS6_DRIVER_MICROSTEPS, AXIS6_DRIVER_MICROSTEPS_GOTO, AXIS6_DRIVER_DECAY, AXIS6_DRIVER_DECAY_GOTO}; + #if AXIS6_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS6_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6); + #elif AXIS6_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_DRIVER_IHOLD, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_IGOTO, AXIS6_DRIVER_INTPOL); + #elif AXIS6_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_DRIVER_IHOLD, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_IGOTO, AXIS6_DRIVER_INTPOL); + #elif AXIS6_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_DRIVER_IHOLD, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_IGOTO, AXIS6_DRIVER_INTPOL); + #elif AXIS6_DRIVER_MODEL == TMC2209 || AXIS6_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_DRIVER_IHOLD, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_IGOTO, AXIS6_DRIVER_INTPOL); + #elif AXIS6_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_DRIVER_IHOLD, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_IGOTO, AXIS6_DRIVER_INTPOL); + #elif AXIS6_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_DRIVER_IHOLD, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_IGOTO, AXIS6_DRIVER_INTPOL); + #elif AXIS6_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver6(6, &DriverPinsAxis6, &DriverSettingsAxis6, AXIS6_DRIVER_IHOLD, AXIS6_DRIVER_IRUN, AXIS6_DRIVER_IGOTO, AXIS6_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis6 = {AXIS6_STEP_PIN, AXIS6_STEP_STATE, AXIS6_DIR_PIN, AXIS6_ENABLE_PIN, AXIS6_ENABLE_STATE}; + StepDirMotor motor_6(6, AXIS6_REVERSE, &StepDirPinsAxis6, ((StepDirDriver*)&driver6)); + #else + #error "Configuration (Config.h): Focuser has an unrecognized AXIS6_DRIVER_MODEL this should never happen!" + #endif + + const AxisPins PinsAxis6 = {AXIS6_SENSE_LIMIT_MIN_PIN, AXIS6_SENSE_HOME_PIN, AXIS6_SENSE_LIMIT_MAX_PIN, {AXIS6_SENSE_HOME, AXIS6_SENSE_HOME_INIT, AXIS6_SENSE_HOME_DIST_LIMIT*1000.0F, AXIS6_SENSE_LIMIT_MIN, AXIS6_SENSE_LIMIT_MAX, AXIS6_SENSE_LIMIT_INIT}}; + const AxisSettings SettingsAxis6 = {AXIS6_STEPS_PER_MICRON, {AXIS6_LIMIT_MIN*1000.0F, AXIS6_LIMIT_MAX*1000.0F}, AXIS6_BACKLASH_RATE}; + } + + Motor& motor6 = motor_6; + + Axis axis6(6, &PinsAxis6, &SettingsAxis6, AXIS_MEASURE_MICRONS); +#endif diff --git a/src/telescope/focuser/local/Focuser.axis7.cpp b/src/telescope/focuser/local/Focuser.axis7.cpp new file mode 100755 index 0000000..a33df53 --- /dev/null +++ b/src/telescope/focuser/local/Focuser.axis7.cpp @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, axis7 + +#include "Focuser.h" + +// note there is a prototype file that should be copy/pasted to build files +// for axes 4 to 9 by replacing all occurances of '@' with the axis number + +#if AXIS7_DRIVER_MODEL != OFF + + namespace { + + #if defined(AXIS7_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis7 = {AXIS7_DRIVER_MODEL, AXIS7_DRIVER_STATUS}; + KTechMotor motor_7(7, AXIS7_REVERSE, &DriverSettingsAxis7); + + #elif defined(AXIS7_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis7 = {AXIS7_DRIVER_MODEL, AXIS7_DRIVER_STATUS}; + Mks42DMotor motor_7(7, AXIS7_REVERSE, &DriverSettingsAxis7, AXIS7_STEPS_PER_DEGREE); + + #elif defined(AXIS7_SERVO_PRESENT) + ServoControl servoControlAxis7; + + #if AXIS7_ENCODER == AB + Quadrature encAxis7(7, AXIS7_ENCODER_A_PIN, AXIS7_ENCODER_B_PIN); + #elif AXIS7_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis7(7, AXIS7_ENCODER_A_PIN, AXIS7_ENCODER_B_PIN); + #elif AXIS7_ENCODER == CW_CCW + CwCcw encAxis7(7, AXIS7_ENCODER_A_PIN, AXIS7_ENCODER_B_PIN); + #elif AXIS7_ENCODER == PULSE_DIR + PulseDir encAxis7(7, AXIS7_ENCODER_A_PIN, AXIS7_ENCODER_B_PIN); + #elif AXIS7_ENCODER == PULSE_ONLY + PulseOnly encAxis7(7, AXIS7_ENCODER_A_PIN, &servoControlAxis7.directionHint); + #elif AXIS7_ENCODER == VIRTUAL + VirtualEnc encAxis7(7); + #elif AXIS7_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis7(7); + #endif + + #if AXIS7_SERVO_FEEDBACK == PID + Pid feedbackAxis7(AXIS7_PID_P, AXIS7_PID_I, AXIS7_PID_D); + #elif AXIS7_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis7(AXIS7_PID_P, AXIS7_PID_I, AXIS7_PID_D, AXIS7_PID_P_GOTO, AXIS7_PID_I_GOTO, AXIS7_PID_D_GOTO, AXIS7_PID_SENSITIVITY); + #endif + + #if AXIS7_SERVO_FLTR == KALMAN + KalmanFilter filterAxis7(AXIS7_SERVO_FLTR_MEAS_U, AXIS7_SERVO_FLTR_VARIANCE); + #elif AXIS7_SERVO_FLTR == ROLLING + RollingFilter filterAxis7(AXIS7_SERVO_FLTR_WSIZE); + #elif AXIS7_SERVO_FLTR == OFF + Filter filterAxis7; + #endif + + const ServoPins DriverPinsAxis7 = {AXIS7_SERVO_PH1_PIN, AXIS7_SERVO_PH1_STATE, AXIS7_SERVO_PH2_PIN, AXIS7_SERVO_PH2_STATE, AXIS7_ENABLE_PIN, AXIS7_ENABLE_STATE, AXIS7_M0_PIN, AXIS7_M1_PIN, AXIS7_M2_PIN, AXIS7_M3_PIN, AXIS7_FAULT_PIN}; + const ServoSettings DriverSettingsAxis7 = {AXIS7_DRIVER_MODEL, AXIS7_DRIVER_STATUS, AXIS7_SERVO_ACCELERATION}; + + #if AXIS7_DRIVER_MODEL == SERVO_EE + ServoEE driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_SERVO_DC_PWR_MIN, AXIS7_SERVO_DC_PWR_MAX); + #elif AXIS7_DRIVER_MODEL == SERVO_PE + ServoPE driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_SERVO_DC_PWR_MIN, AXIS7_SERVO_DC_PWR_MAX); + #elif AXIS7_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_SERVO_DC_PWR_MIN, AXIS7_SERVO_DC_PWR_MAX); + #elif AXIS7_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_SERVO_DC_PWR_MIN, AXIS7_SERVO_DC_PWR_MAX); + #elif AXIS7_DRIVER_MODEL == TMC2209 || AXIS7_DRIVER_MODEL == TMC2226 + ServoTmc2209 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_MOTOR_STEPS_PER_MICRON/AXIS7_STEPS_PER_MICRON, AXIS7_DRIVER_MICROSTEPS, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_DECAY, AXIS7_DRIVER_DECAY_GOTO); + #elif AXIS7_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_MOTOR_STEPS_PER_MICRON/AXIS7_STEPS_PER_MICRON, AXIS7_DRIVER_MICROSTEPS, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_DECAY, AXIS7_DRIVER_DECAY_GOTO); + #elif AXIS7_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver7(7, &DriverSettingsAxis7, AXIS7_MOTOR_STEPS_PER_MICRON/AXIS7_STEPS_PER_MICRON); + #endif + + ServoMotor motor_7(7, AXIS7_REVERSE, ((ServoDriver*)&driver7), &filterAxis7, &encAxis7, AXIS7_ENCODER_ORIGIN, AXIS7_ENCODER_REVERSE == ON, &feedbackAxis7, &servoControlAxis7); + + #elif defined(AXIS7_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis7 = {AXIS7_M0_PIN, AXIS7_M1_PIN, AXIS7_M2_PIN, AXIS7_M2_ON_STATE, AXIS7_M3_PIN, AXIS7_DECAY_PIN, AXIS7_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis7 = {AXIS7_DRIVER_MODEL, AXIS7_DRIVER_STATUS, AXIS7_DRIVER_MICROSTEPS, AXIS7_DRIVER_MICROSTEPS_GOTO, AXIS7_DRIVER_DECAY, AXIS7_DRIVER_DECAY_GOTO}; + #if AXIS7_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS7_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7); + #elif AXIS7_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_DRIVER_IHOLD, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_IGOTO, AXIS7_DRIVER_INTPOL); + #elif AXIS7_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_DRIVER_IHOLD, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_IGOTO, AXIS7_DRIVER_INTPOL); + #elif AXIS7_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_DRIVER_IHOLD, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_IGOTO, AXIS7_DRIVER_INTPOL); + #elif AXIS7_DRIVER_MODEL == TMC2209 || AXIS7_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_DRIVER_IHOLD, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_IGOTO, AXIS7_DRIVER_INTPOL); + #elif AXIS7_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_DRIVER_IHOLD, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_IGOTO, AXIS7_DRIVER_INTPOL); + #elif AXIS7_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_DRIVER_IHOLD, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_IGOTO, AXIS7_DRIVER_INTPOL); + #elif AXIS7_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver7(7, &DriverPinsAxis7, &DriverSettingsAxis7, AXIS7_DRIVER_IHOLD, AXIS7_DRIVER_IRUN, AXIS7_DRIVER_IGOTO, AXIS7_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis7 = {AXIS7_STEP_PIN, AXIS7_STEP_STATE, AXIS7_DIR_PIN, AXIS7_ENABLE_PIN, AXIS7_ENABLE_STATE}; + StepDirMotor motor_7(7, AXIS7_REVERSE, &StepDirPinsAxis7, ((StepDirDriver*)&driver7)); + #else + #error "Configuration (Config.h): Focuser has an unrecognized AXIS7_DRIVER_MODEL this should never happen!" + #endif + + const AxisPins PinsAxis7 = {AXIS7_SENSE_LIMIT_MIN_PIN, AXIS7_SENSE_HOME_PIN, AXIS7_SENSE_LIMIT_MAX_PIN, {AXIS7_SENSE_HOME, AXIS7_SENSE_HOME_INIT, AXIS7_SENSE_HOME_DIST_LIMIT*1000.0F, AXIS7_SENSE_LIMIT_MIN, AXIS7_SENSE_LIMIT_MAX, AXIS7_SENSE_LIMIT_INIT}}; + const AxisSettings SettingsAxis7 = {AXIS7_STEPS_PER_MICRON, {AXIS7_LIMIT_MIN*1000.0F, AXIS7_LIMIT_MAX*1000.0F}, AXIS7_BACKLASH_RATE}; + } + + Motor& motor7 = motor_7; + + Axis axis7(7, &PinsAxis7, &SettingsAxis7, AXIS_MEASURE_MICRONS); +#endif diff --git a/src/telescope/focuser/local/Focuser.axis8.cpp b/src/telescope/focuser/local/Focuser.axis8.cpp new file mode 100755 index 0000000..89433cc --- /dev/null +++ b/src/telescope/focuser/local/Focuser.axis8.cpp @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, axis8 + +#include "Focuser.h" + +// note there is a prototype file that should be copy/pasted to build files +// for axes 4 to 9 by replacing all occurances of '@' with the axis number + +#if AXIS8_DRIVER_MODEL != OFF + + namespace { + + #if defined(AXIS8_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis8 = {AXIS8_DRIVER_MODEL, AXIS8_DRIVER_STATUS}; + KTechMotor motor_8(8, AXIS8_REVERSE, &DriverSettingsAxis8); + + #elif defined(AXIS8_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis8 = {AXIS8_DRIVER_MODEL, AXIS8_DRIVER_STATUS}; + Mks42DMotor motor_8(8, AXIS8_REVERSE, &DriverSettingsAxis8, AXIS8_STEPS_PER_DEGREE); + + #elif defined(AXIS8_SERVO_PRESENT) + ServoControl servoControlAxis8; + + #if AXIS8_ENCODER == AB + Quadrature encAxis8(8, AXIS8_ENCODER_A_PIN, AXIS8_ENCODER_B_PIN); + #elif AXIS8_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis8(8, AXIS8_ENCODER_A_PIN, AXIS8_ENCODER_B_PIN); + #elif AXIS8_ENCODER == CW_CCW + CwCcw encAxis8(8, AXIS8_ENCODER_A_PIN, AXIS8_ENCODER_B_PIN); + #elif AXIS8_ENCODER == PULSE_DIR + PulseDir encAxis8(8, AXIS8_ENCODER_A_PIN, AXIS8_ENCODER_B_PIN); + #elif AXIS8_ENCODER == PULSE_ONLY + PulseOnly encAxis8(8, AXIS8_ENCODER_A_PIN, &servoControlAxis8.directionHint); + #elif AXIS8_ENCODER == VIRTUAL + VirtualEnc encAxis8(8); + #elif AXIS8_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis8(8); + #endif + + #if AXIS8_SERVO_FEEDBACK == PID + Pid feedbackAxis8(AXIS8_PID_P, AXIS8_PID_I, AXIS8_PID_D); + #elif AXIS8_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis8(AXIS8_PID_P, AXIS8_PID_I, AXIS8_PID_D, AXIS8_PID_P_GOTO, AXIS8_PID_I_GOTO, AXIS8_PID_D_GOTO, AXIS8_PID_SENSITIVITY); + #endif + + #if AXIS8_SERVO_FLTR == KALMAN + KalmanFilter filterAxis8(AXIS8_SERVO_FLTR_MEAS_U, AXIS8_SERVO_FLTR_VARIANCE); + #elif AXIS8_SERVO_FLTR == ROLLING + RollingFilter filterAxis8(AXIS8_SERVO_FLTR_WSIZE); + #elif AXIS8_SERVO_FLTR == OFF + Filter filterAxis8; + #endif + + const ServoPins DriverPinsAxis8 = {AXIS8_SERVO_PH1_PIN, AXIS8_SERVO_PH1_STATE, AXIS8_SERVO_PH2_PIN, AXIS8_SERVO_PH2_STATE, AXIS8_ENABLE_PIN, AXIS8_ENABLE_STATE, AXIS8_M0_PIN, AXIS8_M1_PIN, AXIS8_M2_PIN, AXIS8_M3_PIN, AXIS8_FAULT_PIN}; + const ServoSettings DriverSettingsAxis8 = {AXIS8_DRIVER_MODEL, AXIS8_DRIVER_STATUS, AXIS8_SERVO_ACCELERATION}; + + #if AXIS8_DRIVER_MODEL == SERVO_EE + ServoEE driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_SERVO_DC_PWR_MIN, AXIS8_SERVO_DC_PWR_MAX); + #elif AXIS8_DRIVER_MODEL == SERVO_PE + ServoPE driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_SERVO_DC_PWR_MIN, AXIS8_SERVO_DC_PWR_MAX); + #elif AXIS8_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_SERVO_DC_PWR_MIN, AXIS8_SERVO_DC_PWR_MAX); + #elif AXIS8_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_SERVO_DC_PWR_MIN, AXIS8_SERVO_DC_PWR_MAX); + #elif AXIS8_DRIVER_MODEL == TMC2209 || AXIS8_DRIVER_MODEL == TMC2226 + ServoTmc2209 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_MOTOR_STEPS_PER_MICRON/AXIS8_STEPS_PER_MICRON, AXIS8_DRIVER_MICROSTEPS, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_DECAY, AXIS8_DRIVER_DECAY_GOTO); + #elif AXIS8_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_MOTOR_STEPS_PER_MICRON/AXIS8_STEPS_PER_MICRON, AXIS8_DRIVER_MICROSTEPS, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_DECAY, AXIS8_DRIVER_DECAY_GOTO); + #elif AXIS8_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver8(8, &DriverSettingsAxis8, AXIS8_MOTOR_STEPS_PER_MICRON/AXIS8_STEPS_PER_MICRON); + #endif + + ServoMotor motor_8(8, AXIS8_REVERSE, ((ServoDriver*)&driver8), &filterAxis8, &encAxis8, AXIS8_ENCODER_ORIGIN, AXIS8_ENCODER_REVERSE == ON, &feedbackAxis8, &servoControlAxis8); + + #elif defined(AXIS8_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis8 = {AXIS8_M0_PIN, AXIS8_M1_PIN, AXIS8_M2_PIN, AXIS8_M2_ON_STATE, AXIS8_M3_PIN, AXIS8_DECAY_PIN, AXIS8_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis8 = {AXIS8_DRIVER_MODEL, AXIS8_DRIVER_STATUS, AXIS8_DRIVER_MICROSTEPS, AXIS8_DRIVER_MICROSTEPS_GOTO, AXIS8_DRIVER_DECAY, AXIS8_DRIVER_DECAY_GOTO}; + #if AXIS8_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS8_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8); + #elif AXIS8_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_DRIVER_IHOLD, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_IGOTO, AXIS8_DRIVER_INTPOL); + #elif AXIS8_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_DRIVER_IHOLD, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_IGOTO, AXIS8_DRIVER_INTPOL); + #elif AXIS8_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_DRIVER_IHOLD, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_IGOTO, AXIS8_DRIVER_INTPOL); + #elif AXIS8_DRIVER_MODEL == TMC2209 || AXIS8_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_DRIVER_IHOLD, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_IGOTO, AXIS8_DRIVER_INTPOL); + #elif AXIS8_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_DRIVER_IHOLD, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_IGOTO, AXIS8_DRIVER_INTPOL); + #elif AXIS8_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_DRIVER_IHOLD, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_IGOTO, AXIS8_DRIVER_INTPOL); + #elif AXIS8_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver8(8, &DriverPinsAxis8, &DriverSettingsAxis8, AXIS8_DRIVER_IHOLD, AXIS8_DRIVER_IRUN, AXIS8_DRIVER_IGOTO, AXIS8_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis8 = {AXIS8_STEP_PIN, AXIS8_STEP_STATE, AXIS8_DIR_PIN, AXIS8_ENABLE_PIN, AXIS8_ENABLE_STATE}; + StepDirMotor motor_8(8, AXIS8_REVERSE, &StepDirPinsAxis8, ((StepDirDriver*)&driver8)); + #else + #error "Configuration (Config.h): Focuser has an unrecognized AXIS8_DRIVER_MODEL this should never happen!" + #endif + + const AxisPins PinsAxis8 = {AXIS8_SENSE_LIMIT_MIN_PIN, AXIS8_SENSE_HOME_PIN, AXIS8_SENSE_LIMIT_MAX_PIN, {AXIS8_SENSE_HOME, AXIS8_SENSE_HOME_INIT, AXIS8_SENSE_HOME_DIST_LIMIT*1000.0F, AXIS8_SENSE_LIMIT_MIN, AXIS8_SENSE_LIMIT_MAX, AXIS8_SENSE_LIMIT_INIT}}; + const AxisSettings SettingsAxis8 = {AXIS8_STEPS_PER_MICRON, {AXIS8_LIMIT_MIN*1000.0F, AXIS8_LIMIT_MAX*1000.0F}, AXIS8_BACKLASH_RATE}; + } + + Motor& motor8 = motor_8; + + Axis axis8(8, &PinsAxis8, &SettingsAxis8, AXIS_MEASURE_MICRONS); +#endif diff --git a/src/telescope/focuser/local/Focuser.axis9.cpp b/src/telescope/focuser/local/Focuser.axis9.cpp new file mode 100755 index 0000000..991ba07 --- /dev/null +++ b/src/telescope/focuser/local/Focuser.axis9.cpp @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, axis9 + +#include "Focuser.h" + +// note there is a prototype file that should be copy/pasted to build files +// for axes 4 to 9 by replacing all occurances of '@' with the axis number + +#if AXIS9_DRIVER_MODEL != OFF + + namespace { + + #if defined(AXIS9_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis9 = {AXIS9_DRIVER_MODEL, AXIS9_DRIVER_STATUS}; + KTechMotor motor_9(9, AXIS9_REVERSE, &DriverSettingsAxis9); + + #elif defined(AXIS9_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis9 = {AXIS9_DRIVER_MODEL, AXIS9_DRIVER_STATUS}; + Mks42DMotor motor_9(9, AXIS9_REVERSE, &DriverSettingsAxis9, AXIS9_STEPS_PER_DEGREE); + + #elif defined(AXIS9_SERVO_PRESENT) + ServoControl servoControlAxis9; + + #if AXIS9_ENCODER == AB + Quadrature encAxis9(9, AXIS9_ENCODER_A_PIN, AXIS9_ENCODER_B_PIN); + #elif AXIS9_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis9(9, AXIS9_ENCODER_A_PIN, AXIS9_ENCODER_B_PIN); + #elif AXIS9_ENCODER == CW_CCW + CwCcw encAxis9(9, AXIS9_ENCODER_A_PIN, AXIS9_ENCODER_B_PIN); + #elif AXIS9_ENCODER == PULSE_DIR + PulseDir encAxis9(9, AXIS9_ENCODER_A_PIN, AXIS9_ENCODER_B_PIN); + #elif AXIS9_ENCODER == PULSE_ONLY + PulseOnly encAxis9(9, AXIS9_ENCODER_A_PIN, &servoControlAxis9.directionHint); + #elif AXIS9_ENCODER == VIRTUAL + VirtualEnc encAxis9(9); + #elif AXIS9_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis9(9); + #endif + + #if AXIS9_SERVO_FEEDBACK == PID + Pid feedbackAxis9(AXIS9_PID_P, AXIS9_PID_I, AXIS9_PID_D); + #elif AXIS9_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis9(AXIS9_PID_P, AXIS9_PID_I, AXIS9_PID_D, AXIS9_PID_P_GOTO, AXIS9_PID_I_GOTO, AXIS9_PID_D_GOTO, AXIS9_PID_SENSITIVITY); + #endif + + #if AXIS9_SERVO_FLTR == KALMAN + KalmanFilter filterAxis9(AXIS9_SERVO_FLTR_MEAS_U, AXIS9_SERVO_FLTR_VARIANCE); + #elif AXIS9_SERVO_FLTR == ROLLING + RollingFilter filterAxis9(AXIS9_SERVO_FLTR_WSIZE); + #elif AXIS9_SERVO_FLTR == OFF + Filter filterAxis9; + #endif + + const ServoPins DriverPinsAxis9 = {AXIS9_SERVO_PH1_PIN, AXIS9_SERVO_PH1_STATE, AXIS9_SERVO_PH2_PIN, AXIS9_SERVO_PH2_STATE, AXIS9_ENABLE_PIN, AXIS9_ENABLE_STATE, AXIS9_M0_PIN, AXIS9_M1_PIN, AXIS9_M2_PIN, AXIS9_M3_PIN, AXIS9_FAULT_PIN}; + const ServoSettings DriverSettingsAxis9 = {AXIS9_DRIVER_MODEL, AXIS9_DRIVER_STATUS, AXIS9_SERVO_ACCELERATION}; + + #if AXIS9_DRIVER_MODEL == SERVO_EE + ServoEE driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_SERVO_DC_PWR_MIN, AXIS9_SERVO_DC_PWR_MAX); + #elif AXIS9_DRIVER_MODEL == SERVO_PE + ServoPE driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_SERVO_DC_PWR_MIN, AXIS9_SERVO_DC_PWR_MAX); + #elif AXIS9_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_SERVO_DC_PWR_MIN, AXIS9_SERVO_DC_PWR_MAX); + #elif AXIS9_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_SERVO_DC_PWR_MIN, AXIS9_SERVO_DC_PWR_MAX); + #elif AXIS9_DRIVER_MODEL == TMC2209 || AXIS9_DRIVER_MODEL == TMC2226 + ServoTmc2209 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_MOTOR_STEPS_PER_MICRON/AXIS9_STEPS_PER_MICRON, AXIS9_DRIVER_MICROSTEPS, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_DECAY, AXIS9_DRIVER_DECAY_GOTO); + #elif AXIS9_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_MOTOR_STEPS_PER_MICRON/AXIS9_STEPS_PER_MICRON, AXIS9_DRIVER_MICROSTEPS, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_DECAY, AXIS9_DRIVER_DECAY_GOTO); + #elif AXIS9_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver9(9, &DriverSettingsAxis9, AXIS9_MOTOR_STEPS_PER_MICRON/AXIS9_STEPS_PER_MICRON); + #endif + + ServoMotor motor_9(9, AXIS9_REVERSE, ((ServoDriver*)&driver9), &filterAxis9, &encAxis9, AXIS9_ENCODER_ORIGIN, AXIS9_ENCODER_REVERSE == ON, &feedbackAxis9, &servoControlAxis9); + + #elif defined(AXIS9_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis9 = {AXIS9_M0_PIN, AXIS9_M1_PIN, AXIS9_M2_PIN, AXIS9_M2_ON_STATE, AXIS9_M3_PIN, AXIS9_DECAY_PIN, AXIS9_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis9 = {AXIS9_DRIVER_MODEL, AXIS9_DRIVER_STATUS, AXIS9_DRIVER_MICROSTEPS, AXIS9_DRIVER_MICROSTEPS_GOTO, AXIS9_DRIVER_DECAY, AXIS9_DRIVER_DECAY_GOTO}; + #if AXIS9_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS9_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9); + #elif AXIS9_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_DRIVER_IHOLD, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_IGOTO, AXIS9_DRIVER_INTPOL); + #elif AXIS9_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_DRIVER_IHOLD, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_IGOTO, AXIS9_DRIVER_INTPOL); + #elif AXIS9_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_DRIVER_IHOLD, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_IGOTO, AXIS9_DRIVER_INTPOL); + #elif AXIS9_DRIVER_MODEL == TMC2209 || AXIS9_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_DRIVER_IHOLD, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_IGOTO, AXIS9_DRIVER_INTPOL); + #elif AXIS9_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_DRIVER_IHOLD, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_IGOTO, AXIS9_DRIVER_INTPOL); + #elif AXIS9_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_DRIVER_IHOLD, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_IGOTO, AXIS9_DRIVER_INTPOL); + #elif AXIS9_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver9(9, &DriverPinsAxis9, &DriverSettingsAxis9, AXIS9_DRIVER_IHOLD, AXIS9_DRIVER_IRUN, AXIS9_DRIVER_IGOTO, AXIS9_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis9 = {AXIS9_STEP_PIN, AXIS9_STEP_STATE, AXIS9_DIR_PIN, AXIS9_ENABLE_PIN, AXIS9_ENABLE_STATE}; + StepDirMotor motor_9(9, AXIS9_REVERSE, &StepDirPinsAxis9, ((StepDirDriver*)&driver9)); + #else + #error "Configuration (Config.h): Focuser has an unrecognized AXIS9_DRIVER_MODEL this should never happen!" + #endif + + const AxisPins PinsAxis9 = {AXIS9_SENSE_LIMIT_MIN_PIN, AXIS9_SENSE_HOME_PIN, AXIS9_SENSE_LIMIT_MAX_PIN, {AXIS9_SENSE_HOME, AXIS9_SENSE_HOME_INIT, AXIS9_SENSE_HOME_DIST_LIMIT*1000.0F, AXIS9_SENSE_LIMIT_MIN, AXIS9_SENSE_LIMIT_MAX, AXIS9_SENSE_LIMIT_INIT}}; + const AxisSettings SettingsAxis9 = {AXIS9_STEPS_PER_MICRON, {AXIS9_LIMIT_MIN*1000.0F, AXIS9_LIMIT_MAX*1000.0F}, AXIS9_BACKLASH_RATE}; + } + + Motor& motor9 = motor_9; + + Axis axis9(9, &PinsAxis9, &SettingsAxis9, AXIS_MEASURE_MICRONS); +#endif diff --git a/src/telescope/focuser/local/Focuser.can.command.cpp b/src/telescope/focuser/local/Focuser.can.command.cpp new file mode 100755 index 0000000..92ebbc9 --- /dev/null +++ b/src/telescope/focuser/local/Focuser.can.command.cpp @@ -0,0 +1,272 @@ +//-------------------------------------------------------------------------------------------------- +// local telescope focuser control, CAN server (remote node) + +#include "Focuser.h" + +#ifdef FOCUSER_CAN_SERVER_PRESENT + +#include "../../../lib/axis/Axis.h" + +extern Axis axis4; +#define FOCUSER_CAN_INDEX 0 // for the first focuser at axis 4 + +// default command semantics (mirrors the common command contract): +// handled=true, numericReply=true, suppressFrame=false, commandError=CE_NONE +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +void Focuser::processCommand() { + if (!ready) return; + + bool handled = true; + bool numericReply = true; + bool suppressFrame = false; + CommandError commandError = CE_NONE; + + // scratchpad variables + CommandError e = CE_NONE; + float f; + int32_t i; + uint8_t u; + + // ctrl byte: focuser number + units bit (1=microns, 0=steps) + uint8_t ctrl; + if (!readU8(ctrl)) ctrl = 0; + const uint8_t FocuserNumber = (uint8_t)(ctrl & FOC_CTRL_FOCUSER_MASK); // 1..6 + const bool UseMicrons = (ctrl & FOC_CTRL_UNIT_MICRONS) != 0; + + // Gate: make sure we're ready and only answer requests meant for this node + if (!ready || FocuserNumber != (uint8_t)CAN_FOCUSER_NUMBER) return; + + // Unit conversions (apply ONLY to I/M/G/R/S/B/D) + const float MicronsToSteps = axis4.getStepsPerMeasure(); // steps per micron + const float StepsToMicrons = MicronsToSteps != 0.0F ? (1.0F / MicronsToSteps) : 0.0F; + float MicronsToUnits = 1.0F; + float StepsToUnits = StepsToMicrons; + float UnitsToSteps = MicronsToSteps; + if (UseMicrons) { + MicronsToUnits = MicronsToSteps; + StepsToUnits = 1.0F; + UnitsToSteps = 1.0F; + } + + switch (opCode()) { + + // :hP# -> numeric 1/0 + case FOC_OP_PARK_HP: + e = park(FOCUSER_CAN_INDEX); + commandError = (e == CE_NONE) ? CE_1 : e; + break; + + // :hR# -> numeric 1/0 + case FOC_OP_UNPARK_HR: + e = unpark(FOCUSER_CAN_INDEX); + commandError = (e == CE_NONE) ? CE_1 : e; + break; + + // :FT# -> "M"/"S" + rateDigit (ASCII) + case FOC_OP_GET_STATUS_T: + writeU8(axis4.isSlewing() ? (uint8_t)'M' : (uint8_t)'S'); + writeU8((uint8_t)('0' + getGotoRate(FOCUSER_CAN_INDEX))); + numericReply = false; + break; + + // :Fp# -> status-only boolean (matches local: CE_0 if not DC) + case FOC_OP_GET_MODE_p: + if (!isDC(FOCUSER_CAN_INDEX)) commandError = CE_0; // numeric 0/1 synthesized by caller + // numericReply stays true, no payload + break; + + // :FI# -> i32 units + case FOC_OP_GET_MIN_I: + writeI32(axis4.getLimitMin()*MicronsToUnits); + numericReply = false; + break; + + // :FM# -> i32 units + case FOC_OP_GET_MAX_M: + writeI32(axis4.getLimitMax()*MicronsToUnits); + numericReply = false; + break; + + // :Fe# -> f32 (deg C) + case FOC_OP_GET_TEMPDIFF_e: + // Local behavior: if TCF disabled, diff = 0 + writeF32(getTcfEnable(FOCUSER_CAN_INDEX) ? (getTemperature() - getTcfT0(FOCUSER_CAN_INDEX)) : 0.0F); + numericReply = false; + break; + + // :Ft# -> f32 (deg C) + case FOC_OP_GET_TEMP_t: + writeF32(getTemperature()); + numericReply = false; + break; + + // :Fu# -> f32 (microns per step) + case FOC_OP_GET_UM_PER_STEP_u: + writeF32((MicronsToSteps != 0.0F) ? (1.0F/MicronsToSteps) : 0.0F); + numericReply = false; + break; + + // :FB# (get) or :FB[n]# (set) -> values in units (microns or steps) + case FOC_OP_BACKLASH_B: + if (remaining() == 0) { + writeI32(getBacklashSteps(FOCUSER_CAN_INDEX)*StepsToUnits); + numericReply = false; + } else { + if (!readI32(i)) { commandError = CE_PARAM_FORM; break; } + const long vSteps = (long)i*UnitsToSteps; + CommandError e = setBacklashSteps(FOCUSER_CAN_INDEX, vSteps); + commandError = (e == CE_NONE) ? CE_1 : e; + } + break; + + // :FC# (get f32) or :FC[sn.n]# (set f32) + case FOC_OP_TCF_COEF_C: + if (remaining() == 0) { + writeF32(getTcfCoef(FOCUSER_CAN_INDEX)); + numericReply = false; + } else { + if (!readF32(f)) { commandError = CE_PARAM_FORM; break; } + if (!setTcfCoef(FOCUSER_CAN_INDEX, f)) commandError = CE_PARAM_RANGE; + } + break; + + // :Fc# (get) or :Fc[n]# (set) -> status-only boolean (matches local) + case FOC_OP_TCF_ENABLE_c: + if (remaining() == 0) { + if (!getTcfEnable(FOCUSER_CAN_INDEX)) commandError = CE_0; // disabled => "0" + } else { + if (!readU8(u)) { commandError = CE_PARAM_FORM; break; } + commandError = setTcfEnable(FOCUSER_CAN_INDEX, u != 0); + } + break; + + // :FD# (get) or :FD[n]# (set) -> values in units + case FOC_OP_TCF_DEADBAND_D: + if (remaining() == 0) { + writeI32(getTcfDeadband(FOCUSER_CAN_INDEX)*StepsToUnits); + numericReply = false; + } else { + if (!readI32(i)) { commandError = CE_PARAM_FORM; break; } + if (!setTcfDeadband(FOCUSER_CAN_INDEX, i*UnitsToSteps)) commandError = CE_PARAM_RANGE; + } + break; + + // :FP# (get) or :FP[n]# (set) -> u8 percent (no unit-bit conversion) + case FOC_OP_DC_POWER_P: + if (!isDC(FOCUSER_CAN_INDEX)) { commandError = CE_CMD_UNKNOWN; break; } + if (remaining() == 0) { + writeU8(getDcPower(FOCUSER_CAN_INDEX)); + numericReply = false; + } else { + if (!readU8(u)) { commandError = CE_PARAM_FORM; break; } + if (!setDcPower(FOCUSER_CAN_INDEX, u)) commandError = CE_PARAM_RANGE; + } + break; + + // :FQ# -> no reply text + case FOC_OP_STOP_Q: + if (axis4.isHoming()) { + axis4.autoSlewAbort(); + homing[FOCUSER_CAN_INDEX] = true; + } else + axis4.autoSlewStop(); + numericReply = false; + break; + + // :F[1..9]# -> no reply text + case FOC_OP_SET_RATE_1_9: + if (!readU8(u)) { commandError = CE_PARAM_FORM; break; } + if (u < 1 || u > 9) { commandError = CE_PARAM_RANGE; break; } + if (u < 5) setMoveRate(FOCUSER_CAN_INDEX, (int)u); else setGotoRate(FOCUSER_CAN_INDEX, (int)u - 4); + numericReply = false; + break; + + // :FW# -> i32 µm/s (NO unit-bit conversion per your clarification) + case FOC_OP_GET_WORKRATE_W: + writeI32(settings[FOCUSER_CAN_INDEX].gotoRate); + numericReply = false; + break; + + // :F+# / :F-# -> no reply text + case FOC_OP_MOVE_IN_PLUS: + commandError = move(FOCUSER_CAN_INDEX, DIR_FORWARD); + numericReply = false; + break; + + case FOC_OP_MOVE_OUT_MINUS: + commandError = move(FOCUSER_CAN_INDEX, DIR_REVERSE); + numericReply = false; + break; + + // :FG# -> i32 units (position) + case FOC_OP_GET_POS_G: + // Mirror local: (instrumentSteps - tcfSteps) converted to units + i = (int32_t)(axis4.getInstrumentCoordinateSteps() - tcfSteps[FOCUSER_CAN_INDEX]); + writeI32(i*StepsToUnits); + numericReply = false; + break; + + // :FR[sn]# -> no reply text (relative) + case FOC_OP_GOTO_REL_R: + if (!readI32(i)) { commandError = CE_PARAM_FORM; break; } + commandError = gotoTarget(FOCUSER_CAN_INDEX, target[FOCUSER_CAN_INDEX] + i*UnitsToSteps); + numericReply = false; + break; + + // :FS[n]# -> numeric 1/0 (absolute) + case FOC_OP_GOTO_ABS_S: + if (!readI32(i)) { commandError = CE_PARAM_FORM; break; } + commandError = gotoTarget(FOCUSER_CAN_INDEX, i*UnitsToSteps); + // numericReply stays true to match local :FS behavior + break; + + // :FZ# -> no reply text + case FOC_OP_ZERO_Z: + commandError = resetTarget(FOCUSER_CAN_INDEX, 0); + numericReply = false; + break; + + // :FH# -> no reply text (set home) + case FOC_OP_SET_HOME_H: + commandError = resetTarget(FOCUSER_CAN_INDEX, lround(getHomePosition(FOCUSER_CAN_INDEX)*MicronsToSteps)); + numericReply = false; + break; + + // :Fh# -> no reply text (goto home) + case FOC_OP_GOTO_HOME_h: + if (axis4.hasHomeSense()) + commandError = moveHome(FOCUSER_CAN_INDEX); + else + commandError = gotoTarget(FOCUSER_CAN_INDEX, lround(getHomePosition(FOCUSER_CAN_INDEX)*MicronsToSteps)); + numericReply = false; + break; + + // :GXU[n]# + case FOC_OP_DRIVER_STATUS: { + DriverStatus status = axis4.motor->getDriverStatus(); + + if (status.active) { + uint8_t packedStatus = 0; + packedStatus |= (status.standstill ? 1: 0); + packedStatus |= (status.outputA.openLoad ? 1 : 0) << 1; + packedStatus |= (status.outputB.openLoad ? 1 : 0) << 2; + packedStatus |= (status.outputA.shortToGround ? 1 : 0) << 3; + packedStatus |= (status.outputB.shortToGround ? 1 : 0) << 4; + packedStatus |= (status.overTemperature ? 1 : 0) << 5; + packedStatus |= (status.overTemperatureWarning ? 1 : 0) << 6; + packedStatus |= (status.fault ? 1 : 0) << 7; + writeU8(packedStatus); + numericReply = false; + } else commandError = CE_0; + } break; + + default: + handled = false; + commandError = CE_CMD_UNKNOWN; + break; + } + + sendResponse(handled, suppressFrame, numericReply, commandError); +} + +#endif diff --git a/src/telescope/focuser/local/Focuser.command.cpp b/src/telescope/focuser/local/Focuser.command.cpp new file mode 100755 index 0000000..1705f8d --- /dev/null +++ b/src/telescope/focuser/local/Focuser.command.cpp @@ -0,0 +1,341 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control, commands + +#include "Focuser.h" + +#ifdef FOCUSER_PRESENT + +#include "../../../lib/convert/Convert.h" +#include "../../../lib/axis/Axis.h" + +extern Axis *axes[6]; + +// by default reply[80] == "", suppressFrame == false, numericReply == true, and commandError == CE_NONE +// return true if the command has been completely handled and no further command() will be called, or false if not +// for commands that are handled repeatedly commandError might contain CE_NONE or CE_1 to indicate success +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +bool Focuser::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + if (!ready) return false; + + static int index = 0; + + // process any focuser axis commands + for (int index = 0; index < FOCUSER_MAX; index++) { + if (axes[index] != NULL) { + if (axes[index]->command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + } + } + + if (command[0] == 'h') { + // :hP# Moves focuser(s) to the park position + // Return: 0 on failure + // 1 on success + if (command[1] == 'P' && parameter[0] == 0) { + CommandError e = CE_NONE; + for (int index = 0; index < FOCUSER_MAX; index++) { + CommandError e1 = park(index); + if (e1 != CE_NONE) e = e1; + } + if (e == CE_NONE) *commandError = CE_1; else { VF("MSG: Focusers, park error "); VL(e); *commandError = e; } + return false; + } else + + // :hR# Restore parked focuser(s) to operation + // Return: 0 on failure + // 1 on success + if (command[1] == 'R' && parameter[0] == 0) { + CommandError e = CE_NONE; + for (int index = 0; index < FOCUSER_MAX; index++) { + CommandError e1 = unpark(index); + if (e1 != CE_NONE) e = e1; + } + if (e == CE_NONE) *commandError = CE_1; else { VF("MSG: Focusers, unpark error "); VL(e); *commandError = e; } + return false; + } else return false; + } else + + if (command[0] == 'F' && command[1] == 'A') { + // :FA# Focuser Active? + // Return: 0 on failure (no focusers) + // 1 to 6 on success + if (parameter[0] == 0) { + if (active < 0 || axes[active] == NULL) { *commandError = CE_0; return true; } + sprintf(reply, "%d", active + 1); + *numericReply = false; + *suppressFrame = true; + } else + + // :FA[n]# Select focuser where [n] = 1 to 6 + // Return: 0 on failure + // 1 on success + if (parameter[1] == 0) { + int i = parameter[0] - '1'; + if (i >= 0 && i < FOCUSER_MAX && axes[i] != NULL) active = i; else *commandError = CE_PARAM_RANGE; + } else return false; + } else + + // :F[...]# Use selected focuser (defaults to the first focuser) + // :F1[...]# Focuser #1 (Axis4) + // :F2[...]# Focuser #2 (Axis5) + // :F3[...]# Focuser #3 (Axis6) + // :F4[...]# Focuser #4 (Axis7) + // :F5[...]# Focuser #5 (Axis8) + // :F6[...]# Focuser #6 (Axis9) + if (command[0] == 'F') { + + // check that the requested focuser is active + int i = command[1] - '1'; + if (i >= 0 && i < 6 && parameter[0] != 0) { + // if not active return false so other focuser devices may process the command + if (axes[i] == NULL) return false; + index = i; + command[1] = parameter[0]; + char temp[32]; + strcpy(temp, ¶meter[1]); + strcpy(¶meter[0], temp); + } else index = active; + + // check for no default focuser + if (index < 0) { *commandError = CE_CMD_UNKNOWN; return true; } + + // check for commands that shouldn't have a parameter + if (strchr("aTpIMtuQF1234+-GZHh", command[1]) && parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + + // get ready for commands that convert to microns or steps (these commands are upper-case for microns OR lower-case for steps) + const float MicronsToSteps = axes[index]->getStepsPerMeasure(); + const float StepsToMicrons = MicronsToSteps != 0.0F ? (1.0F / MicronsToSteps) : 0.0F; + float MicronsToUnits = 1.0F; + float StepsToUnits = StepsToMicrons; + float UnitsToSteps = MicronsToSteps; + if (strchr("bdgimrs", command[1])) { + MicronsToUnits = MicronsToSteps; + StepsToUnits = 1.0F; + UnitsToSteps = 1.0F; + } + + // :Fa# Get primary focuser + // Returns: 1 true if active + if (command[1] == 'a') { + // always return true + } else + + // :FT# Get status + // Returns: s# + if (command[1] == 'T') { + if (axes[index]->isSlewing()) strcpy(reply,"M"); else strcpy(reply,"S"); // [M] for moving or [S] for stopped + char temp[2] = "0"; + temp[0] = '0' + getGotoRate(index); + strcat(reply, temp); // [1] to [5] for 0.5x to 2x goto rate + *numericReply = false; + } else + + // :Fp# Get mode + // Return: 0 for absolute + // 1 for pseudo absolute + if (command[1] == 'p') { + if (!isDC(index)) *commandError = CE_0; + } else + + // :FI# Get full in position (in microns or steps) + // Returns: n# + if (toupper(command[1]) == 'I') { + sprintf(reply, "%ld", lround(axes[index]->getLimitMin()*MicronsToUnits)); + *numericReply = false; + } else + + // :FM# Get max position (in microns or steps) + // Returns: n# + if (toupper(command[1]) == 'M') { + sprintf(reply, "%ld", lround(axes[index]->getLimitMax()*MicronsToUnits)); + *numericReply = false; + } else + + // :Fe# Get focuser temperature differential + // Returns: n# temperature in deg. C + if (command[1] == 'e') { + if (getTcfEnable(index)) sprintF(reply, "%3.1f", getTemperature() - getTcfT0(index)); else sprintF(reply, "%3.1f", 0.0); + *numericReply = false; + } else + + // :Ft# Get focuser temperature + // Returns: n# temperature in deg. C + if (command[1] == 't') { + sprintF(reply, "%3.1f", getTemperature()); + *numericReply = false; + } else + + // :Fu# Get focuser microns per step + // Returns: n.n# + if (command[1] == 'u') { + sprintF(reply, "%7.5f", (MicronsToSteps != 0.0F) ? (1.0F/MicronsToSteps) : 0.0F); + *numericReply = false; + } else + + // :FB# Get focuser backlash amount (in steps or microns) + // Return: n# + if (toupper(command[1]) == 'B' && parameter[0] == 0) { + sprintf(reply, "%ld", lround(getBacklashSteps(index)*StepsToUnits)); + *numericReply = false; + } else + + // :FB[n]# Set focuser backlash amount (in steps or microns) + // Return: 0 on failure + // 1 on success + if (toupper(command[1]) == 'B') { + *commandError = setBacklashSteps(index, lround(atol(parameter)*UnitsToSteps)); + } else + + // :FC# Get focuser temperature compensation coefficient in microns per °C) + // Return: n.n# + if (command[1] == 'C' && parameter[0] == 0) { + sprintF(reply, "%7.5f", getTcfCoef(index)); + *numericReply = false; + } else + + // :FC[sn.n]# Set focuser temperature compensation coefficient in microns per °C (+ moves out as temperature falls) + // Return: 0 on failure + // 1 on success + if (command[1] == 'C') { + float value = atof(parameter); + if (!setTcfCoef(index, value)) *commandError = CE_PARAM_RANGE; + } else + + // :Fc# Get focuser temperature compensation enable status + // Return: 0 if disabled + // 1 if enabled + if (command[1] == 'c' && parameter[0] == 0) { + if (!getTcfEnable(index)) *commandError = CE_0; + } else + + // :Fc[n]# Enable/disable focuser temperature compensation where [n] = 0 or 1 + // Return: 0 on failure + // 1 on success + if (command[1] == 'c' && parameter[1] == 0) { + *commandError = setTcfEnable(index, parameter[0] != '0'); + } else + + // :FD# Get focuser temperature compensation deadband amount (in steps or microns) + // Return: n# + if (toupper(command[1]) == 'D' && parameter[0] == 0) { + sprintf(reply, "%ld", lround(getTcfDeadband(index)*StepsToUnits)); + *numericReply = false; + } else + + // :FD[n]# Set focuser temperature compensation deadband amount (in steps or microns) + // Return: 0 on failure + // 1 on success + if (toupper(command[1]) == 'D') { + if (!setTcfDeadband(index, lround(atol(parameter)*UnitsToSteps))) *commandError = CE_PARAM_RANGE; + } else + + // :FP# Get focuser DC Motor Power Level (in %) + // Returns: n# + // :FP[n]# Set focuser DC Motor Power Level (in %) + // Return: 0 on failure + // 1 on success + if (command[1] == 'P') { + if (isDC(index)) { + if (parameter[0] == 0) { + sprintf(reply, "%d",(int)getDcPower(index)); *numericReply = false; + } else { + if (!setDcPower(index, atol(parameter))) *commandError = CE_PARAM_RANGE; + } + } else *commandError = CE_CMD_UNKNOWN; + } else + + // :FQ# Stop the focuser + // Returns: Nothing + if (command[1] == 'Q') { + if (axes[index]->isHoming()) { + axes[index]->autoSlewAbort(); + homing[index] = true; + } else axes[index]->autoSlewStop(); + *numericReply = false; + } else + + // :F[n]# Set focuser move or goto rate + // move rate where n = 1 for 1um/sec, 2 for 10um/sec, 3 for 100um/sec, 4 for 0.5x goto rate + // goto rate where n = 5 for 0.5x, 6 for 0.66x, 7 for 1x, 8 for 1.5x, 9 for 2x AXISn_SLEW_RATE_BASE_DESIRED + // Returns: Nothing + if (command[1] >= '1' && command[1] <= '9') { + if (strlen(parameter) == 0) { + int v = command[1] - '0'; + if (v < 5) setMoveRate(index, v); else setGotoRate(index, v - 4); + } else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + + // :FW# Get focuser working slew rate + // goto rate in um/s + // Returns: n# + if (command[1] == 'W') { + sprintf(reply, "%d", settings[index].gotoRate); + *numericReply = false; + } else + + // :F+# Move focuser in (toward objective) + // Returns: Nothing + if (command[1] == '+') { + *commandError = move(index, DIR_FORWARD); + *numericReply = false; + } else + + // :F-# Move focuser out (away from objective) + // Returns: Nothing + if (command[1] == '-') { + *commandError = move(index, DIR_REVERSE); + *numericReply = false; + } else + + // :FG# Get focuser current position (in microns or steps) + // Returns: sn# + if (toupper(command[1]) == 'G') { + sprintf(reply, "%ld", lround((axes[index]->getInstrumentCoordinateSteps() - tcfSteps[index])*StepsToUnits)); + *numericReply = false; + } else + + // :FR[sn]# Goto focuser target position relative (in microns or steps) + // Returns: Nothing + if (toupper(command[1]) == 'R') { + if (strlen(parameter) > 0) *commandError = gotoTarget(index, target[index] + atol(parameter)*UnitsToSteps); else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + + // :FS[n]# Goto focuser target position (in microns or steps) + // Return: 0 on failure + // 1 on success + if (toupper(command[1]) == 'S') { + if (strlen(parameter) > 0) *commandError = gotoTarget(index, atol(parameter)*UnitsToSteps); else *commandError = CE_PARAM_FORM; + } else + + // :FZ# Set focuser position as zero + // Returns: Nothing + if (command[1] == 'Z') { + *commandError = resetTarget(index, 0); + *numericReply = false; + } else + + // :FH# Set focuser position as home + // Returns: Nothing + if (command[1] == 'H') { + *commandError = resetTarget(index, lround(getHomePosition(index)*MicronsToSteps)); + *numericReply = false; + } else + + // :Fh# Move focuser target position to home + // Returns: Nothing + if (command[1] == 'h') { + if (axes[index]->hasHomeSense()) { + *commandError = moveHome(index); + } else { + *commandError = gotoTarget(index, lround(getHomePosition(index)*MicronsToSteps)); + } + *numericReply = false; + } else *commandError = CE_CMD_UNKNOWN; + + } else return false; + + return true; +} + +#endif \ No newline at end of file diff --git a/src/telescope/focuser/local/Focuser.cpp b/src/telescope/focuser/local/Focuser.cpp new file mode 100755 index 0000000..09b10dd --- /dev/null +++ b/src/telescope/focuser/local/Focuser.cpp @@ -0,0 +1,700 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control + +#include "Focuser.h" + +#ifdef FOCUSER_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/sense/Sense.h" +#include "../../../lib/nv/Nv.h" + +#include "../../../libApp/weather/Weather.h" +#include "../../../libApp/temperature/Temperature.h" + +#include "../../Telescope.h" + +typedef struct FocuserConfiguration { + bool present; + int32_t homeDefault; + uint16_t slewRateDesired; + uint8_t slewRateMinimum; + float accelerationTime; + float rapidStopTime; + bool powerDown; + uint16_t powerDownTime; + uint8_t slavedToFocuser; +} FocuserConfiguration; + +static const FocuserConfiguration configuration[] = { +#if FOCUSER_MAX >= 1 + {AXIS4_DRIVER_MODEL != OFF, AXIS4_HOME_DEFAULT, AXIS4_SLEW_RATE_BASE_DESIRED, AXIS4_SLEW_RATE_MINIMUM, AXIS4_ACCELERATION_TIME, AXIS4_RAPID_STOP_TIME, AXIS4_POWER_DOWN == ON, AXIS4_POWER_DOWN_TIME, AXIS4_SLAVED_TO_FOCUSER}, +#endif +#if FOCUSER_MAX >= 2 + {AXIS5_DRIVER_MODEL != OFF, AXIS5_HOME_DEFAULT, AXIS5_SLEW_RATE_BASE_DESIRED, AXIS5_SLEW_RATE_MINIMUM, AXIS5_ACCELERATION_TIME, AXIS5_RAPID_STOP_TIME, AXIS5_POWER_DOWN == ON, AXIS5_POWER_DOWN_TIME, AXIS5_SLAVED_TO_FOCUSER}, +#endif +#if FOCUSER_MAX >= 3 + {AXIS6_DRIVER_MODEL != OFF, AXIS6_HOME_DEFAULT, AXIS6_SLEW_RATE_BASE_DESIRED, AXIS6_SLEW_RATE_MINIMUM, AXIS6_ACCELERATION_TIME, AXIS6_RAPID_STOP_TIME, AXIS6_POWER_DOWN == ON, AXIS6_POWER_DOWN_TIME, AXIS6_SLAVED_TO_FOCUSER}, +#endif +#if FOCUSER_MAX >= 4 + {AXIS7_DRIVER_MODEL != OFF, AXIS7_HOME_DEFAULT, AXIS7_SLEW_RATE_BASE_DESIRED, AXIS7_SLEW_RATE_MINIMUM, AXIS7_ACCELERATION_TIME, AXIS7_RAPID_STOP_TIME, AXIS7_POWER_DOWN == ON, AXIS7_POWER_DOWN_TIME, AXIS7_SLAVED_TO_FOCUSER}, +#endif +#if FOCUSER_MAX >= 5 + {AXIS8_DRIVER_MODEL != OFF, AXIS8_HOME_DEFAULT, AXIS8_SLEW_RATE_BASE_DESIRED, AXIS8_SLEW_RATE_MINIMUM, AXIS8_ACCELERATION_TIME, AXIS8_RAPID_STOP_TIME, AXIS8_POWER_DOWN == ON, AXIS8_POWER_DOWN_TIME, AXIS8_SLAVED_TO_FOCUSER}, +#endif +#if FOCUSER_MAX >= 6 + {AXIS9_DRIVER_MODEL != OFF, AXIS9_HOME_DEFAULT, AXIS9_SLEW_RATE_BASE_DESIRED, AXIS9_SLEW_RATE_MINIMUM, AXIS9_ACCELERATION_TIME, AXIS9_RAPID_STOP_TIME, AXIS9_POWER_DOWN == ON, AXIS9_POWER_DOWN_TIME, AXIS9_SLAVED_TO_FOCUSER}, +#endif +}; + +static int slavedToFocuser[6] = {-1, -1, -1, -1, -1, -1}; + +static void focWrapper() { focuser.monitor(); } + +#if FOCUSER_BUTTON_SENSE_IN != OFF && FOCUSER_BUTTON_SENSE_OUT != OFF +static void focButtonsWrapper() { focuser.buttons(); } +#endif + +#if defined(FOCUSER_CAN_SERVER_PRESENT) +// 1 Hz heartbeat TX for this remote focuser node +static void focHeartbeatWrapper() { + if (!canPlus.ready) return; + const uint16_t hbId = (uint16_t)(CAN_FOCUSER_HB_ID_BASE + (CAN_FOCUSER_NUMBER - 1)); + const uint8_t b = 0; // payload reserved; currently unused + canPlus.writePacket((int)hbId, &b, 1); +} +#endif + +// setup arrays for easy access to focuser axes +Axis *axes[6] = {NULL, NULL, NULL, NULL, NULL, NULL}; + +bool Focuser::init() { + #if defined(FOCUSER_CAN_SERVER_PRESENT) + if (!CanTransportServer::init(true, 2)) return false; + #endif + + // get the motion controllers ready + #if AXIS4_DRIVER_MODEL != OFF + if (!axis4.init(&motor4)) { initError.driver = true; DLF("ERR: Focuser::init(), no motion controller for Axis4!"); return false; } else { + VLF("MSG: Focuser1, init (Axis4)"); + axes[0] = &axis4; + } + #endif + #if AXIS5_DRIVER_MODEL != OFF + if (!axis5.init(&motor5)) { initError.driver = true; DLF("ERR: Focuser::init(), no motion controller for Axis5!"); return false; } else { + VLF("MSG: Focuser2, init (Axis5)"); + axes[1] = &axis5; + } + #endif + #if AXIS6_DRIVER_MODEL != OFF + if (!axis6.init(&motor6)) { initError.driver = true; DLF("ERR: Focuser::init(), no motion controller for Axis6!"); return false; } else { + VLF("MSG: Focuser3, init (Axis6)"); + axes[2] = &axis6; + } + #endif + #if AXIS7_DRIVER_MODEL != OFF + if (!axis7.init(&motor7)) { initError.driver = true; DLF("ERR: Focuser::init(), no motion controller for Axis7!"); return false; } else { + VLF("MSG: Focuser4, init (Axis7)"); + axes[3] = &axis7; + } + #endif + #if AXIS8_DRIVER_MODEL != OFF + if (!axis8.init(&motor8)) { initError.driver = true; DLF("ERR: Focuser::init(), no motion controller for Axis8!"); return false; } else { + VLF("MSG: Focuser5, init (Axis8)"); + axes[4] = &axis8; + } + #endif + #if AXIS9_DRIVER_MODEL != OFF + if (!axis9.init(&motor9)) { initError.driver = true; DLF("ERR: Focuser::init(), no motion controller for Axis9!"); return false; } else { + VLF("MSG: Focuser6, init (Axis9)"); + axes[5] = &axis9; + } + #endif + + // time to wait before writing position to nv after last movement of focuser + if (nv().device().endurance() == NvDevice::Endurance::Mid) writeDelay = 60; else + if (nv().device().endurance() == NvDevice::Endurance::High) writeDelay = 5; + + // init settings stored in NV + for (uint8_t index = 0; index < FOCUSER_MAX; index++) { + // make sure defaults are set + settings[index].tcf.enabled = false; + settings[index].tcf.coef = 0.0F; + settings[index].tcf.deadband = 1; + settings[index].tcf.t0 = 0.0F; + settings[index].parkState = PS_UNPARKED; + settings[index].backlash = 0; + settings[index].position = 0.0F; + settings[index].gotoRate = configuration[index].slewRateDesired; + + // create a nvKey to hold this focusers settings + char keyStr[26]; + snprintf(keyStr, sizeof(keyStr), "FOCUSER%u_SETTINGS", index); + nvKey[index] = nv().kv().computeKey(keyStr); + + if (!nv().kv().getOrInit(nvKey[index], settings[index])) { DF("WRN: Nv, init failed for "); VL(keyStr[index]); } + settings[index].tcf.enabled = constrain(settings[index].tcf.enabled, false, true); + settings[index].tcf.coef = constrain(settings[index].tcf.coef, 0, 999); + settings[index].tcf.deadband = constrain(settings[index].tcf.deadband, 1, 10000); + settings[index].tcf.t0 = constrain(settings[index].tcf.t0, -60, 60); + settings[index].parkState = constrain(settings[index].parkState, PS_UNPARKED, PS_UNPARKING); + settings[index].backlash = constrain(settings[index].backlash, 0, 10000); + settings[index].position = constrain(settings[index].position, axes[index]->getLimitMin(), axes[index]->getLimitMax()); + settings[index].gotoRate = constrain(settings[index].gotoRate, configuration[index].slewRateDesired/2.0, configuration[index].slewRateDesired*2.0); + } + + // get settings + for (int index = 0; index < FOCUSER_MAX; index++) { + moveRate[index] = 100; + tcfSteps[index] = 0; + target[index] = 0; + writeTime[index] = 0; + parkHandle[index] = 0; + homing[index] = false; + + if (validIndex(index)) { + if (active == -1) active = index; + + // TCF defaults to disabled at startup + settings[index].tcf.enabled = false; + + if (settings[index].position < axes[index]->getLimitMin()) { + settings[index].position = axes[index]->getLimitMin(); + initError.value = true; + DLF("ERR: Focuser.init(), bad NV park pos < _LIMIT_MIN (set to _LIMIT_MIN)"); + } + + if (settings[index].position > axes[index]->getLimitMax()) { + settings[index].position = axes[index]->getLimitMax(); + initError.value = true; + DLF("ERR: Focuser.init(), bad NV park pos > _LIMIT_MAX steps (set to _LIMIT_MAX)"); + } + + axes[index]->resetPositionSteps(0); + axes[index]->setBacklashSteps(settings[index].backlash); + axes[index]->setFrequencyMax(configuration[index].slewRateDesired*2.0F); + axes[index]->setFrequencyMin(configuration[index].slewRateMinimum); + axes[index]->setFrequencySlew(configuration[index].slewRateDesired); + axes[index]->setSlewAccelerationTime(configuration[index].accelerationTime); + axes[index]->setSlewAccelerationTimeAbort(configuration[index].rapidStopTime); + if (configuration[index].powerDown) axes[index]->setPowerDownTime(configuration[index].powerDownTime); + slavedToFocuser[index] = configuration[index].slavedToFocuser - 1; + if (slavedToFocuser[index] < -1 || slavedToFocuser[index] >= FOCUSER_MAX) slavedToFocuser[index] = -1; + } + } + + // some final initialization + bool recursionDetected = false; + for (int index = 0; index < FOCUSER_MAX; index++) { + if (validIndex(index) && hasSlaveCycle(index)) { recursionDetected = true; break; } + } + if (recursionDetected) { + for (int index = 0; index < FOCUSER_MAX; index++) slavedToFocuser[index] = -1; + DLF("WRN: Focusers, slaved focusers have recurse on themselves and are disabled!"); + } + + ready = true; + return true; +} + +void Focuser::begin() { + if (!ready) return; + + // some final initialization + for (int index = 0; index < FOCUSER_MAX; index++) { if (validIndex(index)) axes[index]->calibrateDriver(); } + + // start task for temperature compensated focusing + VF("MSG: Focusers, starting TCF task (rate 1s priority 6)... "); + if (tasks.add(1000, 0, true, 6, focWrapper, "FocPoll")) { VLF("success"); } else { VLF("FAILED!"); } + + // start task for monitor for focuser buttons + #if FOCUSER_BUTTON_SENSE_IN != OFF && FOCUSER_BUTTON_SENSE_OUT != OFF + if (FOCUSER_BUTTON_FOCUSER_INDEX - 1 < 0 || + FOCUSER_BUTTON_FOCUSER_INDEX - 1 >= FOCUSER_MAX || + axes[FOCUSER_BUTTON_FOCUSER_INDEX - 1] == NULL) { + DLF("WRN: Focusers, starting button monitor failed invalid FOCUSER_BUTTON_FOCUSER_INDEX"); + return; + } + + VF("MSG: Focusers, starting button monitor task (rate 10ms priority 6)... "); + if (tasks.add(10, 0, true, 6, focButtonsWrapper, "FocBtns")) { + VLF("success"); + VLF("Focusers, adding any in and out button senses"); + inButtonHandle = sense.add(FOCUSER_BUTTON_SENSE_IN_PIN, FOCUSER_BUTTON_SENSE_INIT, FOCUSER_BUTTON_SENSE_IN); + outButtonHandle = sense.add(FOCUSER_BUTTON_SENSE_OUT_PIN, FOCUSER_BUTTON_SENSE_INIT, FOCUSER_BUTTON_SENSE_OUT); + } else { VLF("FAILED!"); } + #endif + + #if defined(FOCUSER_CAN_SERVER_PRESENT) + VF("MSG: Focusers, starting CAN heartbeat task (rate 1s priority 6)... "); + if (tasks.add(1000, 0, true, 6, focHeartbeatWrapper, "FocHB")) { VLF("success"); } else { VLF("FAILED!"); } + #endif + + for (int index = 0; index < FOCUSER_MAX; index++) { + if (configuration[index].present && axes[index] != NULL) unpark(index); + } +} + +bool Focuser::validIndex(int index) { + if (index < 0 || index >= FOCUSER_MAX) return false; + if (!configuration[index].present) return false; + if (axes[index] == nullptr) return false; + return true; +} + +// get focuser temperature in deg. C +float Focuser::getTemperature() { + float t = temperature.getChannel(0); + return t; +} + +// check for DC motor focuser +bool Focuser::isDC(int index) { + UNUSED(index); + return false; +} + +// get DC power in % +int Focuser::getDcPower(int index) { + UNUSED(index); + return 0; +} + +// set DC power in % +bool Focuser::setDcPower(int index, int value) { + UNUSED(index); + if (value < 0 || value > 100) return false; + return true; +} + +// get TCF enable +bool Focuser::getTcfEnable(int index) { + if (!validIndex(index)) return false; + if (isnan(getTemperature())) setTcfEnable(index, false); + return settings[index].tcf.enabled; +} + +// set TCF enable +CommandError Focuser::setTcfEnable(int index, bool value) { + if (!validIndex(index)) return CE_CMD_UNKNOWN; + if (settings[index].parkState >= PS_PARKED) return CE_PARKED; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) setTcfEnable(si, value); } + + if (isnan(getTemperature())) value = false; + settings[index].tcf.enabled = value; + if (value) { + settings[index].tcf.t0 = getTemperature(); + } else { + target[index] += tcfSteps[index]; + tcfSteps[index] = 0; + } + nv().kv().put(nvKey[index], settings[index]); + return CE_NONE; +} + +// get TCF coefficient, in microns per deg. C +float Focuser::getTcfCoef(int index) { + if (!validIndex(index)) return 0.0F; + return settings[index].tcf.coef; +} + +// set TCF coefficient, in microns per deg. C +bool Focuser::setTcfCoef(int index, float value) { + if (!validIndex(index)) return false; + if (fabs(value) >= 1000.0F) return false; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) setTcfCoef(si, value); } + + settings[index].tcf.coef = value; + nv().kv().put(nvKey[index], settings[index]); + return true; +} + +// get TCF deadband, in steps +int Focuser::getTcfDeadband(int index) { + if (!validIndex(index)) return 0; + return settings[index].tcf.deadband; +} + +// set TCF deadband, in steps +bool Focuser::setTcfDeadband(int index, int value) { + if (!validIndex(index)) return false; + if (value < 1 || value > 10000) return false; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) setTcfDeadband(si, value); } + + settings[index].tcf.deadband = value; + nv().kv().put(nvKey[index], settings[index]); + return true; +} + +// get TCF T0, in deg. C +float Focuser::getTcfT0(int index) { + if (!validIndex(index)) return 0.0F; + return settings[index].tcf.t0; +} + +// set TCF T0, in deg. C +bool Focuser::setTcfT0(int index, float value) { + if (!validIndex(index)) return false; + if (fabs(value) > 60.0F) return false; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) setTcfT0(si, value); } + + settings[index].tcf.t0 = value; + nv().kv().put(nvKey[index], settings[index]); + return true; +} + +// get home position in microns +float Focuser::getHomePosition(int index) { + if (!validIndex(index)) return 0; + + int mi = slavedToFocuser[index]; + if (mi >= 0 && mi < FOCUSER_MAX && axes[mi] != NULL) { return axes[mi]->getInstrumentCoordinate(); } + + if (configuration[index].homeDefault >= 0 && configuration[index].homeDefault <= 500000) return configuration[index].homeDefault; + + switch (configuration[index].homeDefault) { + case MAXIMUM: + return lround(axes[index]->getLimitMax()); + case MIDDLE: + return lround((axes[index]->getLimitMax() + axes[index]->getLimitMin())/2.0F); + case MINIMUM: + default: + return lround(axes[index]->getLimitMin()); + } +} + +// get backlash in steps +int Focuser::getBacklashSteps(int index) { + if (!validIndex(index)) return 0; + return settings[index].backlash; +} + +// set backlash in steps +CommandError Focuser::setBacklashSteps(int index, int value) { + if (!validIndex(index)) return CE_CMD_UNKNOWN; + if (value < 0 || value > 10000) return CE_PARAM_RANGE; + if (settings[index].parkState >= PS_PARKED) return CE_PARKED; + + #if FOCUSER_SLAVE_BACKLASH == ON + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) setBacklashSteps(si, value); } + #endif + + settings[index].backlash = value; + nv().kv().put(nvKey[index], settings[index]); + axes[index]->setBacklashSteps(getBacklashSteps(index)); + return CE_NONE; +} + +// set move rate, 1 for 1um/sec slew, 2 for 10um/sec, 3 for 100um/sec, 4 for 0.5x goto rate +void Focuser::setMoveRate(int index, int value) { + if (!validIndex(index)) return; + if (value < 1 || value > 4) return; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) setMoveRate(si, value); } + + switch (value) { + case 1: moveRate[index] = 1; break; + case 2: moveRate[index] = 10; break; + case 3: moveRate[index] = 100; break; + case 4: moveRate[index] = settings[index].gotoRate/2; break; + default: moveRate[index] = 10; break; + } +} + +// start move in the specified direction +CommandError Focuser::move(int index, Direction dir) { + if (!validIndex(index)) return CE_CMD_UNKNOWN; + if (settings[index].parkState >= PS_PARKED) return CE_PARKED; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) move(si, dir); } + + axes[index]->enable(true); + if (!axes[index]->isSlewing()) { + axes[index]->setSynchronizedFrequency(0.0F); + axes[index]->resetTargetToMotorPosition(); + } + return axes[index]->autoSlew(dir, moveRate[index]); +} + +// start move to the home position +CommandError Focuser::moveHome(int index) { + if (!validIndex(index)) return CE_CMD_UNKNOWN; + if (settings[index].parkState >= PS_PARKED) return CE_PARKED; + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) moveHome(si); } + + axes[index]->enable(true); + axes[index]->setFrequencySlew(settings[index].gotoRate); + CommandError e = axes[index]->autoSlewHome(); + if (e == CE_NONE) homing[index] = true; + + return e; +} + +// get goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base +int Focuser::getGotoRate(int index) { + if (!validIndex(index)) return 1; + + if (settings[index].gotoRate < configuration[index].slewRateDesired/1.75) return 1; + if (settings[index].gotoRate < configuration[index].slewRateDesired/1.25) return 2; + if (settings[index].gotoRate < configuration[index].slewRateDesired*1.25) return 3; + if (settings[index].gotoRate < configuration[index].slewRateDesired*1.75) return 4; else return 5; +} + +// set goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base +void Focuser::setGotoRate(int index, int value) { + if (!validIndex(index)) return; + if (value < 1 || value > 5) value = 1; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) setGotoRate(si, value); } + + switch (value) { + case 1: settings[index].gotoRate = configuration[index].slewRateDesired/2.0; break; + case 2: settings[index].gotoRate = configuration[index].slewRateDesired/1.5; break; + case 3: settings[index].gotoRate = configuration[index].slewRateDesired*1.0; break; + case 4: settings[index].gotoRate = configuration[index].slewRateDesired*1.5; break; + case 5: settings[index].gotoRate = configuration[index].slewRateDesired*2.0; break; + default: settings[index].gotoRate = configuration[index].slewRateDesired; break; + } + nv().kv().put(nvKey[index], settings[index]); +} + +// move focuser to a specific location (in steps) +CommandError Focuser::gotoTarget(int index, long target) { + if (!validIndex(index)) return CE_CMD_UNKNOWN; + if (settings[index].parkState >= PS_PARKED) return CE_PARKED; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) gotoTarget(si, target); } + + VF("MSG: Focuser"); V(index + 1); VF(", goto target coordinate set ("); V(target/axes[index]->getStepsPerMeasure()); VLF("um)"); + VF("MSG: Focuser"); V(index + 1); VLF(", attempting goto"); + + axes[index]->enable(true); + axes[index]->setSynchronizedFrequency(0.0F); + axes[index]->setTargetCoordinateSteps(target + tcfSteps[index]); + CommandError e = axes[index]->autoGoto(settings[index].gotoRate); + if (e != CE_NONE) { VF("MSG: Focuser"); V(index + 1); VLF(", goto failed"); } + + return e; +} + +// reset focuser to a specific location (in steps) +CommandError Focuser::resetTarget(int index, long target) { + if (!validIndex(index)) return CE_CMD_UNKNOWN; + if (settings[index].parkState >= PS_PARKED) return CE_PARKED; + + for (int i = 0; i < FOCUSER_MAX; i++) { int si = slavedFocuserIndex(index, i); if (si >= 0) resetTarget(si, target); } + + VF("MSG: Focuser"); V(index + 1); VF(", reset target coordinate set ("); V(target/axes[index]->getStepsPerMeasure()); VLF("um)"); + + CommandError e = axes[index]->resetPositionSteps(target); + axes[index]->setBacklashSteps(getBacklashSteps(index)); + + if (e != CE_NONE) { VF("MSG: Focuser"); V(index + 1); VLF(", reset failed"); } + + return e; +} + +// park focuser at its current location +CommandError Focuser::park(int index) { + if (!validIndex(index)) return CE_PARAM_RANGE; + if (settings[index].parkState == PS_PARKED) return CE_NONE; + if (settings[index].parkState == PS_PARKING) return CE_PARK_FAILED; + if (settings[index].parkState == PS_UNPARKING) return CE_PARK_FAILED; + if (settings[index].parkState == PS_PARK_FAILED) return CE_PARK_FAILED; + + setTcfEnable(index, false); + + axes[index]->enable(true); + VF("MSG: Focuser"); V(index + 1); VLF(", parking"); + axes[index]->setBacklashSteps(0); + float targetMicrons = axes[index]->getInstrumentCoordinate(); + axes[index]->setTargetCoordinatePark(targetMicrons); + + CommandError e = axes[index]->autoGoto(settings[index].gotoRate); + + if (e == CE_NONE) { + settings[index].position = targetMicrons; + settings[index].parkState = PS_PARKING; + nv().kv().put(nvKey[index], settings[index]); + } + + return e; +} + +// unpark focuser +CommandError Focuser::unpark(int index) { + if (!validIndex(index)) return CE_PARAM_RANGE; + if (settings[index].parkState == PS_PARKING) return CE_PARK_FAILED; + if (settings[index].parkState == PS_UNPARKING) return CE_PARK_FAILED; + if (settings[index].parkState == PS_PARK_FAILED) return CE_PARK_FAILED; + + // setting write delay to 0 disables on-the-fly position writes and forces strict parking + if (writeDelay == 0) { + if (settings[index].parkState != PS_PARKED) return CE_NOT_PARKED; + } + + axes[index]->enable(true); + VF("MSG: Focuser"); V(index + 1); VF(", unpark position "); V(settings[index].position); VL("um"); + + // simple unpark if we didn't actually park + if (settings[index].parkState == PS_UNPARKED) { + axes[index]->setInstrumentCoordinate(settings[index].position); + target[index] = lround(settings[index].position*axes[index]->getStepsPerMeasure()) - tcfSteps[index]; + nv().kv().put(nvKey[index], settings[index]); + return CE_NONE; + } + + axes[index]->setBacklashSteps(0); + axes[index]->setInstrumentCoordinatePark(settings[index].position); + + axes[index]->setBacklashSteps(settings[index].backlash); + axes[index]->setTargetCoordinate(settings[index].position); + + CommandError e = axes[index]->autoGoto(settings[index].gotoRate); + + if (e == CE_NONE) { + settings[index].parkState = PS_UNPARKING; + target[index] = lround(settings[index].position*axes[index]->getStepsPerMeasure()) - tcfSteps[index]; + nv().kv().put(nvKey[index], settings[index]); + } + + return e; +} + +// poll focusers to handle parking and TCF +void Focuser::monitor() { + secs++; + + float t = getTemperature(); + for (int index = 0; index < FOCUSER_MAX; index++) { + if (axes[index] != NULL) { + + target[index] = axes[index]->getTargetCoordinateSteps() - tcfSteps[index]; + float targetMicrons = target[index]/axes[index]->getStepsPerMeasure(); + + if (axes[index]->isSlewing() || settings[index].position == targetMicrons) writeTime[index] = secs + writeDelay; + + if (!axes[index]->isSlewing()) { + + if (settings[index].parkState == PS_PARKING) { + if (axes[index]->atTarget()) { + axes[index]->enable(false); + settings[index].parkState = PS_PARKED; + nv().kv().put(nvKey[index], settings[index]); + #if DEBUG == VERBOSE + long offset = axes[index]->getInstrumentCoordinateSteps() - axes[index]->getMotorPositionSteps(); + VF("MSG: Focuser"); V(index + 1); VF(", park motor target "); VL(axes[index]->getTargetCoordinateSteps() - offset); + VF("MSG: Focuser"); V(index + 1); VF(", park motor position "); VL(axes[index]->getMotorPositionSteps()); + #endif + } + } else + + if (settings[index].parkState == PS_UNPARKING) { + if (axes[index]->atTarget()) { + settings[index].parkState = PS_UNPARKED; + nv().kv().put(nvKey[index], settings[index]); + } + } else + + if (settings[index].parkState == PS_UNPARKED) { + if (settings[index].tcf.enabled) { + Y; + if (!isnan(t)) { + float offset = settings[index].tcf.coef * (settings[index].tcf.t0 - t); + offset *= (float)axes[index]->getStepsPerMeasure(); + long steps = lroundf(offset/settings[index].tcf.deadband)*settings[index].tcf.deadband; + if (tcfSteps[index] != steps) { + VF("MSG: Focuser"); V(index + 1); VF(", TCF offset changed moving to target "); + if (steps >= 0) { V("+ "); } else { VF("- "); } V(fabs(steps/axes[index]->getStepsPerMeasure())); VLF("um"); + tcfSteps[index] = steps; + axes[index]->setTargetCoordinateSteps(target[index] + tcfSteps[index]); + } + } + if (!axes[index]->atTarget()) { + axes[index]->setSynchronized(false); + axes[index]->setSynchronizedFrequency(20.0F); // 20um/s + } else { + axes[index]->setSynchronizedFrequency(0.0F); + } + } else tcfSteps[index] = 0; + + if (homing[index]) { + long p = round((axes[index]->getLimitMax() + axes[index]->getLimitMin())/2.0F)*axes[index]->getStepsPerMeasure(); + axes[index]->resetPositionSteps(p); + axes[index]->setBacklashSteps(getBacklashSteps(index)); + homing[index] = false; + } + + // delayed write of focuser position + if (writeDelay != 0) { + if (secs > writeTime[index]) { + settings[index].position = targetMicrons; + nv().kv().put(nvKey[index], settings[index]); + VF("MSG: Focuser"); V(index + 1); VF(", writing position ("); V(targetMicrons); VLF("um) to NV"); + } + } + + } + } + } + } +} + +int Focuser::slavedFocuserIndex(int masterIndex, int candidateIndex) { + if (candidateIndex < 0 || candidateIndex >= FOCUSER_MAX) return -1; + + if (configuration[candidateIndex].present && + axes[candidateIndex] != NULL && + slavedToFocuser[candidateIndex] == masterIndex) { + return candidateIndex; + } + + return -1; +} + +bool Focuser::hasSlaveCycle(int candidateIndex) { + bool seen[FOCUSER_MAX] = {false}; + int i = candidateIndex; + + while (true) { + int mi = slavedToFocuser[i]; + + // no master => chain ends cleanly + if (mi < 0 || mi >= FOCUSER_MAX) return false; + + // broken chain => treat as invalid (block) + if (!configuration[mi].present || axes[mi] == NULL) return true; + + if (mi == i || seen[mi]) return true; + seen[mi] = true; + i = mi; + } +} + +// poll focuser buttons to start/stop movement +#if FOCUSER_BUTTON_SENSE_IN != OFF && FOCUSER_BUTTON_SENSE_OUT != OFF + void Focuser::buttons() { + if (sense.changed(inButtonHandle) || sense.changed(outButtonHandle)) { + bool in = sense.isOn(inButtonHandle); + bool out = sense.isOn(outButtonHandle); + if (in && out) { in = false; out = false; } + if (in) { + if (FOCUSER_BUTTON_MOVE_RATE > 0) moveRate[FOCUSER_BUTTON_FOCUSER_INDEX - 1] = FOCUSER_BUTTON_MOVE_RATE; + move(FOCUSER_BUTTON_FOCUSER_INDEX - 1, DIR_FORWARD); + } else + if (out) { + if (FOCUSER_BUTTON_MOVE_RATE > 0) moveRate[FOCUSER_BUTTON_FOCUSER_INDEX - 1] = FOCUSER_BUTTON_MOVE_RATE; + move(FOCUSER_BUTTON_FOCUSER_INDEX - 1, DIR_REVERSE); + } else + axes[FOCUSER_BUTTON_FOCUSER_INDEX - 1]->autoSlewStop(); + } + } +#endif + +Focuser focuser; + +#endif diff --git a/src/telescope/focuser/local/Focuser.h b/src/telescope/focuser/local/Focuser.h new file mode 100755 index 0000000..3a36275 --- /dev/null +++ b/src/telescope/focuser/local/Focuser.h @@ -0,0 +1,230 @@ +//-------------------------------------------------------------------------------------------------- +// OnStepX focuser control +#pragma once + +#include "../../../Common.h" + +#include "../FocuserBase.h" + +#ifdef FOCUSER_PRESENT + +#include "../../../lib/nv/Nv.h" +#include "../../../lib/axis/Axis.h" +#include "../../../libApp/commands/ProcessCmds.h" +#include "../../Telescope.h" + +#if AXIS9_DRIVER_MODEL != OFF + #define FOCUSER_MAX 6 +#else + #if AXIS8_DRIVER_MODEL != OFF + #define FOCUSER_MAX 5 + #else + #if AXIS7_DRIVER_MODEL != OFF + #define FOCUSER_MAX 4 + #else + #if AXIS6_DRIVER_MODEL != OFF + #define FOCUSER_MAX 3 + #else + #if AXIS5_DRIVER_MODEL != OFF + #define FOCUSER_MAX 2 + #else + #if AXIS4_DRIVER_MODEL != OFF + #define FOCUSER_MAX 1 + #else + #define FOCUSER_MAX 0 + #endif + #endif + #endif + #endif + #endif +#endif + +// switch to allow backlash to be slaved +#ifndef FOCUSER_SLAVE_BACKLASH + #define FOCUSER_SLAVE_BACKLASH OFF +#endif + +#pragma pack(1) +typedef struct Tcf { + bool enabled; + float coef; // in um/°C + int16_t deadband; // in steps + float t0; // in °C, temperature when first enabled +} Tcf; + +#define FocuserSettingsSize 20 +typedef struct FocuserSettings { + Tcf tcf; + ParkState parkState; + int16_t backlash; // in steps + float position; // in microns + int16_t gotoRate; // in microns/s +} FocuserSettings; +#pragma pack() + +#ifdef FOCUSER_CAN_SERVER_PRESENT +#include "../../../lib/canTransport/CanTransportServer.h" +class Focuser : public CanTransportServer { +private: + void processCommand() override; +public: + Focuser() : CanTransportServer((uint16_t)(CAN_FOCUSER_REQ_ID), (uint16_t)(CAN_FOCUSER_RSP_ID)) {} +#else +class Focuser { +public: +#endif + + bool init(); + void begin(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // poll focusers to handle parking and TCF + void monitor(); + + // poll focuser buttons to start/stop movement + #if FOCUSER_BUTTON_SENSE_IN != OFF && FOCUSER_BUTTON_SENSE_OUT != OFF + void buttons(); + #endif + + private: + // make sure our index is valid for this focuser + bool validIndex(int index); + + // get focuser temperature in deg. C + float getTemperature(); + + // check for DC motor focuser + bool isDC(int index); + + // get DC power in % + int getDcPower(int index); + + // set DC power in % + bool setDcPower(int index, int value); + + // get TCF enable + bool getTcfEnable(int index); + + // set TCF enable + CommandError setTcfEnable(int index, bool value); + + // get TCF coefficient, in microns per deg. C + float getTcfCoef(int index); + + // set TCF coefficient, in microns per deg. C + bool setTcfCoef(int index, float value); + + // get TCF deadband, in steps + int getTcfDeadband(int index); + + // set TCF deadband, in steps + bool setTcfDeadband(int index, int value); + + // get TCF T0, in deg. C + float getTcfT0(int index); + + // set TCF T0, in deg. C + bool setTcfT0(int index, float value); + + // get home position in microns + float getHomePosition(int index); + + // get backlash in steps + int getBacklashSteps(int index); + + // set backlash in steps + CommandError setBacklashSteps(int index, int value); + + // set move rate + void setMoveRate(int index, int value); + + // start move in the specified direction + CommandError move(int index, Direction dir); + + // start move to the home position + CommandError moveHome(int index); + + // get goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base + int getGotoRate(int index); + + // set goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base + void setGotoRate(int index, int value); + + // move focuser to a specific location (in steps) + CommandError gotoTarget(int index, long target); + + // reset focuser to a specific location (in steps) + CommandError resetTarget(int index, long target); + + // park focuser at its current location + CommandError park(int index); + + // unpark focuser + CommandError unpark(int index); + + int slavedFocuserIndex(int masterIndex, int candidateIndex); + bool hasSlaveCycle(int candidateIndex); + + bool ready = false; + + int moveRate[FOCUSER_MAX]; + long tcfSteps[FOCUSER_MAX]; + + FocuserSettings settings[FOCUSER_MAX]; + + long target[FOCUSER_MAX]; // in steps + + unsigned long writeTime[FOCUSER_MAX]; + + uint8_t parkHandle[FOCUSER_MAX]; + + bool homing[FOCUSER_MAX]; + + unsigned long secs = 0; + + // the default focuser is the first found + int active = -1; + + #if FOCUSER_BUTTON_SENSE_IN != OFF && FOCUSER_BUTTON_SENSE_OUT != OFF + uint8_t inButtonHandle = 0; + uint8_t outButtonHandle = 0; + #endif + + uint16_t writeDelay = 300; + uint16_t nvKey[6]; +}; + +#if AXIS4_DRIVER_MODEL != OFF + extern Motor& motor4; + extern Axis axis4; +#endif + +#if AXIS5_DRIVER_MODEL != OFF + extern Motor& motor5; + extern Axis axis5; +#endif + +#if AXIS6_DRIVER_MODEL != OFF + extern Motor& motor6; + extern Axis axis6; +#endif + +#if AXIS7_DRIVER_MODEL != OFF + extern Motor& motor7; + extern Axis axis7; +#endif + +#if AXIS8_DRIVER_MODEL != OFF + extern Motor& motor8; + extern Axis axis8; +#endif + +#if AXIS9_DRIVER_MODEL != OFF + extern Motor& motor9; + extern Axis axis9; +#endif + +extern Focuser focuser; + +#endif diff --git a/src/telescope/focuser/remote/Focuser.can.decode.cpp b/src/telescope/focuser/remote/Focuser.can.decode.cpp new file mode 100755 index 0000000..54ec4de --- /dev/null +++ b/src/telescope/focuser/remote/Focuser.can.decode.cpp @@ -0,0 +1,126 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope focuser control, using the CANbus interface (CAN client decode) + +#include "Focuser.h" + +#if defined(FOCUSER_CAN_CLIENT_PRESENT) + +#include "../../../lib/convert/Convert.h" + +bool Focuser::decodeResponse(char *reply) { + if (!reply) return false; + reply[0] = 0; + + // scratchpad variables + float f; + int32_t i; + uint8_t u; + + switch (opCode()) { + + // :FT# -> RX: ASCII bytes "M"/"S" + rateDigit + case FOC_OP_GET_STATUS_T: + u = readRemainingBytes((uint8_t*)reply); + reply[u] = 0; + break; + + // :FI# / :FM# -> RX: i32LE (units already selected by ctrl on request) + case FOC_OP_GET_MIN_I: + case FOC_OP_GET_MAX_M: + if (!readI32(i)) return false; + sprintf(reply, "%ld", (long)i); + break; + + // :Fe# / :Ft# -> RX: f32LE temperature values + case FOC_OP_GET_TEMPDIFF_e: + case FOC_OP_GET_TEMP_t: + if (!readF32(f)) return false; + // local code uses "%3.1f" + sprintF(reply, "%3.1f", f); + break; + + // :Fu# -> RX: f32LE microns per step + case FOC_OP_GET_UM_PER_STEP_u: + if (!readF32(f)) return false; + // local code uses "%7.5f" + sprintF(reply, "%7.5f", f); + break; + + // :FB# (get) -> RX: i32LE backlash (units) + // :FB[n]# (set) -> typically no payload (status-only) + case FOC_OP_BACKLASH_B: + if (remaining() == 0) return true; + if (!readI32(i)) return false; + sprintf(reply, "%ld", (long)i); + break; + + // :FC# (get) -> RX: f32LE + // :FC[sn.n]# (set) -> usually no payload + case FOC_OP_TCF_COEF_C: + if (remaining() == 0) return true; + if (!readF32(f)) return false; + // local code uses "%7.5f" + sprintF(reply, "%7.5f", f); + break; + + // :Fc# (get) -> RX: u8 0/1 (if server returns payload) OR status-only + // :Fc[n]# (set) -> no payload + case FOC_OP_TCF_ENABLE_c: + if (remaining() == 0) return true; + if (!readU8(u)) return false; + sprintf(reply, "%u", (unsigned)(u ? 1 : 0)); + break; + + // :FD# (get) -> RX: i32LE (units) + // :FD[n]# (set) -> no payload + case FOC_OP_TCF_DEADBAND_D: + if (remaining() == 0) return true; + if (!readI32(i)) return false; + sprintf(reply, "%ld", (long)i); + break; + + // :FP# (get) -> RX: u8 percent (or i32 if you choose; server I gave uses u8) + // :FP[n]# (set) -> no payload + case FOC_OP_DC_POWER_P: + if (remaining() == 0) return true; + if (!readU8(u)) return false; + sprintf(reply, "%u", (unsigned)u); + break; + + // :FW# -> RX: i32LE work rate (um/s or whatever your server returns) + case FOC_OP_GET_WORKRATE_W: + if (remaining() == 0) return true; + if (!readI32(i)) return false; + sprintf(reply, "%ld", (long)i); + break; + + // :FG# -> RX: i32LE position (units) + case FOC_OP_GET_POS_G: + if (!readI32(i)) return false; + sprintf(reply, "%ld", (long)i); + break; + + // :GXU[n]# -> RX u8 packed status + case FOC_OP_DRIVER_STATUS: { + uint8_t packedStatus; + readU8(packedStatus); + strcat(reply, ( packedStatus & 1) ? "ST," : ","); + strcat(reply, ((packedStatus << 1) & 1) ? "OA," : ","); + strcat(reply, ((packedStatus << 2) & 1) ? "OB," : ","); + strcat(reply, ((packedStatus << 3) & 1) ? "GA," : ","); + strcat(reply, ((packedStatus << 4) & 1) ? "GB," : ","); + strcat(reply, ((packedStatus << 5) & 1) ? "OT," : ","); // > 150C + strcat(reply, ((packedStatus << 6) & 1) ? "PW," : ","); // > 120C + strcat(reply, ((packedStatus << 7) & 1) ? "GF" : ""); + } break; + + default: + // Most ops are "no reply payload"; empty reply is fine. + reply[0] = 0; + break; + } + + return true; +} + +#endif diff --git a/src/telescope/focuser/remote/Focuser.can.encode.cpp b/src/telescope/focuser/remote/Focuser.can.encode.cpp new file mode 100755 index 0000000..97eb9a5 --- /dev/null +++ b/src/telescope/focuser/remote/Focuser.can.encode.cpp @@ -0,0 +1,208 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope focuser control, using the CANbus interface + +#include "Focuser.h" + +#if defined(FOCUSER_CAN_CLIENT_PRESENT) + +#include "../../../lib/convert/Convert.h" + +bool Focuser::encodeRequest(char *command, char *parameter) { + + // on explicit focuser override the active focuser and reform the command/parameter + uint8_t selectedFocuser = (uint8_t)(active + 1); // 1..6 + + if (command[0] == 'F') { + if (command[1] >= '1' && command[1] <= '6' && parameter[0] != 0) { + selectedFocuser = (uint8_t)(command[1] - '0'); // 1..6 + command[1] = parameter[0]; + int len = strlen(parameter); + for (int i = 0; i <= len; i++) { parameter[i] = parameter[i + 1]; } + } + + // check to see if the selected focuser is present + if (!heartbeatFresh(selectedFocuser - 1)) return false; + + // check for commands that shouldn't have a parameter + if (strchr("aTpIMtuQF1234+-GZHh", command[1]) && parameter[0] != 0) return false; + + // flag if this command is using microns + bool useMicrons = strchr("bdgimrs", command[1]); + + // ctrl byte: focuser number + units bit (1=microns, 0=steps) + uint8_t ctrl = (uint8_t)(selectedFocuser & FOC_CTRL_FOCUSER_MASK); + if (useMicrons) ctrl |= FOC_CTRL_UNIT_MICRONS; + + // :FT# + if (command[1] == 'T') { + if (!beginNewRequest(FOC_OP_GET_STATUS_T)) return false; else writeU8(ctrl); + } else + + // :Fp# + if (command[1] == 'p') { + if (!beginNewRequest(FOC_OP_GET_MODE_p)) return false; else writeU8(ctrl); + } else + + // :FI# / :Fi# (min) (units via ctrl) + if (toupper(command[1]) == 'I') { + if (!beginNewRequest(FOC_OP_GET_MIN_I)) return false; else writeU8(ctrl); + } else + + // :FM# / :Fm# (max) (units via ctrl) + if (toupper(command[1]) == 'M') { + if (!beginNewRequest(FOC_OP_GET_MAX_M)) return false; else writeU8(ctrl); + } else + + // :Fe# + if (command[1] == 'e') { + if (!beginNewRequest(FOC_OP_GET_TEMPDIFF_e)) return false; else writeU8(ctrl); + } else + + // :Ft# + if (command[1] == 't') { + if (!beginNewRequest(FOC_OP_GET_TEMP_t)) return false; else writeU8(ctrl); + } else + + // :Fu# + if (command[1] == 'u') { + if (!beginNewRequest(FOC_OP_GET_UM_PER_STEP_u)) return false; else writeU8(ctrl); + } else + + // :FB# (get) or :FB[n]# (set) (units via ctrl) + if (toupper(command[1]) == 'B') { + if (!beginNewRequest(FOC_OP_BACKLASH_B)) return false; else writeU8(ctrl); + if (parameter[0] != 0) { + const long v = atol(parameter); + if (!writeI32((int32_t)v)) return false; + } + } else + + // :FC# (get f32) or :FC[sn.n]# (set f32) + if (command[1] == 'C') { + if (!beginNewRequest(FOC_OP_TCF_COEF_C)) return false; else writeU8(ctrl); + if (parameter[0] != 0) { + const float v = (float)atof(parameter); + if (!writeF32(v)) return false; + } + } else + + // :Fc# (get) or :Fc[n]# (set u8) + if (command[1] == 'c') { + if (!beginNewRequest(FOC_OP_TCF_ENABLE_c)) return false; else writeU8(ctrl); + if (parameter[0] != 0) { + if (parameter[1] != 0) return false; + const uint8_t en = (parameter[0] != '0') ? 1 : 0; + if (!writeU8(en)) return false; + } + } else + + // :FD# (get) or :FD[n]# (set i32) + if (toupper(command[1]) == 'D') { + if (!beginNewRequest(FOC_OP_TCF_DEADBAND_D)) return false; else writeU8(ctrl); + if (parameter[0] != 0) { + const long v = atol(parameter); + if (!writeI32((int32_t)v)) return false; + } + } else + + // :FP# (get) or :FP[n]# (set u8) + if (command[1] == 'P') { + if (!beginNewRequest(FOC_OP_DC_POWER_P)) return false; else writeU8(ctrl); + if (parameter[0] != 0) { + const long v = atol(parameter); + if (v < 0 || v > 100) return false; + if (!writeU8((uint8_t)v)) return false; + } + } else + + // :FQ# + if (command[1] == 'Q') { + if (!beginNewRequest(FOC_OP_STOP_Q)) return false; else writeU8(ctrl); + } else + + // :F[1..9]# (rate digit; no param) + if (command[1] >= '1' && command[1] <= '9') { + if (parameter[0] != 0) return false; + const uint8_t digit = (uint8_t)(command[1] - '0'); // 1..9 + if (!beginNewRequest(FOC_OP_SET_RATE_1_9)) return false; else writeU8(ctrl); + if (!writeU8(digit)) return false; + } else + + // :FW# + if (command[1] == 'W') { + if (!beginNewRequest(FOC_OP_GET_WORKRATE_W)) return false; else writeU8(ctrl); + } else + + // :F+# + if (command[1] == '+') { + if (!beginNewRequest(FOC_OP_MOVE_IN_PLUS)) return false; else writeU8(ctrl); + } else + + // :F-# + if (command[1] == '-') { + if (!beginNewRequest(FOC_OP_MOVE_OUT_MINUS)) return false; else writeU8(ctrl); + } else + + // :FG# / :Fg# (get pos i32 units via ctrl) + if (toupper(command[1]) == 'G') { + if (!beginNewRequest(FOC_OP_GET_POS_G)) return false; else writeU8(ctrl); + } else + + // :FR[sn]# / :Fr[sn]# (relative i32 units via ctrl) + if (toupper(command[1]) == 'R') { + if (parameter[0] == 0) return false; + if (!beginNewRequest(FOC_OP_GOTO_REL_R)) return false; else writeU8(ctrl); + if (!writeI32((int32_t)atol(parameter))) return false; + } else + + // :FS[n]# / :Fs[n]# (absolute i32 units via ctrl) + if (toupper(command[1]) == 'S') { + if (parameter[0] == 0) return false; + if (!beginNewRequest(FOC_OP_GOTO_ABS_S)) return false; else writeU8(ctrl); + if (!writeI32((int32_t)atol(parameter))) return false; + } else + + // :FZ# (zero) + if (command[1] == 'Z') { + if (!beginNewRequest(FOC_OP_ZERO_Z)) return false; else writeU8(ctrl); + } else + + // :FH# (set home) + if (command[1] == 'H') { + if (!beginNewRequest(FOC_OP_SET_HOME_H)) return false; else writeU8(ctrl); + } else + + // :Fh# (goto home) + if (command[1] == 'h') { + if (!beginNewRequest(FOC_OP_GOTO_HOME_h)) return false; else writeU8(ctrl); + } else + return false; + + return true; + } else + + if (command[0] == 'G' && command[1] == 'X') { + // :GXU[n]# Get focuser driver status + if (parameter[0] == 'U' && parameter[1] >= '4' && parameter[1] <= '9' && parameter[2] == 0) { + + // figure out which CAN focuser this would logically map to + uint8_t associatedFocuser = parameter[1] - '3'; // 1..6 + + // ctrl byte: focuser number + uint8_t ctrl = (uint8_t)(associatedFocuser & FOC_CTRL_FOCUSER_MASK); + + // check to see if the associated focuser is present + if (!heartbeatFresh(associatedFocuser - 1)) return false; + + // if present get its driver status + if (!beginNewRequest(FOC_OP_DRIVER_STATUS)) return false; else writeU8(ctrl); + } else + return false; + + return true; + } + + return true; +} + +#endif diff --git a/src/telescope/focuser/remote/Focuser.cpp b/src/telescope/focuser/remote/Focuser.cpp new file mode 100755 index 0000000..ad3e729 --- /dev/null +++ b/src/telescope/focuser/remote/Focuser.cpp @@ -0,0 +1,166 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control local node, using the CANbus interface + +#include "Focuser.h" + +#ifdef FOCUSER_CAN_CLIENT_PRESENT + +// -------------------------------------------------------------------------- +// Heartbeat RX (local node): update last-heard timestamps for focuser 1..6. +// -------------------------------------------------------------------------- +static void focHb0(uint8_t data[8], uint8_t len) { (void)data; (void)len; focuser.heartbeat(0); } +static void focHb1(uint8_t data[8], uint8_t len) { (void)data; (void)len; focuser.heartbeat(1); } +static void focHb2(uint8_t data[8], uint8_t len) { (void)data; (void)len; focuser.heartbeat(2); } +static void focHb3(uint8_t data[8], uint8_t len) { (void)data; (void)len; focuser.heartbeat(3); } +static void focHb4(uint8_t data[8], uint8_t len) { (void)data; (void)len; focuser.heartbeat(4); } +static void focHb5(uint8_t data[8], uint8_t len) { (void)data; (void)len; focuser.heartbeat(5); } + +void Focuser::begin() { + canPlus.callbackRegisterId((int)(CAN_FOCUSER_HB_ID_BASE + 0), focHb0); + canPlus.callbackRegisterId((int)(CAN_FOCUSER_HB_ID_BASE + 1), focHb1); + canPlus.callbackRegisterId((int)(CAN_FOCUSER_HB_ID_BASE + 2), focHb2); + canPlus.callbackRegisterId((int)(CAN_FOCUSER_HB_ID_BASE + 3), focHb3); + canPlus.callbackRegisterId((int)(CAN_FOCUSER_HB_ID_BASE + 4), focHb4); + canPlus.callbackRegisterId((int)(CAN_FOCUSER_HB_ID_BASE + 5), focHb5); +} + +// by default reply[80] == "", suppressFrame == false, numericReply == true, and commandError == CE_NONE +// return true if the command has been completely handled and no further command() will be called, or false if not +// for commands that are handled repeatedly commandError might contain CE_NONE or CE_1 to indicate success +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +bool Focuser::command(char *reply, char *command, char *parameter, + bool *suppressFrame, bool *numericReply, CommandError *commandError) { + if (!canPlus.ready) return false; + + // find the default focuser if the current one isn't active + // if the current focuser is unassigned (-1) or disappears scan again + if (!heartbeatFresh(active)) { + active = -1; + for (int focuserIdx = 0; focuserIdx < 6; focuserIdx++) { + if (heartbeatFresh(focuserIdx)) { active = focuserIdx; break; } + } + } + + // -------------------------------------------------------------------------- + // :hP# / :hR# park/unpark ALL present focusers + // Numeric result only. Defaults: numericReply==true, suppressFrame==false. + // -------------------------------------------------------------------------- + if (command[0] == 'h' && (command[1] == 'P' || command[1] == 'R') && parameter[0] == 0) { + const uint8_t op = (command[1] == 'P') ? (uint8_t)FOC_OP_PARK_HP : (uint8_t)FOC_OP_UNPARK_HR; + + bool anySent = false; + CommandError worst = CE_NONE; + + for (uint8_t focuserIdx = 0; focuserIdx < 6; focuserIdx++) { + uint8_t ctrl = (uint8_t)(focuserIdx & FOC_CTRL_FOCUSER_MASK); + + if (!heartbeatFresh(focuserIdx)) continue; + anySent = true; + + if (beginNewRequest(op)) { + writeU8(ctrl); + bool handled = false; + if (transactRequest(handled, *suppressFrame, *numericReply, *commandError)) { + if (!handled) worst = CE_CMD_UNKNOWN; + } else *commandError = CE_REPLY_UNKNOWN; + } + + if (*commandError != CE_NONE && *commandError != CE_1) worst = *commandError; + } + + *commandError = (!anySent) ? CE_0 : ((worst == CE_NONE) ? CE_1 : worst); + return true; + } + + // -------------------------------------------------------------------------- + // active focuser return / selection + // -------------------------------------------------------------------------- + + if (command[0] == 'F' && command[1] == 'A') { + + // :FA# Focuser Active? + // Return: 0 on failure (no focusers) + // 1 to 6 on success + if (parameter[0] == 0) { + if (active < 0) { *commandError = CE_0; return true; } + + const uint8_t focuserNum = (uint8_t)(active + 1); + sprintf(reply, "%u", (unsigned)focuserNum); + *numericReply = false; + *suppressFrame = true; + *commandError = CE_NONE; + return true; + } else + + // :FA[n]# Select focuser where [n] = 1 to 6 + // Return: 0 on failure + // 1 on success + if (parameter[1] == 0) { + const int i = parameter[0] - '1'; + if (i < 0 || i > 5 || !heartbeatFresh(i)) { *commandError = CE_PARAM_RANGE; return true; } + active = i; + return true; + } else return false; + } + + // -------------------------------------------------------------------------- + // :Fa# Focuser presence detection + // Returns: 1 true if present + // -------------------------------------------------------------------------- + if (command[0] && command[1] == 'a' && parameter[0] == 0) { + const int i = parameter[0] - '1'; + if (i < 0 || i > 5 || !heartbeatFresh(i)) { *commandError = CE_PARAM_RANGE; return true; } + return true; + } + + // :F[n]a# Focuser presence detection + if ((command[0] == 'F' && command[1] >= '1' && command[1] <= '6' && parameter[0] == 'a' && parameter[1] == 0)) { + sprintf(reply, "%u", heartbeatFresh((int)(command[1] - '1')) ? 1u : 0u); + *numericReply = false; + *suppressFrame = true; + *commandError = CE_NONE; + return true; + } + + // -------------------------------------------------------------------------- + // F - focuser commands only + // -------------------------------------------------------------------------- + if (!(command[0] == 'F') && + !(command[0] == 'G' && command[1] == 'X' && parameter[0] == 'U')) return false; + + // -------------------------------------------------------------------------- + // Generic CAN request/response path + // -------------------------------------------------------------------------- + + if (!encodeRequest(command, parameter)) return false; + + // Transport failure => reply unknown + bool handled = false; + if (!transactRequest(handled, *suppressFrame, *numericReply, *commandError)) { *commandError = CE_REPLY_UNKNOWN; return true; } + + // Remote explicitly did not recognize/handle the command + if (!handled) { + *commandError = CE_CMD_UNKNOWN; + *numericReply = true; + *suppressFrame = false; + reply[0] = 0; + return true; + } + + // Numeric reply: nothing to decode; caller will render numeric based on commandError + if (*numericReply) return true; + + // Payload reply: decode into reply buffer; decode failure => reply unknown + if (!decodeResponse(reply)) { + *commandError = CE_REPLY_UNKNOWN; + *numericReply = true; + *suppressFrame = false; + reply[0] = 0; + } + + return true; +} + +Focuser focuser; + +#endif diff --git a/src/telescope/focuser/remote/Focuser.h b/src/telescope/focuser/remote/Focuser.h new file mode 100755 index 0000000..e6b2d2b --- /dev/null +++ b/src/telescope/focuser/remote/Focuser.h @@ -0,0 +1,51 @@ +//-------------------------------------------------------------------------------------------------- +// telescope focuser control local node, using the CANbus interface +#pragma once + +#include "../../../Common.h" + +#include "../FocuserBase.h" + +#ifdef FOCUSER_CAN_CLIENT_PRESENT + +#include "../../../lib/canTransport/CanTransportClient.h" +#include "../../../libApp/commands/ProcessCmds.h" + +class Focuser : public CanTransportClient { + public: + Focuser() : CanTransportClient((uint16_t)(CAN_FOCUSER_REQ_ID), (uint16_t)(CAN_FOCUSER_RSP_ID)) {} + + void begin(); + + bool command(char *reply, char *command, char *parameter, + bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // Heartbeat event, indexed by focuser 1..6 => focuserIdx 0..5 + inline void heartbeat(uint8_t focuserIdx) { lastHeartbeatMs[focuserIdx] = hb_stamp_2ms_odd(); } + + private: + bool encodeRequest(char *command, char *parameter); + bool decodeResponse(char *reply); + + static inline uint32_t hb_stamp_2ms_odd() { + return millis() | 1UL; + } + + // Heartbeat presence, indexed by focuser 1..6 => focuserIdx 0..5 + inline bool heartbeatFresh(int focuserIdx) const { + if (focuserIdx < 0 || focuserIdx > 5) return false; + const uint32_t last = lastHeartbeatMs[focuserIdx]; + if (last == 0U) return false; + const uint32_t now = hb_stamp_2ms_odd(); + return (uint32_t)(now - last) <= 2000U; + } + + int active = -1; + + // Heartbeat timestamps (ms), indexed by focuser 1..6 => [0..5] + volatile uint32_t lastHeartbeatMs[6] = {0, 0, 0, 0, 0, 0}; +}; + +extern Focuser focuser; + +#endif diff --git a/src/telescope/mount/Mount.axis.cpp b/src/telescope/mount/Mount.axis.cpp new file mode 100755 index 0000000..90f27b4 --- /dev/null +++ b/src/telescope/mount/Mount.axis.cpp @@ -0,0 +1,242 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, axis2 and axis3 + +#include "Mount.h" + +#ifdef MOUNT_PRESENT + +// Axis2 motion controller + +namespace { + +#if defined(AXIS1_ODRIVE_PRESENT) + const ODriveDriverSettings DriverSettingsAxis1 = {AXIS1_DRIVER_MODEL, AXIS1_DRIVER_STATUS}; + ODriveMotor motor_1(1, AXIS1_REVERSE, &DriverSettingsAxis1, (AXIS1_STEPS_PER_DEGREE*RAD_DEG_RATIO)); + +#elif defined(AXIS1_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis1 = {AXIS1_DRIVER_MODEL, AXIS1_DRIVER_STATUS}; + KTechMotor motor_1(1, AXIS1_REVERSE, &DriverSettingsAxis1, AXIS1_STEPS_PER_DEGREE); + +#elif defined(AXIS1_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis1 = {AXIS1_DRIVER_MODEL, AXIS1_DRIVER_STATUS}; + Mks42DMotor motor_1(1, AXIS1_REVERSE, &DriverSettingsAxis1, AXIS1_STEPS_PER_DEGREE); + +#elif defined(AXIS1_SERVO_PRESENT) + ServoControl servoControlAxis1; + + #if AXIS1_ENCODER == AB + Quadrature encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == CW_CCW + CwCcw encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == PULSE_DIR + PulseDir encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == PULSE_ONLY + PulseOnly encAxis1(1, AXIS1_ENCODER_A_PIN, &servoControlAxis1.directionHint); + #elif AXIS1_ENCODER == VIRTUAL + VirtualEnc encAxis1(1); + #elif AXIS1_ENCODER == AS37_H39B_B + As37h39bb encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == JTW_24BIT + Jtw24 encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == JTW_26BIT + Jtw26 encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == LIKA_ASC85 + LikaAsc85 encAxis1(1, AXIS1_ENCODER_A_PIN, AXIS1_ENCODER_B_PIN); + #elif AXIS1_ENCODER == KTECH_IME + KTechIME encAxis1(1); + #elif AXIS1_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis1(1); + #endif + + #if AXIS1_SERVO_FEEDBACK == PID + Pid feedbackAxis1(AXIS1_PID_P, AXIS1_PID_I, AXIS1_PID_D); + #elif AXIS1_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis1(AXIS1_PID_P, AXIS1_PID_I, AXIS1_PID_D, AXIS1_PID_P_GOTO, AXIS1_PID_I_GOTO, AXIS1_PID_D_GOTO, AXIS1_PID_SENSITIVITY); + #endif + + #if AXIS1_SERVO_FLTR == KALMAN + KalmanFilter filterAxis1(AXIS1_SERVO_FLTR_MEAS_U, AXIS1_SERVO_FLTR_VARIANCE); + #elif AXIS1_SERVO_FLTR == LEARNING + // nothing to do as this is auotmatically created... LearningFilter filterAxis1(AXIS1_SERVO_FLTR_WSIZE, 240); + #elif AXIS1_SERVO_FLTR == ROLLING + RollingFilter filterAxis1(AXIS1_SERVO_FLTR_WSIZE); + #elif AXIS1_SERVO_FLTR == OFF + Filter filterAxis1; + #endif + + const ServoPins DriverPinsAxis1 = {AXIS1_SERVO_PH1_PIN, AXIS1_SERVO_PH1_STATE, AXIS1_SERVO_PH2_PIN, AXIS1_SERVO_PH2_STATE, AXIS1_ENABLE_PIN, AXIS1_ENABLE_STATE, AXIS1_M0_PIN, AXIS1_M1_PIN, AXIS1_M2_PIN, AXIS1_M3_PIN, AXIS1_FAULT_PIN}; + const ServoSettings DriverSettingsAxis1 = {AXIS1_DRIVER_MODEL, AXIS1_DRIVER_STATUS, AXIS1_SERVO_ACCELERATION}; + + #if AXIS1_DRIVER_MODEL == SERVO_EE + ServoEE driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_SERVO_DC_PWR_MIN, AXIS1_SERVO_DC_PWR_MAX); + #elif AXIS1_DRIVER_MODEL == SERVO_PE + ServoPE driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_SERVO_DC_PWR_MIN, AXIS1_SERVO_DC_PWR_MAX); + #elif AXIS1_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc5160DC driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_SERVO_DC_PWR_MIN, AXIS1_SERVO_DC_PWR_MAX); + #elif AXIS1_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc2130DC driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_SERVO_DC_PWR_MIN, AXIS1_SERVO_DC_PWR_MAX); + #elif AXIS1_DRIVER_MODEL == SERVO_TMC2209 + ServoTmc2209 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_MOTOR_STEPS_PER_DEGREE/AXIS1_STEPS_PER_DEGREE, AXIS1_DRIVER_MICROSTEPS, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_DECAY, AXIS1_DRIVER_DECAY_GOTO); + #elif AXIS1_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_MOTOR_STEPS_PER_DEGREE/AXIS1_STEPS_PER_DEGREE, AXIS1_DRIVER_MICROSTEPS, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_DECAY, AXIS1_DRIVER_DECAY_GOTO); + #elif AXIS1_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver1(1, &DriverSettingsAxis1, AXIS1_MOTOR_STEPS_PER_DEGREE/AXIS1_STEPS_PER_DEGREE); + #endif + + ServoMotor motor_1(1, AXIS1_REVERSE, ((ServoDriver*)&driver1), &filterAxis1, &encAxis1, AXIS1_ENCODER_ORIGIN, AXIS1_ENCODER_REVERSE == ON, &feedbackAxis1, &servoControlAxis1); + +#elif defined(AXIS1_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis1 = {AXIS1_M0_PIN, AXIS1_M1_PIN, AXIS1_M2_PIN, AXIS1_M2_ON_STATE, AXIS1_M3_PIN, AXIS1_DECAY_PIN, AXIS1_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis1 = {AXIS1_DRIVER_MODEL, AXIS1_DRIVER_STATUS, AXIS1_DRIVER_MICROSTEPS, AXIS1_DRIVER_MICROSTEPS_GOTO, AXIS1_DRIVER_DECAY, AXIS1_DRIVER_DECAY_GOTO}; + #if AXIS1_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS1_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1); + #elif AXIS1_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_DRIVER_IHOLD, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_IGOTO, AXIS1_DRIVER_INTPOL); + #elif AXIS1_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_DRIVER_IHOLD, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_IGOTO, AXIS1_DRIVER_INTPOL); + #elif AXIS1_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_DRIVER_IHOLD, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_IGOTO, AXIS1_DRIVER_INTPOL); + #elif AXIS1_DRIVER_MODEL == TMC2209 || AXIS1_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_DRIVER_IHOLD, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_IGOTO, AXIS1_DRIVER_INTPOL); + #elif AXIS1_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_DRIVER_IHOLD, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_IGOTO, AXIS1_DRIVER_INTPOL); + #elif AXIS1_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_DRIVER_IHOLD, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_IGOTO, AXIS1_DRIVER_INTPOL); + #elif AXIS1_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver1(1, &DriverPinsAxis1, &DriverSettingsAxis1, AXIS1_DRIVER_IHOLD, AXIS1_DRIVER_IRUN, AXIS1_DRIVER_IGOTO, AXIS1_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis1 = {AXIS1_STEP_PIN, AXIS1_STEP_STATE, AXIS1_DIR_PIN, AXIS1_ENABLE_PIN, AXIS1_ENABLE_STATE}; + StepDirMotor motor_1(1, AXIS1_REVERSE, &StepDirPinsAxis1, ((StepDirDriver*)&driver1)); + +#else + #error "Configuration (Config.h): MOUNT_PRESENT without AXIS1_DRIVER_MODEL should never happen!" +#endif + +const AxisPins PinsAxis1 = {AXIS1_SENSE_LIMIT_MIN_PIN, AXIS1_SENSE_HOME_PIN, AXIS1_SENSE_LIMIT_MAX_PIN, {AXIS1_SENSE_HOME, AXIS1_SENSE_HOME_INIT, degToRadF(AXIS1_SENSE_HOME_DIST_LIMIT), AXIS1_SENSE_LIMIT_MIN, AXIS1_SENSE_LIMIT_MAX, AXIS1_SENSE_LIMIT_INIT}}; +const AxisSettings SettingsAxis1 = {AXIS1_STEPS_PER_DEGREE*RAD_DEG_RATIO, {degToRadF(AXIS1_LIMIT_MIN), degToRadF(AXIS1_LIMIT_MAX)}, siderealToRad(TRACK_BACKLASH_RATE)}; +} + +Motor& motor1 = motor_1; + +Axis axis1(1, &PinsAxis1, &SettingsAxis1, AXIS_MEASURE_RADIANS, arcsecToRad(AXIS1_TARGET_TOLERANCE)); + +// Axis2 motion controller + +namespace { + +#if defined(AXIS2_ODRIVE_PRESENT) + const ODriveDriverSettings DriverSettingsAxis2 = {AXIS2_DRIVER_MODEL, AXIS2_DRIVER_STATUS}; + ODriveMotor motor_2(2, AXIS2_REVERSE, &DriverSettingsAxis2, (AXIS2_STEPS_PER_DEGREE*RAD_DEG_RATIO)); + +#elif defined(AXIS2_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis2 = {AXIS2_DRIVER_MODEL, AXIS2_DRIVER_STATUS}; + KTechMotor motor_2(2, AXIS2_REVERSE, &DriverSettingsAxis2); + +#elif defined(AXIS2_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis2 = {AXIS2_DRIVER_MODEL, AXIS2_DRIVER_STATUS}; + Mks42DMotor motor_2(2, AXIS2_REVERSE, &DriverSettingsAxis2, AXIS2_STEPS_PER_DEGREE); + +#elif defined(AXIS2_SERVO_PRESENT) + ServoControl servoControlAxis2; + + #if AXIS2_ENCODER == AB + Quadrature encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == CW_CCW + CwCcw encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == PULSE_DIR + PulseDir encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == PULSE_ONLY + PulseOnly encAxis2(2, AXIS2_ENCODER_A_PIN, &servoControlAxis2.directionHint); + #elif AXIS2_ENCODER == VIRTUAL + VirtualEnc encAxis2(2); + #elif AXIS2_ENCODER == AS37_H39B_B + As37h39bb encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == JTW_24BIT + Jtw24 encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == JTW_26BIT + Jtw26 encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == LIKA_ASC85 + LikaAsc85 encAxis2(2, AXIS2_ENCODER_A_PIN, AXIS2_ENCODER_B_PIN); + #elif AXIS2_ENCODER == KTECH_IME + KTechIME encAxis2(2); + #elif AXIS2_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis2(2); + #endif + + #if AXIS2_SERVO_FEEDBACK == PID + Pid feedbackAxis2(AXIS2_PID_P, AXIS2_PID_I, AXIS2_PID_D); + #elif AXIS2_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis2(AXIS2_PID_P, AXIS2_PID_I, AXIS2_PID_D, AXIS2_PID_P_GOTO, AXIS2_PID_I_GOTO, AXIS2_PID_D_GOTO, AXIS2_PID_SENSITIVITY); + #endif + + #if AXIS2_SERVO_FLTR == KALMAN + KalmanFilter filterAxis2(AXIS2_SERVO_FLTR_MEAS_U, AXIS2_SERVO_FLTR_VARIANCE); + #elif AXIS2_SERVO_FLTR == ROLLING + RollingFilter filterAxis2(AXIS2_SERVO_FLTR_WSIZE); + #elif AXIS2_SERVO_FLTR == OFF + Filter filterAxis2; + #endif + + const ServoPins DriverPinsAxis2 = {AXIS2_SERVO_PH1_PIN, AXIS2_SERVO_PH1_STATE, AXIS2_SERVO_PH2_PIN, AXIS2_SERVO_PH2_STATE, AXIS2_ENABLE_PIN, AXIS2_ENABLE_STATE, AXIS2_M0_PIN, AXIS2_M1_PIN, AXIS2_M2_PIN, AXIS2_M3_PIN, AXIS2_FAULT_PIN}; + const ServoSettings DriverSettingsAxis2 = {AXIS2_DRIVER_MODEL, AXIS2_DRIVER_STATUS, AXIS2_SERVO_ACCELERATION}; + + #if AXIS2_DRIVER_MODEL == SERVO_EE + ServoEE driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_SERVO_DC_PWR_MIN, AXIS2_SERVO_DC_PWR_MAX); + #elif AXIS2_DRIVER_MODEL == SERVO_PE + ServoPE driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_SERVO_DC_PWR_MIN, AXIS2_SERVO_DC_PWR_MAX); + #elif AXIS2_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_SERVO_DC_PWR_MIN, AXIS2_SERVO_DC_PWR_MAX); + #elif AXIS2_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_SERVO_DC_PWR_MIN, AXIS2_SERVO_DC_PWR_MAX); + #elif AXIS2_DRIVER_MODEL == SERVO_TMC2209 + ServoTmc2209 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_MOTOR_STEPS_PER_DEGREE/AXIS2_STEPS_PER_DEGREE, AXIS2_DRIVER_MICROSTEPS, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_DECAY, AXIS2_DRIVER_DECAY_GOTO); + #elif AXIS2_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_MOTOR_STEPS_PER_DEGREE/AXIS2_STEPS_PER_DEGREE, AXIS2_DRIVER_MICROSTEPS, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_DECAY, AXIS2_DRIVER_DECAY_GOTO); + #elif AXIS2_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver2(2, &DriverSettingsAxis2, AXIS2_MOTOR_STEPS_PER_DEGREE/AXIS2_STEPS_PER_DEGREE); + #endif + + ServoMotor motor_2(2, AXIS2_REVERSE, ((ServoDriver*)&driver2), &filterAxis2, &encAxis2, AXIS2_ENCODER_ORIGIN, AXIS2_ENCODER_REVERSE == ON, &feedbackAxis2, &servoControlAxis2); + +#elif defined(AXIS2_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis2 = {AXIS2_M0_PIN, AXIS2_M1_PIN, AXIS2_M2_PIN, AXIS2_M2_ON_STATE, AXIS2_M3_PIN, AXIS2_DECAY_PIN, AXIS2_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis2 = {AXIS2_DRIVER_MODEL, AXIS2_DRIVER_STATUS, AXIS2_DRIVER_MICROSTEPS, AXIS2_DRIVER_MICROSTEPS_GOTO, AXIS2_DRIVER_DECAY, AXIS2_DRIVER_DECAY_GOTO}; + #if AXIS2_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS2_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2); + #elif AXIS2_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_DRIVER_IHOLD, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_IGOTO, AXIS2_DRIVER_INTPOL); + #elif AXIS2_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_DRIVER_IHOLD, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_IGOTO, AXIS2_DRIVER_INTPOL); + #elif AXIS2_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_DRIVER_IHOLD, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_IGOTO, AXIS2_DRIVER_INTPOL); + #elif AXIS2_DRIVER_MODEL == TMC2209 || AXIS2_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_DRIVER_IHOLD, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_IGOTO, AXIS2_DRIVER_INTPOL); + #elif AXIS2_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_DRIVER_IHOLD, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_IGOTO, AXIS2_DRIVER_INTPOL); + #elif AXIS2_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_DRIVER_IHOLD, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_IGOTO, AXIS2_DRIVER_INTPOL); + #elif AXIS2_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver2(2, &DriverPinsAxis2, &DriverSettingsAxis2, AXIS2_DRIVER_IHOLD, AXIS2_DRIVER_IRUN, AXIS2_DRIVER_IGOTO, AXIS2_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis2 = {AXIS2_STEP_PIN, AXIS2_STEP_STATE, AXIS2_DIR_PIN, AXIS2_ENABLE_PIN, AXIS2_ENABLE_STATE}; + StepDirMotor motor_2(2, AXIS2_REVERSE, &StepDirPinsAxis2, ((StepDirDriver*)&driver2)); + +#else + #error "Configuration (Config.h): MOUNT_PRESENT without AXIS2_DRIVER_MODEL should never happen!" +#endif + +const AxisPins PinsAxis2 = {AXIS2_SENSE_LIMIT_MIN_PIN, AXIS2_SENSE_HOME_PIN, AXIS2_SENSE_LIMIT_MAX_PIN, {AXIS2_SENSE_HOME, AXIS2_SENSE_HOME_INIT, degToRadF(AXIS2_SENSE_HOME_DIST_LIMIT), AXIS2_SENSE_LIMIT_MIN, AXIS2_SENSE_LIMIT_MAX, AXIS2_SENSE_LIMIT_INIT}}; +const AxisSettings SettingsAxis2 = {AXIS2_STEPS_PER_DEGREE*RAD_DEG_RATIO, {degToRadF(AXIS2_LIMIT_MIN), degToRadF(AXIS2_LIMIT_MAX)}, siderealToRad(TRACK_BACKLASH_RATE)}; +} + +Motor& motor2 = motor_2; + +Axis axis2(2, &PinsAxis2, &SettingsAxis2, AXIS_MEASURE_RADIANS, arcsecToRad(AXIS2_TARGET_TOLERANCE)); + +#endif diff --git a/src/telescope/mount/Mount.command.cpp b/src/telescope/mount/Mount.command.cpp new file mode 100755 index 0000000..4f8a963 --- /dev/null +++ b/src/telescope/mount/Mount.command.cpp @@ -0,0 +1,489 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, commands + +#include "Mount.h" + +#ifdef MOUNT_PRESENT + +#include "../../lib/tasks/OnTask.h" +#include "../../lib/nv/Nv.h" + +#include "site/Site.h" +#include "coordinates/Transform.h" +#include "goto/Goto.h" +#include "guide/Guide.h" +#include "limits/Limits.h" +#include "park/Park.h" + +bool Mount::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + char *conv_end; + PrecisionMode precisionMode = PM_HIGH; + + if (command[0] == 'G') { + // :GA# Get Mount Altitude + // Returns: sDD*MM# or sDD*MM'SS# (based on precision setting) + // :GAH# High precision + // Returns: sDD*MM'SS.SSS# (high precision) + if (command[1] == 'A' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + double a = getPosition(CR_MOUNT_ALT).a; + #if AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + if (guide.state == GU_HOME_GUIDE || guide.state == GU_HOME_GUIDE_ABORT) { + Coordinate homePosition = home.getPosition(CR_MOUNT_ALT); + a = transform.mountToNative(&homePosition).a; + } + #endif + convert.doubleToDms(reply, radToDeg(a), false, true, precisionMode); + *numericReply = false; + } else + + // :GD# Get Mount Declination + // Returns: sDD*MM# or sDD*MM:SS# (based on precision setting) + // :GDH# Returns: sDD*MM:SS.SSS# (high precision) + if (command[1] == 'D' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H' || parameter[0] == 'e') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + double d = getPosition().d; + #if AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + if (guide.state == GU_HOME_GUIDE || guide.state == GU_HOME_GUIDE_ABORT) { + Coordinate homePosition = home.getPosition(); + d = transform.mountToNative(&homePosition).d; + } + #endif + convert.doubleToDms(reply, radToDeg(d), false, true, precisionMode); + *numericReply = false; + } else + + // :GR# Get Mount Right Ascension + // Returns: HH:MM.T# or HH:MM:SS# (based on precision setting) + // :GRH# Returns: HH:MM:SS.SSSS# (high precision) + if (command[1] == 'R' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H' || parameter[0] == 'a') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + double r = getPosition().r; + #if AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + if (guide.state == GU_HOME_GUIDE || guide.state == GU_HOME_GUIDE_ABORT) { + Coordinate homePosition = home.getPosition(); + r = transform.mountToNative(&homePosition).r; + } + #endif + convert.doubleToHms(reply, radToHrs(r), false, precisionMode); + *numericReply = false; + } else + + // :GT# Get tracking rate, 0.0 unless TrackingSidereal + // Returns: n.n# (OnStep returns more decimal places than LX200 standard) + if (command[1] == 'T' && parameter[0] == 0) { + if (trackingState == TS_NONE) strcpy(reply,"0"); else sprintF(reply, "%0.5f", siderealToHz(trackingRate)); + *numericReply = false; + } else + + // :GX OnStep extended command + if (command[1] == 'X' && parameter[2] == 0) { + + // :GX4[n]# Get angles [n] + // (0 and 1) Returns instrument angle in: DDD:MM:SS# + // (2 and 3) Returns instrument angle in: n.nnnnnn# + // (4 and 5) Returns encoder angle in counts: n# + if (parameter[0] == '4') { + *numericReply = false; + switch (parameter[1]) { + case '0': convert.doubleToDms(reply, radToDeg(axis1.getInstrumentCoordinate()), true, true, PM_HIGH); break; + case '1': convert.doubleToDms(reply, radToDeg(axis2.getInstrumentCoordinate()), true, true, PM_HIGH); break; + case '2': sprintF(reply, "%0.6f", radToDeg(axis1.getInstrumentCoordinate())); break; + case '3': sprintF(reply, "%0.6f", radToDeg(axis2.getInstrumentCoordinate())); break; + case '4': sprintf(reply, "%ld", (long)axis1.motor->getEncoderCount()); break; + case '5': sprintf(reply, "%ld", (long)axis2.motor->getEncoderCount()); break; + default: *numericReply = true; *commandError = CE_CMD_UNKNOWN; + } + } else + + // :GXE[m]# Get mount setting + // Returns: n# + if (parameter[0] == 'E') { + switch (parameter[1]) { + case '4': sprintf(reply, "%ld", lround(axis1.getStepsPerMeasure()/RAD_DEG_RATIO)); *numericReply = false; break; + case '5': sprintf(reply, "%ld", lround(axis2.getStepsPerMeasure()/RAD_DEG_RATIO)); *numericReply = false; break; + case 'E': reply[0] = '0' + (MOUNT_COORDS - 1); *suppressFrame = true; *numericReply = false; break; + case 'F': if (AXIS2_TANGENT_ARM != ON) *commandError = CE_0; break; + case 'G': if (AXIS1_SECTOR_GEAR != ON) *commandError = CE_0; break; + case 'M': sprintf(reply, "%d", (int)settings.mountType); *numericReply = false; break; + default: + return false; + } + } else + + // :GXFn# Get frequency setting and workload + // Returns: Value + if (parameter[0] == 'F') { + switch (parameter[1]) { + case '3': sprintF(reply, "%0.6f", (axis1.getDirection() == DIR_FORWARD) ? axis1.getFrequencySteps() : -axis1.getFrequencySteps()); *numericReply = false; break; + case '4': sprintF(reply, "%0.6f", (axis2.getDirection() == DIR_FORWARD) ? axis2.getFrequencySteps() : -axis2.getFrequencySteps()); *numericReply = false; break; + case 'A': sprintf(reply, "%d%%", 50); *numericReply = false; break; // workload + case 'F': // index position for Axis1 + sprintF(reply, "%0.6f", radToDeg(transform.instrumentToMount(axis1.getIndexPosition(), axis2.getIndexPosition()).a1)); + *numericReply = false; + break; + case 'G': // index position for Axis2 + sprintF(reply, "%0.6f", radToDeg(transform.instrumentToMount(axis1.getIndexPosition(), axis2.getIndexPosition()).a2)); + *numericReply = false; + break; + default: + return false; + } + } else + + // :GXTD# Get tracking rate offset Dec in arc-seconds/sidereal second + // Returns: n.nnnnnn# + if (parameter[0] == 'T' && parameter[1] == 'D' && parameter[2] == 0) { + sprintF(reply, "%0.8f", trackingRateOffsetDec*15.0F); + *numericReply = false; + } else + + // :GXTR# Get tracking rate offset RA in arc-seconds/sidereal second + // Returns: n.nnnnnn# + if (parameter[0] == 'T' && parameter[1] == 'R' && parameter[2] == 0) { + sprintF(reply, "%0.8f", trackingRateOffsetRA*15.0F); + *numericReply = false; + } else return false; + + } else + + // :GZ# Get Mount Azimuth + // Returns: DDD*MM# or DDD*MM'SS# (based on precision setting) + // :GZH# High precision + // Returns: DDD*MM'SS.SSS# (high precision) + if (command[1] == 'Z' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + double z = getPosition(CR_MOUNT_HOR).z; + #if AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + if (guide.state == GU_HOME_GUIDE || guide.state == GU_HOME_GUIDE_ABORT) { + Coordinate homePosition = home.getPosition(CR_MOUNT_ALT); + z = transform.mountToNative(&homePosition).z; + } + #endif + convert.doubleToDms(reply, NormalizeAzimuth(radToDeg(z)), true, false, precisionMode); + *numericReply = false; + } else return false; + } else + + if (command[0] == 'S') { + // :ST[H.H]# Set Tracking Rate in Hz where 60.0 is the solar rate + // Return: 0 on failure + // 1 on success + if (command[1] == 'T') { + double f = strtod(parameter,&conv_end); + if (¶meter[0] != conv_end && ((f >= 30.0 && f < 90.0) || fabs(f) < 0.1)) { + if (fabs(f) < 0.1) tracking(false); else { + #if GOTO_FEATURE == ON + if (park.state != PS_PARKED) { + trackingRate = hzToSidereal(f); + tracking(true); + } + #endif + } + } else *commandError = CE_PARAM_RANGE; + } else + + // :SEO# Set coordinate origin for absolute encoders or mount coordinate memory + // Return: 0 on failure + // 1 on success + if (command[1] == 'E' && parameter[0] == 'O' && parameter[1] == 0) { + if (park.state == PS_PARKED) { *commandError = CE_PARKED; return true; } + if (!mount.isTracking() && !mount.isSlewing()) { + bool handled = false; + + #ifdef HAS_ABSOLUTE_ENCODER_SERVO + handled = true; + VLF("MSG: Mount, setting absolute encoder origin"); + + #if AXIS1_ENCODER == SERIAL_BRIDGE && AXIS2_ENCODER == SERIAL_BRIDGE && defined(SERIAL_ENCODER) + SERIAL_ENCODER.print(":SO#"); + #else + uint32_t zero = (uint32_t)axis1.motor->encoderZero(); + V("MSG: Mount, absolute encoder saving AXIS1_ENCODER_OFFSET "); V(uint32_t(zero)); VLF(" to NV/EEPROM"); + nv().kv().put("AXIS1_ENCODER_ORIGIN", zero); + + zero = (uint32_t)axis2.motor->encoderZero(); + V("MSG: Mount, absolute encoder saving AXIS2_ENCODER_OFFSET "); V(uint32_t(zero)); VLF(" to NV/EEPROM"); + nv().kv().put("AXIS2_ENCODER_ORIGIN", zero); + #endif + + #ifdef HAL_RESET + enable(false); + VLF("MSG: Mount, resetting OnStep..."); + if (nv().device().hasCommit()) { nv().device().commit(); } + const uint32_t startMs = millis(); + const uint32_t timeoutMs = 5000; + while (!nv().device().commitDone() && (uint32_t)(millis() - startMs) < timeoutMs) { tasks.yield(1); } + tasks.yield(1000); + HAL_RESET(); + #endif + #endif + + #if MOUNT_COORDS_MEMORY == ON + if (!handled && !goTo.absoluteEncodersPresent) { + CommandError e = home.reset(true, true); + if (e != CE_NONE) { *commandError = e; return true; } + + limits.enabled(site.isDateTimeReady()); + + syncFromOnStepToEncoders = true; + handled = true; + + VLF("MSG: Mount, setting mount coordinate memory origin"); + } + #endif + + if (!handled) { + *commandError = CE_0; + DLF("MSG: Mount, setting coordinate origin failed; feature unavailable"); + } + } else { + *commandError = CE_0; + DLF("MSG: Mount, setting coordinate origin failed; the mount is in motion!"); + } + } else + + if (command[1] == 'X') { + if (parameter[2] != ',') { *commandError = CE_PARAM_FORM; return true; } + + // :SX4[m],[n]# Set encoder axis [m] to value [n] + // Return: 0 on failure + // 1 on success + if (parameter[0] == '4') { + double d; + static double encoderAxis1 = NAN; + static double encoderAxis2 = NAN; + + switch (parameter[1]) { + + // stage encoder Axis1 value (depreciated) + case '0': + d = strtod(¶meter[3], &conv_end); + if (¶meter[3] != conv_end && fabs(d) <= 360.0L) { encoderAxis1 = degToRad(d); } else { encoderAxis1 = NAN; *commandError = CE_PARAM_RANGE; } + break; + + // stage encoder Axis2 value (depreciated) + case '1': + d = strtod(¶meter[3], &conv_end); + if (¶meter[3] != conv_end && fabs(d) <= 360.0L) { encoderAxis2 = degToRad(d); } else { encoderAxis2 = NAN; *commandError = CE_PARAM_RANGE; } + break; + + // sync from encoder values (depreciated) + case '2': + if (parameter[3] == '1' && parameter[4] == 0) { + #if GOTO_FEATURE == ON + CommandError e = goTo.validate(); + if (e != CE_NONE) { *commandError = e; return true; } + #endif + if (isnan(encoderAxis1) || isnan(encoderAxis2) || syncFromOnStepToEncoders) { *commandError = CE_0; return true; } + *commandError = limits.validateInstrumentCoordinate(1, encoderAxis1, true); + if (*commandError == CE_NONE) *commandError = limits.validateInstrumentCoordinate(2, encoderAxis2, true); + if (*commandError != CE_NONE) return true; + *commandError = limits.setInstrumentCoordinate(1, encoderAxis1, true); + if (*commandError == CE_NONE) *commandError = limits.setInstrumentCoordinate(2, encoderAxis2, true); + if (*commandError != CE_NONE) return true; + } + break; + + // allow sws to control sync mode + case '3': syncFromOnStepToEncoders = false; break; + + // set and sync from encoder Axis1 and Axis2 values + // append 'a' if both SWS encoder values are absolute and trusted + case '4': { + bool absoluteAuthority = false; + d = strtod(¶meter[3], &conv_end); + if (¶meter[3] != conv_end && fabs(d) <= 360.0L) { encoderAxis1 = degToRad(d); } else { encoderAxis1 = NAN; } + + char *parameter2 = strchr(¶meter[3], ','); parameter2++; + d = strtod(parameter2, &conv_end); + if (parameter2 != conv_end && fabs(d) <= 360.0L) { + encoderAxis2 = degToRad(d); + if (conv_end[0] == 'a' && conv_end[1] == 0) absoluteAuthority = true; else + if (conv_end[0] != 0) encoderAxis2 = NAN; + } else { encoderAxis2 = NAN; } + + #if GOTO_FEATURE == ON + CommandError e = goTo.validate(); + if (e != CE_NONE && e != CE_SLEW_ERR_IN_STANDBY && e != CE_SLEW_IN_SLEW) { *commandError = e; return true; } + #endif + + if ( isnan(encoderAxis1) || + isnan(encoderAxis2) || + syncFromOnStepToEncoders || + (goTo.state != GS_NONE && goTo.stage != GG_NEAR_DESTINATION_WAIT) || + guide.state != GU_NONE) { *commandError = CE_0; return true; } + + *commandError = limits.validateInstrumentCoordinate(1, encoderAxis1, true); + if (*commandError == CE_NONE) *commandError = limits.validateInstrumentCoordinate(2, encoderAxis2, true); + if (*commandError != CE_NONE) return true; + *commandError = limits.setInstrumentCoordinate(1, encoderAxis1, true); + if (*commandError == CE_NONE) *commandError = limits.setInstrumentCoordinate(2, encoderAxis2, true); + if (*commandError != CE_NONE) return true; + if (absoluteAuthority) { + captureNominalIndexPositions(); + setStartupAuthorityTrusted(true); + goTo.absoluteEncodersPresent = true; + goTo.encodersPresent = true; + VLF("MSG: Mount, sync from trusted paired absolute SWS encoders"); + } + break; } + + default: *commandError = CE_CMD_UNKNOWN; break; + } + } else + + // :SXEM,[n]# Set mount type (for next restart) where n=0 for default + // Return: 0 on failure + // 1 on success + if (parameter[0] == 'E' && parameter[1] == 'M') { + long mountType = atol(¶meter[3]); + if (mountType == 0 || + (mountType == GEM && AXIS1_WRAP == OFF) || + (mountType == FORK && AXIS1_WRAP == OFF) || + (mountType == ALTAZM && AXIS2_TANGENT_ARM == OFF)) { + settings.mountType = mountType; + nv().kv().put(nvKey, settings); + } else *commandError = CE_PARAM_RANGE; + } else + + // :SXTD,n.n# Set tracking rate offset Dec in arc-seconds/sidereal second + // Return: 0 on failure + // 1 on success + if (parameter[0] == 'T' && parameter[1] == 'D') { + float f = strtod(¶meter[3], &conv_end); + if (f < -1800.0F) f = -1800.0F; + if (f > 1800.0F) f = 1800.0F; + trackingRateOffsetDec = f/15.0F; + } else + + // :SXTR,n.n# Set tracking rate offset RA in arc-seconds/sidereal second + // Return: 0 failure, 1 success + if (parameter[0] == 'T' && parameter[1] == 'R') { + float f = strtod(¶meter[3], &conv_end); + if (f < -1800.0F) f = -1800.0F; + if (f > 1800.0F) f = 1800.0F; + trackingRateOffsetRA = f/15.0F; + } else return false; + + } else return false; + } else + + // T - Tracking Commands + // + // :TS# Track rate solar 60Hz + // :TK# Track rate king 60.136Hz + // :TL# Track rate lunar 57.9Hz + // :TQ# Track rate sidereal 60.164Hz + // + // :T+# Master sidereal clock faster by 0.02 Hertz (stored in EEPROM) + // :T-# Master sidereal clock slower by 0.02 Hertz (stored in EEPROM) + // :TR# Master sidereal clock reset (to calculated sidereal rate, stored in EEPROM) + // Returns: Nothing + // + // :Te# Tracking enable + // :Td# Tracking disable + // + // :To# Track full compensation on + // :Tr# Track refraction compensation on + // :Tn# Track compensation off + // :T1# Track dual axis off (disable Dec tracking on Eq mounts) + // :T2# Track dual axis on + // Return: 0 on failure + // 1 on success + if (command[0] == 'T' && parameter[0] == 0) { + if (command[1] == 'o') { settings.rc = RC_MODEL; } else + if (command[1] == 'r') { settings.rc = RC_REFRACTION; } else + if (command[1] == 'n') { settings.rc = RC_NONE; } else + if (command[1] == '1') { + if (settings.rc == RC_REFRACTION_DUAL) settings.rc = RC_REFRACTION; else + if (settings.rc == RC_MODEL_DUAL) settings.rc = RC_MODEL; + } else + if (command[1] == '2') { + if (settings.rc == RC_REFRACTION) settings.rc = RC_REFRACTION_DUAL; else + if (settings.rc == RC_MODEL) settings.rc = RC_MODEL_DUAL; + } else + if (command[1] == 'S') { trackingRate = hzToSidereal(SOLAR_RATE_HZ); } else + if (command[1] == 'K') { trackingRate = hzToSidereal(KING_RATE_HZ); } else + if (command[1] == 'L') { trackingRate = hzToSidereal(LUNAR_RATE_HZ); } else + if (command[1] == 'Q') { trackingRate = hzToSidereal(SIDEREAL_RATE_HZ); } else + if (command[1] == '+') { site.setSiderealPeriod(site.getSiderealPeriod() - hzToSubMicros(0.02F)); } else + if (command[1] == '-') { site.setSiderealPeriod(site.getSiderealPeriod() + hzToSubMicros(0.02F)); } else + if (command[1] == 'R') { site.setSiderealPeriod(SIDEREAL_PERIOD); } else + if (command[1] == 'e') { + #if GOTO_FEATURE == ON + if (park.state != PS_PARKED) { + #endif + tracking(true); + #if GOTO_FEATURE == ON + } else *commandError = CE_PARKED; + #endif + } else + if (command[1] == 'd') { + tracking(false); + } else *commandError = CE_CMD_UNKNOWN; + + if (!transform.isEquatorial()) { + if (settings.rc == RC_MODEL) settings.rc = RC_MODEL_DUAL; + if (settings.rc == RC_REFRACTION) settings.rc = RC_REFRACTION_DUAL; + } + + if (*commandError == CE_NONE) { + switch (command[1]) { case 'S': case 'K': case 'L': case 'Q': case '+': case '-': case 'R': *numericReply = false; } + nv().kv().put(nvKey, settings); + update(); + } + } else + + // $ - Set parameter + // :$BD[n]# Set Dec/Alt backlash in arc-seconds + // Return: 0 on failure + // 1 on success + // :$BR[n]# Set RA/Azm backlash in arc-seconds + // Return: 0 on failure + // 1 on success + // Set the Backlash values. Units are arc-seconds + if (command[0] == '$' && command[1] == 'B') { + int16_t arcSecs; + if (convert.atoi2((char*)¶meter[1], &arcSecs)) { + if (arcSecs >= 0 && arcSecs <= 3600) { + if (parameter[0] == 'D') { + settings.backlash.axis2 = arcsecToRad(arcSecs); + axis2.setBacklash(settings.backlash.axis2); + nv().kv().put(nvKey, settings); + } else + if (parameter[0] == 'R') { + settings.backlash.axis1 = arcsecToRad(arcSecs); + axis1.setBacklash(settings.backlash.axis1); + nv().kv().put(nvKey, settings); + } else *commandError = CE_CMD_UNKNOWN; + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + // % - Return parameter + // :%BD# Get Dec/Alt Antibacklash value in arc-seconds + // Return: n# + // :%BR# Get RA/Azm Antibacklash value in arc-seconds + // Return: n# + if (command[0] == '%' && command[1] == 'B' && parameter[1] == 0) { + if (parameter[0] == 'D') { + int arcSec = round(radToArcsec(settings.backlash.axis2)); + if (arcSec < 0) arcSec = 0; + if (arcSec > 3600) arcSec = 3600; + sprintf(reply, "%d", arcSec); + *numericReply = false; + } else + if (parameter[0] == 'R') { + int arcSec = round(radToArcsec(settings.backlash.axis1)); + if (arcSec < 0) arcSec = 0; + if (arcSec > 3600) arcSec = 3600; + sprintf(reply, "%d", arcSec); + *numericReply = false; + } else *commandError = CE_CMD_UNKNOWN; + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/mount/Mount.cpp b/src/telescope/mount/Mount.cpp new file mode 100755 index 0000000..f13f9af --- /dev/null +++ b/src/telescope/mount/Mount.cpp @@ -0,0 +1,597 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control + +#include "Mount.h" + +#ifdef MOUNT_PRESENT + +#include "../../lib/tasks/OnTask.h" +#include "../../lib/nv/Nv.h" + +#include "../Telescope.h" +#include "coordinates/Transform.h" +#include "goto/Goto.h" +#include "guide/Guide.h" +#include "home/Home.h" +#include "library/Library.h" +#include "limits/Limits.h" +#include "park/Park.h" +#include "pec/Pec.h" +#include "site/Site.h" +#include "st4/St4.h" +#include "status/Status.h" + +#if MOUNT_COORDS_MEMORY == ON && (NV_DRIVER != NV_MB85RC32 && NV_DRIVER != NV_MB85RC64 && NV_DRIVER != NV_MB85RC256) + #error "Configuration (Config.h): Setting MOUNT_COORDS_MEMORY requires a NV storage device with very high write endurance (FRAM)" +#endif + +inline void mountWrapper() { mount.poll(); } +inline void autostartWrapper() { mount.autostartPostponed(); } + +void Mount::init() { + absoluteCoordinateOriginsEstablished = true; + + nvKey = nv().kv().computeKey("MOUNT_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings)) { DLF("WRN: Nv, init failed for MOUNT_SETTINGS"); } + + // get the main axes ready + delay(100); + if (!axis1.init(&motor1)) { initError.driver = true; DLF("ERR: Mount::init(), no motion controller for Axis1!"); } else { + axis1.setBacklash(settings.backlash.axis1); + axis1.setMotionLimitsCheck(false); + if (AXIS1_POWER_DOWN == ON) axis1.setPowerDownTime(AXIS1_POWER_DOWN_TIME); + #ifdef AXIS1_ENCODER_ORIGIN + uint32_t origin = UINT32_MAX; + if (AXIS1_ENCODER_ORIGIN == 0) { + nv().kv().getOrInit("AXIS1_ENCODER_ORIGIN", origin); + if (origin != UINT32_MAX) axis1.motor->encoderSetOrigin(origin); else absoluteCoordinateOriginsEstablished = false; + } + #endif + } + + delay(100); + if (!axis2.init(&motor2)) { initError.driver = true; DLF("ERR: Mount::init(), no motion controller for Axis2!"); } else { + axis2.setBacklash(settings.backlash.axis2); + axis2.setMotionLimitsCheck(false); + if (AXIS2_POWER_DOWN == ON) axis2.setPowerDownTime(AXIS2_POWER_DOWN_TIME); + #ifdef AXIS2_ENCODER_ORIGIN + uint32_t origin = UINT32_MAX; + if (AXIS2_ENCODER_ORIGIN == 0) { + nv().kv().getOrInit("AXIS2_ENCODER_ORIGIN", origin); + if (origin != UINT32_MAX) axis2.motor->encoderSetOrigin(origin); else absoluteCoordinateOriginsEstablished = false; + } + #endif + } +} + +void Mount::begin() { + startupAuthorityTrustedValue = false; + + axis1.calibrateDriver(); + axis1.enable(MOUNT_ENABLE_IN_STANDBY == ON); + axis2.calibrateDriver(); + axis2.enable(MOUNT_ENABLE_IN_STANDBY == ON); + + // initialize the critical subsystems + site.init(); + + // invalid mount type, set default + if (settings.mountType < MOUNT_SUBTYPE_FIRST || settings.mountType > MOUNT_SUBTYPE_LAST) { + settings.mountType = MOUNT_SUBTYPE; + nv().kv().put(nvKey, settings); + VLF("MSG: Transform, revert mount type to default"); + } + + if (MOUNT_TYPE == ALTALT) settings.mountType = MOUNT_SUBTYPE; + transform.init(settings.mountType); + + // setup compensated tracking as configured + if (TRACK_COMPENSATION_MEMORY == OFF) settings.rc = RC_DEFAULT; + if (!transform.isEquatorial()) { + if (settings.rc == RC_MODEL) settings.rc = RC_MODEL_DUAL; + if (settings.rc == RC_REFRACTION) settings.rc = RC_REFRACTION_DUAL; + } + + // initialize the other subsystems + // Paired mount-axis absolute position sources can establish the initial + // coordinate basis at startup, so allow reset() to bypass sync-threshold + // checks only in that case. + const bool absoluteAxisAuthority = axis1.motor->hasAbsoluteEncoder() && axis2.motor->hasAbsoluteEncoder(); + home.reset(true, absoluteAxisAuthority && absoluteCoordinateOriginsEstablished); + limits.init(); + guide.init(); + + if (AXIS1_WRAP == ON) { + axis1.coordinateWrap(Deg360); + axis1.setLimitMin(-Deg360); + axis1.setLimitMax(Deg360); + } + + goTo.init(); + library.init(); + park.init(); + + #if AXIS1_PEC == ON + pec.init(); + #endif + + #if ST4_INTERFACE == ON + st4.init(); + #endif + + tracking(false); + + // restore where we were pointing + #if MOUNT_COORDS_MEMORY == ON + lastPosition.a1 = (float)axis1.getInstrumentCoordinate(); + lastPosition.a2 = (float)axis2.getInstrumentCoordinate(); + lastPosition.mountType = (uint8_t)(transform.mountType & 0x0Fu); + lastPosition.seq = 0; + lastPosition.untrusted = true; + + nvKeyLastA = nv().kv().computeKey("MOUNT_LAST_POS_A"); + nvKeyLastB = nv().kv().computeKey("MOUNT_LAST_POS_B"); + + if (!goTo.absoluteEncodersPresent && park.state != PS_PARKED) { + + MountPositionMemory a{}, b{}; + const bool foundA = (nv().kv().get(nvKeyLastA, a) == KvPartition::Status::Ok); + const bool foundB = (nv().kv().get(nvKeyLastB, b) == KvPartition::Status::Ok); + + const bool va = foundA && (a.mountType == (uint8_t)(transform.mountType & 0x0Fu)) && !a.untrusted; + const bool vb = foundB && (b.mountType == (uint8_t)(transform.mountType & 0x0Fu)) && !b.untrusted; + + const MountPositionMemory* best = nullptr; + + if (va && vb) { + const uint8_t d = (uint8_t)((a.seq - b.seq) & 3u); + best = (d == 1u) ? &a : (d == 3u) ? &b : &a; + } else if (va) { + best = &a; + } else if (vb) { + best = &b; + } + + if (best) { + lastPosition = *best; + if (limits.validateInstrumentCoordinate(1, lastPosition.a1, true) == CE_NONE && + limits.validateInstrumentCoordinate(2, lastPosition.a2, true) == CE_NONE) { + CommandError e = limits.setInstrumentCoordinate(1, lastPosition.a1, true); + if (e == CE_NONE) e = limits.setInstrumentCoordinate(2, lastPosition.a2, true); + if (e == CE_NONE) { + captureNominalIndexPositions(); + mount.syncFromOnStepToEncoders = true; + setStartupAuthorityTrusted(true); + } else { DLF("WRN: Mount, coordinate memory restore failed"); } + } else { + DLF("WRN: Mount, coordinate memory restore failed"); + } + } + } + #endif + + #if ALIGN_MAX_NUM_STARS > 1 && ALIGN_MODEL_MEMORY == ON + transform.align.modelRead(); + #endif + + #if MOUNT_STARTUP_MODE == SA_PERMISSIVE + setStartupAuthorityTrusted(true); + #elif MOUNT_STARTUP_MODE == SA_AUTO + #if MOUNT_COORDS_MEMORY == OFF + if (!axis1.motor->hasAbsoluteEncoder() && !axis2.motor->hasAbsoluteEncoder()) { + setStartupAuthorityTrusted(true); + } + #endif + #endif + + VF("MSG: Mount, start tracking monitor task (rate 1000ms priority 6)... "); + if (tasks.add(1000, 0, true, 6, mountWrapper, "MtTrack")) { VLF("success"); } else { VLF("FAILED!"); } + + update(); + autostart(); +} + +void Mount::setStartupAuthorityTrusted(bool state) { + #if NV_INIT_ERROR_REVOKES_AUTHORITY == ON + if (state && initError.nv) return; + #endif + + startupAuthorityTrustedValue = state; + + // If we have a trusted coordinate basis and date/time is ready, limits can + // be enforced immediately without waiting for a later goto/reset/unpark path. + if (startupAuthorityTrustedValue && site.isDateTimeReady()) limits.enabled(true); +} + +void Mount::captureNominalIndexPositions() { + nominalIndexAxis1Steps = axis1.getIndexPositionSteps(); + nominalIndexAxis2Steps = axis2.getIndexPositionSteps(); +} + +long Mount::getNominalIndexPositionSteps(uint8_t axisNumber) const { + switch (axisNumber) { + case 1: return nominalIndexAxis1Steps; + case 2: return nominalIndexAxis2Steps; + default: return 0; + } +} + +#if MOUNT_COORDS_MEMORY == ON +void Mount::saveCoordinateMemory(bool trusted) { + if (goTo.absoluteEncodersPresent) return; + if (nv().device().endurance() != NvDevice::Endurance::High) return; + + lastPosition.a1 = (float)axis1.getInstrumentCoordinate(); + lastPosition.a2 = (float)axis2.getInstrumentCoordinate(); + lastPosition.mountType = (uint8_t)(transform.mountType & 0x0Fu); + lastPosition.untrusted = !trusted; + if (lastPosition.seq == 3) lastPosition.seq = 0; else lastPosition.seq++; + nv().kv().put((lastPosition.seq & 1)? nvKeyLastA : nvKeyLastB, lastPosition); +} +#endif + +// get current equatorial position (Native coordinate system) +Coordinate Mount::getPosition(CoordReturn coordReturn) { + updatePosition(coordReturn); + return transform.mountToNative(¤t, false); +} + +// get current equatorial position (Mount coordinate system) +Coordinate Mount::getMountPosition(CoordReturn coordReturn) { + updatePosition(coordReturn); + return current; +} + +// handle all autostart tasks +void Mount::autostart() { + tasks.setDurationComplete(tasks.getHandleByName("MtAuto")); + tasks.add(2000, 0, true, 7, autostartWrapper, "MtAuto"); +} + +void Mount::autostartPostponed() { + // wait until OnStepX is fully up and running + if (!telescope.ready) return; + + // stop this task if already completed + static bool autoStartDone = false; + if (autoStartDone) { + tasks.setDurationComplete(tasks.getHandleByName("MtAuto")); + return; + } + + // handle the one case where this completes without the date/time available + static bool autoTrackDone = false; + if (!autoTrackDone && TRACK_AUTOSTART == ON && transform.isEquatorial() && park.state != PS_PARKED && !home.settings.automaticAtBoot) { + VLF("MSG: Mount, autostart tracking"); + tracking(true); + autoStartDone = true; + return; + } + + // wait for the date/time to be set + if (!site.isDateTimeReady()) return; + + // auto unpark + static bool autoUnparkDone = false; + if (!autoUnparkDone && park.state == PS_PARKED) { + #if GOTO_FEATURE == ON + VLF("MSG: Mount, autostart park restore"); + CommandError e = park.restore(TRACK_AUTOSTART == ON); + if (e != CE_NONE) { + DF("WRN: Mount, autostart park restore failed with code "); DL(e); + autoStartDone = true; + return; + } + #endif + } + autoUnparkDone = true; + + // auto home + static bool autoHomeDone = false; + if (!autoHomeDone && home.settings.automaticAtBoot) { + VLF("MSG: Mount, autostart home"); + CommandError e = home.request(); + if (e != CE_NONE) { + DF("WRN: Mount, autostart home request failed with code "); DL(e); + autoStartDone = true; + return; + } + } + autoHomeDone = true; + + // wait for any homing operation to complete + if (home.state == HS_HOMING) return; + if (home.failed) { + DLF("WRN: Mount, autostart home failed"); + autoStartDone = true; + return; + } + + // auto tracking + if (!autoTrackDone && TRACK_AUTOSTART == ON) { + VLF("MSG: Mount, autostart tracking"); + tracking(true); + } + autoTrackDone = true; + + autoStartDone = true; +} + +// enables or disables tracking, enabling tracking powers on the motors if necessary +void Mount::tracking(bool state) { + if (state == true) { + enable(state); + if (isEnabled()) trackingState = TS_SIDEREAL; + } else + + if (state == false) { + trackingState = TS_NONE; + } + + update(); +} + +// enables or disables power to the mount motors +void Mount::enable(bool state) { + if (state == true) { + #if LIMIT_STRICT == ON + if (!site.dateIsReady || !site.timeIsReady) return; + #endif + + mountStatus.wake(); + } else { + trackingState = TS_NONE; + update(); + } + + axis1.enable(state); + axis2.enable(state); +} + +// updates the tracking rates, etc. as appropriate for the mount state +// called once a second by poll() but available here for immediate action +void Mount::update() { + static int lastStatusFlashMs = 0; + int statusFlashMs = 0; + + #if GOTO_FEATURE == ON + if (goTo.state == GS_NONE && guide.state < GU_GUIDE) { + #else + if (guide.state < GU_GUIDE) { + #endif + + if (trackingState != TS_SIDEREAL) { + trackingRateAxis1 = 0.0F; + trackingRateAxis2 = 0.0F; + } + + float f1 = 0, f2 = 0; + if (!guide.activeAxis1() || guide.state == GU_PULSE_GUIDE) { + f1 = trackingRateAxis1; + if (transform.mountType != ALTAZM && transform.mountType != ALTALT) { + f1 += guide.rateAxis1; + #if AXIS1_PEC == ON + f1 += pec.rate; + #endif + } + axis1.setSynchronizedFrequency(siderealToRadF(f1)*SIDEREAL_RATIO_F*site.getSiderealRatio()); + } + + if (!guide.activeAxis2() || guide.state == GU_PULSE_GUIDE) { + f2 = trackingRateAxis2; + if (transform.mountType != ALTAZM && transform.mountType != ALTALT) f2 += guide.rateAxis2; + axis2.setSynchronizedFrequency(siderealToRadF(f2)*SIDEREAL_RATIO_F*site.getSiderealRatio()); + } + + f1 = fabs(f1); + f2 = fabs(f2); + if (f2 > f1) f1 = f2; + if (f1 < 0.20F) statusFlashMs = SF_STOPPED; else + if (f1 > 3.0F) statusFlashMs = SF_SLEWING; else statusFlashMs = 500.0F/f1; + } else { + statusFlashMs = SF_SLEWING; + axis2.setSynchronizedFrequency(0.0F); + } + + if (statusFlashMs != lastStatusFlashMs) { + lastStatusFlashMs = statusFlashMs; + mountStatus.flashRate(statusFlashMs); + xBusy = statusFlashMs == SF_SLEWING; + } +} + +void Mount::poll() { + #if NV_INIT_ERROR_REVOKES_AUTHORITY == ON + if (initError.nv && startupAuthorityTrustedValue) { + startupAuthorityTrustedValue = false; + DLF("WRN: Mount, startup authority trust cleared due to NV fault"); + } + #endif + + // stop any movement then disable on motor hardware fault + if (mount.motorFault()) { + if (startupAuthorityTrustedValue) { + startupAuthorityTrustedValue = false; + DLF("WRN: Mount, startup authority trust cleared due to motor/encoder fault"); + } + if (goTo.state > GS_NONE) goTo.abort(); else + if (guide.state > GU_NONE) guide.abort(); else + if (axis1.isEnabled() || axis2.isEnabled()) enable(false); + } + + #ifdef HAL_NO_DOUBLE_PRECISION + #define DiffRange 0.0087266463F // 30 arc-minutes in radians + #define DiffRange2 0.017453292F // 60 arc-minutes in radians + #else + #define DiffRange 2.908882086657216e-4L // 1 arc-minute in radians + #define DiffRange2 5.817764173314432e-4L // 2 arc-minutes in radians + #endif + #define DegenerateRange (DiffRange*1.5) // we were using Deg85 + + // keep track of where we are pointing + #if MOUNT_COORDS_MEMORY == ON + saveCoordinateMemory(startupAuthorityTrustedValue && home.state != HS_HOMING); + #endif + + if (trackingState == TS_NONE) { + trackingRateAxis1 = 0.0F; + trackingRateAxis2 = 0.0F; + update(); + return; + } + + if (transform.isEquatorial() && settings.rc == RC_NONE && trackingRateOffsetRA == 0.0F && trackingRateOffsetDec == 0.0F) { + trackingRateAxis1 = trackingRate; + trackingRateAxis2 = 0.0F; + update(); + return; + } + + // get positions 1 (or 30) arc-min ahead and behind the current + updatePosition(CR_MOUNT_ALL); + double altitude = current.a; + double declination = current.d; + double altitude2 = current.aa2; + + // on fast processors calculate true coordinate for a little more accuracy + #ifndef HAL_SLOW_PROCESSOR + transform.mountToTopocentric(¤t); + if (transform.mountType == ALTAZM) transform.horToEqu(¤t); else + if (transform.mountType == ALTALT) transform.aaToEqu(¤t); + #endif + + Y; + Coordinate ahead = current; + Coordinate behind = current; + double trackingRange = DiffRange*trackingRate; + ahead.h += trackingRange; + behind.h -= trackingRange; + + // create horizon coordinates that would exist ahead and behind the current position + if (transform.mountType == ALTAZM) { transform.equToHor(&ahead); transform.equToHor(&behind); Y; } else + if (transform.mountType == ALTALT) { transform.equToAa(&ahead); transform.equToAa(&behind); Y; } + + // apply (optional) pointing model and refraction + if (settings.rc == RC_MODEL || settings.rc == RC_MODEL_DUAL) { + transform.topocentricToObservedPlace(&ahead); Y; + transform.topocentricToObservedPlace(&behind); Y; + transform.observedPlaceToMount(&ahead); Y; + transform.observedPlaceToMount(&behind); Y; + } else if (settings.rc == RC_REFRACTION || settings.rc == RC_REFRACTION_DUAL) { + transform.topocentricToObservedPlace(&ahead); Y; + transform.topocentricToObservedPlace(&behind); Y; + } + + // drop the dual axis if not enabled + if (settings.rc != RC_REFRACTION_DUAL && settings.rc != RC_MODEL_DUAL) { behind.d = ahead.d; } + + // apply tracking rate offset to equatorial coordinates + float timeInSeconds = radToHrs(DiffRange)*3600.0F; + float trackingRateOffsetRadsRA = siderealToRad(trackingRateOffsetRA)*timeInSeconds; + float trackingRateOffsetRadsDec = siderealToRad(trackingRateOffsetDec)*timeInSeconds; + ahead.h -= trackingRateOffsetRadsRA; + behind.h += trackingRateOffsetRadsRA; + ahead.d += trackingRateOffsetRadsDec; + behind.d -= trackingRateOffsetRadsDec; + + // apply non-equatorial guide rate offset to equatorial coordinates + if (guide.state == GU_PULSE_GUIDE && (transform.mountType == ALTAZM || transform.mountType == ALTALT)) { + float trackingRateGuideRadsRA = siderealToRad(guide.rateAxis1)*timeInSeconds*2.0; + float trackingRateGuideRadsDec = siderealToRad(guide.rateAxis2)*timeInSeconds*2.0; + ahead.h += trackingRateGuideRadsRA; + behind.h -= trackingRateGuideRadsRA; + ahead.d += trackingRateGuideRadsDec; + behind.d -= trackingRateGuideRadsDec; + } + + // transfer to variables named appropriately for mount coordinates + float aheadAxis1, aheadAxis2, behindAxis1, behindAxis2; + if (transform.mountType == ALTAZM) { + transform.equToHor(&ahead); + aheadAxis1 = ahead.z; + aheadAxis2 = ahead.a; + behindAxis1 = behind.z; + behindAxis2 = behind.a; + } else + if (transform.mountType == ALTALT) { + transform.equToAa(&ahead); + aheadAxis1 = ahead.aa1; + aheadAxis2 = ahead.aa2; + behindAxis1 = behind.aa1; + behindAxis2 = behind.aa2; + } else { + aheadAxis1 = ahead.h; + aheadAxis2 = ahead.d; + behindAxis1 = behind.h; + behindAxis2 = behind.d; + } + + // calculate the Axis1 tracking rate + if (aheadAxis1 < -Deg90 && behindAxis1 > Deg90) aheadAxis1 += Deg360; + if (behindAxis1 < -Deg90 && aheadAxis1 > Deg90) behindAxis1 += Deg360; + float rate1 = (aheadAxis1 - behindAxis1)/DiffRange2; + if (fabs(trackingRateAxis1 - rate1) <= 0.005F) trackingRateAxis1 = (trackingRateAxis1*9.0F + rate1)/10.0F; else trackingRateAxis1 = rate1; + + // calculate the Axis2 Dec/Alt tracking rate + float rate2 = (aheadAxis2 - behindAxis2)/DiffRange2; + if (current.pierSide == PIER_SIDE_WEST) rate2 = -rate2; + if (fabs(trackingRateAxis2 - rate2) <= 0.005F) trackingRateAxis2 = (trackingRateAxis2*9.0F + rate2)/10.0F; else trackingRateAxis2 = rate2; + + // override for special case of near a celestial pole + if (fabs(declination) > Deg90 - DegenerateRange) { + if (transform.isEquatorial()) trackingRateAxis1 = trackingRate; else + if (transform.mountType == ALTAZM) trackingRateAxis1 = 0.0F; else + if (transform.mountType == ALTALT) trackingRateAxis1 = 0.0F; + trackingRateAxis2 = 0.0F; + } + + // override for both rates for special case near the Azm axis of rotation (Zenith) + if (altitude > Deg90 - DegenerateRange) { +// if (transform.isEquatorial()) trackingRateAxis1 = ztr(current.a); else +// if (transform.mountType == ALTALT) trackingRateAxis1 = ztr(current.a); else + if (transform.mountType == ALTAZM) trackingRateAxis1 = 0.0F; + trackingRateAxis2 = 0.0F; + } + + // override for both rates for special case near the aa1 axis of rotation + if (transform.mountType == ALTALT && fabs(altitude2) > Deg90 - DegenerateRange) { trackingRateAxis1 = 0.0F; } + + update(); +} + +// alternate tracking rate calculation method +float Mount::ztr(float a) { + if (a > degToRadF(89.8F)) return 0.99998667F; else if (a > degToRadF(89.5F)) return 0.99996667F; + + float altH = a + degToRadF(0.25F); if (altH < 0.0F) altH = 0.0F; + float altL = a - degToRadF(0.25F); if (altL < 0.0F) altL = 0.0F; + + float altHr = altH - transform.trueRefrac(altH); + float altLr = altL - transform.trueRefrac(altL); + + float r = (altH - altL)/(altHr - altLr); if (r > 1.0F) r = 1.0F; + return r; +} + +// update where we are pointing *now* +void Mount::updatePosition(CoordReturn coordReturn) { + current = transform.instrumentToMount(axis1.getInstrumentCoordinate(), axis2.getInstrumentCoordinate()); + if (isHome()) { + transform.mountToInstrument(¤t, ¤t.a1, ¤t.a2); + current.pierSide = PIER_SIDE_NONE; + } + + if (transform.mountType == ALTAZM) { + if (coordReturn == CR_MOUNT_EQU || coordReturn == CR_MOUNT_ALL) transform.horToEqu(¤t); + } else + if (transform.mountType == ALTALT) { + transform.aaToEqu(¤t); + } else { + if (coordReturn == CR_MOUNT_ALT) transform.equToAlt(¤t); else + if (coordReturn == CR_MOUNT_HOR || coordReturn == CR_MOUNT_ALL) transform.equToHor(¤t); + } +} + +Mount mount; + +#endif diff --git a/src/telescope/mount/Mount.h b/src/telescope/mount/Mount.h new file mode 100755 index 0000000..3bda29e --- /dev/null +++ b/src/telescope/mount/Mount.h @@ -0,0 +1,163 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control +#pragma once + +#include "../../Common.h" + +#ifdef MOUNT_PRESENT + +#include "../../lib/axis/Axis.h" +#include "../../libApp/commands/ProcessCmds.h" +#include "coordinates/Transform.h" +#include "home/Home.h" + +enum RateCompensation: uint8_t {RC_NONE, RC_REFRACTION, RC_REFRACTION_DUAL, RC_MODEL, RC_MODEL_DUAL}; + +#if TRACK_COMPENSATION_DEFAULT == OFF + #define RC_DEFAULT RC_NONE +#elif TRACK_COMPENSATION_DEFAULT == REFRACTION + #define RC_DEFAULT RC_REFRACTION +#elif TRACK_COMPENSATION_DEFAULT == REFRACTION_DUAL + #define RC_DEFAULT RC_REFRACTION_DUAL +#elif TRACK_COMPENSATION_DEFAULT == MODEL + #define RC_DEFAULT RC_FULL +#elif TRACK_COMPENSATION_DEFAULT == MODEL_DUAL + #define RC_DEFAULT RC_MODEL_DUAL +#endif + +enum TrackingState: uint8_t {TS_NONE, TS_SIDEREAL}; + +#pragma pack(1) +#define MountSettingsSize 9 + +typedef struct Backlash { + float axis1; + float axis2; +} Backlash; + +typedef struct MountSettings { + RateCompensation rc; + Backlash backlash; + uint8_t mountType; +} MountSettings; + +typedef struct MountPositionMemory { + float a1; + float a2; + uint8_t mountType:4; + uint8_t seq:2; + uint8_t untrusted:1; + uint8_t reserved:1; +} MountPositionMemory; + +#pragma pack() + +extern Axis axis1; +extern Axis axis2; + +class Mount { + public: + void init(); + void begin(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // get current equatorial position (Native coordinate system) + Coordinate getPosition(CoordReturn coordReturn = CR_MOUNT_EQU); + + // get current equatorial position (Mount coordinate system) + Coordinate getMountPosition(CoordReturn coordReturn = CR_MOUNT_EQU); + + // returns true if either of the mount motor drivers report a fault + inline bool motorFault() { return axis1.motorFault() || axis2.motorFault(); } + + // returns true if the mount is at the home (startup) position + inline bool isHome() { + return abs(axis1.getInstrumentCoordinate() - home.getPosition(CR_MOUNT).a1) <= arcsecToRad(AXIS1_HOME_TOLERANCE) && + abs(axis2.getInstrumentCoordinate() - home.getPosition(CR_MOUNT).a2) <= arcsecToRad(AXIS2_HOME_TOLERANCE); + } + + // returns true if the mount is slewing (doing a goto or guide > 2X) + inline bool isSlewing() { return axis1.isSlewing() || axis2.isSlewing(); } + + // one time initialization of tracking + void autostart(); + void autostartPostponed(); + + // enables or disables tracking, enabling tracking powers on the motors if necessary + void tracking(bool state); + + // returns true if the mount is tracking + inline bool isTracking() { return trackingState == TS_SIDEREAL; } + + // enables or disables power to the mount motors + // first enable starts the mount status indications + void enable(bool state); + + // returns true if the mount motors are powered on + inline bool isEnabled() { return axis1.isEnabled() && axis2.isEnabled(); } + + // true if syncing only from OnStep to the Encoders + bool syncFromOnStepToEncoders = false; + + inline bool startupAuthorityTrusted() const { return startupAuthorityTrustedValue; } + void setStartupAuthorityTrusted(bool state); + void captureNominalIndexPositions(); + long getNominalIndexPositionSteps(uint8_t axisNumber) const; + + #if MOUNT_COORDS_MEMORY == ON + void saveCoordinateMemory(bool trusted); + #endif + + // updates the tracking rates, etc. as appropriate for the mount state + // called once a second by poll() but available here for immediate action + void update(); + + void poll(); + + // all in sidereal units 1x = 15 arc-seconds/sidereal second + float trackingRate = hzToSidereal(TRACKING_RATE_DEFAULT_HZ); + float trackingRateAxis1 = 0.0F; + float trackingRateAxis2 = 0.0F; + float trackingRateOffsetRA = 0.0F; + float trackingRateOffsetDec = 0.0F; + + MountSettings settings = {RC_DEFAULT, { 0, 0 }, MOUNT_SUBTYPE}; + + private: + // alternate tracking rate calculation method + float ztr(float a); + + // update where we are pointing *now* + void updatePosition(CoordReturn coordReturn); + + // current position in Mount coordinates (Observed Place with no corrections except index offset) + // coordinates are either Horizon (a, z) or Equatorial (h, d) depending on the mountType + // also includes Mount normalized axis coordinates (a1, a2) where a2 is an instrument coordinate in tangent arm mode + Coordinate current; + + TrackingState trackingState = TS_NONE; + + uint32_t nvKey; + + #if MOUNT_COORDS_MEMORY == ON + uint32_t nvKeyLastA, nvKeyLastB; + MountPositionMemory lastPosition; + #endif + + long nominalIndexAxis1Steps = 0; + long nominalIndexAxis2Steps = 0; + + bool absoluteCoordinateOriginsEstablished = true; + bool startupAuthorityTrustedValue = false; +}; + +extern Motor& motor1; +extern Axis axis1; + +extern Motor& motor2; +extern Axis axis2; + +extern Mount mount; + +#endif diff --git a/src/telescope/mount/coordinates/Align.hs.cpp b/src/telescope/mount/coordinates/Align.hs.cpp new file mode 100755 index 0000000..6feee1d --- /dev/null +++ b/src/telescope/mount/coordinates/Align.hs.cpp @@ -0,0 +1,569 @@ +// ----------------------------------------------------------------------------------- +// GOTO ASSIST GEOMETRIC ALIGN FOR ALT/AZM AND EQ MOUNTS (HIGH SPEED) +// +// by Howard Dutton +// +// Copyright (C) 2012 to 2024 Howard Dutton +// + +#include "Align.hs.h" + +#if defined(MOUNT_PRESENT) && defined(HIGH_SPEED_ALIGN) + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../coordinates/Transform.h" +#include "../../Telescope.h" + +#if ALIGN_MAX_NUM_STARS > 1 + +uint8_t modelNumberStars = 0; +void autoModelWrapper() { transform.align.autoModel(modelNumberStars); } + +void GeoAlign::init(int8_t mountType, float latitude) { + modelClear(); + + this->mountType = mountType; + if (mountType == ALTAZM) { + cosLat = cosf(Deg90); + sinLat = sinf(Deg90); + } else + if (mountType == ALTALT) { + cosLat = cosf(0.0F); + sinLat = sinf(0.0F); + } else { + cosLat = cosf(latitude); + sinLat = sinf(latitude); + } +} + +void GeoAlign::modelRead() { + if (!nv().kv().getOrInit("ALIGN_MODEL", model)) { DLF("WRN: Nv, init failed for ALIGN_MODEL"); } + + if (model.ax1Cor < -Deg360 || model.ax1Cor > Deg360) { model.ax1Cor = 0; DLF("ERR: GeoAlign::readModel(), bad NV ax1Cor"); } + if (model.ax2Cor < -Deg360 || model.ax2Cor > Deg360) { model.ax2Cor = 0; DLF("ERR: GeoAlign::readModel(), bad NV ax2Cor"); } + if (model.dfCor < -256 || model.dfCor > 256) { model.dfCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV dfCor"); } + if (model.tfCor < -128 || model.tfCor > 128) { model.tfCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV tfCor"); } + if (model.doCor < -8192 || model.doCor > 8192) { model.doCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV doCor"); } + if (model.pdCor < -256 || model.pdCor > 256) { model.pdCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV pdCor"); } + if (model.altCor < -16384 || model.altCor > 16384) { model.altCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV altCor"); } + if (model.azmCor < -16384 || model.azmCor > 16384) { model.azmCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV azmCor"); } + if (model.hcp < -Deg360 || model.hcp > Deg360) { model.hcp = 0; DLF("ERR: GeoAlign::readModel(), bad NV hcp"); } + if (model.hca < -16384 || model.hca > 16384) { model.hca = 0; DLF("ERR: GeoAlign::readModel(), bad NV hca"); } + if (model.dcp < -Deg360 || model.dcp > Deg360) { model.dcp = 0; DLF("ERR: GeoAlign::readModel(), bad NV dcp"); } + if (model.dca < -16384 || model.dca > 16384) { model.dca = 0; DLF("ERR: GeoAlign::readModel(), bad NV dca"); } +} + +void GeoAlign::modelWrite() { + nv().kv().put("ALIGN_MODEL", model); +} + +void GeoAlign::modelClear() { + modelNumberStars = 0; + model.ax1Cor = 0; // align internal index for Axis1 + model.ax2Cor = 0; // align internal index for Axis2 + model.altCor = 0; // polar error relative to NCP/SCP/Zenith, - is below & + above + model.azmCor = 0; // - is right & + is left + model.doCor = 0; // altitude axis/optics orthogonal correction + model.pdCor = 0; // altitude axis/Azimuth orthogonal correction + model.dfCor = 0; // altitude axis axis flex + model.tfCor = 0; // tube flex + model.hcp = 0; // cos(Axis1) phase + model.hca = 0; // cos(Axis1) amplitude + model.dcp = 0; // cos(Axis2) phase + model.dca = 0; // cos(Axis2) amplitude + modelIsReady = false; +} + +bool GeoAlign::modelReady() { + return modelIsReady; +} + +CommandError GeoAlign::addStar(int thisStar, int numberStars, Coordinate *actual, Coordinate *mount) { + // just return if we are processing a model or the star count is out of range, this should never happen + if (autoModelTask != 0 || thisStar < 1 || thisStar > ALIGN_MAX_NUM_STARS || numberStars < 1 || numberStars > ALIGN_MAX_NUM_STARS) return CE_ALIGN_FAIL; + + int i = thisStar - 1; + + this->mount[i].h = mount->h; + this->mount[i].d = mount->d; + this->actual[i].h = actual->h; + this->actual[i].d = actual->d; + + if (mountType == ALTAZM) { + transform.equToHor(mount); + this->mount[i].ax1 = mount->z; + this->mount[i].ax2 = mount->a; + + transform.equToHor(actual); + this->actual[i].ax1 = actual->z; + this->actual[i].ax2 = actual->a; + } else + if (mountType == ALTALT) { + transform.equToAa(mount); + this->mount[i].ax1 = mount->aa1; + this->mount[i].ax2 = mount->aa2; + + transform.equToAa(actual); + this->actual[i].ax1 = actual->aa1; + this->actual[i].ax2 = actual->aa2; + } else { + this->mount[i].ax1 = mount->h; + this->mount[i].ax2 = mount->d; + + this->actual[i].ax1 = actual->h; + this->actual[i].ax2 = actual->d; + } + + if (mount->pierSide == PIER_SIDE_WEST) { + this->actual[i].side = -1; + this->mount[i].side = -1; + } else { + this->actual[i].side = 1; + this->mount[i].side = 1; + } + + // two or more stars and finished + if (thisStar >= 2 && thisStar == numberStars) { + createModel(numberStars); + } + + return CE_NONE; +} + +void GeoAlign::createModel(int numberStars) { + if (autoModelTask != 0) return; + + // start a task to solve for the model + modelNumberStars = numberStars; + autoModelTask = tasks.add(1, 0, false, 6, autoModelWrapper, "Align"); +} + +// returns the correction to be added to the requested RA,Dec to yield the actual RA,Dec that we will arrive at +void GeoAlign::correct(AlignCoordinate &mount, float sf, float _deo, float _pd, float _pz, float _pe, float _df, float _ff, float _tf, float *a1r, float *a2r) { + float DO1,DOh; + float PD,PDh; + float PZ,PA; + float DF,DFd,TF,FF,FFd,TFh,TFd; + + // ------------------------------------------------------------ + // A. Misalignment due to tube/optics not being perp. to Dec axis + // negative numbers are further (S) from the NCP, swing to the + // equator and the effect on declination is 0. At the SCP it + // becomes a (N) offset. Unchanged with meridian flips. + DO1 = _deo*sf; + // works on HA. meridian flips effect this in HA + DOh = DO1*(1.0F/mount.cosA2)*mount.side; + + // ------------------------------------------------------------ + // B. Misalignment, Declination axis relative to Polar axis + // expressed as a correction to where the Polar axis is pointing + // negative numbers are further (S) from the NCP, swing to the + // equator and the effect on declination is 0. + // At the SCP it is, again, a (S) offset + PD = _pd*sf; + // works on HA. + PDh = -PD*mount.tanA2*mount.side; + + // ------------------------------------------------------------ + // Misalignment, relative to NCP + // negative numbers are east of the pole + // C. polar left-right misalignment + PZ = _pz*sf; + // D. negative numbers are below the pole + // polar below-above misalignment + PA = _pe*sf; + + // ------------------------------------------------------------ + // Axis flex + DF = _df*sf; + DFd = -DF*(cosLat*mount.cosA1 + sinLat*mount.tanA2); + + // ------------------------------------------------------------ + // Fork flex + FF = _ff*sf; + FFd = FF*mount.cosA1; + + // ------------------------------------------------------------ + // Optical axis sag + TF = _tf*sf; + + TFh = TF*(cosLat*mount.sinA1*(1.0/mount.cosA2)); + TFd = TF*(cosLat*mount.cosA1*mount.sinA2-sinLat*mount.cosA2); + + // ------------------------------------------------------------ + *a1r = (-PZ*mount.cosA1*mount.tanA2 + PA*mount.sinA1*mount.tanA2 + DOh + PDh + TFh); + *a2r = (+PZ*mount.sinA1 + PA*mount.cosA1 + DFd + FFd + TFd); +} + +void GeoAlign::doSearch(float sf, int p1, int p2, int p3, int p4, int p5, int p6, int p7, int p8, int p9) { + long l, + + _deo_m,_deo_p, + _pd_m,_pd_p, + _pz_m,_pz_p, + _pe_m,_pe_p, + _df_m,_df_p, + _tf_m,_tf_p, + _ff_m,_ff_p, + _oh_m,_oh_p, + _od_m,_od_p, + + _deo,_pd,_pz,_pe, _df,_tf,_ff, _ode,_ohe; + + float sf1 = arcsecToRad(sf); + + // search + // set Parameter Space + _deo_m= -p1 + round(best_deo/sf); _deo_p= p1 + round(best_deo/sf); + _pd_m = -p2 + round(best_pd/sf); _pd_p = p2 + round(best_pd/sf); + _pz_m = -p3 + round(best_pz/sf); _pz_p = p3 + round(best_pz/sf); + _pe_m = -p4 + round(best_pe/sf); _pe_p = p4 + round(best_pe/sf); + _tf_m = -p5 + round(best_tf/sf); _tf_p = p5 + round(best_tf/sf); + _ff_m = -p6 + round(best_ff/sf); _ff_p = p6 + round(best_ff/sf); + _df_m = -p7 + round(best_df/sf); _df_p = p7 + round(best_df/sf); + _od_m = -p8 + round(best_ode/sf); _od_p = p8 + round(best_ode/sf); + _oh_m = -p9 + round(best_ohe/sf); _oh_p = p9 + round(best_ohe/sf); + + float ma2, ma1; + for (_ohe = _oh_m; _ohe <= _oh_p; _ohe++) + for (_ode = _od_m; _ode <= _od_p; _ode++) { + ode = _ode*sf1; + odw = -ode; + ohe = _ohe*sf1; + ohw = ohe; + + for (l = 0; l < num; l++) { + ma1 = mount[l].ax1; + ma2 = mount[l].ax2; + + if (mount[l].side == -1) // west of the mount + { + ma1 = ma1 + ohw; + ma2 = ma2 + odw; + } else + if (mount[l].side == 1) // east of the mount, default (fork mounts) + { + ma1 = ma1 + ohe; + ma2 = ma2 + ode; + } + + mount[l].ma1 = ma1; + mount[l].ma2 = ma2; + mount[l].sinA1 = sinf(ma1); + mount[l].cosA1 = cosf(ma1); + mount[l].sinA2 = sinf(ma2); + mount[l].cosA2 = cosf(ma2); + mount[l].tanA2 = mount[l].sinA2/mount[l].cosA2; + } + + for (_deo = _deo_m; _deo <= _deo_p; _deo++) + for (_pd = _pd_m; _pd <= _pd_p; _pd++) + for (_pz = _pz_m; _pz <= _pz_p; _pz++) + for (_pe = _pe_m; _pe <= _pe_p; _pe++) + for (_df = _df_m; _df <= _df_p; _df++) + for (_ff = _ff_m; _ff <= _ff_p; _ff++) + for (_tf = _tf_m; _tf <= _tf_p; _tf++) { + + // check the combinations for all samples + for (l = 0; l < num; l++) { + float ma1r, ma2r; + correct(mount[l], sf1, _deo, _pd, _pz, _pe, _df, _ff, _tf, &ma1r, &ma2r); + + delta[l].ax1 = actual[l].ax1 - (mount[l].ma1 - ma1r); + if (delta[l].ax1 > Deg180) delta[l].ax1 = delta[l].ax1 - Deg360; else + if (delta[l].ax1 < -Deg180) delta[l].ax1 = delta[l].ax1 + Deg360; + delta[l].ax2 = actual[l].ax2 - (mount[l].ma2 - ma2r); + delta[l].side = mount[l].side; + } + + // calculate the standard deviations + float a, b; + + sum1 = 0.0; + for (l = 0; l < num; l++) sum1 = sum1 + sq(delta[l].ax1*cosf(actual[l].ax2)); + a = sum1/(num - 1); // was sqrt(sum1/(num - 1)) + + sum1 = 0.0; + for (l = 0; l < num; l++) sum1 = sum1 + sq(delta[l].ax2); + b = sum1/(num - 1); // was sqrt(sum1/(num - 1)) + + max_dist = sqrtf(a + b); // was sq(a) + sq(b) + + // remember the best fit + if (max_dist < best_dist) { + best_dist = max_dist; + best_deo = _deo*sf; + best_pd = _pd*sf; + best_pz = _pz*sf; + best_pe = _pe*sf; + + best_tf = _tf*sf; + best_df = _df*sf; + best_ff = _ff*sf; + + if (p8 != 0) best_odw = radToArcsec(odw); else best_odw = best_pe/2.0; + if (p8 != 0) best_ode = radToArcsec(ode); else best_ode = -best_pe/2.0; + if (p9 != 0) best_ohw = radToArcsec(ohw); + if (p9 != 0) best_ohe = radToArcsec(ohe); + } + + Y; + } + } +} + +void GeoAlign::autoModel(int n) { + modelIsReady = false; + + VLF("MSG: Align, calculate pointing model start"); + + // how many stars? + num = n; + + best_dist = 3600.0F*180.0F; + best_deo = 0.0F; + best_pd = 0.0F; + best_pz = 0.0F; + best_pe = 0.0F; + best_tf = 0.0F; + best_ff = 0.0F; + best_df = 0.0F; + best_ode = 0.0F; + best_ohe = 0.0F; + + // figure out the average Axis1 offset as a starting point + ohe = 0; + float diff; + for (l = 0; l < num; l++) { + diff = actual[l].ax1 - mount[l].ax1; + if (diff > Deg180) diff = diff - Deg360; + if (diff < -Deg180) diff = diff + Deg360; + ohe = ohe + diff; + } + ohe = ohe/num; + best_ohe = round(radToArcsec(ohe)); + best_ohw = best_ohe; + + // fork flex or dec axis flex, as appropriate + if (mountType == ALTAZM) { Ff = 0; Df = 0; } else + if (mountType == FORK) { Ff = 1; Df = 0; } else { Ff = 0; Df = 1; } + + // only search for cone error if > 2 stars + int Do = 0; + if (num > 2) Do = 1; + + // search, this can handle about 9 degrees of polar misalignment, and 4 degrees of cone error + // DoPdPzPeTfFf Df OdOh + doSearch(16384,0 ,0,1,1,0, 0, 0,1,1); + doSearch( 8192,Do,0,1,1,0, 0, 0,1,1); + doSearch( 4096,Do,0,1,1,0, 0, 0,1,1); + doSearch( 2048,Do,0,1,1,0, 0, 0,1,1); + doSearch( 1024,Do,0,1,1,0, 0, 0,1,1); + doSearch( 512,Do,0,1,1,0, 0, 0,1,1); + #ifdef HAL_SLOW_PROCESSOR + doSearch(256,Do,0,1,1,0, 0, 0,1,1); + doSearch(128,Do,0,1,1,0, 0, 0,1,1); + doSearch( 64,Do,0,1,1,0, 0, 0,1,1); + #else + if (num > 4) { + doSearch(256,Do,1,1,1,0,Ff,Df,1,1); + doSearch(128,Do,1,1,1,1,Ff,Df,1,1); + doSearch( 64,Do,1,1,1,1,Ff,Df,1,1); + #ifdef HAL_FAST_PROCESSOR + doSearch( 32,Do,1,1,1,1,Ff,Df,1,1); + doSearch( 16,Do,1,1,1,1,Ff,Df,1,1); + doSearch( 8,Do,1,1,1,1,Ff,Df,1,1); + #ifdef HAL_VFAST_PROCESSOR + doSearch( 4,Do,1,1,1,1,Ff,Df,1,1); + #endif + #endif + } else { + doSearch(256,Do,0,1,1,0, 0, 0,1,1); + doSearch(128,Do,0,1,1,0, 0, 0,1,1); + doSearch( 64,Do,0,1,1,0, 0, 0,1,1); + doSearch( 32,Do,0,1,1,0, 0, 0,1,1); + #ifdef HAL_FAST_PROCESSOR + doSearch( 16,Do,0,1,1,0, 0, 0,1,1); + doSearch( 8,Do,0,1,1,0, 0, 0,1,1); + #ifdef HAL_VFAST_PROCESSOR + doSearch( 4,Do,0,1,1,0, 0, 0,1,1); + #endif + #endif + } + #endif + + // geometric corrections + model.doCor = arcsecToRad(best_deo); + model.pdCor = arcsecToRad(best_pd); + model.azmCor = arcsecToRad(best_pz); + model.altCor = arcsecToRad(best_pe); + + model.tfCor = arcsecToRad(best_tf); + if (mountType == FORK || mountType == ALTAZM) model.dfCor = arcsecToRad(best_ff); else model.dfCor = arcsecToRad(best_df); + + model.ax1Cor = arcsecToRad(best_ohw); + model.ax2Cor = arcsecToRad(best_odw); + + model.hcp = 0; + model.hca = 0; + model.dcp = 0; + model.dca = 0; + + // update status and exit + modelIsReady = true; + + VLF("MSG: Align, calculate pointing model done"); + tasks.setDurationComplete(autoModelTask); + autoModelTask = 0; +} + +void GeoAlign::observedPlaceToMount(Coordinate *coord) { + if (!modelIsReady) return; + + float p = 1.0F; + if (coord->pierSide == PIER_SIDE_WEST) p = -1.0F; + + float ax1, ax2; + if (mountType == ALTAZM) { ax1 = coord->z; ax2 = coord->a; } else + if (mountType == ALTALT) { ax1 = coord->aa1; ax2 = coord->aa2; } else { ax1 = coord->h; ax2 = coord->d; } + + if (ax2 > Deg90) ax2 = Deg90; + if (ax2 < -Deg90) ax2 = -Deg90; + + // initial rough guess at instrument coordinate + float a1 = ax1; + float a2 = ax2; + + // breaks-down near the poles (limited to > 1' from pole) + if (fabs(ax2) < degToRadF(89.98333333F)) { + for (int pass = 0; pass < 3; pass++) { + float sinAx2 = sinf(a2); + float cosAx2 = cosf(a2); + float sinAx1 = sinf(a1); + float cosAx1 = cosf(a1); + + // ------------------------------------------------------------ + // misalignment due to tube/optics not being perp. to Alt axis + // negative numbers are further (down) from the Zenith, swing to the + // horizon and the effect on Alt is 0. At the Nadir it + // becomes an (up) offset. Unchanged with meridian flips. + // expressed as a correction to the Zenith axis misalignment + float DOh = model.doCor*(1.0F/cosAx2)*p; + + // ------------------------------------------------------------ + // misalignment due to Alt axis being perp. to Azm axis + float PDh = -model.pdCor*(sinAx2/cosAx2)*p; + + // Fork or Axis flex + float DFd; + if (mountType == FORK || mountType == ALTAZM) DFd = model.dfCor*cosAx1; else DFd = -model.dfCor*(cosLat*cosAx1 + sinLat*(sinAx2/cosAx2)); + + // Tube flex + float TFh = model.tfCor*(cosLat*sinAx1*(1.0F/cosAx2)); + float TFd = model.tfCor*(cosLat*cosAx1*sinAx2 - sinLat*cosAx2); + + // polar misalignment + float ax1c = -model.azmCor*cosAx1*(sinAx2/cosAx2) + model.altCor*sinAx1*(sinAx2/cosAx2); + float ax2c = +model.azmCor*sinAx1 + model.altCor*cosAx1; + + // cos() linearization for main drive mechanism runout etc. + float COSh = cos(a1 + model.hcp)*model.hca*p; + float COSd = cos(a2 + model.dcp)*model.dca*p; + + // improved guess at instrument coordinate + a1 = ax1 + (ax1c + PDh + DOh + TFh + COSh); + a2 = ax2 + (ax2c + DFd + TFd + COSd); + } + } + + // finally, apply the index offsets + a1 = a1 - model.ax1Cor; + a2 = a2 - model.ax2Cor*-p; + + if (mountType == ALTAZM) { coord->z = a1; coord->a = a2; } else + if (mountType == ALTALT) { coord->aa1 = a1; coord->aa2 = a2; } else { coord->h = a1; coord->d = a2; } +} + +void GeoAlign::mountToObservedPlace(Coordinate *coord) { + if (!modelIsReady) return; + + float p = 1.0F; + if (coord->pierSide == PIER_SIDE_WEST) p = -1.0F; + + float ax1, ax2; + if (mountType == ALTAZM) { ax1 = coord->z; ax2 = coord->a; } else + if (mountType == ALTALT) { ax1 = coord->aa1; ax2 = coord->aa2; } else { ax1 = coord->h; ax2 = coord->d; } + + ax1 = ax1 + model.ax1Cor; + ax2 = ax2 + model.ax2Cor*-p; + + if (ax2 > Deg90) ax2 = Deg90; + if (ax2 < -Deg90) ax2 = -Deg90; + + // breaks-down near the Zenith (limited to > 1' from Zenith) + if (fabs(ax2) < degToRadF(89.98333333F)) { + float sinAx2 = sinf(ax2); + float cosAx2 = cosf(ax2); + float sinAx1 = sinf(ax1); + float cosAx1 = cosf(ax1); + + // ------------------------------------------------------------ + // misalignment due to tube/optics not being perp. to Alt axis + // negative numbers are further (S) from the Zenith, swing to the + // horizon and the effect on Alt is 0. At the Nadir it + // becomes a (N) offset. Unchanged with meridian flips. + // expressed as a correction to the Azm axis misalignment + float DOh = model.doCor*(1.0F/cosAx2)*p; + + // as the above offset becomes zero near the horizon, the affect + // works on Azm instead. meridian flips affect this in Azm + float PDh = -model.pdCor*(sinAx2/cosAx2)*p; + + // Fork or Axis flex + float DFd; + if (mountType == FORK || mountType == ALTAZM) DFd = model.dfCor*cosAx1; else DFd = -model.dfCor*(cosLat*cosAx1 + sinLat*(sinAx2/cosAx2)); + + // Tube flex + float TFh = model.tfCor*(cosLat*sinAx1*(1.0F/cosAx2)); + float TFd = model.tfCor*(cosLat*cosAx1*sinAx2 - sinLat*cosAx2); + + // ------------------------------------------------------------ + // polar misalignment + float a1 = -model.azmCor*cosAx1*(sinAx2/cosAx2) + model.altCor*sinAx1*(sinAx2/cosAx2); + float a2 = +model.azmCor*sinAx1 + model.altCor*cosAx1; + + // cos() linearization for main drive mechanism runout etc. + float COSh = cos(a1 + model.hcp)*model.hca*p; + float COSd = cos(a2 + model.dcp)*model.dca*p; + + ax1 = ax1 - (a1 + PDh + DOh + TFh + COSh); + ax2 = ax2 - (a2 + DFd + TFd + COSd); + } + + if (ax2 > Deg90) ax2 = Deg90; + if (ax2 < -Deg90) ax2 = -Deg90; + + if (mountType == ALTAZM) { + while (ax1 > Deg360) ax1 -= Deg360; + while (ax1 < -Deg360) ax1 += Deg360; + coord->z = ax1; + coord->a = ax2; + } else + if (mountType == ALTALT) { + while (ax1 > Deg360) ax1 -= Deg360; + while (ax1 < -Deg360) ax1 += Deg360; + coord->aa1 = ax1; + coord->aa2 = ax2; + } else { + while (ax1 > Deg180) ax1 -= Deg360; + while (ax1 < -Deg180) ax1 += Deg360; + coord->h = ax1; + coord->d = ax2; + } +} + +#endif + +#endif diff --git a/src/telescope/mount/coordinates/Align.hs.h b/src/telescope/mount/coordinates/Align.hs.h new file mode 100755 index 0000000..560b499 --- /dev/null +++ b/src/telescope/mount/coordinates/Align.hs.h @@ -0,0 +1,134 @@ +// ----------------------------------------------------------------------------------- +// GOTO ASSIST GEOMETRIC ALIGN FOR ALT/AZM AND EQ MOUNTS (HIGH SPEED) +// +// by Howard Dutton +// +// Copyright (C) 2012 to 2024 Howard Dutton +// +#pragma once + +#include "../../../Common.h" + +#if defined(MOUNT_PRESENT) && defined(HIGH_SPEED_ALIGN) + +#include "../../../libApp/commands/ProcessCmds.h" + +#if defined(ALIGN_MAX_STARS) && ALIGN_MAX_STARS != AUTO + #if (ALIGN_MAX_STARS < 3 || ALIGN_MAX_STARS > 9) && ALIGN_MAX_STARS != 1 + #error "ALIGN_MAX_STARS must be 1, or in the range of 3 to 9" + #endif + #define ALIGN_MAX_NUM_STARS ALIGN_MAX_STARS +#else + #if defined(HAL_FAST_PROCESSOR) + #define ALIGN_MAX_NUM_STARS 9 + #else + #define ALIGN_MAX_NUM_STARS 6 + #endif +#endif + +enum PierSide: uint8_t {PIER_SIDE_NONE, PIER_SIDE_EAST, PIER_SIDE_WEST}; + +typedef struct Coordinate { + double r; + double h; + double d; + double a; + double z; + double aa1; + double aa2; + double a1; + double a1Correction; + double a2; + PierSide pierSide; +} Coordinate; + +#if ALIGN_MAX_NUM_STARS > 1 + +// ----------------------------------------------------------------------------------- +// ADVANCED GEOMETRIC ALIGN FOR EQUATORIAL MOUNTS (GOTO ASSIST) + +typedef struct AlignCoordinate { + float ax1, ma1; + float ax2, ma2; + float cosA1, sinA1, cosA2, sinA2, tanA2; + float h; + float d; + int side; +} AlignCoordinate; + +#define AlignModelSize 48 +typedef struct AlignModel { + float ax1Cor; + float ax2Cor; + float altCor; + float azmCor; + float doCor; + float pdCor; + float dfCor; + float tfCor; + float hcp; + float hca; + float dcp; + float dca; +} AlignModel; + +class GeoAlign +{ + public: + // prepare goto assist for operation, also clears the alignment model; + void init(int8_t mountType, float latitude); + + // reads the last saved alignment model from NV + void modelRead(); + // writes the alignment model to NV + void modelWrite(); + // clear the alignment model + void modelClear(); + // reports if ready for operation + bool modelReady(); + + // add a star to an alignment model + // thisStar: 1 for 1st star, 2 for 2nd star, etc. up to numberStars (at which point the mount model is calculated) + // numberStars: total number of stars for this align (1 to 9) + // actual: equatorial or horizon coordinate (depending on the mount type) for where the star should be (in mount coordinates) + // mount: equatorial or horizon coordinate (depending on the mount type) for where the star is (in mount coordinates) + CommandError addStar(int thisStar, int numberStars, Coordinate *actual, Coordinate *mount); + + void createModel(int numberStars); + + // convert equatorial (h,d) or horizon (a,z) coordinate from observed place to mount + void observedPlaceToMount(Coordinate *coord); + // convert equatorial (h,d) or horizon (a,z) coordinate from mount to observed place + void mountToObservedPlace(Coordinate *coord); + + void autoModel(int n); + + bool modelIsReady = false; + + AlignCoordinate mount[ALIGN_MAX_NUM_STARS]; + AlignCoordinate actual[ALIGN_MAX_NUM_STARS]; + AlignCoordinate delta[ALIGN_MAX_NUM_STARS]; + AlignModel model; + + private: + void correct(AlignCoordinate &mount, float sf, float _deo, float _pd, float _pz, float _pe, float _da, float _ff, float _tf, float *h1, float *d1); + void doSearch(float sf, int p1, int p2, int p3, int p4, int p5, int p6, int p7, int p8, int p9); + + int8_t mountType; + float cosLat, sinLat; + + long num, l; + long Ff, Df; + float best_deo, best_pd, best_pz, best_pe, best_ohw, best_odw, best_ohe, best_ode, best_tf, best_df, best_ff; + float avg_ha, avg_dec; + float dist, sumd, rms; + float best_dist; + float ohe, ode, ohw, odw, dh; + float sum1; + float max_dist; + + uint8_t autoModelTask = 0; +}; + +#endif +#endif diff --git a/src/telescope/mount/coordinates/Align.ref.cpp b/src/telescope/mount/coordinates/Align.ref.cpp new file mode 100755 index 0000000..e19d660 --- /dev/null +++ b/src/telescope/mount/coordinates/Align.ref.cpp @@ -0,0 +1,543 @@ +// ----------------------------------------------------------------------------------- +// GOTO ASSIST GEOMETRIC ALIGN FOR ALT/AZM AND EQ MOUNTS (REFERENCE DESIGN) +// +// by Howard Dutton +// +// Copyright (C) 2012 to 2024 Howard Dutton +// + +#include "Align.ref.h" + +#if defined(MOUNT_PRESENT) && !defined(HIGH_SPEED_ALIGN) + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../coordinates/Transform.h" +#include "../../Telescope.h" + +#if ALIGN_MAX_NUM_STARS > 1 + +uint8_t modelNumberStars = 0; +void autoModelWrapper() { transform.align.autoModel(modelNumberStars); } + +void GeoAlign::init(int8_t mountType, float latitude) { + modelClear(); + + this->mountType = mountType; + if (mountType == ALTAZM) { + cosLat = cosf(Deg90); + sinLat = sinf(Deg90); + } else { + cosLat = cosf(latitude); + sinLat = sinf(latitude); + } +} + +void GeoAlign::modelRead() { + if (!nv().kv().getOrInit("ALIGN_MODEL", model)) { DLF("WRN: Nv, init failed for ALIGN_MODEL"); } + + if (model.ax1Cor < -Deg360 || model.ax1Cor > Deg360) { model.ax1Cor = 0; DLF("ERR: GeoAlign::readModel(), bad NV ax1Cor"); } + if (model.ax2Cor < -Deg360 || model.ax2Cor > Deg360) { model.ax2Cor = 0; DLF("ERR: GeoAlign::readModel(), bad NV ax2Cor"); } + if (model.dfCor < -256 || model.dfCor > 256) { model.dfCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV dfCor"); } + if (model.tfCor < -128 || model.tfCor > 128) { model.tfCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV tfCor"); } + if (model.doCor < -8192 || model.doCor > 8192) { model.doCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV doCor"); } + if (model.pdCor < -256 || model.pdCor > 256) { model.pdCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV pdCor"); } + if (model.altCor < -16384 || model.altCor > 16384) { model.altCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV altCor"); } + if (model.azmCor < -16384 || model.azmCor > 16384) { model.azmCor = 0; DLF("ERR: GeoAlign::readModel(), bad NV azmCor"); } + if (model.hcp < -Deg360 || model.hcp > Deg360) { model.hcp = 0; DLF("ERR: GeoAlign::readModel(), bad NV hcp"); } + if (model.hca < -16384 || model.hca > 16384) { model.hca = 0; DLF("ERR: GeoAlign::readModel(), bad NV hca"); } + if (model.dcp < -Deg360 || model.dcp > Deg360) { model.dcp = 0; DLF("ERR: GeoAlign::readModel(), bad NV dcp"); } + if (model.dca < -16384 || model.dca > 16384) { model.dca = 0; DLF("ERR: GeoAlign::readModel(), bad NV dca"); } +} + +void GeoAlign::modelWrite() { + nv().kv().put("ALIGN_MODEL", model); +} + +void GeoAlign::modelClear() { + modelNumberStars = 0; + model.ax1Cor = 0; // align internal index for Axis1 + model.ax2Cor = 0; // align internal index for Axis2 + model.altCor = 0; // polar error relative to NCP/SCP/Zenith, - is below & + above + model.azmCor = 0; // - is right & + is left + model.doCor = 0; // altitude axis/optics orthogonal correction + model.pdCor = 0; // altitude axis/Azimuth orthogonal correction + model.dfCor = 0; // altitude axis axis flex + model.tfCor = 0; // tube flex + model.hcp = 0; // cos(Axis1) phase + model.hca = 0; // cos(Axis1) amplitude + model.dcp = 0; // cos(Axis2) phase + model.dca = 0; // cos(Axis2) amplitude + modelIsReady = false; +} + +bool GeoAlign::modelReady() { + return modelIsReady; +} + +CommandError GeoAlign::addStar(int thisStar, int numberStars, Coordinate *actual, Coordinate *mount) { + // just return if we are processing a model or the star count is out of range, this should never happen + if (autoModelTask != 0 || thisStar < 1 || thisStar > ALIGN_MAX_NUM_STARS || numberStars < 1 || numberStars > ALIGN_MAX_NUM_STARS) return CE_ALIGN_FAIL; + + int i = thisStar - 1; + + this->mount[i].h = mount->h; + this->mount[i].d = mount->d; + this->actual[i].h = actual->h; + this->actual[i].d = actual->d; + + if (mountType == ALTAZM) { + transform.equToHor(mount); + this->mount[i].ax1 = mount->z; + this->mount[i].ax2 = mount->a; + + transform.equToHor(actual); + this->actual[i].ax1 = actual->z; + this->actual[i].ax2 = actual->a; + } else { + this->mount[i].ax1 = mount->h; + this->mount[i].ax2 = mount->d; + + this->actual[i].ax1 = actual->h; + this->actual[i].ax2 = actual->d; + } + + if (mount->pierSide == PIER_SIDE_WEST) { + this->actual[i].side = -1; + this->mount[i].side = -1; + } else { + this->actual[i].side = 1; + this->mount[i].side = 1; + } + + // two or more stars and finished + if (thisStar >= 2 && thisStar == numberStars) { + createModel(numberStars); + } + + return CE_NONE; +} + +void GeoAlign::createModel(int numberStars) { + if (autoModelTask != 0) return; + + // start a task to solve for the model + modelNumberStars = numberStars; + autoModelTask = tasks.add(1, 0, false, 6, autoModelWrapper, "Align"); +} + +// returns the correction to be added to the requested RA,Dec to yield the actual RA,Dec that we will arrive at +void GeoAlign::correct(float a1, float a2, float pierSide, float sf, float _deo, float _pd, float _pz, float _pe, float _df, float _ff, float _tf, float *a1r, float *a2r) { + float DO1,DOh; + float PD,PDh; + float PZ,PA; + float DF,DFd,TF,FF,FFd,TFh,TFd; + + float cosA2 = cosf(a2); + float sinA2 = sinf(a2); + float tanA2 = sinA2/cosA2; + float sinA1 = sinf(a1); + float cosA1 = cosf(a1); + + // ------------------------------------------------------------ + // A. Misalignment due to tube/optics not being perp. to Dec axis + // negative numbers are further (S) from the NCP, swing to the + // equator and the effect on declination is 0. At the SCP it + // becomes a (N) offset. Unchanged with meridian flips. + DO1 = _deo*sf; + // works on HA. meridian flips effect this in HA + DOh = DO1*(1.0F/cosA2)*pierSide; + + // ------------------------------------------------------------ + // B. Misalignment, Declination axis relative to Polar axis + // expressed as a correction to where the Polar axis is pointing + // negative numbers are further (S) from the NCP, swing to the + // equator and the effect on declination is 0. + // At the SCP it is, again, a (S) offset + PD = _pd*sf; + // works on HA. + PDh = -PD*tanA2*pierSide; + + // ------------------------------------------------------------ + // Misalignment, relative to NCP + // negative numbers are east of the pole + // C. polar left-right misalignment + PZ = _pz*sf; + // D. negative numbers are below the pole + // polar below-above misalignment + PA = _pe*sf; + + // ------------------------------------------------------------ + // Axis flex + DF = _df*sf; + DFd = -DF*(cosLat*cosA1 + sinLat*tanA2); + + // ------------------------------------------------------------ + // Fork flex + FF = _ff*sf; + FFd = FF*cosA1; + + // ------------------------------------------------------------ + // Optical axis sag + TF = _tf*sf; + + TFh = TF*(cosLat*sinA1*(1.0/cosA2)); + TFd = TF*(cosLat*cosA1*sinA2-sinLat*cosA2); + + // ------------------------------------------------------------ + *a1r = (-PZ*cosA1*tanA2 + PA*sinA1*tanA2 + DOh + PDh + TFh); + *a2r = (+PZ*sinA1 + PA*cosA1 + DFd + FFd + TFd); +} + +void GeoAlign::doSearch(float sf, int p1, int p2, int p3, int p4, int p5, int p6, int p7, int p8, int p9) { + long l, + + _deo_m,_deo_p, + _pd_m,_pd_p, + _pz_m,_pz_p, + _pe_m,_pe_p, + _df_m,_df_p, + _tf_m,_tf_p, + _ff_m,_ff_p, + _oh_m,_oh_p, + _od_m,_od_p, + + _deo,_pd,_pz,_pe, _df,_tf,_ff, _ode,_ohe; + + float sf1 = arcsecToRad(sf); + + // search + // set Parameter Space + _deo_m= -p1 + round(best_deo/sf); _deo_p= p1 + round(best_deo/sf); + _pd_m = -p2 + round(best_pd/sf); _pd_p = p2 + round(best_pd/sf); + _pz_m = -p3 + round(best_pz/sf); _pz_p = p3 + round(best_pz/sf); + _pe_m = -p4 + round(best_pe/sf); _pe_p = p4 + round(best_pe/sf); + _tf_m = -p5 + round(best_tf/sf); _tf_p = p5 + round(best_tf/sf); + _ff_m = -p6 + round(best_ff/sf); _ff_p = p6 + round(best_ff/sf); + _df_m = -p7 + round(best_df/sf); _df_p = p7 + round(best_df/sf); + _od_m = -p8 + round(best_ode/sf); _od_p = p8 + round(best_ode/sf); + _oh_m = -p9 + round(best_ohe/sf); _oh_p = p9 + round(best_ohe/sf); + + float ma2, ma1; + for (_deo = _deo_m; _deo <= _deo_p; _deo++) + for (_pd = _pd_m; _pd <= _pd_p; _pd++) + for (_pz = _pz_m; _pz <= _pz_p; _pz++) + for (_pe = _pe_m; _pe <= _pe_p; _pe++) + for (_df = _df_m; _df <= _df_p; _df++) + for (_ff = _ff_m; _ff <= _ff_p; _ff++) + for (_tf = _tf_m; _tf <= _tf_p; _tf++) + for (_ohe = _oh_m; _ohe <= _oh_p; _ohe++) + for (_ode = _od_m; _ode <= _od_p; _ode++) { + ode = _ode*sf1; + odw = -ode; + ohe = _ohe*sf1; + ohw = ohe; + + // check the combinations for all samples + for (l = 0; l < num; l++) { + ma1 = mount[l].ax1; + ma2 = mount[l].ax2; + + if (mount[l].side == -1) // west of the mount + { + ma1 = ma1 + ohw; + ma2 = ma2 + odw; + } else + if (mount[l].side == 1) // east of the mount, default (fork mounts) + { + ma1 = ma1 + ohe; + ma2 = ma2 + ode; + } + + float ma1r, ma2r; + correct(ma1, ma2, mount[l].side, sf1, _deo, _pd, _pz, _pe, _df, _ff, _tf, &ma1r, &ma2r); + + delta[l].ax1 = actual[l].ax1 - (ma1 - ma1r); + if (delta[l].ax1 > Deg180) delta[l].ax1 = delta[l].ax1 - Deg360; else + if (delta[l].ax1 < -Deg180) delta[l].ax1 = delta[l].ax1 + Deg360; + delta[l].ax2 = actual[l].ax2 - (ma2 - ma2r); + delta[l].side = mount[l].side; + } + + // calculate the standard deviations + float a, b; + sum1 = 0.0; for (l = 0; l < num; l++) sum1 = sum1 + sq(delta[l].ax1*cosf(actual[l].ax2)); a = sqrtf(sum1/(num - 1)); + sum1 = 0.0; for (l = 0; l < num; l++) sum1 = sum1 + sq(delta[l].ax2); b = sqrtf(sum1/(num - 1)); + max_dist = sqrtf(sq(a) + sq(b)); + + // remember the best fit + if (max_dist < best_dist) { + best_dist = max_dist; + best_deo = _deo*sf; + best_pd = _pd*sf; + best_pz = _pz*sf; + best_pe = _pe*sf; + + best_tf = _tf*sf; + best_df = _df*sf; + best_ff = _ff*sf; + + if (p8 != 0) best_odw = radToArcsec(odw); else best_odw = best_pe/2.0; + if (p8 != 0) best_ode = radToArcsec(ode); else best_ode = -best_pe/2.0; + if (p9 != 0) best_ohw = radToArcsec(ohw); + if (p9 != 0) best_ohe = radToArcsec(ohe); + } + + Y; + } +} + +void GeoAlign::autoModel(int n) { + modelIsReady = false; + + VLF("MSG: Align, calculate pointing model start"); + + // how many stars? + num = n; + + best_dist = 3600.0F*180.0F; + best_deo = 0.0F; + best_pd = 0.0F; + best_pz = 0.0F; + best_pe = 0.0F; + best_tf = 0.0F; + best_ff = 0.0F; + best_df = 0.0F; + best_ode = 0.0F; + best_ohe = 0.0F; + + // figure out the average Axis1 offset as a starting point + ohe = 0; + float diff; + for (l = 0; l < num; l++) { + diff = actual[l].ax1 - mount[l].ax1; + if (diff > Deg180) diff = diff - Deg360; + if (diff < -Deg180) diff = diff + Deg360; + ohe = ohe + diff; + } + ohe = ohe/num; + best_ohe = round(radToArcsec(ohe)); + best_ohw = best_ohe; + + // fork flex or dec axis flex, as appropriate + if (mountType == ALTAZM) { Ff = 0; Df = 0; } else if (mountType == FORK) { Ff = 1; Df = 0; } else { Ff = 0; Df = 1; } + + // only search for cone error if > 2 stars + int Do = 0; + if (num > 2) Do = 1; + + // search, this can handle about 9 degrees of polar misalignment, and 4 degrees of cone error + // DoPdPzPeTfFf Df OdOh + doSearch(16384,0 ,0,1,1,0, 0, 0,1,1); + doSearch( 8192,Do,0,1,1,0, 0, 0,1,1); + doSearch( 4096,Do,0,1,1,0, 0, 0,1,1); + doSearch( 2048,Do,0,1,1,0, 0, 0,1,1); + doSearch( 1024,Do,0,1,1,0, 0, 0,1,1); + doSearch( 512,Do,0,1,1,0, 0, 0,1,1); + #ifdef HAL_SLOW_PROCESSOR + doSearch(256,Do,0,1,1,0, 0, 0,1,1); + doSearch(128,Do,0,1,1,0, 0, 0,1,1); + #else + if (num > 4) { + doSearch(256,Do,1,1,1,0,Ff,Df,1,1); + doSearch(128,Do,1,1,1,1,Ff,Df,1,1); + doSearch( 64,Do,1,1,1,1,Ff,Df,1,1); + #ifdef HAL_FAST_PROCESSOR + doSearch( 32,Do,1,1,1,1,Ff,Df,1,1); + doSearch( 16,Do,1,1,1,1,Ff,Df,1,1); + #endif + } else { + doSearch(256,Do,0,1,1,0, 0, 0,1,1); + doSearch(128,Do,0,1,1,0, 0, 0,1,1); + doSearch( 64,Do,0,1,1,0, 0, 0,1,1); + doSearch( 32,Do,0,1,1,0, 0, 0,1,1); + #ifdef HAL_FAST_PROCESSOR + doSearch( 16,Do,0,1,1,0, 0, 0,1,1); + #endif + } + #endif + + // geometric corrections + model.doCor = arcsecToRad(best_deo); + model.pdCor = arcsecToRad(best_pd); + model.azmCor = arcsecToRad(best_pz); + model.altCor = arcsecToRad(best_pe); + + model.tfCor = arcsecToRad(best_tf); + if (mountType == FORK || mountType == ALTAZM) model.dfCor = arcsecToRad(best_ff); else model.dfCor = arcsecToRad(best_df); + + model.ax1Cor = arcsecToRad(best_ohw); + model.ax2Cor = arcsecToRad(best_odw); + + model.hcp = 0; + model.hca = 0; + model.dcp = 0; + model.dca = 0; + + // update status and exit + modelIsReady = true; + + VLF("MSG: Align, calculate pointing model done"); + tasks.setDurationComplete(autoModelTask); + autoModelTask = 0; +} + +void GeoAlign::observedPlaceToMount(Coordinate *coord) { + if (!modelIsReady) return; + + float p = 1.0F; + if (coord->pierSide == PIER_SIDE_WEST) p = -1.0F; + + float ax1, ax2; + if (mountType == ALTAZM) { + ax1 = coord->z; + ax2 = coord->a; + } else { + ax1 = coord->h; + ax2 = coord->d; + } + + if (ax2 > Deg90) ax2 = Deg90; + if (ax2 < -Deg90) ax2 = -Deg90; + + // initial rough guess at instrument coordinate + float a1 = ax1; + float a2 = ax2; + + // breaks-down near the poles (limited to > 1' from pole) + if (fabs(ax2) < degToRadF(89.98333333F)) { + for (int pass = 0; pass < 3; pass++) { + float sinAx2 = sinf(a2); + float cosAx2 = cosf(a2); + float sinAx1 = sinf(a1); + float cosAx1 = cosf(a1); + + // ------------------------------------------------------------ + // misalignment due to tube/optics not being perp. to Alt axis + // negative numbers are further (down) from the Zenith, swing to the + // horizon and the effect on Alt is 0. At the Nadir it + // becomes an (up) offset. Unchanged with meridian flips. + // expressed as a correction to the Zenith axis misalignment + float DOh = model.doCor*(1.0F/cosAx2)*p; + + // ------------------------------------------------------------ + // misalignment due to Alt axis being perp. to Azm axis + float PDh = -model.pdCor*(sinAx2/cosAx2)*p; + + // Fork or Axis flex + float DFd; + if (mountType == FORK || mountType == ALTAZM) DFd = model.dfCor*cosAx1; else DFd = -model.dfCor*(cosLat*cosAx1 + sinLat*(sinAx2/cosAx2)); + + // Tube flex + float TFh = model.tfCor*(cosLat*sinAx1*(1.0F/cosAx2)); + float TFd = model.tfCor*(cosLat*cosAx1*sinAx2 - sinLat*cosAx2); + + // polar misalignment + float ax1c = -model.azmCor*cosAx1*(sinAx2/cosAx2) + model.altCor*sinAx1*(sinAx2/cosAx2); + float ax2c = +model.azmCor*sinAx1 + model.altCor*cosAx1; + + // cos() linearization for main drive mechanism runout etc. + float COSh = cos(a1 + model.hcp)*model.hca*p; + float COSd = cos(a2 + model.dcp)*model.dca*p; + + // improved guess at instrument coordinate + a1 = ax1 + (ax1c + PDh + DOh + TFh + COSh); + a2 = ax2 + (ax2c + DFd + TFd + COSd); + } + } + + // finally, apply the index offsets + a1 = a1 - model.ax1Cor; + a2 = a2 - model.ax2Cor*-p; + + if (mountType == ALTAZM) { coord->z = a1; coord->a = a2; } else { + if (mountType == ALTALT) { coord->aa1 = a1; coord->aa2 = a2; } else { coord->h = a1; coord->d = a2; } +} + +void GeoAlign::mountToObservedPlace(Coordinate *coord) { + if (!modelIsReady) return; + + float p = 1.0F; + if (coord->pierSide == PIER_SIDE_WEST) p = -1.0F; + + float ax1, ax2; + if (mountType == ALTAZM) { + ax1 = coord->z; + ax2 = coord->a; + } else { + ax1 = coord->h; + ax2 = coord->d; + } + + ax1 = ax1 + model.ax1Cor; + ax2 = ax2 + model.ax2Cor*-p; + + if (ax2 > Deg90) ax2 = Deg90; + if (ax2 < -Deg90) ax2 = -Deg90; + + // breaks-down near the Zenith (limited to > 1' from Zenith) + if (fabs(ax2) < degToRadF(89.98333333F)) { + float sinAx2 = sinf(ax2); + float cosAx2 = cosf(ax2); + float sinAx1 = sinf(ax1); + float cosAx1 = cosf(ax1); + + // ------------------------------------------------------------ + // misalignment due to tube/optics not being perp. to Alt axis + // negative numbers are further (S) from the Zenith, swing to the + // horizon and the effect on Alt is 0. At the Nadir it + // becomes a (N) offset. Unchanged with meridian flips. + // expressed as a correction to the Azm axis misalignment + float DOh = model.doCor*(1.0F/cosAx2)*p; + + // as the above offset becomes zero near the horizon, the affect + // works on Azm instead. meridian flips affect this in Azm + float PDh = -model.pdCor*(sinAx2/cosAx2)*p; + + // Fork or Axis flex + float DFd; + if (mountType == FORK || mountType == ALTAZM) DFd = model.dfCor*cosAx1; else DFd = -model.dfCor*(cosLat*cosAx1 + sinLat*(sinAx2/cosAx2)); + + // Tube flex + float TFh = model.tfCor*(cosLat*sinAx1*(1.0F/cosAx2)); + float TFd = model.tfCor*(cosLat*cosAx1*sinAx2 - sinLat*cosAx2); + + // ------------------------------------------------------------ + // polar misalignment + float a1 = -model.azmCor*cosAx1*(sinAx2/cosAx2) + model.altCor*sinAx1*(sinAx2/cosAx2); + float a2 = +model.azmCor*sinAx1 + model.altCor*cosAx1; + + // cos() linearization for main drive mechanism runout etc. + float COSh = cos(a1 + model.hcp)*model.hca*p; + float COSd = cos(a2 + model.dcp)*model.dca*p; + + ax1 = ax1 - (a1 + PDh + DOh + TFh + COSh); + ax2 = ax2 - (a2 + DFd + TFd + COSd); + } + + if (ax2 > Deg90) ax2 = Deg90; + if (ax2 < -Deg90) ax2 = -Deg90; + + if (mountType == ALTAZM) { + while (ax1 > Deg360) ax1 -= Deg360; + while (ax1 < -Deg360) ax1 += Deg360; + coord->z = ax1; + coord->a = ax2; + } else + if (mountType == ALTALT) { + while (ax1 > Deg360) ax1 -= Deg360; + while (ax1 < -Deg360) ax1 += Deg360; + coord->aa1 = ax1; + coord->aa2 = ax2; + } else { + while (ax1 > Deg180) ax1 -= Deg360; + while (ax1 < -Deg180) ax1 += Deg360; + coord->h = ax1; + coord->d = ax2; + } +} + +#endif + +#endif diff --git a/src/telescope/mount/coordinates/Align.ref.h b/src/telescope/mount/coordinates/Align.ref.h new file mode 100755 index 0000000..d2d0f04 --- /dev/null +++ b/src/telescope/mount/coordinates/Align.ref.h @@ -0,0 +1,132 @@ +// ----------------------------------------------------------------------------------- +// GOTO ASSIST GEOMETRIC ALIGN FOR ALT/AZM AND EQ MOUNTS (REFERENCE DESIGN) +// +// by Howard Dutton +// +// Copyright (C) 2012 to 2024 Howard Dutton +// +#pragma once + +#include "../../../Common.h" + +#if defined(MOUNT_PRESENT) && !defined(HIGH_SPEED_ALIGN) + +#include "../../../libApp/commands/ProcessCmds.h" + +#if defined(ALIGN_MAX_STARS) && ALIGN_MAX_STARS != AUTO + #if (ALIGN_MAX_STARS < 3 || ALIGN_MAX_STARS > 9) && ALIGN_MAX_STARS != 1 + #error "ALIGN_MAX_STARS must be 1, or in the range of 3 to 9" + #endif + #define ALIGN_MAX_NUM_STARS ALIGN_MAX_STARS +#else + #if defined(HAL_FAST_PROCESSOR) + #define ALIGN_MAX_NUM_STARS 9 + #else + #define ALIGN_MAX_NUM_STARS 6 + #endif +#endif + +enum PierSide: uint8_t {PIER_SIDE_NONE, PIER_SIDE_EAST, PIER_SIDE_WEST}; + +typedef struct Coordinate { + double r; + double h; + double d; + double a; + double z; + double aa1; + double aa2; + double a1; + double a1Correction; + double a2; + PierSide pierSide; +} Coordinate; + +#if ALIGN_MAX_NUM_STARS > 1 + +// ----------------------------------------------------------------------------------- +// ADVANCED GEOMETRIC ALIGN FOR EQUATORIAL MOUNTS (GOTO ASSIST) + +typedef struct AlignCoordinate { + float ax1; + float ax2; + float h; + float d; + int side; +} AlignCoordinate; + +#define AlignModelSize 48 +typedef struct AlignModel { + float ax1Cor; + float ax2Cor; + float altCor; + float azmCor; + float doCor; + float pdCor; + float dfCor; + float tfCor; + float hcp; + float hca; + float dcp; + float dca; +} AlignModel; + +class GeoAlign +{ + public: + // prepare goto assist for operation, also clears the alignment model; + void init(int8_t mountType, float latitude); + + // reads the last saved alignment model from NV + void modelRead(); + // writes the alignment model to NV + void modelWrite(); + // clear the alignment model + void modelClear(); + // reports if ready for operation + bool modelReady(); + + // add a star to an alignment model + // thisStar: 1 for 1st star, 2 for 2nd star, etc. up to numberStars (at which point the mount model is calculated) + // numberStars: total number of stars for this align (1 to 9) + // actual: equatorial or horizon coordinate (depending on the mount type) for where the star should be (in mount coordinates) + // mount: equatorial or horizon coordinate (depending on the mount type) for where the star is (in mount coordinates) + CommandError addStar(int thisStar, int numberStars, Coordinate *actual, Coordinate *mount); + + void createModel(int numberStars); + + // convert equatorial (h,d) or horizon (a,z) coordinate from observed place to mount + void observedPlaceToMount(Coordinate *coord); + // convert equatorial (h,d) or horizon (a,z) coordinate from mount to observed place + void mountToObservedPlace(Coordinate *coord); + + void autoModel(int n); + + AlignCoordinate mount[ALIGN_MAX_NUM_STARS]; + AlignCoordinate actual[ALIGN_MAX_NUM_STARS]; + AlignCoordinate delta[ALIGN_MAX_NUM_STARS]; + AlignModel model; + + private: + void correct(float ha, float dec, float pierSide, float sf, float _deo, float _pd, float _pz, float _pe, float _da, float _ff, float _tf, float *h1, float *d1); + void doSearch(float sf, int p1, int p2, int p3, int p4, int p5, int p6, int p7, int p8, int p9); + + bool modelIsReady = false; + int8_t mountType; + float cosLat, sinLat; + + long num, l; + long Ff, Df; + float best_deo, best_pd, best_pz, best_pe, best_ohw, best_odw, best_ohe, best_ode, best_tf, best_df, best_ff; + float avg_ha, avg_dec; + float dist, sumd, rms; + float best_dist; + float ohe, ode, ohw, odw, dh; + float sum1; + float max_dist; + + uint8_t autoModelTask = 0; +}; + +#endif +#endif diff --git a/src/telescope/mount/coordinates/Transform.cpp b/src/telescope/mount/coordinates/Transform.cpp new file mode 100755 index 0000000..f90e04f --- /dev/null +++ b/src/telescope/mount/coordinates/Transform.cpp @@ -0,0 +1,310 @@ +//-------------------------------------------------------------------------------------------------- +// coordinate transformation + +#include "Transform.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/nv/Nv.h" + +#include "../../../libApp/weather/Weather.h" + +#include "../../Telescope.h" + +extern volatile unsigned long fracLAST; + +#define fsToRad(x) ((x)/(13750.98708313976*FRACTIONAL_SEC)) +#define radToFs(x) ((x)*(13750.98708313976*FRACTIONAL_SEC)) + +#if DEBUG != OFF + void Transform::print(Coordinate *coord) { + UNUSED(coord); + VF("(a="); V(radToDeg(coord->a)); VF(", z="); V(radToDeg(coord->z)); + VF("), (r="); V(radToDeg(coord->r)); VF(", h="); V(radToDeg(coord->h)); VF(", d="); V(radToDeg(coord->d)); + VF("), pierSide="); VL(coord->pierSide); + } +#endif + +void Transform::init(uint8_t mountType) { + this->mountType = mountType; + + #if DEBUG == VERBOSE + const char* MountTypeStr[5] = {"", "GEM", "FORK", "ALTAZM", "ALTALT"}; + VF("MSG: Mount, type "); VL(MountTypeStr[mountType]); + #endif + + if (mountType == GEM) meridianFlips = true; else meridianFlips = (MOUNT_ALTERNATE_ORIENTATION == ON); + + #if ALIGN_MAX_NUM_STARS > 1 + align.init(mountType, site.location.latitude); + #endif +} + +Coordinate Transform::mountToNative(Coordinate *coord, bool returnHorizonCoords) { + Coordinate result = *coord; + + #if MOUNT_COORDS == OBSERVED_PLACE + mountToObservedPlace(&result); + #elif MOUNT_COORDS == TOPOCENTRIC || MOUNT_COORDS == TOPO_STRICT + mountToTopocentric(&result); + #else + #error "Configuration (Config.h): MOUNT_COORDS, Unknown native mount coordinate system!" + #endif + + if (mountType == ALTAZM) horToEqu(&result); else + if (mountType == ALTALT) aaToEqu(&result); + + if (isEquatorial() && returnHorizonCoords) equToHor(&result); + + hourAngleToRightAscension(&result, true); + return result; +} + +void Transform::nativeToMount(Coordinate *coord, double *a1, double *a2) { + rightAscensionToHourAngle(coord, true); + + if (mountType == ALTAZM) equToHor(coord); else + if (mountType == ALTALT) equToAa(coord); + + #if MOUNT_COORDS == OBSERVED_PLACE + observedPlaceToMount(coord); + #elif MOUNT_COORDS == TOPOCENTRIC || MOUNT_COORDS == TOPO_STRICT + topocentricToMount(coord); + #else + #error "Configuration (Config.h): MOUNT_COORDS, Unknown native mount coordinate system!" + #endif + + if (a1 != NULL && a2 != NULL) { + if (mountType == ALTAZM) { *a1 = coord->z; *a2 = coord->a; } else + if (mountType == ALTALT) { *a1 = coord->aa1; *a2 = coord->aa2; } else + if (isEquatorial()) { *a1 = coord->h; *a2 = coord->d; } + } +} + +void Transform::mountToTopocentric(Coordinate *coord) { + mountToObservedPlace(coord); + observedPlaceToTopocentric(coord); +} + +void Transform::mountToObservedPlace(Coordinate *coord) { + #if ALIGN_MAX_NUM_STARS > 1 + if (coord->pierSide != PIER_SIDE_NONE) align.mountToObservedPlace(coord); + #else + (void)(*coord); + #endif +} + +void Transform::observedPlaceToTopocentric(Coordinate *coord) { + if (mountType != ALTAZM) { + // within about 1/20 arc-second of the "refracted" NCP or SCP + #if MOUNT_COORDS == TOPO_STRICT + if (fabs(coord->d - Deg90) < OneArcSec) { coord->z = 0.0; coord->a = site.latitude.value; } else + if (fabs(coord->d + Deg90) < OneArcSec) { coord->z = Deg180; coord->a = -site.latitude.value; } else equToHor(coord); + #else + if (fabs(coord->d - Deg90) < OneArcSec || fabs(coord->d + Deg90) < OneArcSec) return; else equToHor(coord); + #endif + coord->a -= apparentRefrac(coord->a); + horToEqu(coord); + } else coord->a -= apparentRefrac(coord->a); +} + +void Transform::topocentricToMount(Coordinate *coord) { + topocentricToObservedPlace(coord); + observedPlaceToMount(coord); +} + +void Transform::observedPlaceToMount(Coordinate *coord) { + #if ALIGN_MAX_NUM_STARS > 1 + if (coord->pierSide != PIER_SIDE_NONE) align.observedPlaceToMount(coord); + #else + (void)(*coord); + #endif + + if (mountType == ALTAZM) { + coord->a1 = coord->z; + coord->a2 = coord->a; + } else { + coord->a1 = coord->h; + coord->a2 = coord->d; + } +} + +void Transform::topocentricToObservedPlace(Coordinate *coord) { + if (mountType != ALTAZM) { + // within about 1/20 arc-second of NCP or SCP + #if MOUNT_COORDS == TOPO_STRICT + if (fabs(coord->d - Deg90) < OneArcSec) { coord->z = 0.0; coord->a = site.latitude.value; } else + if (fabs(coord->d + Deg90) < OneArcSec) { coord->z = Deg180; coord->a = -site.latitude.value; } else equToHor(coord); + #else + if (fabs(coord->d - Deg90) < OneArcSec || fabs(coord->d + Deg90) < OneArcSec) return; else equToHor(coord); + #endif + coord->a += trueRefrac(coord->a); + horToEqu(coord); + } else coord->a += trueRefrac(coord->a); +} + +Coordinate Transform::instrumentToMount(double a1, double a2) { + Coordinate mount = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, PIER_SIDE_NONE}; + + #if AXIS2_TANGENT_ARM_CORRECTION == ON + a2 = TANGENT_ARM_INSTRUMENT_TO_MOUNT(a2) + #endif + + if (site.location.latitude >= 0.0 || mountType == ALTAZM) { + if (a2 > Deg90) { + mount.pierSide = PIER_SIDE_WEST; + a1 -= Deg180; + a2 = Deg180 - a2; + } else mount.pierSide = PIER_SIDE_EAST; + } else { + if (a2 < -Deg90) { + mount.pierSide = PIER_SIDE_WEST; + a1 -= Deg180; + a2 = (-Deg180) - a2; + } else mount.pierSide = PIER_SIDE_EAST; + } + + if (mountType == ALTAZM) { mount.z = a1; mount.a = a2; } else + if (mountType == ALTALT) { mount.aa1 = a1; mount.aa2 = a2; } else + if (isEquatorial()) { mount.h = a1; mount.d = a2; } + + mount.a1 = a1; + mount.a2 = a2; + + mount.a1Correction = 0.0; + + return mount; +} + +void Transform::mountToInstrument(Coordinate *coord, double *a1, double *a2) { + if (mountType == ALTAZM) { *a1 = coord->z; *a2 = coord->a; } else + if (mountType == ALTALT) { *a1 = coord->aa1; *a2 = coord->aa2; } else + if (isEquatorial()) { *a1 = coord->h; *a2 = coord->d; } + + if (site.location.latitude >= 0.0 || !isEquatorial()) { + if (coord->pierSide == PIER_SIDE_WEST) { + *a1 += Deg180; + *a2 = Deg180 - *a2; + } + } else { + if (coord->pierSide == PIER_SIDE_WEST) { + *a1 += Deg180; + *a2 = (-Deg180) - *a2; + } + } + + #if AXIS2_TANGENT_ARM_CORRECTION == ON + *a2 = TANGENT_ARM_MOUNT_TO_INSTRUMENT(*a2) + #endif +} + +void Transform::hourAngleToRightAscension(Coordinate *coord, bool native) { + noInterrupts(); + unsigned long fs = fracLAST; + interrupts(); + coord->r = fsToRad(fs) - coord->h; + if (native) coord->r = backInRads(coord->r); +} + +void Transform::rightAscensionToHourAngle(Coordinate *coord, bool native) { + if (isnan(coord->r)) return; // NAN flags mount coordinates + noInterrupts(); + unsigned long fs = fracLAST; + interrupts(); + coord->h = fsToRad(fs) - coord->r; + if (native) coord->h = backInRads2(coord->h); +} + +void Transform::equToHor(Coordinate *coord) { + double cosHA = cos(coord->h); + double sinAlt = sin(coord->d)*site.locationEx.latitude.sine + cos(coord->d)*site.locationEx.latitude.cosine*cosHA; + coord->a = asin(sinAlt); + double t1 = sin(coord->h); + double t2 = cosHA*site.locationEx.latitude.sine - tan(coord->d)*site.locationEx.latitude.cosine; + // handle degenerate coordinates near the poles + if (fabs(coord->d - Deg90) < TenthArcSec) coord->z = 0.0; else + if (fabs(coord->d + Deg90) < TenthArcSec) coord->z = Deg180; else { + coord->z = atan2(t1, t2); + coord->z += Deg180; + } + if (coord->z > Deg180) coord->z -= Deg360; +} + +void Transform::equToAlt(Coordinate *coord) { + double cosHA = cos(coord->h); + double sinAlt = sin(coord->d)*site.locationEx.latitude.sine + cos(coord->d)*site.locationEx.latitude.cosine*cosHA; + coord->a = asin(sinAlt); +} + +void Transform::horToEqu(Coordinate *coord) { + double cosAzm = cos(coord->z); + double sinDec = sin(coord->a)*site.locationEx.latitude.sine + cos(coord->a)*site.locationEx.latitude.cosine*cosAzm; + coord->d = asin(sinDec); + double t1 = sin(coord->z); + double t2 = cosAzm*site.locationEx.latitude.sine - tan(coord->a)*site.locationEx.latitude.cosine; + coord->h = atan2(t1, t2); + coord->h += Deg180; + if (coord->h > Deg180) coord->h -= Deg360; +} + +void Transform::aaToHor(Coordinate *coord) { + double cosAA1 = cos(coord->aa1); + double sinAlt = cos(coord->aa2)*cosAA1; + coord->a = asin(sinAlt); + double t1 = sin(coord->aa1); + double t2 = -tan(coord->aa2); + // handle degenerate coordinates near the poles + if (fabs(coord->aa2 - Deg90) < TenthArcSec) coord->z = 0.0; else + if (fabs(coord->aa2 + Deg90) < TenthArcSec) coord->z = Deg180; else { + coord->z = atan2(t1, t2); + coord->z += Deg180; + } + if (coord->z > Deg180) coord->z -= Deg360; +} + +void Transform::horToAa(Coordinate *coord) { + double cosAzm = cos(coord->z); + double sinAA2 = cos(coord->a)*cosAzm; + coord->aa2 = asin(sinAA2); + double t1 = sin(coord->z); + double t2 = -tan(coord->a); + coord->aa1 = atan2(t1, t2); + coord->aa1 += Deg180; + if (coord->aa1 > Deg180) coord->aa1 -= Deg360; +} + +double Transform::trueRefrac(double altitude) { + float pressure = 1010.0F; + float temperature = 10.0F; + if (!isnan(weather.getPressure())) pressure = weather.getPressure(); + if (!isnan(weather.getTemperature())) temperature = weather.getTemperature(); + float TPC = (pressure/1010.0F)*(283.0F/(273.0F + temperature)); + float r = 2.9670597e-4F*cotf(altitude + 0.0031375594F/(altitude + 0.089186324F))*TPC; + if (r < 0.0F) r = 0.0F; + return r; +} + +double Transform::apparentRefrac(double altitude) { + double r = trueRefrac(altitude); + return trueRefrac(altitude - r); +} + +float Transform::cotf(float n) { + return 1.0F/tanf(n); +} + +double Transform::backInRads(double angle) { + while (angle >= Deg360) angle -= Deg360; + while (angle < 0.0) angle += Deg360; + return angle; +} + +double Transform::backInRads2(double angle) { + while (angle >= Deg180) angle -= Deg360; + while (angle < -Deg180) angle += Deg360; + return angle; +} + +Transform transform; + +#endif \ No newline at end of file diff --git a/src/telescope/mount/coordinates/Transform.h b/src/telescope/mount/coordinates/Transform.h new file mode 100755 index 0000000..198dc60 --- /dev/null +++ b/src/telescope/mount/coordinates/Transform.h @@ -0,0 +1,114 @@ +//-------------------------------------------------------------------------------------------------- +// coordinate transformation +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +#include +#include "../site/Site.h" +#include "Align.ref.h" +#include "Align.hs.h" + +// MOTOR <--> apply index offset and backlash <--> INSTRUMENT (Axis) +// INSTRUMENT <--> apply celestial coordinate conventions <--> MOUNT (Transform) +// MOUNT <--> apply pointing model <--> OBSERVED (Transform) +// OBSERVED <--> apply refraction <--> TOPOCENTRIC (Transform) + +// CR_MOUNT for Horizon or Equatorial mount coordinates, depending on the mount type +// CR_MOUNT_EQU for Equatorial mount coordinates +// CR_MOUNT_ALT for altitude (a) and Horizon or Equatorial mount coordinates +// CR_MOUNT_HOR for Horizon mount coordinates +// CR_MOUNT_ALL for both Equatorial and Horizon mount coordinates +enum CoordReturn: uint8_t {CR_MOUNT, CR_MOUNT_EQU, CR_MOUNT_ALT, CR_MOUNT_HOR, CR_MOUNT_ALL}; + +class Transform { + public: + // setup for coordinate transformation + void init(uint8_t mountType); + + #if DEBUG != OFF + // prints a coordinate to the debug serial port + void print(Coordinate *coord); + #endif + + // converts from Mount (equatorial or horizon) to Native (equatorial and optionally horizon also) coordinates + // returns equatorial RA + Coordinate mountToNative(Coordinate *coord, bool returnHorizonCoords = false); + // converts from Native (equatorial) to Mount (equatorial or horizon) coordinates (optional: a1 is h or z, a2 is d or a) + // accepts equatorial RA + void nativeToMount(Coordinate *coord, double *a1 = NULL, double *a2 = NULL); + + // converts from Mount to Topocentric coordinates (removes pointing model and refraction from equatorial coordinates) + void mountToTopocentric(Coordinate *coord); + // converts from Topocentric to Mount coordinates (adds pointing model and refraction to equatorial coordinates) + void topocentricToMount(Coordinate *coord); + + // converts from Mount to Observed coordinates (removes pointing model from coordinates) + void mountToObservedPlace(Coordinate *coord); + // converts from Observed to Mount coordinates (adds pointing model to coordinates) + void observedPlaceToMount(Coordinate *coord); + + // converts from Instrument (angular) to Mount (equatorial or horizon) coordinates + Coordinate instrumentToMount(double a1, double a2); + // converts from Mount (equatorial or horizon) to Instrument (angular) coordinates + void mountToInstrument(Coordinate *coord, double *a1, double *a2); + + // converts from Topocentric to Observed coordinates (removes refraction effects from equatorial coordinates) + void topocentricToObservedPlace(Coordinate *coord); + // converts from Observed to Topocentric coordinates (adds refraction effects to equatorial coordinates) + void observedPlaceToTopocentric(Coordinate *coord); + + // converts from Hour Angle (h) to Right Ascension (r) coordinates + void hourAngleToRightAscension(Coordinate *coord, bool native); + // converts from Right Ascension (r) to Hour Angle (h) coordinates + // set RA to NAN to use the Hour Angle coordinate as (h) + void rightAscensionToHourAngle(Coordinate *coord, bool native); + + // converts from Equatorial (h,d) to Horizon (a,z) coordinates + void equToHor(Coordinate *coord); + // converts from Equatorial (h,d) to Horizon (a) altitude coordinate + void equToAlt(Coordinate *coord); + // converts from Horizon (a,z) to Equatorial (h,d) coordinates + void horToEqu(Coordinate *coord); + + // converts from AltAlt (aa1,aa2) to Horizon (a,z) coordinates + void aaToHor(Coordinate *coord); + // converts from Horizon (a,z) to AltAlt (aa1,aa2) coordinates + void horToAa(Coordinate *coord); + // converts from AltAlt (aa1,aa2) to Equatorial (h,d) coordinates + void aaToEqu(Coordinate *coord) { aaToHor(coord); horToEqu(coord); }; + // converts from equatorial (h,d) to AltAlt (aa1,aa2) coordinates + void equToAa(Coordinate *coord) { equToHor(coord); horToAa(coord); }; + + // refraction at altitude, pressure (millibars), and temperature (celsius) + // returns amount of refraction at the true altitude + double trueRefrac(double altitude); + // refraction at altitude, pressure (millibars), and temperature (celsius) + // returns the amount of refraction at the apparent altitude + double apparentRefrac(double altitude); + + // flag if this mount is equatorial or not + bool isEquatorial() { return mountType == GEM || mountType == FORK; }; + + #if ALIGN_MAX_NUM_STARS > 1 + GeoAlign align; + #endif + + int8_t mountType = MOUNT_SUBTYPE; + bool meridianFlips; + + private: + + float cotf(float n); + + // adjust coordinate back into 0 to 360 "degrees" range (in radians) + double backInRads(double angle); + // adjust coordinate back into -180 to 180 "degrees" range (in radians) + double backInRads2(double angle); +}; + +extern Transform transform; + +#endif diff --git a/src/telescope/mount/coordinates/coordinates.ino b/src/telescope/mount/coordinates/coordinates.ino new file mode 100755 index 0000000..0f91199 --- /dev/null +++ b/src/telescope/mount/coordinates/coordinates.ino @@ -0,0 +1,4 @@ +// Placeholder file +// Nothing to see here ... +// +// This file is only present so the Arduino IDE can edit the .h file(s) diff --git a/src/telescope/mount/goto/Goto.command.cpp b/src/telescope/mount/goto/Goto.command.cpp new file mode 100755 index 0000000..c4ecb7b --- /dev/null +++ b/src/telescope/mount/goto/Goto.command.cpp @@ -0,0 +1,583 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, commands + +#include "Goto.h" + +#if defined(MOUNT_PRESENT) + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../site/Site.h" +#include "../Mount.h" +#include "../guide/Guide.h" +#include "../home/Home.h" +#include "../limits/Limits.h" +#include "../status/Status.h" + +bool Goto::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + PrecisionMode precisionMode = PM_HIGH; + + if (command[0] == 'A') { + // :AW# Align Write to EEPROM + // Returns: 1 on success + if (command[1] == 'W' && parameter[0] == 0) { + #if ALIGN_MAX_NUM_STARS > 1 + transform.align.modelWrite(); + #endif + } else + + // :A?# Align status + // Returns: mno# + // where m is the maximum number of alignment stars + // n is the current alignment star (0 otherwise) note: this is always 1 greater than the number + // of alignment stars accepted and so can range from '0' to ':' (':' = '0' + 10) + // o is the last required alignment star when an alignment is in progress (0 otherwise) + if (command[1] == '?' && parameter[0] == 0) { + reply[0] = '0' + ALIGN_MAX_NUM_STARS; + reply[1] = '0' + alignState.currentStar; + reply[2] = '0' + alignState.lastStar; + reply[3] = 0; + *numericReply = false; + } else + + // :A[n]# Start Telescope Manual Alignment Sequence + // This is to initiate a n-star alignment for 1..MAX_NUM_ALIGN_STARS: + // 1) Before calling this function, the telescope should be in the polar-home position + // 2) Call this function with the # of align stars you'd like to use + // 3) Set the target location (RA/Dec) to a bright star, etc. (not too near the NCP/SCP) + // 4) Issue a goto command + // 5) Center the star/object using the guide commands (as needed) + // 6) Call :A+# command to accept the correction + // ( for two+ star alignment ) + // 7) Back to #3 above until done, except where possible choose at least one star on both meridian sides + // Return: 0 on failure + // 1 on success + if (command[1] >= '1' && command[1] <= ALIGN_MAX_NUM_STARS + '0' && parameter[0] == 0) { + // set current time and date before calling this routine + #if ALIGN_AUTO_HOME == ON && GOTO_FEATURE == ON + home.requestWithReset(); + #else + home.reset(); + #endif + mount.tracking(true); + + // start align... + alignState.lastStar = command[1] - '0'; + alignState.currentStar = 1; + VLF("MSG: Mount, align requested"); + } else + + // :A+# Align accept target location + // Return: 0 on failure + // 1 on success + if (command[1] == '+' && parameter[0] == 0) { + if (alignActive()) { + CommandError e = alignAddStar(); + if (e != CE_NONE) { + alignState.lastStar = 0; + alignState.currentStar = 0; + *commandError = e; + DLF("ERR: Mount, failed to add align point"); + } else { VLF("MSG: Mount, align point added"); } + } else *commandError = CE_ALIGN_NOT_ACTIVE; + } else *commandError = CE_CMD_UNKNOWN; + } else + + // C - Sync Control + // :CS# Synchonize the telescope with the current right ascension and declination coordinates + // Returns: Nothing (Sync's fail silently) + // :CM# Synchonize the telescope with the current database object (as above) + // Returns: "N/A#" on success, "En#" on failure where n is the error code per the :MS# command + if (command[0] == 'C' && (command[1] == 'S' || command[1] == 'M') && parameter[0] == 0) { + CommandError e; + + if (alignActive()) { + e = alignAddStar(true); + if (e != CE_NONE) { + alignState.lastStar = 0; + alignState.currentStar = 0; + *commandError = e; + DLF("ERR: Mount, failed to add align point"); + } else { VLF("MSG: Mount, align point added"); } + } else { + Coordinate before = mount.getMountPosition(CR_MOUNT); + PierSideSelect pps = settings.preferredPierSide; + if (!mount.isHome() && PIER_SIDE_SYNC_CHANGE_SIDES == OFF) pps = PSS_SAME_ONLY; + e = requestSync(gotoTarget, pps); + Coordinate after = mount.getMountPosition(CR_MOUNT); + if (before.pierSide != PIER_SIDE_NONE && before.pierSide != after.pierSide) mountStatus.soundAlert(); + } + + if (command[1] == 'M') { + if (e == CE_NONE) strcpy(reply, "N/A"); else + if (e >= CE_SLEW_ERR_BELOW_HORIZON && e <= CE_SLEW_ERR_UNSPECIFIED) { + strcpy(reply, "E0"); + reply[1] = (char)(e - CE_SLEW_ERR_BELOW_HORIZON) + '1'; + } else { + DF("ERR: Mount, sync unspecified error occured ("); D(e); DLF(")"); + strcpy(reply, "E9"); + } + } + *numericReply = false; + } else + + // D - Distance Bars + // :D# Return: "\0x7f#" if the mount is moving, otherwise "#". + if (command[0] == 'D' && command[1] == 0) { + if (goTo.state == GS_GOTO) { + reply[0] = (char)127; + reply[1] = 0; + } else { + reply[0] = '#'; + reply[1] = 0; + *suppressFrame = true; + } + *numericReply = false; + } else + + if (command[0] == 'G') { + // :Gr# Get target Right Ascension + // Returns: HH:MM.T# or HH:MM:SS (based on precision setting) + // :GrH# Returns: HH:MM:SS.SSSS# (high precision) + if (command[1] == 'r' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H' || parameter[0] == 'a') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + convert.doubleToHms(reply, radToHrs(gotoTarget.r), false, precisionMode); + *numericReply = false; + } else + + // :Gd# Get target Declination + // Returns: HH:MM.T# or HH:MM:SS (based on precision setting) + // :GdH# High precision + // Returns: HH:MM:SS.SSS# (high precision) + if (command[1] == 'd' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H' || parameter[0] == 'e') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + convert.doubleToDms(reply, radToDeg(gotoTarget.d), false, true, precisionMode); + *numericReply = false; + } else + + // :Gal# Get target Altitude + // Returns: sDD*MM# or sDD*MM'SS# (based on precision setting) + // :GaH# High precision + // Returns: sDD*MM'SS.SSS# (high precision) + if (command[1] == 'a' && (parameter[0] == 'H' || parameter[0] == 'l') && parameter[1] == 0) { + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; + convert.doubleToHms(reply, radToDeg(gotoTarget.a), false, precisionMode); + *numericReply = false; + } else + + // :Gz# Get target Azimuth + // Returns: DDD*MM# or DDD*MM'SS# (based on precision setting) + // :GzH# High precision + // Returns: DDD*MM'SS.SSS# (high precision) + if (command[1] == 'z' ) { + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + convert.doubleToDms(reply, radToDeg(gotoTarget.z), true, false, precisionMode); + *numericReply = false; + } else + + if (command[1] == 'X' && parameter[2] == 0) { + #if ALIGN_MAX_NUM_STARS > 1 + // :GX0[n]# Get align setting [n] + // Returns: Value + if (parameter[0] == '0') { + static int star = 0; + *numericReply = false; + switch (parameter[1]) { + case '0': sprintf(reply, "%ld",(long)round((radToArcsec(transform.align.model.ax1Cor)))); break; // ax1Cor + case '1': sprintf(reply, "%ld",(long)round((radToArcsec(transform.align.model.ax2Cor)))); break; // ax2Cor + case '2': sprintf(reply, "%ld",(long)round((radToArcsec(transform.align.model.altCor)))); break; // altCor + case '3': sprintf(reply, "%ld",(long)round((radToArcsec(transform.align.model.azmCor)))); break; // azmCor + case '4': sprintf(reply, "%ld",(long)round((radToArcsec(transform.align.model.doCor)))); break; // doCor + case '5': sprintf(reply, "%ld",(long)round((radToArcsec(transform.align.model.pdCor)))); break; // pdCor + case '6': if (transform.mountType == FORK || transform.mountType == ALTAZM) // ffCor + sprintf(reply, "%ld",(long)(round(radToArcsec(transform.align.model.dfCor)))); else sprintf(reply, "%ld",(long)(0)); + break; + case '7': if (transform.mountType != FORK && transform.mountType != ALTAZM) // dfCor + sprintf(reply, "%ld",(long)(round(radToArcsec(transform.align.model.dfCor)))); else sprintf(reply, "%ld",(long)(0)); + break; + case '8': sprintf(reply, "%ld",(long)(round(radToArcsec(transform.align.model.tfCor)))); break; // tfCor + case 'a': sprintf(reply, "%ld",(long)(round(radToDeg(transform.align.model.hcp)))); break; // hcp + case 'b': sprintf(reply, "%ld",(long)(round(radToArcsec(transform.align.model.hca)))); break; // hca + case 'c': sprintf(reply, "%ld",(long)(round(radToDeg(transform.align.model.dcp)))); break; // dcp + case 'd': sprintf(reply, "%ld",(long)(round(radToArcsec(transform.align.model.dca)))); break; // dca + // number of stars, reset to first star + case '9': { int n = 0; if (alignState.currentStar > alignState.lastStar) n = alignState.lastStar; sprintf(reply, "%ld",(long)(n)); star = 0; } break; + case 'A': { convert.doubleToHms(reply,radToHrs(transform.align.actual[star].h),true,PM_HIGH); } break; + case 'B': { convert.doubleToDms(reply,radToDeg(transform.align.actual[star].d),false,true,PM_HIGH); } break; + case 'C': { convert.doubleToHms(reply,radToHrs(transform.align.mount[star].h),true,PM_HIGH); } break; + case 'D': { convert.doubleToDms(reply,radToDeg(transform.align.mount[star].d),false,true,PM_HIGH); } break; + // pier side (and increment n) + case 'E': sprintf(reply, "%ld",(long)(transform.align.mount[star].side)); star++; break; + default: *numericReply = true; *commandError = CE_CMD_UNKNOWN; + } + } else + #endif + + // :GX9[n]# Get setting [n] + // Returns: Value + if (parameter[0] == '9') { + switch (parameter[1]) { + // current rate in us/step + case '2': sprintF(reply, "%0.3f", settings.usPerStepCurrent); break; + // default base rate in us/step + case '3': sprintF(reply, "%0.3f", usPerStepBase); break; + // pierSide 0 = None, 1 = East, 2 = West (with suffix 'N' if meridian flips are disabled) + case '4': sprintf(reply, "%d%s", (int)mount.getMountPosition().pierSide, (!transform.meridianFlips)?" N":""); break; + // autoMeridianFlip + case '5': sprintf(reply, "%d", (int)isAutoFlipEnabled()); break; + // preferred pier side + case '6': reply[0] = transform.meridianFlips ? "EWBA"[settings.preferredPierSide - 1] : 'E'; reply[1] = 0; break; + // current step rate in deg/s + case '7': sprintF(reply, "%0.1f", (1000000.0F/settings.usPerStepCurrent)/degToRadF(axis1.getStepsPerMeasure())); break; + // fastest step rate in us/step + case '9': sprintF(reply, "%0.3f",usPerStepLowerLimit()); break; + default: return false; + } + *numericReply = false; + } else return false; + } else return false; + } else + + if (command[0] == 'M') { + // :MA# Goto the target Alt and Az + // Returns: 0..9, see :MS# + if (command[1] == 'A' && parameter[0] == 0) { + transform.horToEqu(&gotoTarget); + transform.hourAngleToRightAscension(&gotoTarget, true); + CommandError e = request(gotoTarget, settings.preferredPierSide); + strcpy(reply,"0"); + if (e >= CE_SLEW_ERR_BELOW_HORIZON && e <= CE_SLEW_ERR_UNSPECIFIED) reply[0] = (char)(e - CE_SLEW_ERR_BELOW_HORIZON) + '1'; + if (e == CE_NONE) reply[0] = '0'; + *numericReply = false; + *suppressFrame = true; + *commandError = e; + } else + + // :MD# Goto Destination pier side for the Target Object + // Returns: + // 0=destination is East of the pier + // 1=destination is West of the pier + // 2=destination is Unknown + if (command[1] == 'D' && parameter[0] == 0) { + Coordinate coords = gotoTarget; + coords.pierSide = PIER_SIDE_NONE; + transform.nativeToMount(&coords); + CommandError e = setTarget(&coords, settings.preferredPierSide); + strcpy(reply, "2"); + if (e == CE_NONE && target.pierSide == PIER_SIDE_EAST) reply[0] = '0'; + if (e == CE_NONE && target.pierSide == PIER_SIDE_WEST) reply[0] = '1'; + *numericReply = false; + *suppressFrame = true; + *commandError = e; + } else + + // :MN# Goto current RA/Dec but opposite Pier side (within meridian limit overlap) + // :MNe# Goto current RA/Dec but East of the Pier (within meridian limit overlap) + // :MNw# Goto current RA/Dec but West of the Pier (within meridian limit overlap) + // Returns: 0..9, see :MS# + if (command[1] == 'N') { + if (transform.meridianFlips) { + Coordinate newTarget = mount.getPosition(); + CommandError e = CE_NONE; + + if (parameter[0] == 0) { + parameter[1] = 0; + if (newTarget.pierSide == PIER_SIDE_EAST) parameter[0] = 'w'; else + if (newTarget.pierSide == PIER_SIDE_WEST) parameter[0] = 'e'; else e = CE_SLEW_ERR_UNSPECIFIED; + } + + if (e != CE_SLEW_ERR_UNSPECIFIED) { + if (parameter[0] == 'e' && parameter[1] == 0) e = request(newTarget, PSS_EAST_ONLY); else + if (parameter[0] == 'w' && parameter[1] == 0) e = request(newTarget, PSS_WEST_ONLY); else e = CE_CMD_UNKNOWN; + } + + if (e != CE_CMD_UNKNOWN) { + if (e >= CE_SLEW_ERR_BELOW_HORIZON && e <= CE_SLEW_ERR_UNSPECIFIED) reply[0] = (char)(e - CE_SLEW_ERR_BELOW_HORIZON) + '1'; + if (e == CE_NONE) reply[0] = '0'; + reply[1] = 0; + *numericReply = false; + *suppressFrame = true; + } + *commandError = e; + } else *commandError = CE_CMD_UNKNOWN; + } else + + // :MP# Goto the Current Position for Polar Align + // Returns: 0..9, see :MS# + if (command[1] == 'P' && parameter[0] == 0) { + if (transform.isEquatorial()) { + Coordinate newTarget = mount.getPosition(); + CommandError e = validate(); + if (e == CE_NONE) e = limits.validateTarget(&newTarget, true); + if (e == CE_NONE) { + #if ALIGN_MAX_NUM_STARS > 1 + transform.align.model.altCor = 0.0; + transform.align.model.azmCor = 0.0; + #endif + e = request(newTarget, PSS_SAME_ONLY); + } + if (e >= CE_SLEW_ERR_BELOW_HORIZON && e <= CE_SLEW_ERR_UNSPECIFIED) reply[0] = (char)(e - CE_SLEW_ERR_BELOW_HORIZON) + '1'; + if (e == CE_NONE) reply[0] = '0'; + reply[1] = 0; + *numericReply = false; + *suppressFrame = true; + *commandError = e; + } else *commandError = CE_CMD_UNKNOWN; + } else + + // :MS# Goto the Target Object + // Returns: + // 0=goto is possible + // 1=below the horizon limit + // 2=above overhead limit + // 3=controller in standby + // 4=mount is parked + // 5=goto in progress + // 6=outside limits + // 7=hardware fault + // 8=already in motion + // 9=unspecified error + if (command[1] == 'S' && parameter[0] == 0) { + CommandError e = request(gotoTarget, settings.preferredPierSide); + strcpy(reply,"0"); + if (e >= CE_SLEW_ERR_BELOW_HORIZON && e <= CE_SLEW_ERR_UNSPECIFIED) reply[0] = (char)(e - CE_SLEW_ERR_BELOW_HORIZON) + '1'; + if (e == CE_NONE) reply[0] = '0'; + *numericReply = false; + *suppressFrame = true; + *commandError = e; + } else return false; + } else + + if (command[0] == 'S') { + // :Sr[HH:MM.T]# or :Sr[HH:MM:SS]# or :Sr[HH:MM:SS.SSSS]# + // Set Target Right Ascension + // Return: 0 on failure + // 1 on success + if (command[1] == 'r') { + if (!convert.hmsToDouble(&gotoTarget.r, parameter)) *commandError = CE_PARAM_RANGE; else + gotoTarget.r = hrsToRad(gotoTarget.r); + } else + + // :Sd[sDD*MM]# or :Sd[sDD*MM:SS]# or :Sd[sDD*MM:SS.SSS]# + // Set Target Declination + // Return: 0 on failure + // 1 on success + if (command[1] == 'd') { + if (!convert.dmsToDouble(&gotoTarget.d, parameter, true)) *commandError = CE_PARAM_RANGE; else + gotoTarget.d = degToRad(gotoTarget.d); + } else + + // :Sa[sDD*MM]# or :Sa[sDD*MM'SS]# or :Sa[sDD*MM'SS.SSS]# + // Set Target Altitude + // Return: 0 on failure + // 1 on success + if (command[1] == 'a') { + if (!convert.dmsToDouble(&gotoTarget.a, parameter, true)) *commandError = CE_PARAM_RANGE; else + gotoTarget.a = degToRad(gotoTarget.a); + } else + + // :Sz[DDD*MM]# or :Sz[DDD*MM'SS]# or :Sz[DDD*MM'SS.SSS]# + // Set Target Azmuith + // Return: 0 on failure + // 1 on success + if (command[1] == 'z') { + if (!convert.dmsToDouble(&gotoTarget.z, parameter, false)) *commandError = CE_PARAM_RANGE; else + gotoTarget.z = degToRad(gotoTarget.z); + } else + + if (command[1] == 'X') { + if (parameter[2] != ',') { *commandError = CE_PARAM_FORM; return true; } + + #if ALIGN_MAX_NUM_STARS > 1 + // :SX0[m],[n]# Set align model coefficient [m] to value [n] + // Return: 0 on failure + // 1 on success + if (parameter[0] == '0') { + static int star; + double d; + switch (parameter[1]) { + case '0': transform.align.model.ax1Cor = arcsecToRad(atol(¶meter[3])); break; // ax1Cor + case '1': transform.align.model.ax2Cor = arcsecToRad(atol(¶meter[3])); break; // ax2Cor + case '2': transform.align.model.altCor = arcsecToRad(atol(¶meter[3])); break; // altCor + case '3': transform.align.model.azmCor = arcsecToRad(atol(¶meter[3])); break; // azmCor + case '4': transform.align.model.doCor = arcsecToRad(atol(¶meter[3])); break; // doCor + case '5': transform.align.model.pdCor = arcsecToRad(atol(¶meter[3])); break; // pdCor + case '6': + if (transform.mountType == FORK || transform.mountType == ALTAZM) // fdCor or ffCor + transform.align.model.dfCor = arcsecToRad(atol(¶meter[3])); + break; + case '7': + if (transform.mountType != FORK && transform.mountType != ALTAZM) // fdCor or ffCor + transform.align.model.dfCor = arcsecToRad(atol(¶meter[3])); + break; + case '8': transform.align.model.tfCor = arcsecToRad(atol(¶meter[3])); break; // tfCor + case 'a': transform.align.model.hcp = degToRad(atol(¶meter[3])); break; // hcp + case 'b': transform.align.model.hca = arcsecToRad(atol(¶meter[3])); break; // hca + case 'c': transform.align.model.dcp = degToRad(atol(¶meter[3])); break; // dcp + case 'd': transform.align.model.dca = arcsecToRad(atol(¶meter[3])); break; // dca + // use :SX09,0# to start upload of stars for align, when done use :SX09,1# to calculate the pointing model, or :SX09,2# to force model activation + case '9': { + int n = atol(¶meter[3]); + if (n == 0) { + star = 0; + alignReset(); + } else + if (n == 1) { + if (star >= 1) { + alignState.lastStar = star; + alignState.currentStar = star + 1; + transform.align.createModel(star); + } + } else + if (n == 2) { + transform.align.modelIsReady = true; + } else *commandError = CE_PARAM_RANGE; + } + break; + // Actual HA (n) + case 'A': if (!convert.hmsToDouble(&d, ¶meter[3], PM_HIGH)) *commandError = CE_PARAM_FORM; else transform.align.actual[star].h = hrsToRad(d); break; + // Actual Dec (n) + case 'B': if (!convert.dmsToDouble(&d, ¶meter[3], true, PM_HIGH)) *commandError = CE_PARAM_FORM; else transform.align.actual[star].d = degToRad(d); break; + // Mount HA (n) + case 'C': if (!convert.hmsToDouble(&d, ¶meter[3], PM_HIGH)) *commandError = CE_PARAM_FORM; else transform.align.mount[star].h = hrsToRad(d); break; + // Mount Dec (n) + case 'D': if (!convert.dmsToDouble(&d, ¶meter[3], true, PM_HIGH)) *commandError = CE_PARAM_FORM; else transform.align.mount[star].d = degToRad(d); break; + // Mount PierSide (and increment n) + case 'E': + if (star <= 8) { + transform.align.actual[star].side = transform.align.mount[star].side = atol(¶meter[3]); + if (transform.mountType == ALTAZM) { + Coordinate temp; + temp.h = transform.align.actual[star].h; + temp.d = transform.align.actual[star].d; + transform.equToHor(&temp); + transform.align.actual[star].ax1 = temp.z; + transform.align.actual[star].ax2 = temp.a; + temp.h = transform.align.mount[star].h; + temp.d = transform.align.mount[star].d; + transform.equToHor(&temp); + transform.align.mount[star].ax1 = temp.z; + transform.align.mount[star].ax2 = temp.a; + } else + if (transform.mountType == ALTALT) { + Coordinate temp; + temp.h = transform.align.actual[star].h; + temp.d = transform.align.actual[star].d; + transform.equToAa(&temp); + transform.align.actual[star].ax1 = temp.aa1; + transform.align.actual[star].ax2 = temp.aa2; + temp.h = transform.align.mount[star].h; + temp.d = transform.align.mount[star].d; + transform.equToAa(&temp); + transform.align.mount[star].ax1 = temp.aa1; + transform.align.mount[star].ax2 = temp.aa2; + } else { + transform.align.actual[star].ax1 = transform.align.actual[star].h; + transform.align.actual[star].ax2 = transform.align.actual[star].d; + transform.align.mount[star].ax1 = transform.align.mount[star].h; + transform.align.mount[star].ax2 = transform.align.mount[star].d; + } + star++; + } + break; + default: *commandError = CE_CMD_UNKNOWN; + } + } else + #endif + + // :SX9[m],[n]# Set mount misc. + // Return: see below + if (parameter[0] == '9') { + switch (parameter[1]) { + // set new slew rate (returns 1 success or 0 failure) + case '2': + if (state == GS_NONE && guide.state == GU_NONE) { + char *conv_end; + settings.usPerStepCurrent = strtod(¶meter[3], &conv_end); + if (settings.usPerStepCurrent < usPerStepBase/2.0) settings.usPerStepCurrent = usPerStepBase/2.0; + if (settings.usPerStepCurrent > usPerStepBase*2.0) settings.usPerStepCurrent = usPerStepBase*2.0; + if (settings.usPerStepCurrent < usPerStepLowerLimit()) settings.usPerStepCurrent = usPerStepLowerLimit(); + if (GOTO_FEATURE == OFF) settings.usPerStepCurrent = usPerStepBase; // force base rate + nv().kv().put(nvKey, settings); + updateAccelerationRates(); + } else *commandError = CE_SLEW_IN_MOTION; + break; + // slew rate preset (returns nothing) + case '3': + *numericReply = false; + if (state == GS_NONE && guide.state == GU_NONE) { + switch (parameter[3]) { + case '5': settings.usPerStepCurrent = usPerStepBase*2.0; break; // 50% + case '4': settings.usPerStepCurrent = usPerStepBase*1.5; break; // 66.7% (2/3x) + case '3': settings.usPerStepCurrent = usPerStepBase; break; // 100% + case '2': settings.usPerStepCurrent = usPerStepBase/1.5; break; // 150% + case '1': settings.usPerStepCurrent = usPerStepBase/2.0; break; // 200% + default: settings.usPerStepCurrent = usPerStepBase; + } + if (settings.usPerStepCurrent < usPerStepLowerLimit()) settings.usPerStepCurrent = usPerStepLowerLimit(); + if (GOTO_FEATURE == OFF) settings.usPerStepCurrent = usPerStepBase; // force base rate + nv().kv().put(nvKey, settings); + updateAccelerationRates(); + } else *commandError = CE_SLEW_IN_MOTION; + break; + + // autoMeridianFlip + case '5': + if ((transform.isEquatorial()) && transform.meridianFlips && GOTO_FEATURE != OFF) { + if (parameter[3] == '0' || parameter[3] == '1') { + settings.meridianFlipAuto = parameter[3] - '0'; + #if MFLIP_AUTOMATIC_MEMORY == ON + nv().kv().put(nvKey, settings); + #endif + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_CMD_UNKNOWN; + break; + // preferred pier side + case '6': + if (transform.meridianFlips && GOTO_FEATURE != OFF) { + switch (parameter[3]) { + case 'E': settings.preferredPierSide = PSS_EAST; break; + case 'W': settings.preferredPierSide = PSS_WEST; break; + case 'B': settings.preferredPierSide = PSS_BEST; break; + case 'A': settings.preferredPierSide = PSS_AUTO; break; + default: *commandError = CE_PARAM_RANGE; + } + #if PIER_SIDE_PREFERRED_MEMORY == ON + nv().kv().put(nvKey, settings); + #endif + } else *commandError = CE_CMD_UNKNOWN; + break; + // pause at home on meridian flip + case '8': + if (transform.meridianFlips && GOTO_FEATURE != OFF) { + if (parameter[3] == '0' || parameter[3] == '1') { + #if GOTO_FEATURE == ON + settings.meridianFlipPause = parameter[3] - '0'; + #if MFLIP_PAUSE_HOME_MEMORY == ON + nv().kv().put(nvKey, settings); + #endif + #endif + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_CMD_UNKNOWN; + break; + // continue if paused at home + case '9': + if (transform.meridianFlips && GOTO_FEATURE != OFF) { + if (parameter[3] == '1') { + #if GOTO_FEATURE == ON + if (meridianFlipHome.paused) meridianFlipHome.resume = true; + #endif + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_CMD_UNKNOWN; + break; + default: return false; + } + } else return false; + } else return false; + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/mount/goto/Goto.cpp b/src/telescope/mount/goto/Goto.cpp new file mode 100755 index 0000000..b72977d --- /dev/null +++ b/src/telescope/mount/goto/Goto.cpp @@ -0,0 +1,734 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, sync and goto + +#include "Goto.h" + +#if defined(MOUNT_PRESENT) + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../Telescope.h" +#include "../Mount.h" +#include "../coordinates/Transform.h" +#include "../guide/Guide.h" +#include "../home/Home.h" +#include "../park/Park.h" +#include "../limits/Limits.h" +#include "../status/Status.h" + +#if GOTO_FEATURE == ON +inline void gotoWrapper() { goTo.poll(); } +#endif + +void Goto::init() { + + nvKey = nv().kv().computeKey("GOTO_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings)) { DLF("WRN: Nv, init failed for GOTO_SETTINGS"); } + + settings.meridianFlipAuto = constrain(settings.meridianFlipAuto, false, true); + settings.meridianFlipPause = constrain(settings.meridianFlipPause, false, true); + settings.preferredPierSide = constrain(settings.preferredPierSide, PSS_NONE, PSS_SAME_ONLY); + + // force defaults if needed + #if MFLIP_PAUSE_HOME_MEMORY != ON + settings.meridianFlipPause = (MFLIP_PAUSE_HOME_DEFAULT == ON); + #endif + + if (MFLIP_AUTOMATIC_MEMORY != ON || !transform.meridianFlips) settings.meridianFlipAuto = (MFLIP_AUTOMATIC_DEFAULT == ON); + + #if PIER_SIDE_PREFERRED_MEMORY != ON + settings.preferredPierSide = (PierSideSelect)PIER_SIDE_PREFERRED_DEFAULT; + #endif + + // calculate base and current maximum step rates + #if GOTO_FEATURE == ON + usPerStepBase = 1000000.0F/((axis1.getStepsPerMeasure()/RAD_DEG_RATIO)*SLEW_RATE_BASE_DESIRED); + #if SLEW_RATE_MEMORY != ON + settings.usPerStepCurrent = usPerStepBase; + #endif + #else + usPerStepBase = 1000000.0F/((axis1.getStepsPerMeasure()/RAD_DEG_RATIO)*1.0F); + settings.usPerStepCurrent = usPerStepBase; + #endif + + settings.usPerStepCurrent = constrain(settings.usPerStepCurrent, usPerStepBase/2.0F, usPerStepBase*2.0F); + if (usPerStepBase < usPerStepLowerLimit()) usPerStepBase = usPerStepLowerLimit()*2.0F; + if (settings.usPerStepCurrent > 1000000.0F) settings.usPerStepCurrent = usPerStepBase; + if (settings.usPerStepCurrent < usPerStepBase/2.0F) settings.usPerStepCurrent = usPerStepBase/2.0F; + if (settings.usPerStepCurrent > usPerStepBase*2.0F) settings.usPerStepCurrent = usPerStepBase*2.0F; + + axis1.setFrequencyMax(((1000000.0F/usPerStepBase)/axis1.getStepsPerMeasure())*2.0F); + axis2.setFrequencyMax(((1000000.0F/usPerStepBase)/axis2.getStepsPerMeasure())*2.0F); + + absoluteEncodersPresent = axis1.motor->hasAbsoluteEncoder() || axis2.motor->hasAbsoluteEncoder(); + if (AXIS1_HOME_TOLERANCE != 0.0F || AXIS2_HOME_TOLERANCE != 0.0F || + AXIS1_TARGET_TOLERANCE != 0.0F || AXIS2_TARGET_TOLERANCE != 0.0F || absoluteEncodersPresent) encodersPresent = true; + + updateAccelerationRates(); +} + +// goto to equatorial target position (Native coordinate system) using the defaut preferredPierSide +CommandError Goto::request() { + return request(target, settings.preferredPierSide); +} + +#if GOTO_FEATURE == ON + +// goto equatorial position (Native or Mount coordinate system) +CommandError Goto::request(Coordinate coords, PierSideSelect pierSideSelect, bool native) { + if (!mount.startupAuthorityTrusted()) { + DLF("WRN: Mount, goto rejected because startup authority is not trusted"); + return CE_SLEW_ERR_UNSPECIFIED; + } + + if (native) { + coords.pierSide = PIER_SIDE_NONE; + transform.nativeToMount(&coords); + } + + CommandError e = setTarget(&coords, pierSideSelect); + if (e == CE_SLEW_IN_SLEW) { abort(); return e; } + if (e != CE_NONE) return e; + + lastAlignTarget = target; + + #if AXIS1_SECTOR_GEAR == ON || AXIS2_TANGENT_ARM == ON + double a1, a2; + + // handle special case of a sector gear RA + #if AXIS1_SECTOR_GEAR == ON + transform.mountToInstrument(&target, &a1, &a2); + a1 = a1 - axis1.getIndexPosition(); + if (a1 < axis1.getLimitMin()) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (a1 > axis1.getLimitMax()) return CE_SLEW_ERR_OUTSIDE_LIMITS; + #endif + + // handle special case of a tangent arm Dec + #if AXIS2_TANGENT_ARM == ON + transform.mountToInstrument(&target, &a1, &a2); + a2 = a2 - axis2.getIndexPosition(); + if (a2 < axis2.getLimitMin()) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (a2 > axis2.getLimitMax()) return CE_SLEW_ERR_OUTSIDE_LIMITS; + #endif + #endif + + limits.enabled(true); + mount.syncFromOnStepToEncoders = false; + if (firstGoto) { + mount.tracking(true); + firstGoto = false; + } + guide.backlashEnableControl(true); + + // allow slewing near target for Eq modes but disable for alt/az, parking, homing if encoders are not present + nearDestinationRefineStages = 0; + slewDestinationDistHA = 0.0; + slewDestinationDistDec = 0.0; + if ((encodersPresent || (park.state != PS_PARKING && home.state != HS_HOMING))) { + nearDestinationRefineStages = GOTO_REFINE_STAGES; + if (transform.isEquatorial()) { + slewDestinationDistHA = degToRad(GOTO_OFFSET); + slewDestinationDistDec = degToRad(GOTO_OFFSET); + if (target.pierSide == PIER_SIDE_WEST) slewDestinationDistDec = -slewDestinationDistDec; + } + } + + // prepare for goto + Coordinate current = mount.getMountPosition(CR_MOUNT_HOR); + state = GS_GOTO; + stage = GG_NEAR_DESTINATION_START; + start = current; + destination = target; + + // add waypoint if needed + if (transform.isEquatorial() && MFLIP_SKIP_HOME == OFF && start.pierSide != destination.pierSide) { + VLF("MSG: Mount, goto changes pier side, setting waypoint at home"); + waypoint(¤t); + } + + // allow goto and enable tracking after any of the limits below are exceeded + // typically these are triggered by tracking into the relevant limit and + // a goto is safe since it should always move away from the limit else it wouldn't be allowed + // finally I enabling tracking again since that allows for easy recovery + #if LIMIT_RECOVERY == ON + if (limits.isBelowHorizon() || limits.isPastMeridianW() || limits.isPastAxis1Max()) { + limits.limitsDisablePeriod(1.0F); + #if LIMIT_RECOVERY_WITH_TRACKING == ON + if (home.state != HS_HOMING && park.state != PS_PARKING) mount.tracking(true); + #endif + } + #endif + + // start the goto monitor + if (taskHandle != 0) tasks.remove(taskHandle); + taskHandle = tasks.add(0, 0, true, 3, gotoWrapper, "MntGoto"); + if (taskHandle) { + VLF("MSG: Mount, create goto monitor task (idle, priority 3)... success"); + VLF("MSG: Mount, attempting goto"); + + e = startAutoSlew(); + if (e != CE_NONE) { + VF("MSG: Mount, goto failed with error "); VL(e); + return e; + } + + tasks.setPeriodMicros(taskHandle, FRACTIONAL_SEC_US); + VF("MSG: Mount, goto monitor task set rate "); V(FRACTIONAL_SEC_US); VL("us"); + + mountStatus.soundAlert(); + + } else { DLF("WRN: Mount, start goto monitor task... FAILED!"); } + + return CE_NONE; +} +#else + +// sync replaces goto to equatorial position (Native or Mount coordinate system) when GOTO_FEATURE is OFF +CommandError Goto::request(Coordinate coords, PierSideSelect pierSideSelect, bool native) { + mountStatus.soundAlert(); + + return requestSync(coords, pierSideSelect, native); +} +#endif + +// sync to equatorial target position (Native coordinate system) using the default preferredPierSide +CommandError Goto::requestSync() { + return requestSync(target, settings.preferredPierSide); +} + +// sync to equatorial position (Native or Mount coordinate system) +CommandError Goto::requestSync(Coordinate coords, PierSideSelect pierSideSelect, bool native) { + if (!mount.startupAuthorityTrusted()) { + DLF("WRN: Mount, sync rejected because startup authority is not trusted"); + return CE_SLEW_ERR_UNSPECIFIED; + } + + if (native) { + coords.pierSide = PIER_SIDE_NONE; + transform.nativeToMount(&coords); + } + + CommandError e = setTarget(&coords, pierSideSelect, false); + if (e != CE_NONE) return e; + + if (mount.isHome()) mount.tracking(true); + + double a1, a2; + transform.mountToInstrument(&target, &a1, &a2); + a1 += target.a1Correction; + + e = limits.validateInstrumentCoordinate(1, a1); + if (e != CE_NONE) return e; + e = limits.validateInstrumentCoordinate(2, a2); + if (e != CE_NONE) return e; + e = limits.setInstrumentCoordinate(1, a1, true); + if (e == CE_NONE) e = limits.setInstrumentCoordinate(2, a2, true); + if (e != CE_NONE) return e; + + limits.enabled(true); + mount.syncFromOnStepToEncoders = true; + + VLF("MSG: Mount, sync instrument coordinates updated"); + + return CE_NONE; +} + +// checks for valid target and determines pier side (Mount coordinate system) +CommandError Goto::setTarget(Coordinate *coords, PierSideSelect pierSideSelect, bool isGoto) { + CommandError e = validate(); + if (e == CE_SLEW_ERR_IN_STANDBY && (encodersPresent || mount.isHome())) { + mount.enable(true); + e = validate(); + } + if (e == CE_NONE && isGoto && limits.isAboveOverhead()) e = CE_SLEW_ERR_OUTSIDE_LIMITS; + if (e != CE_NONE) return e; + + target = *coords; + + if (transform.mountType == ALTAZM) transform.horToEqu(&target); else + if (transform.mountType == ALTALT) transform.aaToEqu(&target); else transform.equToHor(&target); + + // east side of pier is always the default polar-home position + // east side of pier - we're in the western sky and the HA's are positive + // west side of pier - we're in the eastern sky and the HA's are negative + + Coordinate current = mount.getMountPosition(CR_MOUNT); + + if (!transform.meridianFlips) pierSideSelect = PSS_EAST_ONLY; + + bool pierSideBest = false; + if (pierSideSelect == PSS_AUTO) { + if (transform.mountType != ALTAZM && transform.mountType != ALTALT) { + if (isGoto && (current.h < -Deg90 || current.h > Deg90)) pierSideSelect = PSS_WEST; else pierSideSelect = PSS_EAST; + } else pierSideSelect = PSS_BEST; + } + if (pierSideSelect == PSS_BEST) { + if (current.pierSide == PIER_SIDE_WEST) pierSideSelect = PSS_WEST; else pierSideSelect = PSS_EAST; + pierSideBest = true; + } + if (pierSideSelect == PSS_SAME_ONLY) { + if (current.pierSide == PIER_SIDE_WEST) pierSideSelect = PSS_WEST_ONLY; else pierSideSelect = PSS_EAST_ONLY; + } + + bool eastReachable, westReachable; + + target.pierSide = current.pierSide; + target.a1Correction = 0.0; + + double axis1TargetCorrectionE = 0.0; + double axis1TargetCorrectionW = 0.0; + e = limits.validateTarget(&target, &eastReachable, &westReachable, &axis1TargetCorrectionE, &axis1TargetCorrectionW, isGoto); + if (e != CE_NONE) return e; + + double a1, a2, a1e, a1w, a2e, a2w; + a1 = axis1.getInstrumentCoordinate(); + a2 = axis2.getInstrumentCoordinate(); + + target.pierSide = PIER_SIDE_EAST; + transform.mountToInstrument(&target, &a1e, &a2e); + a1e += axis1TargetCorrectionE; + + target.pierSide = PIER_SIDE_WEST; + transform.mountToInstrument(&target, &a1w, &a2w); + a1w += axis1TargetCorrectionW; + + target.pierSide = current.pierSide; + + VF("MSG: Mount, set-target current axis1 "); V(radToDeg(a1)); VF(" and axis2 "); VL(radToDeg(a2)); + VF("MSG: Mount, set-target targetE axis1 "); V(radToDeg(a1e)); VF(" and axis2 "); VL(radToDeg(a2e)); + VF("MSG: Mount, set-target targetW axis1 "); V(radToDeg(a1w)); VF(" and axis2 "); VL(radToDeg(a2w)); + + double eastDistance, westDistance; + if (dist(a1, a1e) > dist(a2, a2e)) eastDistance = dist(a1, a1e); else eastDistance = dist(a2, a2e); + if (dist(a1, a1w) > dist(a2, a2w)) westDistance = dist(a1, a1w); else westDistance = dist(a2, a2w); + + if (mount.isHome() && transform.mountType == GEM) { + VLF("MSG: Mount, set-target destination from home based on HA"); + if (target.h < 0) pierSideSelect = PSS_WEST; else pierSideSelect = PSS_EAST; + pierSideBest = true; + } + + target.pierSide = PIER_SIDE_NONE; + + if (pierSideSelect == PSS_EAST_ONLY) { + VLF("MSG: Mount, set-target using PPS_EAST_ONLY"); + if (eastReachable) target.pierSide = PIER_SIDE_EAST; + } else + if (pierSideSelect == PSS_WEST_ONLY) { + VLF("MSG: Mount, set-target using PPS_WEST_ONLY"); + if (westReachable) target.pierSide = PIER_SIDE_WEST; + } else + if (pierSideSelect == PSS_EAST) { + VLF("MSG: Mount, set-target using PPS_EAST"); + if (westReachable && !eastReachable) target.pierSide = PIER_SIDE_WEST; else + if (isGoto && westReachable && pierSideBest && westDistance < eastDistance) { + VLF("MSG: Mount, set-target destination in alternate (W) orientation is closer"); + target.pierSide = PIER_SIDE_WEST; + } else + if (eastReachable) target.pierSide = PIER_SIDE_EAST; + } else + if (pierSideSelect == PSS_WEST) { + VLF("MSG: Mount, set-target using PPS_WEST"); + if (eastReachable && !westReachable) target.pierSide = PIER_SIDE_EAST; else + if (isGoto && eastReachable && pierSideBest && eastDistance < westDistance) { + VLF("MSG: Mount, set-target destination in normal (E) orientation is closer"); + target.pierSide = PIER_SIDE_EAST; + } else + if (westReachable) target.pierSide = PIER_SIDE_WEST; + } + + if (target.pierSide == PIER_SIDE_NONE) { + VLF("MSG: Mount, set-target destination outside limits"); + return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + + VF("MSG: Mount, set-target destination "); + if (current.pierSide == PIER_SIDE_NONE) { VF("NONE"); } else + if (current.pierSide == PIER_SIDE_EAST) { VF("EAST"); } else + if (current.pierSide == PIER_SIDE_WEST) { VF("WEST"); } else { VF("?"); } + if (current.pierSide == target.pierSide) { VF(" stays "); } else { VF(" to "); } + if (target.pierSide == PIER_SIDE_NONE) { VLF("NONE"); } else + if (target.pierSide == PIER_SIDE_EAST) { VLF("EAST"); } else + if (target.pierSide == PIER_SIDE_WEST) { VLF("WEST"); } else { VLF("?"); } + + if (target.pierSide != PIER_SIDE_EAST && target.pierSide != PIER_SIDE_WEST) { + VLF("MSG: Mount, set-target destination pier side defaults to EAST"); + target.pierSide = PIER_SIDE_EAST; + } + + // adjust Axis1 coordinate range as needed to allow going past +/-180 degrees + if (target.pierSide == PIER_SIDE_EAST) target.a1Correction = axis1TargetCorrectionE; + if (target.pierSide == PIER_SIDE_WEST) target.a1Correction = axis1TargetCorrectionW; + + transform.observedPlaceToMount(&target); + if (transform.mountType == ALTAZM) transform.horToEqu(&target); else + if (transform.mountType == ALTALT) transform.aaToEqu(&target); else transform.equToHor(&target); + + transform.hourAngleToRightAscension(&target, false); + + return CE_NONE; +} + +// stop any presently active goto +void Goto::abort() { + if (state == GS_GOTO && stage > GG_READY_ABORT) stage = GG_READY_ABORT; +} + +// general status checks ahead of sync or goto +CommandError Goto::validate() { + if (!axis1.isEnabled()) return CE_SLEW_ERR_IN_STANDBY; + if (!axis2.isEnabled()) return CE_SLEW_ERR_IN_STANDBY; + if (park.state == PS_PARKED) return CE_SLEW_ERR_IN_PARK; + if (state != GS_NONE) return CE_SLEW_IN_SLEW; + if (guide.state != GU_NONE) return CE_SLEW_IN_MOTION; + if (mount.isSlewing()) return CE_SLEW_IN_MOTION; + if (limits.isGotoError()) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (mount.motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + return CE_NONE; +} + +// add an align star (at the current position relative to target) +CommandError Goto::alignAddStar(bool sync) { + if (alignState.currentStar > alignState.lastStar) return CE_PARAM_RANGE; + + CommandError e = CE_NONE; + + // first star, get ready for a new pointing model, init/sync then call gta.addStar + if (alignState.currentStar == 1) { + #if ALIGN_MAX_NUM_STARS > 1 + transform.align.init(transform.mountType, site.location.latitude); + #endif + e = requestSync(gotoTarget, PSS_SAME_ONLY); + lastAlignTarget = mount.getMountPosition(CR_MOUNT_ALL); + transform.hourAngleToRightAscension(&lastAlignTarget, true); + } + + // add an align star + if (e == CE_NONE) { + Coordinate mountPosition = mount.getMountPosition(CR_MOUNT_ALL); + if (sync) { lastAlignTarget = gotoTarget; } + + // update the targets HA and Horizon coords as necessary + transform.rightAscensionToHourAngle(&lastAlignTarget, true); + if (transform.mountType == ALTAZM) transform.equToHor(&lastAlignTarget); else + if (transform.mountType == ALTALT) transform.equToAa(&lastAlignTarget); + + #if ALIGN_MAX_NUM_STARS > 1 + e = transform.align.addStar(alignState.currentStar, alignState.lastStar, &lastAlignTarget, &mountPosition); + #else + UNUSED(mountPosition); + #endif + + if (e == CE_NONE) alignState.currentStar++; + } + + return e; +} + +// reset the alignment model +void Goto::alignReset() { + alignState.currentStar = 0; + alignState.lastStar = 0; + #if ALIGN_MAX_NUM_STARS > 1 + transform.align.modelClear(); + #endif +} + +#if GOTO_FEATURE == ON +// set any additional destinations required for a goto +void Goto::waypoint(Coordinate *current) { + // HA goes from +90...0..-90 + // W . E + // meridian flip, only happens for equatorial mounts + + stage = GG_WAYPOINT_HOME; + + // default goes straight to the home position + destination = home.getPosition(CR_MOUNT); + + // if the home position is at 0 hours, we're done + if (destination.h == 0.0) return; + + double d60 = degToRad(120); + double d45 = degToRad(135); + if (current->pierSide == PIER_SIDE_EAST) { d60 = Deg180 - d60; d45 = Deg180 - d45; } + + // decide if we should first move to 60 deg. HA (4 hours) to get away from the horizon limits + if (current->a < Deg10 && fabs(start.h) > Deg90) { destination.h = d60; stage = GG_WAYPOINT_AVOID; return; } + + // decide if we should first move to 45 deg. HA (3 hours) to get away from the horizon limits + // if at a low latitude and in the opposite sky, |HA| = 6 is very low on the horizon and we need + // to delay arriving there during a meridian flip. In the extreme case, where the user is very + // near the Earths equator an Horizon limit of -10 or -15 may be necessary for proper operation + if (current->a < Deg20 && site.locationEx.latitude.absval < Deg45) { + if (site.location.latitude >= 0) { + if (current->d <= Deg90 - site.location.latitude) { destination.h = d45; stage = GG_WAYPOINT_AVOID; } + } else { + if (current->d >= -Deg90 - site.location.latitude) { destination.h = d45; stage = GG_WAYPOINT_AVOID; } + } + } +} + +// monitor goto +void Goto::poll() { + // abort if either axis encounters a limit + if (stage != GG_ABORT && + (stage == GG_READY_ABORT || axis1.motionError(DIR_BOTH) || axis2.motionError(DIR_BOTH))) { + VLF("MSG: Mount, goto abort requested"); + stage = GG_ABORT; + meridianFlipHome.paused = false; + meridianFlipHome.resume = false; + axis1.autoSlewAbort(); + axis2.autoSlewAbort(); + } + + const unsigned long now = millis(); + + // abort any goto that might hang! + if (axis1.isSlewing()) { + if (!axis1.nearTarget()) nearTargetTimeoutAxis1 = now; + if (now - nearTargetTimeoutAxis1 > 15000U) { + DLF("WRN: Mount, goto axis1 timed out aborting slew!"); + axis1.autoSlewAbort(); + } + } + if (axis2.isSlewing()) { + if (!axis2.nearTarget()) nearTargetTimeoutAxis2 = now; + if (now - nearTargetTimeoutAxis2 > 15000U) { + DLF("WRN: Mount, goto axis2 timed out aborting slew!"); + axis2.autoSlewAbort(); + } + } + + if (!mount.isSlewing()) { + if (stage == GG_WAYPOINT_AVOID) { + VLF("MSG: Mount, goto waypoint reached"); + stage = GG_WAYPOINT_HOME; + destination = home.getPosition(CR_MOUNT); + startAutoSlew(); + } else + + if (stage == GG_WAYPOINT_HOME) { + if (settings.meridianFlipPause && !meridianFlipHome.resume) { meridianFlipHome.paused = true; goto skip; } + meridianFlipHome.paused = false; + meridianFlipHome.resume = false; + + VLF("MSG: Mount, goto home reached"); + stage = GG_NEAR_DESTINATION_START; + destination = target; + startAutoSlew(); + } else + + if (stage == GG_NEAR_DESTINATION_START) { + if (nearDestinationRefineStages >= 1) { + VLF("MSG: Mount, goto near destination wait started"); + nearDestinationTimeout = now + GOTO_SETTLE_TIME; + stage = GG_NEAR_DESTINATION_WAIT; + } else stage = GG_NEAR_DESTINATION; + } else + + if (stage == GG_NEAR_DESTINATION_WAIT) { + if ((long)(now - nearDestinationTimeout) > 0) { + VLF("MSG: Mount, goto near destination wait done"); + stage = GG_NEAR_DESTINATION; + } + } else + + if (stage == GG_NEAR_DESTINATION) { + if (nearDestinationRefineStages >= 1) { + + if (--nearDestinationRefineStages) stage = GG_NEAR_DESTINATION_START; else stage = GG_DESTINATION; + + VLF("MSG: Mount, goto near destination reached"); + destination = target; + if (!alignActive() || GOTO_OFFSET_ALIGN == OFF) { + slewDestinationDistHA = 0.0; + slewDestinationDistDec = 0.0; + } + startAutoSlew(); + } else { + stage = GG_DESTINATION; + VLF("MSG: Mount, goto near destination skipped"); + } + } else + + if (stage == GG_DESTINATION || stage == GG_ABORT) { + VLF("MSG: Mount, goto destination reached"); + state = GS_NONE; + mount.update(); + + // back to normal motor frequencies + axis1.setFrequencyScale(1.0F); + axis2.setFrequencyScale(1.0F); + + // kill this monitor + tasks.setDurationComplete(taskHandle); + taskHandle = 0; + VLF("MSG: Mount, goto monitor task terminated"); + + // check if parking and mark as finished or unparked as needed + if (park.state == PS_PARKING) { + if (stage == GG_ABORT) park.requestAborted(); else park.requestDone(); + } + + // check if homing + if (home.state == HS_HOMING) { + if (stage == GG_ABORT) home.requestAborted(); else home.requestDone(); + } + + // reset goto stage + stage = GG_NONE; + + mountStatus.soundAlert(); + + return; + } + } + + // adjust rates near the horizon to help avoid exceeding the minimum altitude limit + if (transform.isEquatorial() && MOUNT_HORIZON_AVOIDANCE == ON) { + if (site.locationEx.latitude.absval > degToRad(10.0)) { + static float last_a2 = 0; + Coordinate coords = mount.getMountPosition(CR_MOUNT_ALT); + float a2 = site.locationEx.latitude.sign*coords.d; + + // range 0.2 to 1.0, where a larger distance has less slowdown effect + float slowdownFactor = radToDeg(coords.a - limits.settings.altitude.min)/(SLEW_ACCELERATION_DIST*2.0); + + // constrain + if (slowdownFactor > 1.0F) slowdownFactor = 1.0F; + if (slowdownFactor < 0.2F) slowdownFactor = 0.2F; + + // if Dec is decreasing slow down the Dec axis, if Dec is increasing slow down the RA axis + float sfr = 0.5F/FRACTIONAL_SEC; + float slowdownFactor1 = 1.0F; + float slowdownFactor2 = 1.0F; + static float slowdownFactor1a = 1.0F; + static float slowdownFactor2a = 1.0F; + if (a2 < last_a2) slowdownFactor2 = slowdownFactor; else slowdownFactor1 = slowdownFactor; + + if (slowdownFactor1a < slowdownFactor1) { slowdownFactor1a += sfr; } + if (slowdownFactor1a > slowdownFactor1) { slowdownFactor1a -= sfr; } + if (slowdownFactor1a > 1.0F) slowdownFactor1a = 1.0F; + if (slowdownFactor1a < 0.2F) slowdownFactor1a = 0.2F; + if (slowdownFactor2a < slowdownFactor2) { slowdownFactor2a += sfr; } + if (slowdownFactor2a > slowdownFactor2) { slowdownFactor2a -= sfr; } + if (slowdownFactor2a > 1.0F) slowdownFactor2a = 1.0F; + if (slowdownFactor2a < 0.2F) slowdownFactor2a = 0.2F; + + axis1.setFrequencyScale(slowdownFactor1a); + axis2.setFrequencyScale(slowdownFactor2a); + + last_a2 = a2; + } else { + axis1.setFrequencyScale(1.0F); + axis2.setFrequencyScale(1.0F); + } + } + + skip: + + // keep updating the axis targets to match the mount target + // but allow timeout to stop tracking to guarantee synchronization + if (AXIS1_TARGET_TOLERANCE != 0.0F || AXIS2_TARGET_TOLERANCE != 0.0F || !axis1.nearTarget() || !axis2.nearTarget()) nearTargetTimeout = millis(); + + if (mount.isTracking()) { + target.r += siderealToRad(mount.trackingRateOffsetRA)/FRACTIONAL_SEC; + target.d += siderealToRad(mount.trackingRateOffsetDec)/FRACTIONAL_SEC; + transform.rightAscensionToHourAngle(&target, false); + if (stage >= GG_NEAR_DESTINATION_START) { + if (millis() - nearTargetTimeout < 5000U) { + Coordinate nearTarget = target; + nearTarget.h -= slewDestinationDistHA; + nearTarget.d -= slewDestinationDistDec; + + if (transform.mountType == ALTAZM) transform.equToHor(&nearTarget); else + if (transform.mountType == ALTALT) transform.equToAa(&nearTarget); + + double a1, a2; + transform.mountToInstrument(&nearTarget, &a1, &a2); + + axis1.setTargetCoordinate(a1 + nearTarget.a1Correction); + axis2.setTargetCoordinate(a2); + } + } + } +} + +// start slews with approach correction and parking/homing support +CommandError Goto::startAutoSlew() { + CommandError e; + + const unsigned long now = millis(); + nearTargetTimeoutAxis1 = now; + nearTargetTimeoutAxis2 = now; + + if (stage == GG_NEAR_DESTINATION || stage == GG_DESTINATION) { + destination.h -= slewDestinationDistHA; + destination.d -= slewDestinationDistDec; + } + + double a1, a2; + transform.mountToInstrument(&destination, &a1, &a2); + + if (stage == GG_DESTINATION && park.state == PS_PARKING) { + axis1.setTargetCoordinatePark(a1 + destination.a1Correction); + axis2.setTargetCoordinatePark(a2); + } else { + axis1.setTargetCoordinate(a1 + destination.a1Correction); + axis2.setTargetCoordinate(a2); + } + + VF("MSG: Mount, goto target coordinates set (a1="); V(radToDeg(a1)); VF(" deg, a2="); V(radToDeg(a2)); VLF(" deg)"); + + e = axis1.autoGoto(radsPerSecondCurrent); + if (e == CE_NONE) e = axis2.autoGoto(radsPerSecondCurrent*((float)(AXIS2_SLEW_RATE_PERCENT)/100.0F)); + + nearTargetTimeout = millis(); + + return e; +} +#endif + +// update acceleration rates for goto and guiding +void Goto::updateAccelerationRates() { + radsPerSecondCurrent = (1000000.0F/settings.usPerStepCurrent)/(float)axis1.getStepsPerMeasure(); + rate = radsPerSecondCurrent; + #if GOTO_FEATURE == ON + float secondsToAccelerate = (degToRadF((float)(SLEW_ACCELERATION_DIST))/radsPerSecondCurrent)*2.0F; + float secondsToAccelerateAbort = (degToRadF((float)(SLEW_RAPID_STOP_DIST))/radsPerSecondCurrent)*2.0F; + #else + float secondsToAccelerate = (degToRadF((float)(5.0F))/radsPerSecondCurrent)*2.0F; + float secondsToAccelerateAbort = (degToRadF((float)(2.0F))/radsPerSecondCurrent)*2.0F; + #endif + axis1.setSlewAccelerationRate(radsPerSecondCurrent/secondsToAccelerate); + axis1.setSlewAccelerationRateAbort(radsPerSecondCurrent/secondsToAccelerateAbort); + axis2.setSlewAccelerationRate(radsPerSecondCurrent/secondsToAccelerate); + axis2.setSlewAccelerationRateAbort(radsPerSecondCurrent/secondsToAccelerateAbort); +} + +// estimate average microseconds per step lower limit +float Goto::usPerStepLowerLimit() { + // basis is platform/clock-rate specific (for square wave) + float r_us = HAL_MAXRATE_LOWER_LIMIT; + + // higher speed ISR code path? + #if STEP_WAVE_FORM == PULSE || STEP_WAVE_FORM == DEDGE + r_us /= 1.6F; + #endif + + // average required goto us rates for each axis with any micro-step mode switching applied + float r_us_axis1 = r_us/axis1.getStepsPerStepSlewing(); + float r_us_axis2 = r_us/axis2.getStepsPerStepSlewing(); + + // average in axis2 step rate scaling for drives where the reduction ratio isn't equal + r_us = (1.0F/(1.0F/r_us_axis1 + 1.0F/r_us_axis2))*2.0F; + + // return rate in us units + return r_us; +} + +Goto goTo; + +#endif diff --git a/src/telescope/mount/goto/Goto.h b/src/telescope/mount/goto/Goto.h new file mode 100755 index 0000000..fd57e85 --- /dev/null +++ b/src/telescope/mount/goto/Goto.h @@ -0,0 +1,170 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control +#pragma once + +#include "../../../Common.h" + +#if defined(MOUNT_PRESENT) + +#include "../../../libApp/commands/ProcessCmds.h" +#include "../coordinates/Transform.h" + +enum MeridianFlip: uint8_t {MF_NEVER, MF_ALWAYS}; +enum GotoState: uint8_t {GS_NONE, GS_GOTO}; +enum GotoStage: uint8_t {GG_NONE, GG_ABORT, GG_READY_ABORT, GG_WAYPOINT_HOME, GG_WAYPOINT_AVOID, GG_NEAR_DESTINATION_START, GG_NEAR_DESTINATION_WAIT, GG_NEAR_DESTINATION, GG_DESTINATION}; +enum GotoType: uint8_t {GT_NONE, GT_HOME, GT_PARK}; +enum PierSideSelect: uint8_t {PSS_NONE, PSS_EAST, PSS_WEST, PSS_BEST, PSS_AUTO, PSS_EAST_ONLY, PSS_WEST_ONLY, PSS_SAME_ONLY}; + +typedef struct MeridianFlipHome { + bool paused; + bool resume; +} MeridianFlipHome; + +#pragma pack(1) +#define GotoSettingsSize 6 +typedef struct GotoSettings { + bool meridianFlipAuto :1; + bool meridianFlipPause :1; + PierSideSelect preferredPierSide; + float usPerStepCurrent; +} GotoSettings; +#pragma pack() + +typedef struct AlignState { + uint8_t currentStar; + uint8_t lastStar; +} AlignState; + +class Goto { + public: + void init(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // goto to equatorial target position (Native coordinate system) using the defaut preferredPierSide + CommandError request(); + + // goto equatorial position (Native or Mount coordinate system) + CommandError request(Coordinate coords, PierSideSelect pierSideSelect, bool native = true); + + // sync to equatorial target position (Native coordinate system) using the default preferredPierSide + CommandError requestSync(); + + // sync to equatorial position (Native or Mount coordinate system) + CommandError requestSync(Coordinate coords, PierSideSelect pierSideSelect, bool native = true); + + // get target equatorial position (Native coordinate system) + inline Coordinate getGotoTarget() { return gotoTarget; } + + // set target equatorial position (Native coordinate system) + inline void setGotoTarget(Coordinate *coords) { gotoTarget = *coords; } + + // checks for valid target and determines pier side (Mount coordinate system) + CommandError setTarget(Coordinate *coords, PierSideSelect pierSideSelect, bool isGoto = true); + + // abort any presently active goto + void abort(); + + // general status checks ahead of sync or goto + CommandError validate(); + + // add an align star (at the current position relative to target) + CommandError alignAddStar(bool sync = false); + + // reset the alignment model + void alignReset(); + + // check if an align is in progress + inline bool alignActive() { return alignState.lastStar > 0 && alignState.currentStar <= alignState.lastStar; } + + // check if an align is done + inline bool alignDone() { return alignState.lastStar > 0 && alignState.currentStar > alignState.lastStar; } + + // returns true if the pause at home feature is enabled + inline bool isHomePauseEnabled() { return settings.meridianFlipPause; } + + // returns true if paused at the home position, waiting to continue, during a goto + inline bool isHomePaused() { return meridianFlipHome.paused; } + + // if paused at the home position, continue the goto + inline void homeContinue() { meridianFlipHome.resume = true; } + + // returns true if the automatic meridian flip feature is enabled + inline bool isAutoFlipEnabled() { return settings.meridianFlipAuto && transform.isEquatorial() && transform.meridianFlips; } + + // return selected slew rate + inline float getRadsPerSecond() { return radsPerSecondCurrent; } + + // monitor goto + void poll(); + + // for determining goto state + GotoState state = GS_NONE; + GotoStage stage = GG_NONE; + + // current goto rate in radians per second + float rate; + + // flag to start tracking if this is the first goto + bool firstGoto = true; + + // flag to indicate that encoders are present + bool absoluteEncodersPresent = false; + bool encodersPresent = false; + + private: + + #if GOTO_FEATURE == ON + // set any additional destinations required for a goto + void waypoint(Coordinate *current); + + // start slews with approach correction and parking/homing support + CommandError startAutoSlew(); + #endif + + // update acceleration rates for goto and guiding + void updateAccelerationRates(); + + // estimate average microseconds per step lower limit + float usPerStepLowerLimit(); + + // get least distance between coordinates + inline double dist(double a, double b) { if (a > b) return a - b; else return b - a; } + + // requested goto/sync destination Native coordinate (eq or hor) + Coordinate gotoTarget = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, PIER_SIDE_NONE}; + // goto starts from this Mount coordinate (eq or hor) + Coordinate start; + // goto next destination Mount coordinate (eq or hor) + Coordinate destination; + // goto final destination Mount coordinate (eq or hor) + Coordinate target = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, PIER_SIDE_NONE}; + // last align (goto) target Mount coordinate (eq or hor) + Coordinate lastAlignTarget = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, PIER_SIDE_NONE}; + GotoState stateAbort = GS_NONE; + GotoState stateLast = GS_NONE; + uint8_t taskHandle = 0; + int nearDestinationRefineStages; + unsigned long nearTargetTimeout = 0; + unsigned long nearTargetTimeoutAxis1 = 0; + unsigned long nearTargetTimeoutAxis2 = 0; + unsigned long nearDestinationTimeout = 0; + + MeridianFlipHome meridianFlipHome = {false, false}; + + AlignState alignState = {0, 0}; + + float usPerStepBase = 128.0F; + float radsPerSecondCurrent; + + double slewDestinationDistHA = 0.0; + double slewDestinationDistDec = 0.0; + + GotoSettings settings = {MFLIP_AUTOMATIC_DEFAULT == ON, MFLIP_PAUSE_HOME_DEFAULT == ON, (PierSideSelect)PIER_SIDE_PREFERRED_DEFAULT, 1000001.0F}; + + uint32_t nvKey; +}; + +extern Goto goTo; + +#endif diff --git a/src/telescope/mount/guide/Guide.command.cpp b/src/telescope/mount/guide/Guide.command.cpp new file mode 100755 index 0000000..88ff347 --- /dev/null +++ b/src/telescope/mount/guide/Guide.command.cpp @@ -0,0 +1,196 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, commands + +#include "Guide.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../site/Site.h" +#include "../goto/Goto.h" + +bool Guide::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + *suppressFrame = false; + + // :GX90# Get setting pulse guide rate + // Returns: n.nn# + if (command[0] == 'G' && command[1] == 'X' && parameter[0] == '9' && parameter[1] == '0' && parameter[2] == 0) { + sprintF(reply, "%0.2f", rateSelectToRate(settings.pulseRateSelect)); + *numericReply = false; + } else + + // M - Telescope Movement (Guiding) Commands + if (command[0] == 'M') { + + // :Mgd[n]# Pulse guide command where n is the guide time in milliseconds + // Returns: Nothing + // :MGd[n]# Pulse guide command where n is the guide time in milliseconds + // Return: 0 on failure + // 1 on success + if (command[1] == 'g' || command[1] == 'G') { + int16_t timeMs; + if (convert.atoi2(¶meter[1], &timeMs)) { + if (timeMs >= 0) { + if (parameter[0] == 'w') { + *commandError = startAxis1(GA_FORWARD, (GUIDE_SEPARATE_PULSE_RATE == ON) ? settings.pulseRateSelect : settings.axis1RateSelect, timeMs); + } else + if (parameter[0] == 'e') { + *commandError = startAxis1(GA_REVERSE, (GUIDE_SEPARATE_PULSE_RATE == ON) ? settings.pulseRateSelect : settings.axis1RateSelect, timeMs); + } else + if (parameter[0] == 'n') { + *commandError = startAxis2(GA_FORWARD, (GUIDE_SEPARATE_PULSE_RATE == ON) ? settings.pulseRateSelect : settings.axis2RateSelect, timeMs); + } else + if (parameter[0] == 's') { + *commandError = startAxis2(GA_REVERSE, (GUIDE_SEPARATE_PULSE_RATE == ON) ? settings.pulseRateSelect : settings.axis2RateSelect, timeMs); + } else *commandError = CE_CMD_UNKNOWN; + if (command[1] == 'g') *numericReply = false; + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + // :Mw# Move Telescope West at current guide rate + // Returns: Nothing + if (command[1] == 'w' && parameter[0] == 0) { + *commandError = startAxis1(GA_FORWARD, settings.axis1RateSelect, GUIDE_TIME_LIMIT*1000); + *numericReply = false; + } else + // :Me# Move Telescope East at current guide rate + // Returns: Nothing + if (command[1] == 'e' && parameter[0] == 0) { + *commandError = startAxis1(GA_REVERSE, settings.axis1RateSelect, GUIDE_TIME_LIMIT*1000); + *numericReply = false; + } else + // :Mn# Move Telescope North at current guide rate + // Returns: Nothing + if (command[1] == 'n' && parameter[0] == 0) { + *commandError = startAxis2(GA_FORWARD, settings.axis2RateSelect, GUIDE_TIME_LIMIT*1000); + *numericReply = false; + } else + // :Ms# Move Telescope South at current guide rate + // Returns: Nothing + if (command[1] == 's' && parameter[0] == 0) { + *commandError = startAxis2(GA_REVERSE, settings.axis2RateSelect, GUIDE_TIME_LIMIT*1000); + *numericReply = false; + } else + // :Mp# Move Telescope for sPiral search at current guide rate + // Returns: Nothing + if (command[1] == 'p' && parameter[0] == 0) { + *commandError = startSpiral(settings.axis1RateSelect, GUIDE_SPIRAL_TIME_LIMIT*1000); + *numericReply = false; + } else return false; + } else + + // Q - Movement Commands + // :Q# Halt all slews, stops goto + // Returns: Nothing + if (command[0] == 'Q') { + if (command[1] == 0) { + #if GOTO_FEATURE == ON + goTo.abort(); + #endif + stop(); + *numericReply = false; + } else + // :Qe# Qw# Halt east/westward Slews + // Returns: Nothing + if ((command[1] == 'e' || command[1] == 'w') && parameter[0] == 0) { + stopAxis1(); + *numericReply = false; + } else + // :Qn# Qs# Halt north/southward Slews + // Returns: Nothing + if ((command[1] == 'n' || command[1] == 's') && parameter[0] == 0) { + stopAxis2(); + *numericReply = false; + } else return false; + } else + + // R - Guide Rate Commands + if (command[0] == 'R') { + + #if GOTO_FEATURE == ON + float maxDegsPerSec = radToDegF(goTo.rate); + #else + float maxDegsPerSec = 0.083333333F; + #endif + + // :RA[n.n]# Set Axis1 Guide rate to n.n degrees per second + // Returns: Nothing + // Note: Tracking, if active, is suspended in this mode of operation + if (command[1] == 'A') { + char* conv_end; + float f = strtod(parameter, &conv_end); + if (¶meter[0] != conv_end) { + if (f < 0.0001F/3600.0F) f = 0.0001F/3600.0F; + if (f > maxDegsPerSec) f = maxDegsPerSec; + VF("MSG: set Axis1 custom guide rate to "); + if (f < 0.1F) { + V(f*3600.0F); VLF(" arc-sec/s"); + } else { + V(f); VLF(" deg/s"); + } + settings.axis1RateSelect = GR_CUSTOM; + customRateAxis1 = f*240.0F; + } else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + + // :RE[n.n]# Set Axis2 Guide rate to n.n degrees per second + // Returns: Nothing + // Note: Tracking, if active, is suspended in this mode of operation + if (command[1] == 'E') { + char* conv_end; + float f = strtod(parameter, &conv_end); + if (¶meter[0] != conv_end) { + if (f < 0.0001F/3600.0F) f = 0.0001F/3600.0F; + if (f > maxDegsPerSec) f = maxDegsPerSec; + VF("MSG: set Axis2 custom guide rate to "); + if (f < 0.1F) { + V(f*3600.0F); VLF(" arc-sec/s"); + } else { + V(f); VLF(" deg/s"); + } + settings.axis2RateSelect = GR_CUSTOM; + customRateAxis2 = f*240.0F; + } else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + + // :RG# Set guide rate: Guiding 1X + // :RC# Set guide rate: Centering 8X + // :RM# Set guide rate: Find 20X + // :RF# Set guide rate: Fast 48X + // :RS# Set guide rate: Slew ?X (1/2 of current goto rate) + // :Rn# Set guide rate to n, where n = 0..9 + // Returns: Nothing + // Note: Tracking, if active, is integrated in this mode of operation + if (strchr("GCMFS0123456789", command[1]) && parameter[0] == 0) { + int r; + if (command[1] == 'G') r = 2; else + if (command[1] == 'C') r = 5; else + if (command[1] == 'M') r = 6; else + if (command[1] == 'F') r = 7; else + if (command[1] == 'S') r = 8; else r = command[1] - '0'; + + if (r >= 0 && r <= 9) { + #if GOTO_FEATURE != ON + if (r > 6) r = 6; + #endif + settings.axis1RateSelect = (GuideRateSelect)r; + settings.axis2RateSelect = (GuideRateSelect)r; + VF("MSG: Guide rate "); V(r); VLF(" selected"); + if (GUIDE_SEPARATE_PULSE_RATE == ON && (GuideRateSelect)r <= GR_1X) { + settings.pulseRateSelect = (GuideRateSelect)r; + nv().kv().put(nvKey, settings); + } + } else *commandError = CE_PARAM_RANGE; + *numericReply = false; + } else return false; + } else return false; + + return true; +} + +#endif \ No newline at end of file diff --git a/src/telescope/mount/guide/Guide.cpp b/src/telescope/mount/guide/Guide.cpp new file mode 100755 index 0000000..ffe457b --- /dev/null +++ b/src/telescope/mount/guide/Guide.cpp @@ -0,0 +1,484 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, guiding + +#include "Guide.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../../libApp/commands/ProcessCmds.h" + +#include "../../Telescope.h" +#include "../Mount.h" +#include "../goto/Goto.h" +#include "../home/Home.h" +#include "../park/Park.h" +#include "../home/Home.h" +#include "../limits/Limits.h" +#include "../status/Status.h" + +inline void guideWrapper() { guide.poll(); } + +void Guide::init() { + + nvKey = nv().kv().computeKey("GUIDE_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings.pulseRateSelect)) { DLF("WRN: Nv, init failed for GUIDE_SETTINGS"); } + + settings.axis1RateSelect = GR_20X; + settings.axis2RateSelect = GR_20X; + settings.pulseRateSelect = constrain(settings.pulseRateSelect, GR_QUARTER, GR_1X); + + // start guide monitor task + VF("MSG: Mount, start guide monitor task (rate "); V(FRACTIONAL_SEC_US/2); VF("us priority 3)... "); + int taskHandle = tasks.add(0, 0, true, 3, guideWrapper, "MtGuide"); + tasks.setPeriodMicros(taskHandle, FRACTIONAL_SEC_US/2); + if (taskHandle) { VLF("success"); } else { VLF("FAILED!"); } +} + +// start guide at a given direction and rate on Axis1 +CommandError Guide::startAxis1(GuideAction guideAction, GuideRateSelect rateSelect, unsigned long guideTimeLimit) { + if (guideAction == GA_NONE) return CE_NONE; + if (state == GU_HOME_GUIDE || state == GU_HOME_GUIDE_ABORT) return CE_NONE; + + CommandError e = validate(1, guideAction); + if (e != CE_NONE) return e; + + guideActionAxis1 = guideAction; + float rate = rateSelectToRate(rateSelect, 1); + + // unlimited 0 means the maximum period, about 49 days + if (guideTimeLimit == 0) guideTimeLimit = 0x1FFFFFFF; + guideFinishTimeAxis1 = millis() + guideTimeLimit; + + if (rate <= 2) { + axis1.setPowerDownOverrideTime(300000UL); + axis2.setPowerDownOverrideTime(300000UL); + } + + if (limits.isEnabled() && rate <= 2 && rateSelect != GR_CUSTOM) { + backlashEnableControl(false); + state = GU_PULSE_GUIDE; + if (guideAction == GA_REVERSE) { VF("MSG: Guide, Axis1 rev @"); rateAxis1 = -rate; } else { VF("MSG: Guide, Axis1 fwd @"); rateAxis1 = rate; } + V(rate); VL("X"); + mount.update(); + } else { + state = GU_GUIDE; + backlashEnableControl(true); + if (rateSelect != GR_CUSTOM) { + if (guideAction == GA_REVERSE) rate -= mount.trackingRateAxis1; else rate += mount.trackingRateAxis1; + } + axis1.setFrequencySlew(degToRadF(rate/240.0F)); + axis1AutoSlew(guideAction); + } + + return CE_NONE; +} + +// stop guide on Axis1, use GA_BREAK to stop in either direction or specifiy the direction to be stopped GA_FORWARD or GA_REVERSE +// set abort true to rapidly stop (broken limit, etc) +void Guide::stopAxis1(GuideAction stopDirection, bool abort) { + if (state == GU_HOME_GUIDE && !abort) { this->abort(); return; } + + if (guideActionAxis1 > GA_BREAK) { + if (stopDirection != GA_BREAK && guideActionAxis1 != stopDirection && guideActionAxis1 != GA_HOME) return; + if (rateAxis1 == 0.0F) { + guideActionAxis1 = GA_BREAK; + if (abort) axis1.autoSlewAbort(); else axis1.autoSlewStop(); + } else { + VLF("MSG: Guide, Axis1 stopped"); + guideActionAxis1 = GA_NONE; + rateAxis1 = 0.0F; + mount.update(); + } + } +} + +// start guide at a given direction and rate on Axis2 +CommandError Guide::startAxis2(GuideAction guideAction, GuideRateSelect rateSelect, unsigned long guideTimeLimit) { + if (guideAction == GA_NONE) return CE_NONE; + if (state == GU_HOME_GUIDE || state == GU_HOME_GUIDE_ABORT) return CE_NONE; + + CommandError e = validate(2, guideAction); // if successful always sets pierSide + if (e != CE_NONE) return e; + + guideActionAxis2 = guideAction; + float rate = rateSelectToRate(rateSelect, 2); + float fastestRate = rateSelectToRate(GR_MAX, 2)*((float)(AXIS2_SLEW_RATE_PERCENT)/100.0F); + if (rate > fastestRate) rate = fastestRate; + + // unlimited 0 means the maximum period, about 49 days + if (guideTimeLimit == 0) guideTimeLimit = 0x1FFFFFFF; + guideFinishTimeAxis2 = millis() + guideTimeLimit; + + if (rate <= 2) { + axis1.setPowerDownOverrideTime(300000UL); + axis2.setPowerDownOverrideTime(300000UL); + } + + if (limits.isEnabled() && rate <= 2 && rateSelect != GR_CUSTOM) { + state = GU_PULSE_GUIDE; + backlashEnableControl(false); + if (pierSide == PIER_SIDE_WEST) { if (guideAction == GA_FORWARD) guideAction = GA_REVERSE; else guideAction = GA_FORWARD; }; + if (guideAction == GA_REVERSE) { VF("MSG: Guide, Axis2 rev @"); rateAxis2 = -rate; } else { VF("MSG: Guide, Axis2 fwd @"); rateAxis2 = rate; } + V(rate); VL("X"); + mount.update(); + } else { + state = GU_GUIDE; + backlashEnableControl(true); + if (rateSelect != GR_CUSTOM) { + if (guideAction == GA_REVERSE) rate -= mount.trackingRateAxis2; else rate += mount.trackingRateAxis2; + } + axis2.setFrequencySlew(degToRadF(rate/240.0F)); + axis2AutoSlew(guideAction); + } + + return CE_NONE; +} + +// stop guide on Axis2, use GA_BREAK to stop in either direction or specifiy the direction to be stopped GA_FORWARD or GA_REVERSE +// set abort true to rapidly stop (broken limit, etc) +void Guide::stopAxis2(GuideAction stopDirection, bool abort) { + if (state == GU_HOME_GUIDE && !abort) { this->abort(); return; } + + if (guideActionAxis2 > GA_BREAK) { + if (stopDirection != GA_BREAK && guideActionAxis2 != stopDirection && guideActionAxis2 != GA_HOME) return; + if (rateAxis2 == 0.0F) { + guideActionAxis2 = GA_BREAK; + if (abort) axis2.autoSlewAbort(); else axis2.autoSlewStop(); + } else { + VLF("MSG: Guide, Axis2 stopped"); + guideActionAxis2 = GA_NONE; + rateAxis2 = 0.0F; + mount.update(); + } + } +} + +// start spiral guide at the specified rate (spiral size is porportional to rate) +CommandError Guide::startSpiral(GuideRateSelect rateSelect, unsigned long guideTimeLimit) { + if (state == GU_SPIRAL_GUIDE) { stop(); return CE_NONE; } + if (guideActionAxis1 != GA_NONE || guideActionAxis2 != GA_NONE) return CE_SLEW_IN_MOTION; + CommandError e = validate(0, GA_SPIRAL); if (e != CE_NONE) return e; + + backlashEnableControl(true); + + if (rateSelect < GR_2X) rateSelect = GR_2X; + if (rateSelect > GR_HALF_MAX) rateSelect = GR_HALF_MAX; + spiralGuideRateSelect = rateSelect; + + VF("MSG: guideSpiralStart(), using guide rates to "); V(rateSelectToRate(spiralGuideRateSelect)); VL("X"); + + // unlimited 0 means the maximum period, about 49 days + if (guideTimeLimit == 0) guideTimeLimit = 0x1FFFFFFF; + spiralStartTime = millis(); + guideFinishTimeAxis1 = spiralStartTime + guideTimeLimit; + guideFinishTimeAxis2 = guideFinishTimeAxis1; + + // setup and call the polling routine once to start the guides + Coordinate location = mount.getMountPosition(CR_MOUNT); + spiralScaleAxis1 = cos(location.a2); + spiralPoll(); + + state = GU_SPIRAL_GUIDE; + return CE_NONE; +} + +// start guide home (for use with home switches) +CommandError Guide::startHome() { + #if GOTO_FEATURE == ON + if (state == GU_HOME_GUIDE) { abort(); return CE_NONE; } + if (guideActionAxis1 != GA_NONE || guideActionAxis2 != GA_NONE) return CE_SLEW_IN_MOTION; + CommandError e = validate(0, GA_HOME); if (e != CE_NONE) return e; + + backlashEnableControl(true); + + // use guiding and switches to find home + mountStatus.soundAlert(); + state = GU_HOME_GUIDE; + + #if AXIS1_SECTOR_GEAR == ON || AXIS2_TANGENT_ARM == OFF + guideFinishTimeAxis1 = millis() + (unsigned long)(GUIDE_HOME_TIME_LIMIT * 1000.0); + guideActionAxis1 = GA_HOME; + axis1.setFrequencySlew(goTo.rate); + axis1.autoSlewHome(); + #endif + + guideFinishTimeAxis2 = millis() + (unsigned long)(GUIDE_HOME_TIME_LIMIT * 1000.0); + guideActionAxis2 = GA_HOME; + axis2.setFrequencySlew(goTo.rate*((float)(AXIS2_SLEW_RATE_PERCENT)/100.0F)); + axis2.autoSlewHome(); + #else + DLF("WRN: Mount, sensed home rejected because goto is disabled"); + return CE_SLEW_ERR_UNSPECIFIED; + #endif + return CE_NONE; +} + +// stop both axes of guide +void Guide::stop() { + stopAxis1(); + stopAxis2(); +} + +// abort both axes of guide +void Guide::abort() { + if (state == GU_HOME_GUIDE) { + VLF("MSG: Mount, aborting home guide"); + state = GU_HOME_GUIDE_ABORT; + } + stopAxis1(GA_BREAK, true); + stopAxis2(GA_BREAK, true); +} + +// keep guide rate <= half max +float Guide::limitGuideRate(float rate) { + #if GOTO_FEATURE == ON + float rateLimit = radToDegF(goTo.rate)*120.0F; + if (rate > rateLimit) rate = rateLimit; + #endif + return rate; +} + +// return guide rate (sidereal x) for guide rate selection +float Guide::rateSelectToRate(GuideRateSelect rateSelect, uint8_t axis) { + switch (rateSelect) { + case GR_QUARTER: return 0.25F; + case GR_HALF: return 0.5F; + case GR_1X: return 1.0F; + case GR_2X: return 2.0F; + case GR_4X: return limitGuideRate(4.0F); + case GR_8X: return limitGuideRate(8.0F); + case GR_20X: return limitGuideRate(20.0F); + #if GOTO_FEATURE == ON + case GR_48X: return limitGuideRate(48.0F); + case GR_HALF_MAX: return radToDegF(goTo.rate)*120.0F; + case GR_MAX: return radToDegF(goTo.rate)*240.0F; + #else + case GR_48X: return limitGuideRate(20.0F); + case GR_HALF_MAX: return limitGuideRate(20.0F); + case GR_MAX: return limitGuideRate(20.0F); + #endif + case GR_CUSTOM: if (axis == 1) return customRateAxis1; else if (axis == 2) return customRateAxis2; else return 0.0F; + default: return 0.0F; + } +} + +// valid guide for Axis1 +bool Guide::validAxis1(GuideAction guideAction) { + Coordinate location = mount.getMountPosition(CR_MOUNT_ALT); + + if (!limits.isEnabled()) return true; + + #if AXIS1_SECTOR_GEAR == ON + location.a1 = axis1.getMotorPosition(); + #endif + + // for Fork and Alt/Azm mounts limits are based on shaft angles + // so convert axis1 into normal PIER_SIDE_EAST coordinates + if (transform.mountType != GEM && location.pierSide == PIER_SIDE_WEST) location.a1 += Deg180; + + if (guideAction == GA_REVERSE || guideAction == GA_SPIRAL) { + if (transform.mountType == GEM && location.pierSide == PIER_SIDE_EAST) { + if (location.h < -limits.settings.pastMeridianE) return false; + } + if (location.a1 < axis1.getLimitMin()) return false; + } + + if (guideAction == GA_FORWARD || guideAction == GA_SPIRAL) { + if (transform.mountType == GEM && location.pierSide == PIER_SIDE_WEST) { + if (location.h > limits.settings.pastMeridianW) return false; + } + if (location.a1 > axis1.getLimitMax()) return false; + } + return true; +} + +// valid guide for Axis2 +bool Guide::validAxis2(GuideAction guideAction) { + Coordinate location = mount.getMountPosition(CR_MOUNT_ALT); + pierSide = location.pierSide; + + if (!limits.isEnabled()) return true; + + #if AXIS2_TANGENT_ARM == ON + location.a2 = axis2.getMotorPosition(); + #endif + + if (guideAction == GA_REVERSE || guideAction == GA_SPIRAL) { + if (pierSide == PIER_SIDE_WEST) { + if (fgt(location.a2, axis2.getLimitMax())) return false; + } else { + if (flt(location.a2, axis2.getLimitMin())) return false; + } + } + + if (guideAction == GA_FORWARD || guideAction == GA_SPIRAL) { + if (pierSide == PIER_SIDE_WEST) { + if (flt(location.a2, axis2.getLimitMin())) return false; + } else { + if (fgt(location.a2, axis2.getLimitMax())) return false; + } + } + if (guideAction == GA_SPIRAL) { + if (fabs(location.a2) > degToRad(75.0)) return false; + } + return true; +} + +// general validation of guide request +CommandError Guide::validate(int axis, GuideAction guideAction) { + if (!mount.isEnabled()) { + mount.enable(true); + if (!mount.isEnabled()) return CE_SLEW_ERR_IN_STANDBY; + } + + if ((guideAction == GA_SPIRAL || guideAction == GA_HOME) && mount.isSlewing()) return CE_SLEW_IN_MOTION; + + #if GOTO_FEATURE == ON + if (park.state == PS_PARKED) return CE_SLEW_ERR_IN_PARK; + if (goTo.state != GS_NONE) { goTo.abort(); return CE_SLEW_IN_MOTION; } + #endif + + if (axis == 1 || guideAction == GA_SPIRAL) { + if (!validAxis1(guideAction)) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (settings.axis1RateSelect < 3) { + if (limits.isError() || axis1.motionError(DIR_BOTH)) return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + } + + if (axis == 2 || guideAction == GA_SPIRAL) { + if (!validAxis2(guideAction)) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (settings.axis2RateSelect < 3) { + if (limits.isError() || axis2.motionError(DIR_BOTH)) return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + } + + if (mount.motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + + return CE_NONE; +} + +// start axis1 movement +void Guide::axis1AutoSlew(GuideAction guideAction) { + if (guideAction == GA_REVERSE) axis1.autoSlew(DIR_REVERSE); else + if (guideAction == GA_FORWARD) axis1.autoSlew(DIR_FORWARD); +} + +// start axis2 movement +void Guide::axis2AutoSlew(GuideAction guideAction) { + Coordinate location = mount.getMountPosition(CR_MOUNT); + if (location.pierSide == PIER_SIDE_WEST) { + if (guideAction == GA_REVERSE) axis2.autoSlew(DIR_FORWARD); else + if (guideAction == GA_FORWARD) axis2.autoSlew(DIR_REVERSE); + } else { + if (guideAction == GA_REVERSE) axis2.autoSlew(DIR_REVERSE); else + if (guideAction == GA_FORWARD) axis2.autoSlew(DIR_FORWARD); + } +} + +// set guide spiral rates in RA/Azm and Dec/Alt, rate is in x-sidereal, guide elapsed time is in ms +void Guide::spiralPoll() { + // current elapsed time in seconds + float T = ((long)(millis() - spiralStartTime))/1000.0; + + // actual rate we'll be using (in sidereal X) + float rate = rateSelectToRate(spiralGuideRateSelect); + float maxRate = rateSelectToRate(GR_MAX); + if (rate > maxRate/2.0) rate = maxRate/2.0; + float fastestRate = rateSelectToRate(GR_MAX, 2)*((float)(AXIS2_SLEW_RATE_PERCENT)/100.0F); + if (rate > fastestRate) rate = fastestRate; + + // apparaent FOV (in arc-seconds) = rate*15.0*2.0; + // current radius assuming movement at 2 seconds per fov + double radius = pow(T/6.28318, 1.0/1.74); + + // current angle in radians + float angle = (radius - trunc(radius))*6.28318; + + // calculate the Axis rates for this moment (in sidereal X) + customRateAxis1 = rate*cos(angle); + customRateAxis2 = rate*sin(angle); + + // add the current tracking rates + customRateAxis1 += mount.trackingRateAxis1; + customRateAxis2 += mount.trackingRateAxis2; + + // set any new directions + guideActionAxis1 = GA_FORWARD; + guideActionAxis2 = GA_FORWARD; + if (customRateAxis1 < 0) { customRateAxis1 = fabs(customRateAxis1); guideActionAxis1 = GA_REVERSE; } + if (customRateAxis2 < 0) { customRateAxis2 = fabs(customRateAxis2); guideActionAxis2 = GA_REVERSE; } + axis1AutoSlew(guideActionAxis1); + axis2AutoSlew(guideActionAxis2); + + // adjust Axis1 rate due to spherical coordinates and limit it to rates we can reach. worst case the + // shape of the spiral will degrade to an 2:1 aspect oval at half max rate (fastest allowed) and |Axis2| = 75 deg + customRateAxis1 /= spiralScaleAxis1; + if (customRateAxis1 > maxRate) customRateAxis1 = maxRate; + + // set the new guide rates + axis1.setFrequencySlew(siderealToRadF(customRateAxis1)); + axis2.setFrequencySlew(siderealToRadF(customRateAxis2)); +} + +void Guide::poll() { + // just return if no guide is active + if (state == GU_NONE) return; + + // check fast guide completion axis1 + if (guideActionAxis1 == GA_BREAK && rateAxis1 == 0.0F && !axis1.isSlewing()) { + guideActionAxis1 = GA_NONE; + mount.update(); + } else { + if (guideActionAxis1 > GA_BREAK && (long)(millis() - guideFinishTimeAxis1) >= 0) stopAxis1(); + } + + // check fast guide completion axis2 + if (guideActionAxis2 == GA_BREAK && rateAxis2 == 0.0F && !axis2.isSlewing()) { + guideActionAxis2 = GA_NONE; + mount.update(); + } else { + if (guideActionAxis2 > GA_BREAK && (long)(millis() - guideFinishTimeAxis2) >= 0) stopAxis2(); + } + + // do spiral guiding, change rates and stop both axes at once + if (state == GU_SPIRAL_GUIDE) { + if (guideActionAxis1 > GA_BREAK && guideActionAxis2 > GA_BREAK) spiralPoll(); else + if (guideActionAxis1 > GA_BREAK || guideActionAxis2 > GA_BREAK) stop(); + } + + // handle end of home guiding + if ((state == GU_HOME_GUIDE || state == GU_HOME_GUIDE_ABORT) && !mount.isSlewing()) { + guideActionAxis1 = GA_NONE; + guideActionAxis2 = GA_NONE; + mountStatus.soundAlert(); + if (state == GU_HOME_GUIDE) { + VLF("MSG: Guide, arrival at home detected"); + state = GU_NONE; + home.guideDone(true); + } else { + VLF("MSG: Guide, aborted homing"); + state = GU_NONE; + home.guideDone(false); + } + } + + // watch for finished guides + if (guideActionAxis1 == GA_NONE && guideActionAxis2 == GA_NONE) state = GU_NONE; +} + +// enables or disables backlash for the GUIDE_DISABLE_BACKLASH option +void Guide::backlashEnableControl(bool enable) { + #if GUIDE_DISABLE_BACKLASH == ON + axis1.setBacklash(enable ? mount.settings.backlash.axis1 : 0.0F); + axis2.setBacklash(enable ? mount.settings.backlash.axis2 : 0.0F); + #else + UNUSED(enable); + #endif +} + +Guide guide; + +#endif diff --git a/src/telescope/mount/guide/Guide.h b/src/telescope/mount/guide/Guide.h new file mode 100755 index 0000000..44f20d0 --- /dev/null +++ b/src/telescope/mount/guide/Guide.h @@ -0,0 +1,135 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +#include "../../../libApp/commands/ProcessCmds.h" +#include "../../../lib/sound/Sound.h" +#include "../../../lib/axis/Axis.h" +#include "../coordinates/Transform.h" + +// default time limit for guiding home is 10 minutes +#define GUIDE_HOME_TIME_LIMIT 600.0 + +// default time for spiral guides is 103.4 seconds +#define GUIDE_SPIRAL_TIME_LIMIT 103.4 +enum GuideState: uint8_t {GU_NONE, GU_PULSE_GUIDE, GU_GUIDE, GU_SPIRAL_GUIDE, GU_HOME_GUIDE, GU_HOME_GUIDE_ABORT}; +enum GuideRateSelect: uint8_t {GR_QUARTER, GR_HALF, GR_1X, GR_2X, GR_4X, GR_8X, GR_20X, GR_48X, GR_HALF_MAX, GR_MAX, GR_CUSTOM}; +enum GuideAction: uint8_t {GA_NONE, GA_BREAK, GA_FORWARD, GA_REVERSE, GA_SPIRAL, GA_HOME }; + +#pragma pack(1) +#define GuideSettingsSize 3 +typedef struct GuideSettings { + GuideRateSelect pulseRateSelect; + GuideRateSelect axis1RateSelect; + GuideRateSelect axis2RateSelect; +} GuideSettings; +#pragma pack() + +class Guide { + public: + void init(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // start guide at a given direction and rate on Axis1 + CommandError startAxis1(GuideAction guideAction, GuideRateSelect rateSelect, unsigned long guideTimeLimit); + + // stop guide on Axis1, use GA_BREAK to stop in either direction or specifiy the direction to be stopped GA_FORWARD or GA_REVERSE + // set abort true to rapidly stop (broken limit, etc) + void stopAxis1(GuideAction stopDirection = GA_BREAK, bool abort = false); + + // start guide at a given direction and rate on Axis2 + CommandError startAxis2(GuideAction guideAction, GuideRateSelect rateSelect, unsigned long guideTimeLimit); + + // stop guide on Axis2, use GA_BREAK to stop in either direction or specifiy the direction to be stopped GA_FORWARD or GA_REVERSE + // set abort true to rapidly stop (broken limit, etc) + void stopAxis2(GuideAction stopDirection = GA_BREAK, bool abort = false); + + // start spiral guide at the specified rate (spiral size is porportional to rate) + CommandError startSpiral(GuideRateSelect rateSelect, unsigned long guideTimeLimit); + + // start guide home (for use with home switches) + CommandError startHome(); + + // stop both axes of guide + void stop(); + + // abort both axes of guide + void abort(); + + // returns true if a pulse guide is happening + inline bool activePulseGuide() { return state == GU_PULSE_GUIDE; } + + // returns true if a guide is happening + inline bool active() { + return (guideActionAxis1 != GA_NONE || guideActionAxis2 != GA_NONE) && !activePulseGuide(); + } + + // returns true if any guide is happening on Axis1 + inline bool activeAxis1() { return guideActionAxis1 != GA_NONE; } + + // returns true if any guide is happening on Axis2 + inline bool activeAxis2() { return guideActionAxis2 != GA_NONE; } + + void poll(); + + void spiralPoll(); + + // enables or disables backlash for the GUIDE_DISABLE_BACKLASH option + void backlashEnableControl(bool enable); + + GuideState state = GU_NONE; + + float rateAxis1 = 0.0F; + float rateAxis2 = 0.0F; + + GuideSettings settings = { GR_HALF, GR_20X, GR_20X }; + + private: + // keep guide rate <= half max + float limitGuideRate(float rate); + + // return guide rate (sidereal x) for guide rate selection + float rateSelectToRate(GuideRateSelect rateSelect, uint8_t axis = 1); + + // valid guide for Axis1 + bool validAxis1(GuideAction guideAction); + + // valid guide for Axis2 + bool validAxis2(GuideAction guideAction); + + // general validation of guide request + CommandError validate(int axis, GuideAction guideAction); + + // start axis1 movement + void axis1AutoSlew(GuideAction guideAction); + + // start axis2 movement + void axis2AutoSlew(GuideAction guideAction); + + GuideRateSelect spiralGuideRateSelect = GR_20X; + + GuideAction guideActionAxis1 = GA_NONE; + GuideAction guideActionAxis2 = GA_NONE; + + PierSide pierSide = PIER_SIDE_NONE; + + float customRateAxis1 = 0.0F; + float customRateAxis2 = 0.0F; + + float spiralScaleAxis1 = 0.0F; + unsigned long spiralStartTime = 0; + + unsigned long guideFinishTimeAxis1 = 0; + unsigned long guideFinishTimeAxis2 = 0; + + uint32_t nvKey; +}; + +extern Guide guide; + +#endif diff --git a/src/telescope/mount/home/Home.command.cpp b/src/telescope/mount/home/Home.command.cpp new file mode 100755 index 0000000..262cf08 --- /dev/null +++ b/src/telescope/mount/home/Home.command.cpp @@ -0,0 +1,94 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, home commands + +#include "Home.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/nv/Nv.h" + +#include "../limits/Limits.h" +#include "../park/Park.h" +#include "../site/Site.h" + +bool Home::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + UNUSED(reply); + UNUSED(suppressFrame); + if (command[0] == 'h') { + // :h?# Get home status - has sense, auto home enabled, offset axis1, offset axis2 (in arcseconds.) + // Returns: n,n,n,n# + if (command[1] == '?' && parameter[0] == 0) { + sprintf(reply, "%d,%ld,%ld", (int)hasSense, settings.axis1.senseOffset, settings.axis2.senseOffset); + *numericReply = false; + } else + + // :hAn# Set auto home state. + // Returns: Nothing + if (command[1] == 'A' && parameter[1] == 0) { + switch (parameter[0]) { + case '0': settings.automaticAtBoot = false; break; + case '1': settings.automaticAtBoot = true; break; + default: *commandError = CE_PARAM_RANGE; break; + } + nv().kv().put(nvKey, settings); + + *numericReply = false; + } else + + // :hC# Moves mount to the home position + // Returns: Nothing + if (command[1] == 'C' && parameter[0] == 0) { + *commandError = request(); + *numericReply = false; + } else + + // :hC1,n# Set home sense direction and home offset for axis1 arcseconds + // Returns: Nothing + if (command[1] == 'C' && parameter[0] == '1' && parameter[1] == ',') { + if (parameter[2] == 'R' && parameter[3] == 0) { + settings.axis1.senseReverse = !settings.axis1.senseReverse; + V("MSG: Set Axis1 home reverse "); VL(settings.axis1.senseReverse); + setReversal(); + } else { + long l = atol(¶meter[2]); + if (l >= -648000 || l <= 648000) { + settings.axis1.senseOffset = l; + } else *commandError = CE_PARAM_RANGE; + } + nv().kv().put(nvKey, settings); + *numericReply = false; + } else + + // :hC2,n# Set home sense direction and home offset for axis2 arcseconds + // Returns: Nothing + if (command[1] == 'C' && parameter[0] == '2' && parameter[1] == ',') { + if (parameter[2] == 'R' && parameter[3] == 0) { + settings.axis2.senseReverse = !settings.axis2.senseReverse; + V("MSG: Set Axis2 home reverse "); VL(settings.axis2.senseReverse); + setReversal(); + } else { + long l = atol(¶meter[2]); + if (l >= -648000 || l <= 648000) { + settings.axis2.senseOffset = l; + } else *commandError = CE_PARAM_RANGE; + } + nv().kv().put(nvKey, settings); + *numericReply = false; + } else + + // :hF# Reset mount at the home position. This position is required for a cold Start. + // Point to the celestial pole. GEM w/counterweights pointing downwards (CWD position). Equatorial fork mounts at HA = 0. + // Returns: Nothing + if (command[1] == 'F' && parameter[0] == 0) { + *commandError = reset(true); + park.reset(); + limits.enabled(site.isDateTimeReady()); + *numericReply = false; + } else return false; + + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/mount/home/Home.cpp b/src/telescope/mount/home/Home.cpp new file mode 100755 index 0000000..4cd6594 --- /dev/null +++ b/src/telescope/mount/home/Home.cpp @@ -0,0 +1,424 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, homing + +#include "Home.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../Telescope.h" +#include "../Mount.h" +#include "../goto/Goto.h" +#include "../guide/Guide.h" +#include "../limits/Limits.h" + +// init the home position (according to settings and mount type) +void Home::init() { + nvKey = nv().kv().computeKey("HOME_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings)) { DLF("WRN: Nv, init failed for HOME_SETTINGS"); } + + settings.automaticAtBoot = constrain(settings.automaticAtBoot, false, true); + settings.axis1.senseOffset = constrain(settings.axis1.senseOffset, degToArcsecF(-180), degToArcsecF(180)); + settings.axis1.senseReverse = constrain(settings.axis1.senseReverse, false, true); + settings.axis2.senseOffset = constrain(settings.axis2.senseOffset, degToArcsecF(-180), degToArcsecF(180)); + settings.axis2.senseReverse = constrain(settings.axis2.senseReverse, false, true); + + #ifndef AXIS1_HOME_DEFAULT + if (transform.mountType == GEM) position.h = Deg90; else { position.h = 0.0L; position.z = 0.0L; position.aa1 = 0.0L; } + #else + if (transform.mountType == ALTAZM) position.z = degToRad(AXIS1_HOME_DEFAULT); else + if (transform.mountType == ALTALT) position.aa1 = degToRad(AXIS1_HOME_DEFAULT); else position.h = degToRad(AXIS1_HOME_DEFAULT); + #endif + + #ifndef AXIS2_HOME_DEFAULT + if (transform.mountType == ALTAZM) position.a = 0.0; else + if (transform.mountType == ALTALT) position.aa2 = 0.0; else position.d = site.locationEx.latitude.sign*Deg90; + #else + if (transform.mountType == ALTAZM) position.a = degToRad(AXIS2_HOME_DEFAULT); + if (transform.mountType == ALTALT) position.aa2 = degToRad(AXIS2_HOME_DEFAULT); else position.d = degToRad(AXIS2_HOME_DEFAULT); + #endif + + setReversal(); + + position.pierSide = PIER_SIDE_NONE; +} + +// move mount to the home position +CommandError Home::request() { + #if GOTO_FEATURE == OFF + DLF("WRN: Mount, home rejected because goto is disabled"); + return CE_SLEW_ERR_UNSPECIFIED; + #endif + + failed = false; + if (!mount.startupAuthorityTrusted() && !hasSense) { + DLF("WRN: Mount, home rejected because startup authority is not trusted and no home switches are present"); + return CE_SLEW_ERR_UNSPECIFIED; + } + if (!site.dateIsReady || !site.timeIsReady) return CE_SLEW_ERR_IN_STANDBY; + if (goTo.state != GS_NONE) return CE_SLEW_IN_MOTION; + if (guide.state != GU_NONE) { + if (guide.state == GU_HOME_GUIDE) guide.stop(); + return CE_SLEW_IN_MOTION; + } + + wasTracking = mount.isTracking(); + #if AXIS1_SECTOR_GEAR == ON || AXIS2_TANGENT_ARM == OFF + mount.tracking(false); + #endif + + // make sure the motors are powered on + mount.enable(true); + goTo.firstGoto = false; + + Coordinate homePosition = getPosition(CR_MOUNT_EQU); + + if (hasSense) { + CommandError e; + + #if AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + double a1 = axis1.getInstrumentCoordinate() - homePosition.a1; + double a2 = axis2.getInstrumentCoordinate() - homePosition.a2; + + // both -180 to 180 + VF("MSG: Mount, homing from a1="); V(radToDeg(a1)); VF(" degrees a2="); V(radToDeg(a2)); VLF(" degrees"); + if (abs(a1) > degToRad(AXIS1_SENSE_HOME_DIST_LIMIT) - abs(arcsecToRad(settings.axis1.senseOffset))) return CE_SLEW_ERR_OUTSIDE_LIMITS; + if (abs(a2) > degToRad(AXIS2_SENSE_HOME_DIST_LIMIT) - abs(arcsecToRad(settings.axis2.senseOffset))) return CE_SLEW_ERR_OUTSIDE_LIMITS; + + e = reset(false); + if (e != CE_NONE) return e; + #endif + + VLF("MSG: Mount, guiding to home"); + state = HS_HOMING; + isRequestWithReset = false; + #if MOUNT_COORDS_MEMORY == ON + mount.saveCoordinateMemory(false); + #endif + e = guide.startHome(); + if (e != CE_NONE) { + state = HS_NONE; + return e; + } + } else { + #if AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + VLF("MSG: Mount, moving to home"); + state = HS_HOMING; + + CommandError result = goTo.request(homePosition, PSS_EAST_ONLY, false); + if (result != CE_NONE) { + DF("WRN: Mount, moving to home goto failed (code "); D(result); DLF(")"); + state = HS_NONE; + } + return result; + #else + #if AXIS1_SECTOR_GEAR == ON + VLF("MSG: Mount, moving SG to home"); + axis1.setFrequencySlew(goTo.rate); + axis1.setTargetCoordinate(axis1.getIndexPosition()); + VLF("Mount, axis1 home target coordinates set"); + axis1.autoGoto(degToRadF((float)(SLEW_ACCELERATION_DIST))); + #endif + #if AXIS2_TANGENT_ARM == ON + VLF("MSG: Mount, moving TA to home"); + axis2.setFrequencySlew(goTo.rate*((float)(AXIS2_SLEW_RATE_PERCENT)/100.0F)); + axis2.setTargetCoordinate(axis2.getIndexPosition()); + VLF("Mount, axis2 home target coordinates set"); + axis2.autoGoto(degToRadF((float)(SLEW_ACCELERATION_DIST))); + #endif + #endif + } + return CE_NONE; +} + +// reset mount, moves to the home position first if home switches are present +CommandError Home::requestWithReset() { + #if GOTO_FEATURE == OFF + DLF("WRN: Mount, reset/home rejected because goto is disabled"); + return CE_SLEW_ERR_UNSPECIFIED; + #endif + + if (!mount.startupAuthorityTrusted() && !hasSense) { + DLF("WRN: Mount, reset/home rejected because startup authority is not trusted and no home switches are present"); + return CE_SLEW_ERR_UNSPECIFIED; + } + if (hasSense) { + CommandError result = request(); + isRequestWithReset = true; + return result; + } else return reset(); +} + +// clear home state on abort +void Home::requestAborted() { + state = HS_NONE; + failed = true; + mount.tracking(wasTracking); +} + +// after finding home switches displace the mount axes as specified +void Home::guideDone(bool success) { + if (!success) { + state = HS_NONE; + failed = true; + CommandError e = reset(isRequestWithReset); + if (e != CE_NONE) { + DF("WRN: Home::guideDone(), reset after sensed home failure failed (code "); D(e); DLF(")"); + } else { + DLF("WRN: Home::guideDone(), sensed home failed"); + } + return; + } + + #if AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + if (useOffset()) { + CommandError e = reset(isRequestWithReset, true); + if (e != CE_NONE) { + state = HS_NONE; + failed = true; + DF("WRN: Home::guideDone(), reset after sensed home failed (code "); D(e); DLF(")"); + return; + } + + #if MOUNT_HOME_AT_OFFSETS == OFF + VF("MSG: Mount, finishing move to home with goto to ("); + double a1 = axis1.getInstrumentCoordinate() - arcsecToRad(site.locationEx.latitude.sign*settings.axis1.senseOffset); + double a2 = axis2.getInstrumentCoordinate() - arcsecToRad(settings.axis2.senseOffset); + V(radToDeg(a1)); VF(","); V(radToDeg(a2)); VLF(")"); + axis1.setTargetCoordinate(a1); + axis1.autoGoto(goTo.getRadsPerSecond()); + axis2.setTargetCoordinate(a2); + axis2.autoGoto(goTo.getRadsPerSecond()); + #endif + mount.syncFromOnStepToEncoders = true; + + state = HS_NONE; + failed = false; + } else { + state = HS_NONE; + CommandError e = reset(isRequestWithReset, true); + if (e != CE_NONE) { + failed = true; + DF("WRN: Home::guideDone(), reset after sensed home failed (code "); D(e); DLF(")"); + } else { + failed = false; + mount.syncFromOnStepToEncoders = true; + } + } + #else + state = HS_NONE; + failed = false; + + #if AXIS1_SECTOR_GEAR == ON + VLF("MSG: Mount, sector gear set origin"); + double h = axis1.getInstrumentCoordinate(); + if (axis1.resetPosition(0.0L) != 0) { DLF("WRN: Home::guideDone(), failed to resetPosition Axis1"); exit; } + if (limits.setInstrumentCoordinate(1, h, true) != CE_NONE) { DLF("WRN: Home::guideDone(), failed to setInstrumentCoordinate Axis1"); exit; } + mount.tracking(wasTracking); + #endif + + #if AXIS2_TANGENT_ARM == ON + VLF("MSG: Mount, tangent arm set origin"); + double d = axis2.getInstrumentCoordinate(); + if (axis2.resetPosition(0.0L) != 0) { DLF("WRN: Home::guideDone(), failed to resetPosition Axis2"); exit; } + if (limits.setInstrumentCoordinate(2, d, true) != CE_NONE) { DLF("WRN: Home::guideDone(), failed to setInstrumentCoordinate Axis2"); exit; } + #endif + + mount.captureNominalIndexPositions(); + #endif +} + +// once homed mark as done +void Home::requestDone() { + state = HS_NONE; + failed = false; + reset(false); +} + +// reset mount at home +CommandError Home::reset(bool fullReset, bool authoritative) { + #if GOTO_FEATURE == ON + if (goTo.state != GS_NONE) { + axis1.autoSlewAbort(); + axis2.autoSlewAbort(); + return CE_SLEW_IN_MOTION; + } + #endif + if (guide.state != GU_NONE) { + if (guide.state == GU_HOME_GUIDE) guide.stop(); + return CE_SLEW_IN_MOTION; + } + + // refresh the home position + init(); + + // stop tracking and set default rate + mount.tracking(false); + mount.trackingRate = hzToSidereal(TRACKING_RATE_DEFAULT_HZ); + mount.trackingRateOffsetRA = 0.0F; + mount.trackingRateOffsetDec = 0.0F; + goTo.firstGoto = true; + + tasks.yieldMicros(10000); + + Coordinate homePosition = getPosition(CR_MOUNT); + + if (!goTo.absoluteEncodersPresent) { + #if MOUNT_HOME_AT_OFFSETS == OFF + if (useOffset() && state == HS_HOMING && !fullReset) { + homePosition.a1 += arcsecToRad(site.locationEx.latitude.sign*settings.axis1.senseOffset); + homePosition.a2 += arcsecToRad(settings.axis2.senseOffset); + } + #endif + + bool bypassCoordinateLimit = authoritative; + #if MOUNT_STARTUP_MODE == SA_AUTO && MOUNT_COORDS_MEMORY == OFF + if (!bypassCoordinateLimit && !axis1.motor->hasAbsoluteEncoder() && !axis2.motor->hasAbsoluteEncoder()) { + bypassCoordinateLimit = true; + } + #endif + + CommandError e = limits.validateInstrumentCoordinate(1, homePosition.a1, bypassCoordinateLimit); + if (e == CE_NONE) e = limits.validateInstrumentCoordinate(2, homePosition.a2, bypassCoordinateLimit); + if (e != CE_NONE) { + DLF("WRN: Home::reset(), coordinate reset rejected by sync threshold"); + if (fullReset) mount.enable(MOUNT_ENABLE_IN_STANDBY == ON); + return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + + if (axis1.resetPosition(0.0L) != 0) { + DLF("WRN: Home::reset(), failed to resetPosition Axis1"); + if (fullReset) mount.enable(MOUNT_ENABLE_IN_STANDBY == ON); + return CE_SLEW_IN_MOTION; + } + if (axis2.resetPosition(0.0L) != 0) { + DLF("WRN: Home::reset(), failed to resetPosition Axis2"); + if (fullReset) mount.enable(MOUNT_ENABLE_IN_STANDBY == ON); + return CE_SLEW_IN_MOTION; + } + + e = limits.setInstrumentCoordinate(1, homePosition.a1, true); + if (e == CE_NONE) e = limits.setInstrumentCoordinate(2, homePosition.a2, true); + if (e != CE_NONE) { + if (fullReset) mount.enable(MOUNT_ENABLE_IN_STANDBY == ON); + return e; + } + } + + mount.captureNominalIndexPositions(); + + axis1.setBacklash(mount.settings.backlash.axis1); + axis2.setBacklash(mount.settings.backlash.axis2); + + axis1.setFrequencySlew(degToRadF(0.1F)); + axis2.setFrequencySlew(degToRadF(0.1F)); + + // make sure the motors are powered off + if (fullReset) { + mount.enable(MOUNT_ENABLE_IN_STANDBY == ON); + + #if GOTO_FEATURE == ON + goTo.alignReset(); + #endif + + VF("MSG: Mount, reset at home ("); + V(radToDeg(homePosition.a1)); VF(","); V(radToDeg(homePosition.a2)); + VLF(") and in standby"); + } else { + VF("MSG: Mount, reset at home ("); + V(radToDeg(homePosition.a1)); VF(","); V(radToDeg(homePosition.a2)); + VLF(")"); + } + + if (authoritative) mount.setStartupAuthorityTrusted(true); + + return CE_NONE; +} + +// get the home position (Mount coordinate system) +Coordinate Home::getPosition(CoordReturn coordReturn) { + Coordinate homePosition = position; + + homePosition.pierSide = PIER_SIDE_EAST; + + #if MOUNT_HOME_AT_OFFSETS == ON + if (useOffset()) { + if (transform.mountType == ALTAZM) { + homePosition.z += arcsecToRad(settings.axis1.senseOffset); + homePosition.a += arcsecToRad(settings.axis2.senseOffset); + } else + if (transform.mountType == ALTALT) { + homePosition.aa1 += arcsecToRad(settings.axis1.senseOffset); + homePosition.aa2 += arcsecToRad(settings.axis2.senseOffset); + } else { + homePosition.h += arcsecToRad(site.locationEx.latitude.sign*settings.axis1.senseOffset); + homePosition.d += arcsecToRad(settings.axis2.senseOffset); + } + } + #endif + + if (transform.mountType == ALTAZM) { + homePosition.a1 = homePosition.z; + homePosition.a2 = homePosition.a; + } else + if (transform.mountType == ALTALT) { + homePosition.a1 = homePosition.aa1; + homePosition.a2 = homePosition.aa2; + } else { + homePosition.a1 = homePosition.h; + homePosition.a2 = homePosition.d; + + if (site.location.latitude >= 0.0) { + if (homePosition.d > Deg90) { + homePosition.pierSide = PIER_SIDE_WEST; + homePosition.h -= Deg180; + homePosition.d = Deg180 - homePosition.d; + } + } else { + if (homePosition.d < -Deg90) { + homePosition.pierSide = PIER_SIDE_WEST; + homePosition.h -= Deg180; + homePosition.d = (-Deg180) - homePosition.d; + } + } + } + + switch (coordReturn) { + case CR_MOUNT: + break; + case CR_MOUNT_EQU: + if (transform.mountType == ALTAZM) transform.horToEqu(&homePosition); + if (transform.mountType == ALTALT) transform.aaToEqu(&homePosition); + transform.hourAngleToRightAscension(&homePosition, true); + break; + case CR_MOUNT_ALT: + case CR_MOUNT_HOR: + if (transform.isEquatorial()) transform.equToHor(&homePosition); + else if (transform.mountType == ALTALT) transform.aaToHor(&homePosition); + break; + case CR_MOUNT_ALL: + if (transform.mountType == ALTAZM) transform.horToEqu(&homePosition); else + if (transform.mountType == ALTALT) transform.aaToEqu(&homePosition); else transform.equToHor(&homePosition); + transform.hourAngleToRightAscension(&homePosition, true); + break; + } + + return homePosition; +} + +bool Home::useOffset() { + if (hasSense && (settings.axis1.senseOffset != 0 || settings.axis2.senseOffset != 0)) return true; else return false; +} + +void Home::setReversal() { + bool latitudeReversal = transform.isEquatorial() && site.locationEx.latitude.sign < 0.0; + axis1.setReverse(latitudeReversal); + if (!settings.axis1.senseReverse) axis1.setHomeReverse(latitudeReversal); else axis1.setHomeReverse(!latitudeReversal); + axis2.setHomeReverse(settings.axis2.senseReverse); +} + +Home home; + +#endif diff --git a/src/telescope/mount/home/Home.h b/src/telescope/mount/home/Home.h new file mode 100755 index 0000000..a103e3d --- /dev/null +++ b/src/telescope/mount/home/Home.h @@ -0,0 +1,81 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount home control +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +#include "../../../libApp/commands/ProcessCmds.h" +#include "../coordinates/Transform.h" + +enum HomeState: uint8_t {HS_NONE, HS_HOMING}; + +#pragma pack(1) +#define SettingsSize 11 +typedef struct HomeSense { + long senseOffset; + bool senseReverse; +} HomeSense; +typedef struct Settings { + bool automaticAtBoot; + HomeSense axis1; + HomeSense axis2; +} Settings; +#pragma pack() + +class Home { + public: + // init the home position (according to settings and mount type) + void init(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // move mount to the home position + CommandError request(); + + // reset mount, moves to the home position first if home switches are present + CommandError requestWithReset(); + + // clear home state on abort + void requestAborted(); + + // after finding home switches displace the mount axes as specified + void guideDone(bool success); + + // once homed mark as done + void requestDone(); + + // reset mount at home + CommandError reset(bool fullReset = true, bool authoritative = false); + + // get the home position (Mount coordinate system) + Coordinate getPosition(CoordReturn coordReturn = CR_MOUNT_EQU); + + // home sensing + bool useOffset(); + + // allow axis1 by latitude reversal and home switch reversal + void setReversal(); + + bool hasSense = (((AXIS1_SENSE_HOME) != OFF) && ((AXIS2_SENSE_HOME) != OFF)) || \ + (((AXIS1_SECTOR_GEAR) == ON) && ((AXIS1_SENSE_HOME) != OFF)) || \ + (((AXIS2_TANGENT_ARM) == ON) && ((AXIS2_SENSE_HOME) != OFF)); + + Settings settings = {MOUNT_AUTO_HOME_DEFAULT == ON, {AXIS1_SENSE_HOME_OFFSET, false}, {AXIS2_SENSE_HOME_OFFSET, false}}; + + bool isRequestWithReset = false; + bool failed = false; + + HomeState state; + + private: + bool wasTracking = false; + Coordinate position; + + uint32_t nvKey; +}; + +extern Home home; + +#endif diff --git a/src/telescope/mount/library/Library.command.cpp b/src/telescope/mount/library/Library.command.cpp new file mode 100755 index 0000000..f440bbe --- /dev/null +++ b/src/telescope/mount/library/Library.command.cpp @@ -0,0 +1,183 @@ +//-------------------------------------------------------------------------------------------------- +// telescope celestial object library, commands + +#include "Library.h" + +#if defined(MOUNT_PRESENT) + +#include "../coordinates/Transform.h" +#include "../goto/Goto.h" + +char const *ObjectStr[] = {"UNK", "OC", "GC", "PN", "DN", "SG", "EG", "IG", "KNT", "SNR", "GAL", "CN", "STR", "PLA", "CMT", "AST"}; + +bool Library::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + *suppressFrame = false; + + // L - Object Library Commands + if (command[0] == 'L') { + + // :LB# Find previous catalog object subject to the current constraints + // Returns: Nothing + if (command[1] == 'B' && parameter[0] == 0) { + prevRec(); + *numericReply = false; + } else + + // :LC[n]# Set current target object to catalog object number + // Returns: Nothing + if (command[1] == 'C') { + int16_t i; + if (convert.atoi2(parameter, &i)) { + if (i >= 0) { + gotoRec(i); + *numericReply = false; + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + // :LI# Get Object Information + // Returns: s# (string containing the current target object’s name and object type) + if (command[1] == 'I' && parameter[0] == 0) { + int i; + Coordinate target; + readVars(reply, &i, &target.r, &target.d); + + char const *objType = ObjectStr[i]; + strcat(reply, ","); + strcat(reply, objType); + *numericReply = false; + } else + + // :LIG# Get catalog object information and goto target + // Returns: Nothing + if (command[1] == 'I' && parameter[0] == 'G' && parameter[1] == 0) { + int i; + Coordinate target; + readVars(reply, &i, &target.r, &target.d); + goTo.setGotoTarget(&target); + *numericReply = false; + } else + + // :LR# Get catalog object information including RA and Dec, with advance to next record + // Returns: s# (string containing the current target object’s name, type, RA, and Dec) + if (command[1] == 'R' && parameter[0] == 0) { + int i; + Coordinate target; + readVars(reply, &i, &target.r, &target.d); + goTo.setGotoTarget(&target); + + char const * objType = ObjectStr[i]; + char ws[20]; + + strcat(reply, ","); + strcat(reply, objType); + if (strcmp(reply, ",UNK") != 0) { + convert.doubleToHms(ws, radToHrs(target.r), false, PM_HIGH); strcat(reply, ","); strcat(reply, ws); + convert.doubleToDms(ws, radToDeg(target.d), false, true, PM_HIGH); strcat(reply, ","); strcat(reply, ws); + } + + nextRec(); + *numericReply = false; + } else + + // :LW[s]# Write catalog object information including current target RA,Dec to next available empty record + // If at the end of the object list (:LI# command returns an empty string "#") a new item is automatically added + // [s] is a string of up to eleven chars followed by a comma and a type designation for ex. ":LWM31 AND,GAL#" + // Returns: 0 on failure (memory full, for example) + // 1 on success + if (command[1] == 'W') { + char name[12]; + char objType[4]; + + // extract object name + int l = 0; + do { + name[l] = 0; + if (parameter[l] == ',') break; + name[l] = parameter[l]; + name[l + 1] = 0; + l++; + } while (l < 12); + + // extract object type + int16_t i = 0; + if (parameter[l] == ',') { + l++; + int16_t m = 0; + do { + objType[m + 1] = 0; + objType[m] = parameter[l]; + l++; + m++; + } while (parameter[l] != 0); + // encode + // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 + // "UNK", "OC", "GC", "PN", "DN", "SG", "EG", "IG", "KNT", "SNR", "GAL", "CN", "STR", "PLA", "CMT", "AST" + for (l = 0; l <= 15; l++) { if (strcmp(objType, ObjectStr[l]) == 0) i = l; } + } + + Coordinate target = goTo.getGotoTarget(); + if (firstFreeRec()) writeVars(name, i, target.r, target.d); else *commandError = CE_LIBRARY_FULL; + } else + + // :LN# Find next catalog object subject to the current constraints + // Returns: Nothing + if (command[1] == 'N' && parameter[0] == 0) { + nextRec(); + *numericReply = false; + } else + + // :L$# Move to catalog name record + // Returns: 1 + if (command[1] == '$' && parameter[0] == 0) { + nameRec(); + *numericReply = true; + } else + + // :LD# Clear current record + // Returns: Nothing + if (command[1] == 'D' && parameter[0] == 0) { + clearCurrentRec(); + *numericReply = false; + } else + + // :LL# Clear current catalog + // Returns: Nothing + if (command[1] == 'L' && parameter[0] == 0) { + clearLib(); + *numericReply = false; + } else + + // :L!# Clear library (all catalogs) + // Returns: Nothing + if (command[1] == '!' && parameter[0] == 0) { + clearAll(); + *numericReply = false; + } else + + // :L?# Get library free records (all catalogs) + // Returns: n# + if (command[1] == '?' && parameter[0] == 0) { + sprintf(reply, "%u", recFreeAll()); + *numericReply = false; + } else + + // :Lo[n]# Select Library catalog by catalog number n + // Catalog number ranges from 0..14, catalogs 0..6 are user defined, the remainder are reserved + // Return: 0 on failure + // 1 on success + if (command[1] == 'o') { + int16_t i; + if (convert.atoi2(parameter, &i)) { + if (i >= 0 && i <= 14) { + setCatalog(i); + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else *commandError = CE_CMD_UNKNOWN; + + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/mount/library/Library.cpp b/src/telescope/mount/library/Library.cpp new file mode 100755 index 0000000..a01f30e --- /dev/null +++ b/src/telescope/mount/library/Library.cpp @@ -0,0 +1,324 @@ +// ----------------------------------------------------------------------------------- +// telescope celestial object library + +#include "Library.h" + +#if defined(MOUNT_PRESENT) + +#include "../../../lib/nv/Nv.h" + +#include "../../Telescope.h" + +char const * objectStr[] = {"UNK", "OC", "GC", "PN", "DN", "SG", "EG", "IG", "KNT", "SNR", "GAL", "CN", "STR", "PLA", "CMT", "AST"}; + +bool Library::init() { + if (ready) return true; + + catalog = 0; + NvVolume& nvVolume = nv().volume(); + if (!nvVolume.isMounted()) { + DLF("WRN: Library::init(); NV volume not mounted"); + return false; + } + + // Bind IvPartition to the LIBRARY partition by name. + if (nvIv.init(nvVolume, "LIBRARY")) { + VLF("MSG: Nv, partition 'LIBRARY' mounted"); + } else { + DLF("WRN: Library::init(); can't find library partition!"); + return false; + } + + recMax = nvIv.sizeBytes()/rec_size; // maximum number of records + + if (recMax == 0) { + DLF("WRN: Library::init(); recMax == 0, no library space available"); + return false; + } + + VF("MSG: Mount, library allocated "); V(recMax); VLF(" catalog records"); + + ready = true; + + firstRec(); + + return true; +} + +// select catalog by number (0 to 14) +bool Library::setCatalog(int num) { + if (!ready) return false; + if (num < 0 || num > 14) return false; + + catalog = num; + return firstRec(); +} + +// write current record from data +// \param name: object name (to 12 chars) +// \param code: object classification (0 to 15) +// \param RA: in radians +// \param Dec: in radians +void Library::writeVars(char* name, int code, double RA, double Dec) { + if (!ready) return; + + libRec_t work; + + for (int16_t l = 0; l < 11; l++) work.libRec.name[l] = name[l]; + work.libRec.code = (code | (catalog << 4)); + + // convert from radians to degrees + RA = radToDeg(RA); + Dec = radToDeg(Dec); + + // convert into an unsigned int, RA=0..360 + RA = degRange(RA)/360.0; + // convert into an unsigned int, Dec=0..180 + if (Dec > 90.0) Dec = 90.0; + if (Dec < -90.0) Dec = -90.0; + Dec = Dec + 90.0; Dec = Dec/180.0; + uint16_t r = round(RA*65536.0); + uint16_t d = round(Dec*65536.0); + + work.libRec.RA = r; + work.libRec.Dec = d; + + writeRec(recPos, work); +} + +// read data for the current record +// \param name: object name (to 12 chars) +// \param code: object classification (0 to 15) +// \param RA: in radians +// \param Dec: in radians +void Library::readVars(char* name, int* code, double* RA, double* Dec) { + if (!ready) { name[0] = 0; *code = 0; *RA = 0.0; *Dec = 0.0; return; } + + libRec_t work; + work = readRec(recPos); + + int16_t cat = work.libRec.code >> 4; + + // empty? or not found + if (cat == 15 || cat != catalog) { name[0] = 0; *code = 0; *RA = 0.0; *Dec = 0.0; return; } + + for (int16_t l = 0; l < 11; l++) name[l] = work.libRec.name[l]; + name[11] = 0; + + *code = work.libRec.code & 15; + uint16_t r = work.libRec.RA; + uint16_t d = work.libRec.Dec; + + // convert from unsigned int + *RA = (double)r; + *RA = (*RA/65536.0)*360.0; + *Dec = (double)d; + *Dec = ((*Dec/65536.0)*180.0)-90.0; + + // convert from degrees to radians + *RA = degToRad(*RA); + *Dec = degToRad(*Dec); +} + +// move to catalogs first rec +bool Library::firstRec() { + if (!ready) return false; + + libRec_t work; + + // see if first record is for the currentLib + recPos = 0; + work = readRec(recPos); + int16_t cat = (int16_t)work.libRec.code >> 4; + if (work.libRec.name[0] != '$' && cat == catalog) return true; + + // otherwise find the first one, if it exists + return nextRec(); +} + +// move to the catalog name rec +bool Library::nameRec() { + if (!ready) return false; + + for (recPos = 0; recPos < recMax; ++recPos) { + const libRec_t work = readRec(recPos); + const int16_t cat = (int16_t)(work.libRec.code >> 4); + + if (work.libRec.name[0] == '$' && cat == catalog) return true; + } + + // not found, point at the last record + recPos = (uint16_t)(recMax - 1); + return false; +} + +// move to the first unused record for this catalog +bool Library::firstFreeRec() { + if (!ready) return false; + + for (recPos = 0; recPos < recMax; ++recPos) { + const libRec_t work = readRec(recPos); + const int16_t cat = (int16_t)(work.libRec.code >> 4); + + if (cat == 15) return true; // unused slot + } + + // not found, point at the last record + recPos = (uint16_t)(recMax - 1); + return false; +} + +bool Library::prevRec() { + if (!ready) return false; + + while (true) { + if (recPos == 0) return false; // no earlier record + + recPos--; + libRec_t work = readRec(recPos); + int16_t cat = (int16_t)(work.libRec.code >> 4); + + if (work.libRec.name[0] != '$' && cat == catalog) return true; + } +} + +// move to the next record, if it exists +bool Library::nextRec() { + if (!ready) return false; + + while (true) { + if (recPos >= (uint16_t)(recMax - 1)) return false; + + ++recPos; + + libRec_t work = readRec(recPos); + int16_t cat = (int16_t)(work.libRec.code >> 4); + + if (work.libRec.name[0] != '$' && cat == catalog) return true; + } +} + +// move to the specified record (of this catalog), if it exists +bool Library::gotoRec(uint16_t num) { + if (!ready) return false; + + libRec_t work; + int16_t cat; + uint16_t lastAddress = 0; + uint16_t count = 0; + + for (uint16_t address = 0; address < recMax; address++) { + work = readRec(address); + lastAddress = address; + + cat = (int16_t)work.libRec.code >> 4; + if (work.libRec.name[0] != '$' && cat == catalog) count++; + if (count == num) break; + } + if (count == num) { recPos = lastAddress; return true; } else return false; +} + +// actual number of records for this catalog +uint16_t Library::recCount() { + if (!ready) return 0; + + libRec_t work; + int16_t cat; + uint16_t count = 0; + + for (uint16_t address = 0; address < recMax; address++) { + work = readRec(address); + + cat = (int16_t)work.libRec.code >> 4; + if (work.libRec.name[0] != '$' && cat == catalog) count++; + } + + return count; +} + +// actual number of records for this library +uint16_t Library::recCountAll() { + if (!ready) return 0; + + libRec_t work; + int16_t cat; + uint16_t c = 0; + + for (uint16_t address = 0; address < recMax; address++) { + work = readRec(address); + + cat = (int16_t)work.libRec.code >> 4; + if (cat >= 0 && cat <= 14) c++; + } + + return c; +} + +// clears this record +void Library::clearCurrentRec() { + if (!ready) return; + + libRec_t work; + int16_t cat; + + work = readRec(recPos); + + cat = (int16_t)work.libRec.code >> 4; + if (cat == catalog) clearRec(recPos); +} + +// clears this library +void Library::clearLib() { + if (!ready) return; + + libRec_t work; + int16_t cat; + + for (uint16_t address = 0; address < recMax; address++) { + work = readRec(address); + + cat = (int16_t)work.libRec.code >> 4; + if (cat == catalog) clearRec(address); + } +} + +// clear library (clear all catalogs) +void Library::clearAll() { + if (!ready) return; + for (uint16_t l = 0; l < recMax; l++) clearRec(l); +} + +// number records available for this library +uint16_t Library::recFreeAll() { + if (!ready) return 0; + return recMax - recCountAll(); +} + +libRec_t Library::readRec(uint16_t address) { + libRec_t work{}; + if (!ready) return work; + if (address >= recMax) return work; + + nvIv.readBlock(address, work.libRecBytes); + return work; +} + +void Library::writeRec(uint16_t address, libRec_t data) { + if (!ready) return; + if (address >= recMax) return; + + nvIv.writeBlock(address, data.libRecBytes); +} + +void Library::clearRec(uint16_t address) { + if (!ready) return; + if (address >= recMax) return; + + uint16_t index = address*rec_size; + uint8_t code = 0xF0; + nvIv.writeBytes(index + 11, &code, 1); // catalog code 15 = unused +} + +Library library; + +#endif \ No newline at end of file diff --git a/src/telescope/mount/library/Library.h b/src/telescope/mount/library/Library.h new file mode 100755 index 0000000..865010b --- /dev/null +++ b/src/telescope/mount/library/Library.h @@ -0,0 +1,119 @@ +// ----------------------------------------------------------------------------------- +// telescope celestial object library +#pragma once + +#include "../../../Common.h" + +#if defined(MOUNT_PRESENT) + +#include "../../../lib/convert/Convert.h" +#include "../../../lib/nv/NvIvPartition.h" +#include "../../../libApp/commands/ProcessCmds.h" + +#if AXIS1_PEC == ON + #define NV_LIBRARY_DATA_BASE NV_PEC_BUFFER_BASE + PEC_BUFFER_SIZE_LIMIT +#else + #define NV_LIBRARY_DATA_BASE NV_PEC_BUFFER_BASE + 0 +#endif + +#pragma pack(1) +const int rec_size = 16; +typedef struct { + char name[11]; // 11 + byte code; // 1 (low 4 bits are object class, high are catalog #) + uint16_t RA; // 2 + uint16_t Dec; // 2 +} libRecBase_t; + +typedef union { + libRecBase_t libRec; + byte libRecBytes[rec_size]; +} libRec_t; +#pragma pack() + +class Library +{ + public: + bool init(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // select catalog by number (0 to 14) + bool setCatalog(int num); + + // write current record from data + // \param name: object name (to 12 chars) + // \param code: object classification (0 to 15) + // \param RA: in radians + // \param Dec: in radians + void writeVars(char* name, int code, double RA, double Dec); + + // read data for the current record + // \param name: object name (to 12 chars) + // \param code: object classification (0 to 15) + // \param RA: in radians + // \param Dec: in radians + void readVars(char* name, int* code, double* RA, double* Dec); + + // move to catalogs first rec + bool firstRec(); + + // move to the catalog name rec + bool nameRec(); + + // move to the first unused record for this catalog + bool firstFreeRec(); + + // move to the previous record, if it exists + bool prevRec(); + + // move to the next record, if it exists + bool nextRec(); + + // move to the specified record (of this catalog), if it exists + bool gotoRec(uint16_t num); + + // actual number of records for this catalog + uint16_t recCount(); + + // actual number of records for this library + uint16_t recCountAll(); + + // clears this record + void clearCurrentRec(); + + // clears this library + void clearLib(); + + // clears all libraries + void clearAll(); + + // number records available for this library + uint16_t recFreeAll(); + + private: + // flag for successful initialization + bool ready = false; + + // currently selected record# + uint16_t recPos; + + // last record# + uint16_t recMax; + + // 16 byte record + libRec_t list; + + libRec_t readRec(uint16_t address); + void writeRec(uint16_t address, libRec_t data); + void clearRec(uint16_t address); + inline double degRange(double d) { while (d >= 360.0) d -= 360.0; while (d < 0.0) d += 360.0; return d; } + + int catalog; + + IvPartition nvIv; +}; + +extern Library library; + +#endif diff --git a/src/telescope/mount/limits/Limits.command.cpp b/src/telescope/mount/limits/Limits.command.cpp new file mode 100755 index 0000000..8b0fcbf --- /dev/null +++ b/src/telescope/mount/limits/Limits.command.cpp @@ -0,0 +1,107 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount limits commands + +#include "Limits.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../Telescope.h" +#include "../Mount.h" +#include "../site/Site.h" + +bool Limits::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + *suppressFrame = false; + + if (command[0] == 'G') { + // :Gh# Get Horizon Limit, the minimum elevation of the mount relative to the horizon + // Returns: sDD*# + if (command[1] == 'h' && parameter[0] == 0) { + sprintf(reply, "%+02ld*", lroundf(radToDegF(settings.altitude.min))); + *numericReply = false; + } else + + // :Go# Get Overhead Limit + // Returns: DD*# + // The highest elevation above the horizon that the telescope will goto + if (command[1] == 'o' && parameter[0] == 0) { + sprintf(reply, "%02ld*", lroundf(radToDegF(settings.altitude.max))); + *numericReply=false; + } else + + // :GXE[m]# Get Other Limit [m] + // Returns: n# + if (command[1] == 'X' && parameter[0] == 'E' && parameter[2] == 0) { + *numericReply = false; + switch (parameter[1]) { + case '9': sprintf(reply, "%ld",lroundf(radToDegF(settings.pastMeridianE)*4.0F)); break; // minutes past meridianE + case 'A': sprintf(reply, "%ld",lroundf(radToDegF(settings.pastMeridianW)*4.0F)); break; // minutes past meridianW + case 'e': sprintf(reply, "%ld",lroundf(radToDegF(axis1.getLimitMin()))); break; // RA east or -Az limit, in degrees + case 'w': sprintf(reply, "%ld",lroundf(radToDegF(axis1.getLimitMax()))); break; // RA west or +Az limit, in degrees + case 'B': sprintf(reply, "%ld",lroundf(radToDegF(axis1.getLimitMax())/15.0F)); break; // RA west or +Az limit, in hours + case 'C': sprintf(reply, "%ld",lroundf(radToDegF(axis2.getLimitMin()))); break; // Dec south or -Alt limit, in degrees + case 'D': sprintf(reply, "%ld",lroundf(radToDegF(axis2.getLimitMax()))); break; // Dec north or +Alt limit, in degrees + default: return false; + } + } else return false; + } else + + if (command[0] == 'S') { + // :Sh[sDD]# + // Set the elevation lower limit + // Return: 0 on failure + // 1 on success + if (command[1] == 'h') { + int16_t deg; + if (convert.atoi2(parameter, °)) { + if (deg >= -30.0F && deg <= 30.0F) { + settings.altitude.min = degToRadF(deg); + nv().kv().put(nvKey, settings); + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + // :So[DD]# + // Set the overhead elevation limit in degrees relative to the horizon + // Return: 0 on failure + // 1 on success + if (command[1] == 'o') { + int16_t deg; + if (convert.atoi2(parameter, °)) { + if (deg >= 60.0F && deg <= 90.0F) { + settings.altitude.max = degToRadF(deg); + nv().kv().put(nvKey, settings); + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + // :SXE9,[n]# + // :SXEA,[n]# + // Set meridian limit east (9) or west (A) to value [n] in minutes + // Return: 0 on failure or 1 on success + if (command[1] == 'X' && parameter[0] == 'E' && parameter[2] == ',') { + long l = atol(¶meter[3]); float degs = l/4.0; + switch (parameter[1]) { + case '9': + if (degs >= -360.0F && degs <= 360.0F) { + settings.pastMeridianE = degToRadF(degs); + nv().kv().put(nvKey, settings); + } else *commandError = CE_PARAM_RANGE; + break; + case 'A': + if (degs >= -360.0F && degs <= 360.0F) { + settings.pastMeridianW = degToRadF(degs); + nv().kv().put(nvKey, settings); + } else *commandError = CE_PARAM_RANGE; + break; + default: return false; + } + } else return false; + } else return false; + + return true; +} + +#endif \ No newline at end of file diff --git a/src/telescope/mount/limits/Limits.cpp b/src/telescope/mount/limits/Limits.cpp new file mode 100755 index 0000000..73dbde8 --- /dev/null +++ b/src/telescope/mount/limits/Limits.cpp @@ -0,0 +1,504 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, limits + +#include "Limits.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../../telescope/Telescope.h" +#include "../Mount.h" +#include "../goto/Goto.h" +#include "../guide/Guide.h" +#include "../site/Site.h" + +inline void limitsWrapper() { limits.poll(); } + +void Limits::init() { + + nvKey = nv().kv().computeKey("LIMIT_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings)) { DLF("WRN: Nv, init failed for LIMIT_SETTINGS"); } + + settings.altitude.min = constrain(settings.altitude.min, degToRadF(-30), degToRadF(30)); + settings.altitude.max = constrain(settings.altitude.max, degToRadF(60), degToRadF(90)); + settings.pastMeridianE = constrain(settings.pastMeridianE, degToRadF(-360), degToRadF(360)); + settings.pastMeridianW = constrain(settings.pastMeridianW, degToRadF(-360), degToRadF(360)); + + // start limit monitor task + VF("MSG: Mount, start limits monitor task (rate 100ms priority 2)... "); + if (tasks.add(100, 0, true, 2, limitsWrapper, "MtLimit")) { VLF("success"); } else { VLF("FAILED!"); } +} + +// target coordinate check ahead of sync, goto, etc. +CommandError Limits::validateTarget(Coordinate *coords, bool isGoto) { + bool eastReachable, westReachable; + double eastCorrection, westCorrection; + return validateTarget(coords, &eastReachable, &westReachable, &eastCorrection, &westCorrection, isGoto); +} + +CommandError Limits::validateInstrumentCoordinate(uint8_t axisNumber, double value, bool bypass) { + if (bypass) return CE_NONE; + if (!limitsEnabled) return CE_NONE; + + #if AXIS1_WRAP == ON + return CE_NONE; + #else + #if AXIS1_SECTOR_GEAR == ON || AXIS2_TANGENT_ARM == ON + return CE_NONE; + #endif + + double delta; + long proposedIndexSteps; + long nominalIndexSteps; + + switch (axisNumber) { + case 1: + if (AXIS1_LIMIT_SYNC == OFF) return CE_NONE; + proposedIndexSteps = lround(value*axis1.getStepsPerMeasure()) - (axis1.getInstrumentCoordinateSteps() - axis1.getIndexPositionSteps()); + nominalIndexSteps = mount.getNominalIndexPositionSteps(1); + delta = fabs((double)(proposedIndexSteps - nominalIndexSteps))/axis1.getStepsPerMeasure(); + if (delta > degToRadF((float)AXIS1_LIMIT_SYNC)) { + VLF("MSG: Mount, sync axis1 rejected (exceeds threshold)"); + return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + break; + case 2: + if (AXIS2_LIMIT_SYNC == OFF) return CE_NONE; + proposedIndexSteps = lround(value*axis2.getStepsPerMeasure()) - (axis2.getInstrumentCoordinateSteps() - axis2.getIndexPositionSteps()); + nominalIndexSteps = mount.getNominalIndexPositionSteps(2); + delta = fabs((double)(proposedIndexSteps - nominalIndexSteps))/axis2.getStepsPerMeasure(); + if (delta > degToRadF((float)AXIS2_LIMIT_SYNC)) { + VLF("MSG: Mount, sync axis2 rejected (exceeds threshold)"); + return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + break; + default: + return CE_PARAM_RANGE; + } + + return CE_NONE; + #endif +} + +CommandError Limits::setInstrumentCoordinate(uint8_t axisNumber, double value, bool bypass) { + CommandError e = validateInstrumentCoordinate(axisNumber, value, bypass); + if (e != CE_NONE) return e; + + switch (axisNumber) { + case 1: + axis1.setInstrumentCoordinate(value); + break; + case 2: + axis2.setInstrumentCoordinate(value); + break; + default: + return CE_PARAM_RANGE; + } + + return CE_NONE; +} + +// target coordinate check ahead of sync, goto, etc. +CommandError Limits::validateTarget(Coordinate *coords, bool *eastReachable, bool *westReachable, double *eastCorrection, double *westCorrection, bool isGoto) { + if (flt(coords->a, settings.altitude.min)) return CE_SLEW_ERR_BELOW_HORIZON; + if (fgt(coords->a, settings.altitude.max)) return CE_SLEW_ERR_ABOVE_OVERHEAD; + + double a1e, a2e, a1w, a2w; + + PierSide lastPierSide = coords->pierSide; + coords->pierSide = PIER_SIDE_EAST; + transform.mountToInstrument(coords, &a1e, &a2e); + coords->pierSide = PIER_SIDE_WEST; + transform.mountToInstrument(coords, &a1w, &a2w); + coords->pierSide = lastPierSide; + + float eastLimitMin = axis1.getLimitMin(); + float eastLimitMax = axis1.getLimitMax(); + float westLimitMin = axis1.getLimitMin(); + float westLimitMax = axis1.getLimitMax(); + + if (AXIS1_SECTOR_GEAR == ON) { + if (isGoto) { + *eastReachable = true; + *westReachable = true; + return CE_NONE; + } else { + eastLimitMin = -Deg180; + eastLimitMax = Deg180; + westLimitMin = -Deg180; + westLimitMax = Deg180; + } + } + + if (transform.mountType == GEM) { + if (-limits.settings.pastMeridianE > eastLimitMin) eastLimitMin = -limits.settings.pastMeridianE; + if (limits.settings.pastMeridianW < westLimitMax) westLimitMax = limits.settings.pastMeridianW; + westLimitMin += Deg180; + westLimitMax += Deg180; + } + + double a1 = axis1.getInstrumentCoordinate(); + + bool inRange; + if ((a1e >= eastLimitMin) && (a1e <= eastLimitMax)) inRange = true; else inRange = false; + + if ((a1e + Deg360 > eastLimitMin) && (a1e + Deg360 < eastLimitMax) && inRange && (dist(a1, a1e) > dist(a1, a1e + Deg360))) { + a1e += Deg360; + *eastCorrection = Deg360; + VF("MSG: Mount, validate destination east axis1 best normalized to "); VL(radToDeg(a1e)); + } else + if ((a1e - Deg360 > eastLimitMin) && (a1e - Deg360 < eastLimitMax) && inRange && (dist(a1, a1e) > dist(a1, a1e - Deg360))) { + a1e -= Deg360; + *eastCorrection = -Deg360; + VF("MSG: Mount, validate destination east axis1 best normalized to "); VL(radToDeg(a1e)); + } + + if ((a1e + Deg360 > eastLimitMin) && (a1e + Deg360 < eastLimitMax) && !inRange) { + a1e += Deg360; + *eastCorrection = Deg360; + VF("MSG: Mount, validate destination east axis1 normalized to "); VL(radToDeg(a1e)); + } else + if ((a1e - Deg360 > eastLimitMin) && (a1e - Deg360 < eastLimitMax) && !inRange) { + a1e -= Deg360; + *eastCorrection = -Deg360; + VF("MSG: Mount, validate destination east axis1 normalized to "); VL(radToDeg(a1e)); + } + + if ((a1w >= westLimitMin) && (a1w <= westLimitMax)) inRange = true; else inRange = false; + + if ((a1w + Deg360 > westLimitMin) && (a1w + Deg360 < westLimitMax) && inRange && (dist(a1, a1w) > dist(a1, a1w + Deg360))) { + a1w += Deg360; + *westCorrection = Deg360; + VF("MSG: Mount, validate destination best west axis1 normalized to "); VL(radToDeg(a1w)); + } else + if ((a1w - Deg360 > westLimitMin) && (a1w - Deg360 < westLimitMax) && inRange && (dist(a1, a1w) > dist(a1, a1w - Deg360))) { + a1w -= Deg360; + *westCorrection = -Deg360; + VF("MSG: Mount, validate destination best west axis1 normalized to "); VL(radToDeg(a1w)); + } + + if ((a1w + Deg360 > westLimitMin) && (a1w + Deg360 < westLimitMax) && !inRange) { + a1w += Deg360; + *westCorrection = Deg360; + VF("MSG: Mount, validate destination west axis1 normalized to "); VL(radToDeg(a1w)); + } else + if ((a1w - Deg360 > westLimitMin) && (a1w - Deg360 < westLimitMax) && !inRange) { + a1w -= Deg360; + *westCorrection = -Deg360; + VF("MSG: Mount, validate destination west axis1 normalized to "); VL(radToDeg(a1w)); + } + + *eastReachable = a1e >= eastLimitMin && a1e <= eastLimitMax; + *westReachable = a1w >= westLimitMin && a1w <= westLimitMax; + + VF("MSG: Mount, validate east target axis1 "); V(radToDeg(eastLimitMin)); VF(" < "); V(radToDeg(a1e)); VF(" < "); V(radToDeg(eastLimitMax)); + if (*eastReachable) { VLF(" TRUE"); } else { VLF(" FALSE"); } + VF("MSG: Mount, validate west target axis1 "); V(radToDeg(westLimitMin)); VF(" < "); V(radToDeg(a1w)); VF(" < "); V(radToDeg(westLimitMax)); + if (*westReachable) { VLF(" TRUE"); } else { VLF(" FALSE"); } + + if (!*eastReachable && !*westReachable) { + VLF("MSG: Mount, validate target outside limits"); + *eastCorrection = 0.0; + *westCorrection = 0.0; + return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + + #if AXIS2_TANGENT_ARM == OFF + if (transform.isEquatorial()) { + if (flt(coords->d, axis2.getLimitMin())) { + VF("MSG: Mount, validate failed Dec past min limit by "); + V(radToDeg(coords->d - axis2.getLimitMin())*3600.0); VLF(" arc-secs"); + return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + if (fgt(coords->d, axis2.getLimitMax())) { + VF("MSG: Mount, validate failed Dec past max limit by "); + V(radToDeg(coords->d - axis2.getLimitMax())*3600.0); VLF(" arc-secs"); + return CE_SLEW_ERR_OUTSIDE_LIMITS; + } + } + #endif + + return CE_NONE; +} + +// true if an error exists +bool Limits::isError() { + return initError.nv || + initError.value || + initError.tls || + error.altitude.min || + error.altitude.max || + error.limit.axis1.min || + error.limit.axis1.max || + error.limit.axis2.min || + error.limit.axis2.max || + error.limitSense.axis1.min || + error.limitSense.axis1.max || + error.limitSense.axis2.min || + error.limitSense.axis2.max || + error.meridian.east || + error.meridian.west; +} + +// true if an error exists that impacts goto safety +bool Limits::isGotoError() { + return initError.nv || + initError.value || + !site.dateIsReady || + !site.timeIsReady || + error.limitSense.axis1.min || + error.limitSense.axis1.max || + error.limitSense.axis2.min || + error.limitSense.axis2.max; +} + +// return general error code +uint8_t Limits::errorCode() { + enum GeneralErrors: uint8_t { + ERR_NONE, ERR_MOTOR_FAULT, ERR_ALT_MIN, ERR_LIMIT_SENSE, ERR_DEC, ERR_AZM, + ERR_UNDER_POLE, ERR_MERIDIAN, ERR_SYNC, ERR_PARK, ERR_GOTO_SYNC, ERR_UNSPECIFIED, + ERR_ALT_MAX, ERR_WEATHER_INIT, ERR_SITE_INIT, ERR_NV_INIT}; + + // priority highest to lowest + if (mount.motorFault()) return (uint8_t)ERR_MOTOR_FAULT; + if (error.limitSense.axis1.min || error.limitSense.axis1.max || + error.limitSense.axis2.min || error.limitSense.axis2.max) return (uint8_t)ERR_LIMIT_SENSE; + if (error.altitude.min) return (uint8_t)ERR_ALT_MIN; + if (error.altitude.max) return (uint8_t)ERR_ALT_MAX; + if (transform.isEquatorial()) { + if (error.limit.axis1.min || error.limit.axis1.max) return (uint8_t)ERR_UNDER_POLE; + if (error.limit.axis2.min || error.limit.axis2.max) return (uint8_t)ERR_DEC; + } else { + if (error.limit.axis1.min || error.limit.axis1.max) return (uint8_t)ERR_AZM; + if (error.limit.axis2.min) return (uint8_t)ERR_ALT_MIN; + if (error.limit.axis2.max) return (uint8_t)ERR_ALT_MAX; + } + if (error.meridian.east || error.meridian.west) return (uint8_t)ERR_MERIDIAN; + if (initError.nv || initError.value) return (uint8_t)ERR_NV_INIT; + if (initError.tls) return (uint8_t)ERR_SITE_INIT; + if (initError.weather) return (uint8_t)ERR_WEATHER_INIT; + return ERR_NONE; +} + +void Limits::stop() { + #if GOTO_FEATURE == ON + goTo.abort(); + #endif + guide.stopAxis1(GA_BREAK, true); + guide.stopAxis2(GA_BREAK, true); + mount.tracking(false); +} + +void Limits::stopAxis1(GuideAction stopDirection) { + #if GOTO_FEATURE == ON + goTo.abort(); + #endif + guide.stopAxis1(stopDirection, true); + if (stopDirection == GA_FORWARD || !transform.isEquatorial()) mount.tracking(false); +} + +void Limits::stopAxis2(GuideAction stopDirection) { + #if GOTO_FEATURE == ON + goTo.abort(); + #endif + guide.stopAxis2(stopDirection, true); + if (!transform.isEquatorial()) mount.tracking(false); +} + +void Limits::poll() { + if (limitsDisablePeriodDs > 0) limitsDisablePeriodDs--; + + LimitsError lastError = error; + + Coordinate current = mount.getMountPosition(CR_MOUNT_ALT); + + #if TRACK_AUTOSTART == OFF && TRACK_WITHOUT_LIMITS == OFF + if (!limitsEnabled && mount.isTracking()) { + VLF("MSG: Mount, tracking without limits disallowed"); + mount.tracking(false); + } + #endif + + if (limitsEnabled && guide.state != GU_HOME_GUIDE && guide.state != GU_HOME_GUIDE_ABORT) { + // overhead and horizon limits + if (current.a < settings.altitude.min && limitsDisablePeriodDs == 0) error.altitude.min = true; else error.altitude.min = false; + + if (fabs(settings.altitude.max - Deg90) > OneArcSec) { + if (current.a > settings.altitude.max) error.altitude.max = true; else error.altitude.max = false; + } else error.altitude.max = false; + + // meridian limits + if (transform.mountType == GEM && current.pierSide == PIER_SIDE_EAST) { + if (current.h < -settings.pastMeridianE) { + stopAxis1(GA_REVERSE); + error.meridian.east = true; + } else error.meridian.east = false; + } else error.meridian.east = false; + + if (transform.mountType == GEM && current.pierSide == PIER_SIDE_WEST) { + if (current.h > settings.pastMeridianW && limitsDisablePeriodDs == 0) { + #if GOTO_FEATURE == ON && AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + if (goTo.isAutoFlipEnabled() && mount.isTracking()) { + limitsDisablePeriod(1.0F); + VLF("MSG: Mount, start automatic meridian flip"); + Coordinate target = mount.getMountPosition(); + CommandError e = goTo.request(target, PSS_EAST_ONLY, false); + if (e != CE_NONE) { + stopAxis1(GA_FORWARD); + error.meridian.west = true; + DF("WRN: Limits::limitPoll(), goto for automatic meridian flip failed ("); D(e); DL(")"); + } + } else + #endif + { + stopAxis1(GA_FORWARD); + error.meridian.west = true; + } + } else error.meridian.west = false; + } else error.meridian.west = false; + + // for Fork and Alt/Azm mounts limits are based on shaft angles + // so convert axis1 into normal PIER_SIDE_EAST coordinates + if (transform.mountType != GEM && current.pierSide == PIER_SIDE_WEST) current.a1 += Deg180; + + if (mount.isHome()) current.a1 = home.getPosition(CR_MOUNT).a1; + + #if AXIS1_SECTOR_GEAR == ON + current.a1 = axis1.getMotorPosition(); + #endif + + // min and max limits + if (flt(current.a1, axis1.getLimitMin())) { + stopAxis1(GA_REVERSE); + error.limit.axis1.min = true; + // --------------------------------------------------------- + if (lastError.limit.axis1.min != error.limit.axis1.min) { + D("WRN: Mount, limits min axis1 "); D(radToDeg(current.a1)); D(" > "); D(radToDeg(axis1.getLimitMin())); + D(" ("); D(current.pierSide == PIER_SIDE_WEST ? "W" : "E"); D(")"); + DLF(" FAILED"); + V("MSG: Mount, axis1 = "); VL(degToRad(axis1.getInstrumentCoordinate())); + } + // --------------------------------------------------------- + } else error.limit.axis1.min = false; + + if (fgt(current.a1, axis1.getLimitMax()) && limitsDisablePeriodDs == 0) { + #if GOTO_FEATURE == ON && AXIS1_SECTOR_GEAR == OFF && AXIS2_TANGENT_ARM == OFF + if (current.pierSide == PIER_SIDE_EAST && goTo.isAutoFlipEnabled() && mount.isTracking()) { + limitsDisablePeriod(1.0F); + VLF("MSG: Mount, start automatic meridian flip"); + Coordinate target = mount.getMountPosition(); + CommandError e = goTo.request(target, PSS_WEST_ONLY, false); + if (e != CE_NONE) { + stopAxis1(GA_FORWARD); + error.limit.axis1.max = true; + DF("WRN: Limits::limitPoll(), goto for automatic meridian flip failed ("); D(e); DL(")"); + } + } else + #endif + { + stopAxis1(GA_FORWARD); + error.limit.axis1.max = true; + // ------------------------------------------------------------- + if (lastError.limit.axis1.max != error.limit.axis1.max) { + D("WRN: Mount, limits max axis1 "); D(radToDeg(current.a1)); D(" < "); D(radToDeg(axis1.getLimitMax())); + DLF(" FAILED"); + } + // ------------------------------------------------------------- + } + } else error.limit.axis1.max = false; + + if (mount.isHome()) current.a2 = home.getPosition(CR_MOUNT).a2; + + #if AXIS2_TANGENT_ARM == ON + current.a2 = axis2.getMotorPosition(); + #endif + + if (flt(current.a2, axis2.getLimitMin())) { + stopAxis2((current.pierSide == PIER_SIDE_EAST) ? GA_REVERSE : GA_FORWARD); + error.limit.axis2.min = true; + } else error.limit.axis2.min = false; + + if (fgt(current.a2, axis2.getLimitMax())) { + stopAxis2((current.pierSide == PIER_SIDE_EAST) ? GA_FORWARD : GA_REVERSE); + error.limit.axis2.max = true; + } else error.limit.axis2.max = false; + + } else { + error.altitude.min = false; + error.altitude.max = false; + error.limit.axis1.min = false; + error.limit.axis1.max = false; + error.limit.axis2.min = false; + error.limit.axis2.max = false; + error.meridian.east = false; + error.meridian.west = false; + } + + // min and max limit switches + error.limitSense.axis1.min = axis1.motionErrorSensed(DIR_REVERSE); + if (error.limitSense.axis1.min) { + if (!axis1.commonMinMaxSense || lastError.limitSense.axis1.min != error.limitSense.axis1.min) stopAxis1(GA_REVERSE); + } + + error.limitSense.axis1.max = axis1.motionErrorSensed(DIR_FORWARD); + if (error.limitSense.axis1.max) { + if (!axis1.commonMinMaxSense || lastError.limitSense.axis1.max != error.limitSense.axis1.max) stopAxis1(GA_FORWARD); + } + + error.limitSense.axis2.min = axis2.motionErrorSensed(DIR_REVERSE); + if (error.limitSense.axis2.min) { + if (!axis2.commonMinMaxSense || lastError.limitSense.axis2.min != error.limitSense.axis2.min) + stopAxis2((current.pierSide == PIER_SIDE_EAST) ? GA_REVERSE : GA_FORWARD); + } + + error.limitSense.axis2.max = axis2.motionErrorSensed(DIR_FORWARD); + if (error.limitSense.axis2.max) { + if (!axis2.commonMinMaxSense || lastError.limitSense.axis2.max != error.limitSense.axis2.max) + stopAxis2((current.pierSide == PIER_SIDE_EAST) ? GA_FORWARD : GA_REVERSE); + } + + #if DEBUG == VERBOSE + const char* errPre = "MSG: Mount, limit state: "; + if (lastError.altitude.min != error.altitude.min || + lastError.altitude.max != error.altitude.max || + lastError.meridian.east != error.meridian.east || + lastError.meridian.west != error.meridian.west || + lastError.limit.axis1.min != error.limit.axis1.min || + lastError.limit.axis1.max != error.limit.axis1.max || + lastError.limit.axis2.min != error.limit.axis2.min || + lastError.limit.axis2.max != error.limit.axis2.max) { + V(errPre); + V(error.altitude.min? "Alt-< " :"Alt- "); + V(error.altitude.max? "Alt+< " :"Alt+ "); + V(error.meridian.east? "ME< " :"ME " ); + V(error.meridian.west? "MW< " :"MW " ); + V(error.limit.axis1.min? "A1L-< " :"A1L- "); + V(error.limit.axis1.max? "A1L+< " :"A1L+ "); + V(error.limit.axis2.min? "A2L-< " :"A2L- "); + V(error.limit.axis2.max? "A2L+< " :"A2L+ "); + } + #endif + + // respond to overhead and horizon limits + if (transform.mountType == ALTAZM) { + if (error.altitude.min) stopAxis2(GA_REVERSE); + if (error.altitude.max) stopAxis2(GA_FORWARD); + } else { + if (!lastError.altitude.min && error.altitude.min) stop(); + if (!lastError.altitude.max && error.altitude.max) stop(); + } + + // if first time breaking a meridian or min/max limit stop all guides + if ((!lastError.meridian.east && error.meridian.east) || + (!lastError.meridian.west && error.meridian.west) || + (!lastError.limit.axis1.min && error.limit.axis1.min) || + (!lastError.limit.axis1.max && error.limit.axis1.max) || + (!lastError.limit.axis2.min && error.limit.axis2.min) || + (!lastError.limit.axis2.max && error.limit.axis2.max)) stop(); +} + +Limits limits; + +#endif diff --git a/src/telescope/mount/limits/Limits.h b/src/telescope/mount/limits/Limits.h new file mode 100755 index 0000000..eed9fbc --- /dev/null +++ b/src/telescope/mount/limits/Limits.h @@ -0,0 +1,126 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +#include "../guide/Guide.h" + +// Limit defaults +#ifndef LIMIT_HORIZON +#define LIMIT_HORIZON -10.0F +#endif +#ifndef LIMIT_OVERHEAD +#define LIMIT_OVERHEAD 80.0F +#endif +#ifndef LIMIT_MERIDIAN_EAST +#define LIMIT_MERIDIAN_EAST 15.0F +#endif +#ifndef LIMIT_MERIDIAN_WEST +#define LIMIT_MERIDIAN_WEST 15.0F +#endif + +#pragma pack(1) +typedef struct AltitudeLimits { + float min; // in radians + float max; // in radians +} AltitudeLimits; + +#define LimitSettingsSize 16 +typedef struct LimitSettings { + AltitudeLimits altitude; + float pastMeridianE; // in radians + float pastMeridianW; // in radians +} LimitSettings; +#pragma pack() + +typedef struct MerdianError { + uint8_t east; + uint8_t west; +} MerdianError; + +typedef struct MinMaxError { + uint8_t min; + uint8_t max; +} MinMaxError; + +typedef struct AxisMinMaxError { + MinMaxError axis1; + MinMaxError axis2; +} AxisMinMaxError; + +typedef struct LimitsError { + MinMaxError altitude; + AxisMinMaxError limit; + AxisMinMaxError limitSense; + MerdianError meridian; +} LimitsError; + +class Limits { + public: + void init(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // target coordinate check ahead of sync, goto, etc. + CommandError validateTarget(Coordinate *coords, bool isGoto); + CommandError validateTarget(Coordinate *coords, bool *eastReachable, bool *westReachable, double *eastCorrection, double *westCorrection, bool isGoto); + + // validate and apply an instrument coordinate change for a mount axis + CommandError validateInstrumentCoordinate(uint8_t axisNumber, double value, bool bypass = false); + CommandError setInstrumentCoordinate(uint8_t axisNumber, double value, bool bypass = false); + + // true if an limit related error is exists + bool isError(); + + // true if an error exists that impacts goto safety + bool isGotoError(); + + // true if an below horizon limit + inline bool isBelowHorizon() { return error.altitude.min; } + + // true if an above overhead limit + inline bool isAboveOverhead() { return error.altitude.max; } + + // true if past meridian limit west + inline bool isPastMeridianW() { return error.meridian.west; } + + // true if past the axis1 max limit + inline bool isPastAxis1Max() { return error.limit.axis1.max; } + + // return general error code + uint8_t errorCode(); + + // enable or disable limit enforcement + inline void enabled(bool state) { limitsEnabled = state; } + + // check if limits are being enforced + inline bool isEnabled() { return limitsEnabled; } + + // disable meridian limits for the specified period in seconds + void limitsDisablePeriod(float seconds) { limitsDisablePeriodDs = lroundf(seconds*10.0F); } + + void poll(); + + LimitSettings settings = { { degToRadF(LIMIT_HORIZON), degToRadF(LIMIT_OVERHEAD) }, degToRadF(LIMIT_MERIDIAN_EAST), degToRadF(LIMIT_MERIDIAN_WEST) }; + + private: + void stop(); + void stopAxis1(GuideAction stopDirection = GA_BREAK); + void stopAxis2(GuideAction stopDirection = GA_BREAK); + // get least distance between coordinates + inline double dist(double a, double b) { if (a > b) return a - b; else return b - a; } + + bool limitsEnabled = false; + LimitsError error; + + int limitsDisablePeriodDs = 0; // in deciseconds (0.1s) + + uint32_t nvKey; +}; + +extern Limits limits; + +#endif diff --git a/src/telescope/mount/park/Park.command.cpp b/src/telescope/mount/park/Park.command.cpp new file mode 100755 index 0000000..efd97bc --- /dev/null +++ b/src/telescope/mount/park/Park.command.cpp @@ -0,0 +1,37 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, park commands + +#include "Park.h" + +#if defined(MOUNT_PRESENT) + +bool Park::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + UNUSED(reply); + UNUSED(suppressFrame); + UNUSED(numericReply); + if (command[0] == 'h') { + // :hP# Moves mount to the park position + // Return: 0 on failure + // 1 on success + if (command[1] == 'P' && parameter[0] == 0) { + CommandError e = request(); + if (e == CE_NONE) *commandError = CE_1; else { VF("MSG: Mount, park error "); VL(e); *commandError = e; } + } else + + // :hQ# Set the mount park position + // Return: 0 on failure + // 1 on success + if (command[1] == 'Q' && parameter[0] == 0) *commandError = set(); else + + // :hR# Restore parked mount to operation + // Return: 0 on failure + // 1 on success + if (command[1] == 'R' && parameter[0] == 0) { + CommandError e = restore(true); + if (e == CE_NONE) *commandError = CE_1; else { VF("MSG: Mount, unpark error "); VL(e); *commandError = e; } + } else return false; + } else return false; + + return true; +} +#endif diff --git a/src/telescope/mount/park/Park.cpp b/src/telescope/mount/park/Park.cpp new file mode 100755 index 0000000..81f06af --- /dev/null +++ b/src/telescope/mount/park/Park.cpp @@ -0,0 +1,323 @@ +// ----------------------------------------------------------------------------------- +// telescope mount control, parking + +#include "Park.h" + +#if defined(MOUNT_PRESENT) + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../Telescope.h" +#include "../Mount.h" +#include "../goto/Goto.h" +#include "../guide/Guide.h" +#include "../home/Home.h" +#include "../limits/Limits.h" +#include "../../../lib/sense/Sense.h" + +void parkSignalWrapper() { park.signal(); } + +void Park::init() { + nvKey = nv().kv().computeKey("PARK_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings)) { DLF("WRN: Nv, init failed for PARK_SETTINGS"); } + state = settings.state; + if (!settings.saved) set(true); + + // configure any associated sense/signal pins + #if (PARK_SENSE) != OFF && (PARK_SENSE_PIN) != OFF + VLF("MSG: Mount, park adding sense"); + parkSenseHandle = sense.add(PARK_SENSE_PIN, PARK_SENSE_INIT, PARK_SENSE); + #endif + + #if (PARK_SIGNAL) != OFF && (PARK_SIGNAL_PIN) != OFF + VLF("MSG: Mount, park adding signal"); + parkSignalHandle = sense.add(PARK_SIGNAL_PIN, PARK_SIGNAL_INIT, PARK_SIGNAL); + + VF("MSG: Mount, start park signal monitor task (rate 1000ms priority 4)... "); + if (tasks.add(1000, 0, true, 4, parkSignalWrapper, "ParkSgl")) { VLF("success"); } else { VLF("FAILED!"); } + #endif +} + +// sets the park position +CommandError Park::set(bool ignoreTrust) { + if (state == PS_PARK_FAILED) return CE_PARK_FAILED; + if (state == PS_PARKED) return CE_PARKED; + if (goTo.state != GS_NONE) return CE_SLEW_IN_MOTION; + if (guide.state != GU_NONE) return CE_SLEW_IN_MOTION; + if (mount.motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + if (!ignoreTrust && !mount.startupAuthorityTrusted()) { + DLF("WRN: Mount, set park rejected because startup authority is not trusted"); + return CE_SLEW_ERR_UNSPECIFIED; + } + + VLF("MSG: Mount, setting park position"); + + bool wasTracking = mount.isTracking(); + mount.tracking(false); + + // get our current position in equatorial coordinates + Coordinate current = mount.getPosition(CR_MOUNT_EQU); + + // save as the park position + settings.state = state; + settings.position.h = current.h; + settings.position.d = current.d; + settings.position.pierSide = current.pierSide; + if (settings.position.pierSide == PIER_SIDE_NONE) { + if (transform.meridianFlips) { + if (current.h < 0) settings.position.pierSide = PIER_SIDE_WEST; else settings.position.pierSide = PIER_SIDE_EAST; + } else settings.position.pierSide = PIER_SIDE_EAST; + } + settings.saved = true; + nv().kv().put(nvKey, settings); + + #if ALIGN_MAX_NUM_STARS > 1 + transform.align.modelWrite(); + #endif + + mount.tracking(wasTracking); + + VLF("MSG: Mount, setting park done"); + return CE_NONE; +} + +// move the mount to the park position +CommandError Park::request() { + if (!settings.saved) return CE_NO_PARK_POSITION_SET; + if (state == PS_PARKED) return CE_NONE; + if (state == PS_PARKING) return CE_PARK_FAILED; + if (state == PS_PARK_FAILED) return CE_PARK_FAILED; + if (!mount.startupAuthorityTrusted()) { + DLF("WRN: Mount, park rejected because startup authority is not trusted"); + return CE_SLEW_ERR_UNSPECIFIED; + } + if (!mount.isEnabled()) return CE_SLEW_ERR_IN_STANDBY; + if (goTo.state != GS_NONE) return CE_SLEW_IN_MOTION; + if (guide.state != GU_NONE) return CE_SLEW_IN_MOTION; + if (mount.motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + + CommandError e = goTo.validate(); + if (e != CE_NONE) return e; + + // stop tracking + wasTracking = mount.isTracking(); + mount.tracking(false); + mount.enable(true); + goTo.firstGoto = false; + + #if AXIS1_PEC == ON + // turn off PEC while we park + pecDisable(); + pec.state = PEC_NONE; + settings.wormSensePositionSteps = wormSenseSteps; + #endif + + // record our park status + ParkState priorParkState = state; + + // update state to parking + state = PS_PARKING; + settings.state = state; + nv().kv().put(nvKey, settings); + + // get the park coordinate ready + axis1.setBacklash(0.0L); + axis2.setBacklash(0.0L); + Coordinate parkTarget; + parkTarget.h = settings.position.h; + parkTarget.d = settings.position.d; + parkTarget.pierSide = settings.position.pierSide; + if (transform.mountType == ALTAZM) transform.equToHor(&parkTarget); else + if (transform.mountType == ALTALT) transform.equToAa(&parkTarget); + + // goto the park (mount) target coordinate + VLF("MSG: Mount, parking"); + if (parkTarget.pierSide == PIER_SIDE_EAST) e = goTo.request(parkTarget, PSS_EAST_ONLY, false); else + if (parkTarget.pierSide == PIER_SIDE_WEST) e = goTo.request(parkTarget, PSS_WEST_ONLY, false); + + if (e != CE_NONE) { + mount.tracking(wasTracking); + axis1.setBacklash(mount.settings.backlash.axis1); + axis2.setBacklash(mount.settings.backlash.axis2); + + state = priorParkState; + settings.state = state; + nv().kv().put(nvKey, settings); + + VF(": Mount::parkGoto(), Failed to start goto (CE "); V(e); VL(")"); + return e; + } + return CE_NONE; +} + +// clear park state on abort +void Park::requestAborted() { + state = PS_UNPARKED; + settings.state = state; + nv().kv().put(nvKey, settings); + + // restore backlash settings + axis1.setBacklash(mount.settings.backlash.axis1); + axis2.setBacklash(mount.settings.backlash.axis2); + + mount.tracking(wasTracking); +} + +// once parked save the park state +void Park::requestDone() { + + #if (PARK_SENSE) != OFF && (PARK_SENSE_PIN) != OFF + if (sense.isOn(parkSenseHandle)) { + VLF("MSG: Mount, park sense state indicates success."); + } else { + DLF("WRN: Mount, park sense state failed!"); + axis1.setBacklash(mount.settings.backlash.axis1); + axis2.setBacklash(mount.settings.backlash.axis2); + state = PS_PARK_FAILED; + settings.state = state; + nv().kv().put(nvKey, settings); + } + #endif + + if (state != PS_PARK_FAILED) { + #if DEBUG == VERBOSE + long index = axis1.getInstrumentCoordinateSteps() - axis1.getMotorPositionSteps(); + VF("MSG: Mount, park axis1 motor target "); VL(axis1.getTargetCoordinateSteps() - index); + VF("MSG: Mount, park axis1 motor position "); VL(axis1.getMotorPositionSteps()); + index = axis2.getInstrumentCoordinateSteps() - axis2.getMotorPositionSteps(); + VF("MSG: Mount, park axis2 motor target "); VL(axis2.getTargetCoordinateSteps() - index); + VF("MSG: Mount, park axis2 motor position "); VL(axis2.getMotorPositionSteps()); + #endif + + // save the axis state + state = PS_PARKED; + settings.state = state; + nv().kv().put(nvKey, settings); + + #if ALIGN_MAX_NUM_STARS > 1 + transform.align.modelWrite(); + #endif + + VLF("MSG: Mount, parking done"); + } else { DLF("ERR: Mount::parkFinish(), Parking failed"); } + + mount.enable(MOUNT_ENABLE_IN_STANDBY == ON); +} + +// returns a parked telescope to operation +CommandError Park::restore(bool withTrackingOn) { + if (!settings.saved) return CE_NO_PARK_POSITION_SET; + if (!mount.startupAuthorityTrusted()) { + DLF("WRN: Mount, unpark rejected because startup authority is not trusted"); + return CE_SLEW_ERR_UNSPECIFIED; + } + if (state != PS_PARKED) { + #if MOUNT_STARTUP_MODE == SA_STRICT || MOUNT_COORDS_MEMORY == ON + VLF("MSG: Mount, unpark from home disabled by startup authority policy"); + return CE_NOT_PARKED; + #else + if (goTo.absoluteEncodersPresent) { + VLF("MSG: Mount, unpark from home disabled when absolute position sources are present"); + return CE_NOT_PARKED; + } + if (!mount.isHome()) { + VLF("MSG: Mount, unpark when not parked allowed at home only"); + return CE_NOT_PARKED; + } + #endif + } + if (!site.isDateTimeReady()) { + VLF("MSG: Mount, unpark postponed no date/time"); + return CE_PARKED; + } + if (mount.motorFault()) return CE_SLEW_ERR_HARDWARE_FAULT; + + if (withTrackingOn) { + VLF("MSG: Mount, unparking"); + } else { + VLF("MSG: Mount, recovering unpark position"); + } + + #if AXIS1_PEC == ON + wormSenseSteps = settings.wormSensePositionSteps; + #endif + + if (!goTo.absoluteEncodersPresent) { + + // reset the mount, zero backlash + CommandError e = home.reset(); + if (e != CE_NONE) { + DF("WRN: Mount, unpark reset failed (code "); D(e); DLF(")"); + return e; + } + axis1.setBacklashSteps(0); + axis2.setBacklashSteps(0); + + // load the pointing model + #if ALIGN_MAX_NUM_STARS > 1 + transform.align.modelRead(); + #endif + + // get the park coordinate ready + Coordinate parkTarget; + parkTarget.h = settings.position.h; + parkTarget.d = settings.position.d; + parkTarget.pierSide = settings.position.pierSide; + if (transform.mountType == GEM) { + if (parkTarget.pierSide == PIER_SIDE_EAST && parkTarget.h < -limits.settings.pastMeridianE) parkTarget.h += PI*2.0; + if (parkTarget.pierSide == PIER_SIDE_WEST && parkTarget.h > limits.settings.pastMeridianW) parkTarget.h -= PI*2.0; + } + + // set the mount target + double a1, a2; + if (transform.mountType == ALTAZM) transform.equToHor(&parkTarget); else + if (transform.mountType == ALTALT) transform.equToAa(&parkTarget); + + transform.mountToInstrument(&parkTarget, &a1, &a2); + axis1.setInstrumentCoordinatePark(a1); + axis2.setInstrumentCoordinatePark(a2); + mount.captureNominalIndexPositions(); + + VF("MSG: Mount, unpark axis1 motor position "); VL(axis1.getMotorPositionSteps()); + VF("MSG: Mount, unpark axis2 motor position "); VL(axis2.getMotorPositionSteps()); + + // restore backlash settings + axis1.setBacklash(mount.settings.backlash.axis1); + axis2.setBacklash(mount.settings.backlash.axis2); + } + + limits.enabled(true); + if (!goTo.absoluteEncodersPresent) mount.syncFromOnStepToEncoders = true; + + if (withTrackingOn) { + state = PS_UNPARKED; + settings.state = state; + nv().kv().put(nvKey, settings); + mount.tracking(true); + VLF("MSG: Mount, unparking done"); + } else { + VLF("MSG: Mount, recovering unpark position done"); + } + + return CE_NONE; +} + +void Park::reset() { + state = PS_UNPARKED; + settings.state = state; + nv().kv().put(nvKey, settings); +} + +// check input pin to initiate a park operation, if allowed +void Park::signal() { + #if PARK_SIGNAL != OFF && PARK_SIGNAL_PIN != OFF + if (sense.isOn(parkSignalHandle) && state == PS_UNPARKED && !mount.isSlewing() && !mount.isHome()) { + request(); + } + #endif +} + +Park park; + +#endif diff --git a/src/telescope/mount/park/Park.h b/src/telescope/mount/park/Park.h new file mode 100755 index 0000000..512447d --- /dev/null +++ b/src/telescope/mount/park/Park.h @@ -0,0 +1,69 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control +#pragma once + +#include "../../../Common.h" + +#if defined(MOUNT_PRESENT) + +#include "../../Telescope.h" +#include "../coordinates/Transform.h" + +#pragma pack(1) +typedef struct ParkPosition { + float h; + float d; + PierSide pierSide; +} ParkPosition; + +#define ParkSettingsSize 15 +typedef struct ParkSettings { + ParkPosition position; + bool saved; + ParkState state; + long wormSensePositionSteps; +} ParkSettings; +#pragma pack() + +class Park { + public: + void init(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // sets a park position + CommandError set(bool ignoreTrust = false); + + // move the mount to the park position + CommandError request(); + + // clear park state on abort + void requestAborted(); + + // once parked save the park state + void requestDone(); + + // returns a parked telescope to operation + CommandError restore(bool withTrackingOn); + + // resets park state, clears any errors but does not erase the park position + void reset(); + + // check input pin to initiate park operation + void signal(); + + ParkState state = PS_UNPARKED; + + ParkSettings settings = {{0, 0, PIER_SIDE_NONE}, false, PS_UNPARKED, 0}; + + private: + uint8_t parkSenseHandle = 0; + uint8_t parkSignalHandle = 0; + bool wasTracking = false; + + uint32_t nvKey; +}; + +extern Park park; + +#endif diff --git a/src/telescope/mount/pec/Pec.command.cpp b/src/telescope/mount/pec/Pec.command.cpp new file mode 100755 index 0000000..3084717 --- /dev/null +++ b/src/telescope/mount/pec/Pec.command.cpp @@ -0,0 +1,251 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, commands + +#include "Pec.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../site/Site.h" + +bool Pec::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + *suppressFrame = false; + *commandError = CE_NONE; + + if (command[0] == 'G' && command[1] == 'X' && parameter[2] == 0) { + #if AXIS1_PEC == ON + // :GX91# Get PEC analog value + // Returns: n# + if (parameter[0] == '9' && parameter[1] == '1') { + sprintf(reply, "%d", 0); + *numericReply = false; + } else + #endif + + // :GXE6# Get number of steps per sidereal second + // Returns: n# + if (parameter[0] == 'E' && parameter[1] == '6') { + sprintF(reply, "%0.6f", (axis1.getStepsPerMeasure()/RAD_DEG_RATIO_F)/240.0F); + *numericReply = false; + } else + + // :GXE7# Get PEC worm rotation steps (from NV) + // Returns: n# + if (parameter[0] == 'E' && parameter[1] == '7') { + PecSettings nextSettings = { false, PEC_NONE, PEC_STEPS_PER_WORM_ROTATION }; + nv().kv().get(nvKey, nextSettings); + sprintf(reply, "%ld", nextSettings.wormRotationSteps); + *numericReply = false; + } else + + // :GXE8# Get PEC buffer size in seconds + // Returns: n# + if (parameter[0] == 'E' && parameter[1] == '8') { + sprintf(reply, "%ld", bufferSize); + *numericReply = false; + } else return false; + } else + + #if AXIS1_PEC == ON + // :SXE7,[n]# + // Set PEC steps per worm rotation [n] + // Return: 0 on failure or 1 on success + if (command[0] == 'S' && command[1] == 'X' && parameter[0] == 'E' && parameter[1] == '7' && parameter[2] == ',') { + if (parameter[2] != ',') { *commandError = CE_PARAM_FORM; return true; } + long l = atol(¶meter[3]); + if (l >= 0 && l < 129600000) { + PecSettings nextSettings = settings; + nextSettings.wormRotationSteps = l; + nv().kv().put(nvKey, nextSettings); + } else *commandError = CE_PARAM_RANGE; + } else + #endif + + // V - PEC Readout + if (command[0] == 'V') { + #if AXIS1_PEC == ON + // :VH# PEC index sense position in sidereal seconds + // Returns: n# + if (command[1] == 'H' && parameter[0] == 0) { + long s = lroundf(wormSenseSteps/stepsPerSiderealSecond); + while (s > wormRotationSeconds) s -= wormRotationSeconds; + while (s < 0) s += wormRotationSeconds; + sprintf(reply, "%05ld",s); + *numericReply = false; + } else + + // :VR[n]# Read PEC table entry rate adjustment (in steps +/-) for worm segment n (in seconds) + // Returns: sn# + // :VR# Read PEC table entry rate adjustment (in steps +/-) for currently playing segment and its rate adjustment (in steps +/-) + // Returns: sn,n# + if (command[1] == 'R') { + int16_t i, j; + bool conv_result = true; + if (parameter[0] == 0) i = bufferIndex; else conv_result = convert.atoi2(parameter, &i); + if (conv_result) { + if (i >= 0 && i < bufferSize) { + if (parameter[0] == 0) { + i -= 1; + if (i < 0) i += wormRotationSeconds; + if (i >= wormRotationSeconds) i -= wormRotationSeconds; + j = buffer[i]; + sprintf(reply, "%+04i,%03i", j, i); + } else { + j = buffer[i]; + sprintf(reply, "%+04i", j); + } + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + + // :Vr[n]# Read out RA PEC ten byte frame in hex format starting at worm segment n (in seconds) + // Returns: x0x1x2x3x4x5x6x7x8x9# (hex one byte integers) + // Ten rate adjustment factors for 1s worm segments in steps +/- (steps = x0 - 128, etc.) + if (command[1] == 'r') { + int16_t i, j; + if (convert.atoi2(parameter, &i)) { + if (i >= 0 && i < bufferSize) { + j = 0; + uint8_t b; + char s[3] = " "; + for (j = 0; j < 10; j++) { + if (i + j < bufferSize) b = (int)buffer[i + j] + 128; else b = 128; + snprintf(s, sizeof(s), "%02X", b); + strcat(reply, s); + } + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + #endif + + // :VS# Get PEC number of steps per sidereal second of worm rotation + // Returns: n.n# + if (command[1] == 'S' && parameter[0] == 0) { + stepsPerSiderealSecond = (axis1.getStepsPerMeasure()/RAD_DEG_RATIO_F)/240.0F; + sprintF(reply, "%0.6f", stepsPerSiderealSecond); + *numericReply = false; + } else + + // :VW# Get pec worm rotation steps + // Returns: n# + if (command[1] == 'W' && parameter[0] == 0) { + long steps = 0; + #if AXIS1_PEC == ON + steps = settings.wormRotationSteps; + #endif + sprintf(reply, "%06ld", steps); + *numericReply = false; + } else return false; + } else + + // W - PEC Write + if (command[0] == 'W') { + #if AXIS1_PEC == ON + // :WR+# Move PEC Table ahead by one sidereal second + // Return: 0 on failure + // 1 on success + if (command[1] == 'R' && parameter[0] == '+' && parameter[1] == 0) { + int8_t i = buffer[wormRotationSeconds - 1]; + memmove(&buffer[1], &buffer[0], wormRotationSeconds - 1); + buffer[0] = i; + } else + + // :WR-# Move PEC Table back by one sidereal second + // Return: 0 on failure + // 1 on success + if (command[1] == 'R' && parameter[0] == '-' && parameter[1] == 0) { + int8_t i = buffer[0]; + memmove(&buffer[0], &buffer[1], wormRotationSeconds - 1); + buffer[wormRotationSeconds - 1] = i; + } else + + // :WR[n,sn]# Write PEC table entry for worm segment [n] (in sidereal seconds) + // where [sn] is the correction in steps +/- for this 1 second segment + // Returns: Nothing + if (command[1] == 'R') { + char *parameter2 = strchr(parameter, ','); + if (parameter2) { + int16_t i, j; + parameter2[0] = 0; + parameter2++; + if (convert.atoi2(parameter, &i)) { + if (i >= 0 && i < bufferSize) { + if (convert.atoi2(parameter2, &j)) { + if (j >= -128 && j <= 127) { + buffer[i] = j; + settings.recorded = true; + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + #endif + return false; + } else + + // $QZ - PEC Control + if (command[0] == '$' && command[1] == 'Q' && parameter[0] == 'Z' && parameter[2] == 0) { + *numericReply = false; + #if AXIS1_PEC == ON + // :$QZ+# Enable RA PEC compensation + // Returns: nothing + if (parameter[1] == '+') { + if (settings.state == PEC_NONE && settings.recorded) settings.state = PEC_READY_PLAY; else *commandError = CE_0; + nv().kv().put(nvKey, settings); + } else + // :$QZ-# Disable RA PEC Compensation + // Returns: nothing + if (parameter[1] == '-') { + settings.state = PEC_NONE; + nv().kv().put(nvKey, settings); + } else + // :$QZ/# Ready Record PEC + // Returns: nothing + if (parameter[1] == '/') { + if (settings.state == PEC_NONE && mount.isTracking()) { + settings.state = PEC_READY_RECORD; + nv().kv().put(nvKey, settings); + } else *commandError = CE_0; + } else + // :$QZZ# Clear the PEC data buffer + // Return: Nothing + if (parameter[1] == 'Z') { + for (int i = 0; i < bufferSize; i++) buffer[i] = 0; + settings.state = PEC_NONE; + settings.recorded = false; + nv().kv().put("PEC_SETTINGS", settings); + } else + // :$QZ!# Write PEC data to NV + // Returns: nothing + if (parameter[1] == '!') { + settings.recorded = true; + nv().kv().put("PEC_SETTINGS", settings); + nvIv.writeBytes(0, buffer, bufferSize); + } else + #endif + // :$QZ?# Get PEC status + // Returns: s#, one of "IpPrR" (I)gnore, get ready to (p)lay, (P)laying, get ready to (r)ecord, (R)ecording + // or an optional (.) to indicate an index detect + if (parameter[1] == '?') { + const char *pecStateStr = "IpPrR"; + uint8_t state = 0; + #if AXIS1_PEC == ON + state = settings.state; + #endif + reply[0] = pecStateStr[state]; reply[1] = 0; reply[2] = 0; + #if AXIS1_PEC == ON + if (wormIndexSenseThisSecond) reply[1] = '.'; + #endif + } else { *numericReply = true; return false; } + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/mount/pec/Pec.cpp b/src/telescope/mount/pec/Pec.cpp new file mode 100755 index 0000000..c334dd6 --- /dev/null +++ b/src/telescope/mount/pec/Pec.cpp @@ -0,0 +1,334 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control, PEC + +#include "Pec.h" + +#ifdef MOUNT_PRESENT + +#if AXIS1_PEC == ON + #include "../../../lib/tasks/OnTask.h" + #include "../../../lib/nv/Nv.h" + + #include "../../../lib/sense/Sense.h" + #include "../../Telescope.h" + #include "../goto/Goto.h" + #include "../guide/Guide.h" + #include "../park/Park.h" + + #if (PEC_SENSE) == OFF + bool wormSenseFirst = true; + #else + bool wormSenseFirst = false; + #endif + + inline void pecWrapper() { pec.poll(); } + + void Pec::init() { + nvKey = nv().kv().computeKey("PEC_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings)) { DLF("WRN: Nv, init failed for PEC_SETTINGS"); } + + settings.recorded = constrain(settings.recorded, false, true); + settings.state = constrain(settings.state, PEC_NONE, PEC_RECORD); + settings.wormRotationSteps = constrain(settings.wormRotationSteps, 0, 129600000); + + NvVolume& nvVolume = nv().volume(); + if (!nvVolume.isMounted()) { + DLF("WRN: Pec::init(); NV volume not mounted"); + return; + } + + // Bind IvPartition to the PEC partition by name. + if (nvIv.init(nvVolume, "PEC")) { + VLF("MSG: Nv, partition 'PEC' mounted"); + } else { + DLF("WRN: Pec::init(); can't find PEC partition!"); + return; + } + + stepsPerSiderealSecond = (axis1.getStepsPerMeasure()/RAD_DEG_RATIO)/240.0L; + stepsPerSiderealSecondI = lroundf(stepsPerSiderealSecond); + stepsPerMicroSecond = (stepsPerSiderealSecond*SIDEREAL_RATIO)/1000000.0L; + + wormRotationSeconds = round(settings.wormRotationSteps/stepsPerSiderealSecond); + bufferSize = wormRotationSeconds; + + if (bufferSize > 0) { + if (bufferSize < 61) { + DLF("WRN: Pec::init(), invalid bufferSize (minimum worm rotation 61 seconds)"); + bufferSize = 0; + initError.value = true; + } else + if (bufferSize > nvIv.sizeBytes()) { + DLF("WRN: Pec::init(), bufferSize exceeds PEC partition size - PEC disabled"); + bufferSize = 0; + initError.value = true; + } else { + buffer = (int8_t*)malloc(bufferSize * sizeof(*buffer)); + if (buffer == NULL) { + bufferSize = 0; + initError.value = true; + DLF("WRN: Pec::init(), bufferSize exceeds available RAM - PEC disabled"); + } else { + VF("MSG: Mount, PEC allocated buffer "); V(bufferSize * (long)sizeof(*buffer)); VLF(" bytes"); + + bool bufferNeedsInit = true; + nvIv.readBytes(0, buffer, bufferSize); + for (int i = 0; i < bufferSize; i++) { if (buffer[i] != 0) bufferNeedsInit = false; } + if (bufferNeedsInit) { + for (int i = 0; i < bufferSize; i++) buffer[i] = 0; + nvIv.writeBytes(0, buffer, bufferSize); + } + + if (settings.state > PEC_RECORD) { + settings.state = PEC_NONE; + initError.value = true; + DLF("ERR: Pec::init(), bad NV settings.state"); + } + + if (!settings.recorded) settings.state = PEC_NONE; + + #if (PEC_SENSE) == OFF + #if GOTO_FEATURE == ON + park.settings.wormSensePositionSteps = 0; + #endif + settings.state = PEC_NONE; + #else + VLF("MSG: Mount, PEC adding sense"); + senseHandle = sense.add(PEC_SENSE_PIN, PEC_SENSE_INIT, PEC_SENSE); + #endif + + VF("MSG: Mount, PEC start monitor task (rate 10ms priority 3)... "); + monitorHandle = tasks.add(10, 0, true, 3, pecWrapper, "MntPec"); + if (monitorHandle) { VLF("success"); } else { VLF("FAILED!"); } + } + } + } + if (bufferSize <= 0) { bufferSize = 0; settings.state = PEC_NONE; settings.recorded = false; } + if (wormRotationSeconds > bufferSize) wormRotationSeconds = bufferSize; + } + + void Pec::poll() { + // PEC is only active when we're tracking at the sidereal rate with a guide rate that makes sense + #if GOTO_FEATURE == ON + if (park.state > PS_UNPARKED) { disable(); return; } + #endif + if (!mount.isTracking() || guide.state > GU_PULSE_GUIDE) { disable(); return; } + + // keep track of our current step position, and when the step position on the worm wraps during playback + long axis1Steps = axis1.getMotorPositionSteps(); + + #if (PEC_SENSE) == OFF + wormSenseFirst = true; + #else + static int lastState; + lastState = wormIndexState; + wormIndexState = sense.isOn(senseHandle); + + // digital or analog pec sense, with 60 second delay before redetect + long dist; + if (wormSenseSteps > axis1Steps) dist = wormSenseSteps - axis1Steps; else dist = axis1Steps - wormSenseSteps; + + if (wormIndexSenseThisSecond && dist > stepsPerSiderealSecond*2.0) wormIndexSenseThisSecond = false; + + if (dist > stepsPerSiderealSecond*60.0 && wormIndexState != lastState && wormIndexState == true) { + VLF("MSG: Mount, PEC index detected"); + wormSenseSteps = axis1Steps; + wormSenseFirst = true; + bufferStart = true; + wormIndexSenseThisSecond = true; + } else bufferStart = false; + + #endif + + if (settings.state == PEC_NONE) { rate = 0.0F; return; } + if (!wormSenseFirst) return; + + // worm step position corrected for any index found + #if (PEC_SENSE) == OFF + static long lastWormRotationSteps = wormRotationSteps; + #endif + wormRotationSteps = axis1Steps - wormSenseSteps; + while (wormRotationSteps >= settings.wormRotationSteps) wormRotationSteps -= settings.wormRotationSteps; + while (wormRotationSteps < 0) wormRotationSteps += settings.wormRotationSteps; + + #if (PEC_SENSE) == OFF + if (wormRotationSteps - lastWormRotationSteps < 0) { + VLF("MSG: Mount, PEC virtual index detected"); + bufferStart = true; + } else bufferStart = false; + #endif + + // handle playing back and recording PEC + noInterrupts(); + unsigned long lastFs = fracLAST; + interrupts(); + + // start playing PEC + if (settings.state == PEC_READY_PLAY) { + // makes sure the index is at the start of a second before resuming play + if ((long)fmod(wormRotationSteps, stepsPerSiderealSecond) == 0) { + VLF("MSG: Mount, PEC started playing"); + settings.state = PEC_PLAY; + bufferIndex = lroundf(wormRotationSteps/stepsPerSiderealSecond); + wormRotationStartTimeFs = lastFs; + } + } else + // start recording PEC + if (settings.state == PEC_READY_RECORD) { + if ((long)fmod(wormRotationSteps, stepsPerSiderealSecond) == 0) { + VF("MSG: Mount, PEC started recording at "); + settings.state = PEC_RECORD; + bufferIndex = lroundf(wormRotationSteps/stepsPerSiderealSecond); + firstRecording = !settings.recorded; + wormRotationStartTimeFs = lastFs; + V(wormRotationStartTimeFs); + recordStopTimeFs = wormRotationStartTimeFs + (uint32_t)(wormRotationSeconds*(long)FRACTIONAL_SEC); + V(" and stopping at "); VL(recordStopTimeFs); + accGuideAxis1 = 0.0L; + } + } else + // and once the PEC data is all stored, indicate that it's valid and start using it + if (settings.state == PEC_RECORD && (long)(lastFs - recordStopTimeFs) > 0) { + VLF("MSG: Mount, PEC recording complete switched to playing"); + settings.state = PEC_PLAY; + settings.recorded = true; + cleanup(); + } + + // reset the buffer index to match the worm index + if (bufferStart && settings.state != PEC_RECORD) { + bufferIndex = 0; + wormRotationStartTimeFs = lastFs; + } + + // Increment the PEC index once a second and make it go back to zero when the + // worm finishes a rotation, this code works when crossing zero + if (lastFs - wormRotationStartTimeFs >= FRACTIONAL_SEC) { wormRotationStartTimeFs = lastFs; bufferIndex++; } + bufferIndex = ((bufferIndex % wormRotationSeconds) + wormRotationSeconds) % wormRotationSeconds; + + // accumulate guide steps for PEC + if (guide.rateAxis1 != 0.0F) { + if (accGuideStartTime != 0) accGuideAxis1 += stepsPerMicroSecond*(micros() - accGuideStartTime)*guide.rateAxis1; + accGuideStartTime = micros(); + if (accGuideStartTime == 0) accGuideStartTime = 1; + } else accGuideStartTime = 0; + + // falls in whenever the pecIndex changes, which is once a sidereal second + static long lastBufferIndex = 0; + if (bufferIndex != lastBufferIndex) { + lastBufferIndex = bufferIndex; + + // assume no change to tracking rate + rate = 0.0F; + + if (settings.state == PEC_RECORD) { + // get guide steps taken from the accumulator + int i = round(accGuideAxis1); + + // stay within +/- one sidereal rate for corrections + if (i < -stepsPerSiderealSecondI) i = -stepsPerSiderealSecondI; + if (i > stepsPerSiderealSecondI) i = stepsPerSiderealSecondI; + + // apply weighted average + if (!firstRecording) i = (i + (int)buffer[bufferIndex]*2)/3; + + // restrict to valid range and store + if (i < -127) i = -127; else if (i > 127) i = 127; + + // remove steps from the accumulator + accGuideAxis1 -= i; + + buffer[bufferIndex] = i; + } + + if (settings.state == PEC_PLAY) { + // adjust one second before the value was recorded, an estimate of the latency between image acquisition and response + // if sending values directly to OnStep from PECprep, etc. be sure to account for this + // number of steps ahead or behind for this 1 second slot, up to +/-127 + int j = bufferIndex - 1; if (j == -1) j += wormRotationSeconds; + int i = buffer[j]; + if (i > stepsPerSiderealSecondI) i = stepsPerSiderealSecondI; + if (i < -stepsPerSiderealSecondI) i = -stepsPerSiderealSecondI; + rate = i/stepsPerSiderealSecond; + } + } + } + + // disable PEC + void Pec::disable() { + // give up recording if we stop tracking at the sidereal rate + // don't zero the PEC offset, we don't want things moving and it really doesn't matter + if (settings.state == PEC_RECORD || settings.state == PEC_READY_RECORD) { + VLF("MSG: Mount, PEC recording stopped"); + settings.state = PEC_NONE; + rate = 0.0F; + } + // get ready to re-index when tracking comes back + if (settings.state == PEC_PLAY) { + VLF("MSG: Mount, PEC playing paused"); + settings.state = PEC_READY_PLAY; + rate = 0.0F; + } + } + + // applies low pass filter to smooth noise in PEC data and linear regression + void Pec::cleanup() { + VLF("MSG: Mount, applying low pass filter to PEC data"); + int i,J1,J4,J9,J17; + for (int scc = 3; scc < wormRotationSeconds + 3; scc++) { + i = buffer[scc % wormRotationSeconds]; + + J1 = lroundf(i*0.01F); + J4 = lroundf(i*0.04F); + J9 = lroundf(i*0.09F); + J17 = lroundf(i*0.17F); + buffer[(scc - 4) % wormRotationSeconds] = (buffer[(scc - 4) % wormRotationSeconds]) + J1; + buffer[(scc - 3) % wormRotationSeconds] = (buffer[(scc - 3) % wormRotationSeconds]) + J4; + buffer[(scc - 2) % wormRotationSeconds] = (buffer[(scc - 2) % wormRotationSeconds]) + J9; + buffer[(scc - 1) % wormRotationSeconds] = (buffer[(scc - 1) % wormRotationSeconds]) + J17; + buffer[(scc ) % wormRotationSeconds] = (buffer[(scc ) % wormRotationSeconds]) - (J17+J17+J9+J9+J4+J4+J1+J1); + buffer[(scc + 1) % wormRotationSeconds] = (buffer[(scc + 1) % wormRotationSeconds]) + J17; + buffer[(scc + 2) % wormRotationSeconds] = (buffer[(scc + 2) % wormRotationSeconds]) + J9; + buffer[(scc + 3) % wormRotationSeconds] = (buffer[(scc + 3) % wormRotationSeconds]) + J4; + buffer[(scc + 4) % wormRotationSeconds] = (buffer[(scc + 4) % wormRotationSeconds]) + J1; + } + + // linear regression + VLF("MSG: Mount, applying linear regression to PEC data"); + // the number of steps added should equal the number of steps subtracted (from the cycle) + // first, determine how far we've moved ahead or backward in steps + long stepsSum = 0; for (int scc = 0; scc < wormRotationSeconds; scc++) stepsSum += buffer[scc]; + + // this is the correction coefficient for a given location in the sequence + float Ccf = (float)stepsSum/wormRotationSeconds; + + // now, apply the correction to the sequence to make the PEC adjustments null out + // this process was simulated in a spreadsheet and the roundoff error might leave us at +/- a step which is tacked on at the beginning + long lp2 = 0; stepsSum = 0; + for (int scc = 0; scc < wormRotationSeconds; scc++) { + // the correction, "now" + long lp1 = lroundf(-scc*Ccf); + + // if the correction increases or decreases then add or subtract that many steps + buffer[scc] = buffer[scc] + (lp1 - lp2); + + // sum the values for a final adjustment, if necessary + stepsSum += buffer[scc]; + lp2 = lp1; + } + buffer[0] -= stepsSum; + + // a reality check, make sure the buffer data looks good, if not forget it + if (stepsSum < -2 || stepsSum > 2) { + DF("ERR: Pec::cleanup(), linear regression residual "); D(stepsSum); + if (stepsSum > 2) { DLF(" > threshold of 2"); } else { DLF(" < threshold of -2"); } + settings.recorded = false; + settings.state = PEC_NONE; + } + } + +#endif + + Pec pec; + +#endif diff --git a/src/telescope/mount/pec/Pec.h b/src/telescope/mount/pec/Pec.h new file mode 100755 index 0000000..5492e99 --- /dev/null +++ b/src/telescope/mount/pec/Pec.h @@ -0,0 +1,84 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +#if PEC_BUFFER_SIZE_LIMIT == 0 || PEC_STEPS_PER_WORM_ROTATION == 0 + #define AXIS1_PEC OFF +#else + #define AXIS1_PEC ON +#endif + +#include "../../../lib/nv/NvIvPartition.h" +#include "../Mount.h" + +enum PecState: uint8_t {PEC_NONE, PEC_READY_PLAY, PEC_PLAY, PEC_READY_RECORD, PEC_RECORD}; + +#pragma pack(1) +#define PecSettingsSize 6 +typedef struct PecSettings { + bool recorded:1; + PecState state; + long wormRotationSteps; +} PecSettings; +#pragma pack() + +class Pec { + public: + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // tracking rate (in x) due to PEC playing + float rate = 0.0F; + + #if AXIS1_PEC == ON + void init(); + void poll(); + + PecSettings settings = { false, PEC_NONE, PEC_STEPS_PER_WORM_ROTATION }; + #endif + + private: + #if AXIS1_PEC == ON + // disable PEC + void disable(); + + // applies low pass filter to smooth noise in PEC data and linear regression + void cleanup(); + #endif + + double stepsPerSiderealSecond = 0.0L; + int stepsPerSiderealSecondI = 0; + double stepsPerMicroSecond = 0.0L; + long bufferSize = 0; // in bytes + #if AXIS1_PEC == ON + uint8_t monitorHandle = 0; + uint8_t senseHandle = 0; + + bool wormIndexState = false; + bool wormIndexSenseThisSecond = false; + long wormRotationSteps = 0; // step position in worm rotation sequence + long wormSenseSteps = 0; // step position + + bool firstRecording = false; + uint32_t recordStopTimeFs = 0; + uint32_t wormRotationStartTimeFs = 0; // start time of worm rotation sequence, in fracsecs or milliseconds + long wormRotationSeconds = 0; // time for a worm rotation, in seconds + unsigned long accGuideStartTime = 0; + + double accGuideAxis1 = 0.0L; + + bool bufferStart = false; + long bufferIndex = 0; // index into the pec buffer + int8_t* buffer; + #endif + + IvPartition nvIv; + uint32_t nvKey; +}; + +extern Pec pec; + +#endif diff --git a/src/telescope/mount/site/Site.command.cpp b/src/telescope/mount/site/Site.command.cpp new file mode 100755 index 0000000..ce46941 --- /dev/null +++ b/src/telescope/mount/site/Site.command.cpp @@ -0,0 +1,282 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount time and location, commands + +#include "Site.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/nv/Nv.h" + +#include "../../../libApp/commands/ProcessCmds.h" + +#include "../../Telescope.h" +#include "../home/Home.h" +#include "../Mount.h" + +bool Site::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + *suppressFrame = false; + PrecisionMode precisionMode = convert.precision; + + if (command[0] == 'G') { + // :Ga# Get standard time in 12 hour format + // Returns: HH:MM:SS# + if (command[1] == 'a' && parameter[0] == 0) { + double time = rangeAmPm(getTime() - location.timezone); + convert.doubleToHms(reply, time, false, PM_HIGH); + *numericReply = false; + } else + + // :GC# Get standard calendar date + // Returns: MM/DD/YY# + if (command[1] == 'C' && parameter[0] == 0) { + GregorianDate local = calendars.julianToGregorian(UT1ToLocal(getDateTime())); + sprintf(reply, "%02d/%02d/%02d", (int)local.month, (int)local.day, (int)local.year % 100); + *numericReply = false; + } else + + // :Gc# Get the Local Standard Time format + // Returns: 24# + if (command[1] == 'c' && parameter[0] == 0) { + strcpy(reply, "24"); + *numericReply = false; + } else + + // :GG# Get UTC offset time, hours and minutes to add to local time to convert to UTC + // Returns: [s]HH:MM# + if (command[1] == 'G' && parameter[0] == 0) { + convert.doubleToHms(reply, location.timezone, true, PM_LOWEST); + *numericReply = false; + } else + + // :Gg# Get Current Site Longitude, east is negative + // Returns: sDDD*MM# + // :GgH# Get current site Longitude + // Returns: sDD*MM:SS.SSS# (high precision) + if (command[1] == 'g' && (parameter[0] == 0 || parameter[1] == 0)) { + precisionMode = PM_LOW; + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + convert.doubleToDms(reply, radToDeg(location.longitude), true, true, precisionMode); + *numericReply = false; + } else + + // :GL# Get Local Standard Time in 24 hour format + // Returns: HH:MM:SS# + // :GLH# Returns: HH:MM:SS.SSSS# (high precision) + if (command[1] == 'L' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + convert.doubleToHms(reply, rangeHours(getTime() - location.timezone), false, precisionMode); + *numericReply = false; + } else + + // :GM# Get site 1 name + // :GN# Get site 2 name + // :GO# Get site 3 name + // :GP# Get site 4 name + // Returns: s# + if ((command[1] == 'M' || command[1] == 'N' || command[1] == 'O' || command[1] == 'P') && parameter[0] == 0) { + Location tempLocation; + tempLocation.name[0] = 0; + uint8_t locationNumber = command[1] - 'M'; + if (locationNumber <= 3) { nv().kv().get(nvKey[locationNumber], tempLocation); } + strcpy(reply, tempLocation.name); + if (reply[0] == 0) { strcat(reply,"None"); } + *numericReply = false; + } else + + // :GS# Get the Sidereal Time as sexagesimal value in 24 hour format + // Returns: HH:MM:SS# + // :GSH# Returns: HH:MM:SS.ss# (high precision) + if (command[1] == 'S' && (parameter[0] == 0 || parameter[1] == 0)) { + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + convert.doubleToHms(reply, getSiderealTime(), false, precisionMode); + *numericReply = false; + } else + + // :Gt# Get current site Latitude, positive for North latitudes + // Returns: sDD*MM# + // :GtH# Get current site Latitude, positive for North latitudes + // Returns: sDD*MM:SS.SSS# (high precision) + if (command[1] == 't' && (parameter[0] == 0 || parameter[1] == 0)) { + precisionMode = PM_LOW; + if (parameter[0] == 'H') precisionMode = PM_HIGHEST; else if (parameter[0] != 0) { *commandError = CE_PARAM_FORM; return true; } + convert.doubleToDms(reply, radToDeg(location.latitude), false, true, precisionMode); + *numericReply = false; + } else + + // :Gv# Get the site eleVation in meters + // Returns: +/-n.n + if (command[1] == 'v' && parameter[0] == 0) { + sprintF(reply, "%3.1f", location.elevation); + *numericReply = false; + } else + + if (command[1] == 'X' && parameter[0] == '8' && parameter[2] == 0) { + // :GX80# Get the UT1 Time as sexagesimal value in 24 hour format + // Returns: HH:MM:SS.ss# + if (parameter[1] == '0') { + convert.doubleToHms(reply, rangeHours(getTime()), false, PM_HIGH); + *numericReply = false; + } else + + // :GX81# Get the UT1 Date + // Returns: MM/DD/YY# + if (parameter[1] == '1') { + JulianDate julianDay = ut1; + double hour = getTime(); + while (hour >= 24.0) { hour -= 24.0; julianDay.day += 1.0; } + if (hour < 0.0) { hour += 24.0; julianDay.day -= 1.0; } + GregorianDate date = calendars.julianDayToGregorian(julianDay); + sprintf(reply, "%02d/%02d/%02d", (int)date.month, (int)date.day, (int)date.year % 100); + *numericReply = false; + } else + + // :GX89# Date/time ready status + // Return: 0 ready, 1 not ready + if (parameter[1] == '9') { + if (dateIsReady && timeIsReady) *commandError = CE_0; else *commandError = CE_1; + } else return false; + + } else return false; + } else + + if (command[0] == 'S') { + // :SC[MM/DD/YY]# or :SC[MM/DD/YYYY]# + // Change local standard date + // Return: 0 on failure, 1 on success + if (command[1] == 'C') { + GregorianDate local = calendars.julianToGregorian(UT1ToLocal(getDateTime())); + if (strToDate(parameter, &local)) { + dateIsReady = true; + setDateTime(localToUT1(calendars.gregorianToJulian(local))); + updateTLS(); + } else *commandError = CE_PARAM_FORM; + } else + + // :SG[sHH]# or :SG[sHH:MM]# (where MM is 00, 30, or 45) + // Set the number of hours added to local time to yield UT1 + // Return: 0 failure, 1 success + if (command[1] == 'G') { + double hour; + if (convert.tzToDouble(&hour, parameter)) { + if (hour >= -13.75 || hour <= 12.0) { + location.timezone = hour; + nv().kv().put(nvKey[locationNumber], location); + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + // :Sg[(s)DDD*MM]# or :Sg[(s)DDD*MM:SS]# or :Sg[(s)DDD*MM:SS.SSS]# + // Set current site longitude, east longitudes can be negative or > 180 degrees + // Return: 0 failure, 1 success + if (command[1] == 'g') { + double degs; + int i; + if (parameter[0] == '-' || parameter[0] == '+') i = 1; else i = 0; + if (convert.dmsToDouble(°s, (char *)¶meter[i], false)) { + if (degs >= -180.0 && degs <= 360.0) { + if (degs >= 180.0) degs -= 360.0; + location.longitude = degToRad(degs); + if (parameter[0] == '-') location.longitude = -location.longitude; + updateLocation(); + nv().kv().put(nvKey[locationNumber], location); + } else *commandError = CE_PARAM_RANGE; + } else *commandError = CE_PARAM_FORM; + } else + + // :SL[HH:MM:SS]# or :SL[HH:MM:SS.SSS]# + // Set the local Time + // Return: 0 failure, 1 success + if (command[1] == 'L') { + GregorianDate local = calendars.julianToGregorian(UT1ToLocal(getDateTime())); + if (convert.hmsToDouble(&local.hour, parameter, PM_HIGH) || convert.hmsToDouble(&local.hour, parameter, PM_HIGHEST)) { + timeIsReady = true; + setDateTime(localToUT1(calendars.gregorianToJulian(local))); + updateTLS(); + } else *commandError = CE_PARAM_FORM; + } else + + // :SM[s]# or :SN[s]# or :SO[s]# or :SP[s]# + // Set site name, string may be up to 15 characters + // Return: 0 on failure + // 1 on success + if ((command[1] == 'M' || command[1] == 'N' || command[1] == 'O' || command[1] == 'P')) { + uint8_t tempLocationNumber = command[1] - 'M'; + if (strlen(parameter) <= 15) { + Location tempLocation; + tempLocation.latitude = 0.0; + tempLocation.longitude = 0.0; + tempLocation.timezone = 0.0; + tempLocation.elevation = 100.0; + nv().kv().get(nvKey[tempLocationNumber], tempLocation); + strcpy(tempLocation.name, parameter); + nv().kv().put(nvKey[tempLocationNumber], tempLocation); + if (locationNumber == tempLocationNumber) strcpy(location.name, parameter); + } else *commandError = CE_PARAM_RANGE; + } else + + // :St[sDD*MM]# or :St[sDD*MM:SS]# or :St[sDD*MM:SS.SSS]# + // Set current site latitude in degrees + // Return: 0 failure, 1 success + if (command[1] == 't') { + double degs; + if (convert.dmsToDouble(°s, parameter, true)) { + location.latitude = degToRad(degs); + updateLocation(); + nv().kv().put(nvKey[locationNumber], location); + if (mount.isHome()) home.init(); + } else *commandError = CE_PARAM_FORM; + } else + + // :SU[s.s]# + // Set the number of seconds (up to +/-0.9) added to UTC to yield UT1 (DUT1) + // presently only applied to GPS and NTP UTC time for UT1 conversion + // see: https://datacenter.iers.org/data/latestVersion/bulletinB.txt + // Return: 0 failure, 1 success + if (command[1] == 'U') { + char *conv_end; + double dut1 = strtod(parameter, &conv_end); + if (dut1 >= -0.9 && dut1 <= 0.9) { + #if TIME_LOCATION_SOURCE != OFF + tls->DUT1 = dut1; + #endif + #if TIME_LOCATION_SOURCE_FALLBACK != OFF + tlsFallback->DUT1 = dut1; + #endif + } else *commandError = CE_PARAM_FORM; + } else + + // :Sv[sn.n]# + // Sets current site eleVation in meters + // Return: 0 failure, 1 success + if (command[1] == 'v') { + char *conv_end; + float f = strtod(¶meter[0], &conv_end); + if (¶meter[0] == conv_end) f = NAN; + if (!setElevation(f)) *commandError = CE_PARAM_RANGE; + nv().kv().put(nvKey[locationNumber], location); + } else return false; + } else + + if (command[0] == 'W') { + // :W[n]# Sets current site to n, where n = 0..3 + // Returns: Nothing + if ((command[1] >= '0' && command[1] <= '3') && parameter[0] == 0) { + locationNumber = command[1] - '0'; + readLocation(locationNumber); + updateLocation(); + if (mount.isHome()) home.init(); + *numericReply = false; + } else + + // :W?# Queries current site + // Returns: n# + if (command[1] == '?' && parameter[0] == 0) { + sprintf(reply, "%d", (int)locationNumber); + *numericReply = false; + } else return false; + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/mount/site/Site.cpp b/src/telescope/mount/site/Site.cpp new file mode 100755 index 0000000..18fdd34 --- /dev/null +++ b/src/telescope/mount/site/Site.cpp @@ -0,0 +1,459 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount, time and location + +#include "Site.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../Telescope.h" +#include "../park/Park.h" +#include "../home/Home.h" +#include "../limits/Limits.h" +#include "../Mount.h" + +// fractional second sidereal clock (fracsec or millisecond) +volatile unsigned long fracLAST; +IRAM_ATTR void clockTickWrapper() { fracLAST++; } + +#define fsToHours(x) ((x)/(3600.0*FRACTIONAL_SEC)) +#define hoursToFs(x) ((x)*(3600.0*FRACTIONAL_SEC)) +#define fsToDays(x) ((x)/(86400.0*FRACTIONAL_SEC)) +#define daysToFs(x) ((x)*(86400.0*FRACTIONAL_SEC)) + +#if TIME_LOCATION_SOURCE == GPS + void gpsCheck() { + if (site.tls->isReady()) { + VLF("MSG: Mount, getting site from GPS"); + double latitude, longitude; + float elevation; + site.tls->getSite(latitude, longitude, elevation); + site.location.latitude = degToRad(latitude); + site.location.longitude = degToRad(longitude); + site.location.elevation = degToRad(elevation); + strcpy(site.location.name, "GPS"); + site.updateLocation(); + if (mount.isHome()) home.init(); + + JulianDate jd; + VLF("MSG: Mount, getting date/time from GPS"); + unsigned long syncTimeout = millis() + 1000U; + while (!site.tls->get(jd) && (long)(millis() - syncTimeout) < 0) { }; + if ((long)(millis() - syncTimeout) < 0) { + site.dateIsReady = true; + site.timeIsReady = true; + site.setDateTime(jd); + site.updateTLS(); + } else { + VLF("MSG: Mount, GPS timed out getting date/time from GPS"); + initError.tls = true; + } + + VLF("MSG: Mount, stopping GPS monitor task"); + tasks.setDurationComplete(tasks.getHandleByName("gpsChk")); + } else + + if ((long)(millis() - site.updateTimeoutTime) > 0) { + DLF("WRN: Mount, GPS timed out stopping monitor task"); + tasks.setDurationComplete(tasks.getHandleByName("gpsChk")); + DLF("WRN: TLS, GPS timed out stopping GPS polling task"); + tasks.setDurationComplete(tasks.getHandleByName("gpsPoll")); + initError.tls = true; + } + } +#endif + +#if TIME_LOCATION_SOURCE == NTP + void ntpCheck() { + if (site.tls->isReady()) { + VLF("MSG: Mount, getting date/time from NTP"); + JulianDate jd; + site.tls->get(jd); + site.dateIsReady = true; + site.timeIsReady = true; + site.setDateTime(jd); + site.updateTLS(); + + VLF("MSG: Mount, stopping NTP monitor task"); + tasks.setDurationComplete(tasks.getHandleByName("ntpChk")); + } else + + if ((long)(millis() - site.updateTimeoutTime) > 0) { + DLF("MSG: Mount, NTP timed out stopping monitor task"); + tasks.setDurationComplete(tasks.getHandleByName("ntpChk")); + DLF("WRN: TLS, NTP timed out stopping monitor task"); + tasks.setDurationComplete(tasks.getHandleByName("ntp")); + initError.tls = true; + } + } +#endif + +#if (TIME_LOCATION_PPS_SENSE) != OFF && TIME_LOCATION_PPS_SYNC == ON + void ppsMonitor() { + static long deltaAvg = 0; + + // monitor and adapt to keep PPS in sync with our UT1 + static unsigned long lastMicros = 0; + long p = micros() - pps.lastMicros; + if (pps.lastMicros == lastMicros || p < 250000L) { + // DF("MSG: Site,"); D(pps.synced ? ' ' : '!'); DF("sub-micro delta="); D((long)pps.averageSubMicros - 16000000L); DLF("us"); + return; + } + lastMicros = pps.lastMicros; + + double h = site.getDateTime().hour*3600.0; + char syncc = ' '; + long s = round((h - floor(h))*1000000.0); + if (!pps.synced) { syncc = '!'; p = 1000000L; s = 1000000L; } + + long delta = p - s; + deltaAvg = (delta + deltaAvg*19L)/20L; + // DF("MSG: Site,"); D(syncc); DF("sub-micro delta="); D((long)pps.averageSubMicros - 16000000L); DF("us, pps="); D(p/1000L); DF("ms, time.sec="); D(s/1000L); DF("ms, delta="); D(delta/1000); DLF("ms"); + + if (abs(deltaAvg) < 3000L) return; + pps.averageSubMicrosSkew = (-deltaAvg*SITE_MIN_PPS_SUB_MICRO)/2000L; + if (pps.averageSubMicrosSkew < -128) pps.averageSubMicrosSkew = -128; else + if (pps.averageSubMicrosSkew > 128) pps.averageSubMicrosSkew = 128; + } +#endif + +void Site::init() { + // get location + VLF("MSG: Mount, site get Latitude/Longitude from NV"); + readLocation(locationNumber); + updateLocation(); + + // get date/time from a TLS or NV + #if TIME_LOCATION_SOURCE != OFF + + #if TIME_LOCATION_SOURCE_FALLBACK == DS3231 + tlsFallback = new TlsDs3231; + #elif TIME_LOCATION_SOURCE_FALLBACK == DS3234 + tlsFallback = new TlsDs3234; + #elif TIME_LOCATION_SOURCE_FALLBACK == SD3031 + tlsFallback = new TlsSd3031; + #elif TIME_LOCATION_SOURCE_FALLBACK == TEENSY + tlsFallback = new TlsTeensy; + #endif + + #if TIME_LOCATION_SOURCE == DS3231 + tls = new TlsDs3231; + #elif TIME_LOCATION_SOURCE == DS3234 + tls = new TlsDs3234; + #elif TIME_LOCATION_SOURCE == GPS + tls = new TlsGPS; + #elif TIME_LOCATION_SOURCE == NTP + tls = new TlsNTP; + #elif TIME_LOCATION_SOURCE == SD3031 + tls = new TlsSd3031; + #elif TIME_LOCATION_SOURCE == TEENSY + tls = new TlsTeensy; + #endif + + initError.tls = !tls->init(); + if (!initError.tls) { + if (tls->isReady()) { + tls->get(ut1); + dateIsReady = true; + timeIsReady = true; + VLF("MSG: Mount, site get Date/Time from TLS"); + } else { + #if TIME_LOCATION_SOURCE == GPS + updateTimeoutTime = millis() + GPS_TIMEOUT_MINUTES*60000U; + VF("MSG: Site, start GPS check task (rate 5000ms priority 7)... "); + if (tasks.add(5000, 0, true, 7, gpsCheck, "gpsChk")) { VLF("success"); } else { VLF("FAILED!"); } + #endif + #if TIME_LOCATION_SOURCE == NTP + updateTimeoutTime = millis() + NTP_TIMEOUT_SECONDS*1000U; + VF("MSG: Site, start NTP check task (rate 5000ms priority 7)... "); + if (tasks.add(5000, 0, true, 7, ntpCheck, "ntpChk")) { VLF("success"); } else { VLF("FAILED!"); } + #endif + } + } + + #if TIME_LOCATION_SOURCE_FALLBACK != OFF + if (tlsFallback->init() && tlsFallback->isReady()) { + tlsFallback->get(ut1); + dateIsReady = true; + timeIsReady = true; + #if TIME_LOCATION_SOURCE_FALLBACK == DS3231 + if (initError.tls) tlsFallback->ppsEnable(); + #endif + initError.tls = false; + VLF("MSG: Mount, site get Date/Time from fallback TLS"); + } + #endif + + if (initError.tls) { + DLF("WRN: Site::init(), Warning TLS initialization failed"); + DLF("WRN: Site::init(), fallback to last Date/Time from NV"); + readJD(); + } + #else + VLF("MSG: Mount, site get Date/Time from NV"); + readJD(); + #endif + + setSiderealTime(ut1); + + VF("MSG: Mount, site start sidereal timer task (rate 10ms priority 0)... "); + delay(100); + // period ms (0=idle), duration ms (0=forever), repeat, priority (highest 0..7 lowest), task_handle + taskHandle = tasks.add(0, 0, true, 0, clockTickWrapper, "MtTick"); + if (taskHandle) { + VLF("success"); + if (!tasks.requestHardwareTimer(taskHandle, 1)) { DLF("WRN: Site::init(), didn't get h/w timer for Clock (using s/w timer)"); } + } else { VLF("FAILED!"); } + + setSiderealPeriod(SIDEREAL_PERIOD); + + #if (TIME_LOCATION_PPS_SENSE) != OFF + pps.init(); + #if TIME_LOCATION_PPS_SYNC == ON + VF("MSG: Mount, site start PPS monitor task (rate 333ms priority 6)... "); + delay(100); + // period ms (0=idle), duration ms (0=forever), repeat, priority (highest 0..7 lowest), task_handle + if (tasks.add(333, 0, true, 6, ppsMonitor, "ppsMon")) { VLF("success"); } else { VLF("FAILED!"); } + #endif + #endif +} + +// update/apply the site latitude and longitude, necessary for LAST calculations etc. +void Site::updateLocation() { + locationEx.latitude.cosine = cos(location.latitude); + locationEx.latitude.sine = sin(location.latitude); + locationEx.latitude.absval = fabs(location.latitude); + if (location.latitude >= 0.0) locationEx.latitude.sign = 1.0; else locationEx.latitude.sign = -1.0; + + // same date and time, just calculates the sidereal time again + ut1.hour = getTime(); + setSiderealTime(ut1); +} + +// update the initError status and restore the park position if necessary +void Site::updateTLS() { + #if TIME_LOCATION_SOURCE != OFF + tls->set(ut1); + #endif + #if TIME_LOCATION_SOURCE_FALLBACK != OFF + tlsFallback->set(ut1); + #endif + + if (isDateTimeReady()) { + if (initError.tls) initError.tls = false; + #if GOTO_FEATURE == ON + if (park.state == PS_PARKED) park.restore(false); + #endif + } +} + +// gets the UT1 Julian date/time +JulianDate Site::getDateTime() { + JulianDate now = ut1; + + now.hour = getTime(); + while (now.hour >= 24.0) { now.hour -= 24.0; now.day += 1.0; } + while (now.hour < 0.0) { now.hour += 24.0; now.day -= 1.0; } + + return now; +} + +// sets the UT1 Julian date/time and updates sidereal time +void Site::setDateTime(JulianDate julianDate) { + ut1 = julianDate; + setSiderealTime(julianDate); + + if (writeTime) nv().kv().put("DATE_TIME", ut1); + + if (nv().device().endurance() < NvDevice::Endurance::Mid) writeTime = false; + + #if LIMIT_STRICT == ON + limits.enabled(isDateTimeReady()); + #endif + + if (isDateTimeReady() && mount.startupAuthorityTrusted()) limits.enabled(true); +} + +// gets the time in sidereal hours +double Site::getSiderealTime() { + long fs; + noInterrupts(); + fs = fracLAST; + interrupts(); + return rangeHours(fsToHours(fs)); +} + +// sets the UT time (in hours) that have passed in this Julian Day +void Site::setSiderealTime(JulianDate julianDate) { + setLAST(julianDate, julianDateToLAST(julianDate)); +} + +// checks if the date and time were set +bool Site::isDateTimeReady() { + return dateIsReady && timeIsReady; +} + +// gets sidereal period, in sub-micro counts per second +unsigned long Site::getSiderealPeriod() { + return siderealPeriod; +} + +// sets sidereal period, in sub-micro counts per second +void Site::setSiderealPeriod(unsigned long period) { + siderealPeriod = period; + tasks.setPeriodSubMicros(taskHandle, lround(siderealPeriod/FRACTIONAL_SEC)); +} + +// gets the time in hours that have passed since Julian Day was set (UT1) +double Site::getTime() { + unsigned long cs; + noInterrupts(); + cs = fracLAST; + interrupts(); + return fracHOUR + fsToHours((cs - fracSTART)/SIDEREAL_RATIO); +} + +// sets the time in sidereal hours +void Site::setLAST(JulianDate julianDate, double time) { + long fs = lround(hoursToFs(time)); + fracHOUR = julianDate.hour; + fracSTART = fs; + noInterrupts(); + fracLAST = fs; + interrupts(); +} + +// convert julian date/time to local apparent sidereal time +double Site::julianDateToLAST(JulianDate julianDate) { + // DL("ST 1"); delay(100); + double gast = julianDateToGAST(julianDate); + // DL("ST 2"); delay(100); + return rangeHours(gast - radToHrs(location.longitude)); +} + +// convert julian date/time to greenwich apparent sidereal time +double Site::julianDateToGAST(JulianDate julianDate) { + GregorianDate date; + + date = calendars.julianDayToGregorian(julianDate); + JulianDate julianDay0 = calendars.gregorianToJulianDay(date); + double D= (julianDate.day - 2451545.0) + julianDate.hour/24.0; + double D0=(julianDay0.day - 2451545.0); + double H = julianDate.hour; + double T = D/36525.0; + double gmst = 6.697374558 + 0.06570982441908*D0; + gmst = gmst + SIDEREAL_RATIO*H + 0.000026*T*T; + + // equation of the equinoxes + double O = 125.04 - 0.052954 *D; + double L = 280.47 + 0.98565 *D; + double E = 23.4393 - 0.0000004*D; + double W = -0.000319*sin(degToRad(O)) - 0.000024*sin(degToRad(2*L)); + double eqeq = W*cos(degToRad(E)); + double gast = gmst + eqeq; + + return rangeHours(gast); +} + +// reads the julian date information from NV +void Site::readJD() { + ut1.day = 2451544.5; + ut1.hour = 0.0; + + if (!nv().kv().getOrInit("DATE_TIME", ut1)) { DLF("WRN: Nv, init failed for DATE_TIME"); } + ut1.day = constrain(ut1.day, 2451544.5, 2816787.5); + ut1.hour = constrain(ut1.hour, 0, 24); +} + +// reads the location information from NV +// locationNumber can be 0..3 +void Site::readLocation(uint8_t n) { + if (n > 3) return; + + locationNumber = n; + location.latitude = 0.0; + location.longitude = 0.0; + location.timezone = 0.0; + location.elevation = 100.0; + strcpy(location.name, ""); + + char keyStr[26]; + snprintf(keyStr, sizeof(keyStr), "LOCATION%u_SETTINGS", locationNumber); + nvKey[locationNumber] = nv().kv().computeKey(keyStr); + if (!nv().kv().getOrInit(nvKey[locationNumber], location)) { DF("WRN: Nv, init failed for "); VL(keyStr); } + + location.latitude = constrain(location.latitude, -Deg90, Deg90); + location.longitude = constrain(location.longitude, -Deg360, Deg360); + location.elevation = constrain(location.elevation, -418, 8848); // in meters + location.timezone = constrain(location.timezone, -14, 12); +} + +// sets the site altitude in meters +bool Site::setElevation(float e) { + if (e >= -100.0 && e < 20000.0) location.elevation = e; else return false; + return true; +} + +// convert UT1 to local standard date/time +JulianDate Site::UT1ToLocal(JulianDate ut1) { + ut1.hour -= location.timezone; + while (ut1.hour >= 24.0) { ut1.hour -= 24.0; ut1.day += 1.0; } + while (ut1.hour < 0.0) { ut1.hour += 24.0; ut1.day -= 1.0; } + return ut1; +} + +// convert local standard to UT1 date/time +JulianDate Site::localToUT1(JulianDate local) { + local.hour += location.timezone; + while (local.hour >= 24.0) { local.hour -= 24.0; local.day += 1.0; } + while (local.hour < 0.0) { local.hour += 24.0; local.day -= 1.0; } + return local; +} + +// adjust into the 0 to 24 range +double Site::rangeHours(double time) { + while (time >= 24.0) time -= 24.0; + while (time < 0.0) time += 24.0; + return time; +} + +// adjust into the 0 to 12 range +double Site::rangeAmPm(double time) { + while (time >= 12.0) time -= 12.0; + while (time < 0.0) time += 12.0; + return time; +} + +// convert string in format MM/DD/YY or MM/DD/YYYY to Date (changes only date) +bool Site::strToDate(char *ymd, GregorianDate *date) { + GregorianDate temp; + char m[3], d[3], y[5]; + + if (strlen(ymd) != 8 && strlen(ymd) != 10) return false; + + m[0] = *ymd++; m[1] = *ymd++; m[2] = 0; + if (!convert.atoi2(m, &temp.month, false)) return false; + if (*ymd++ != '/') return false; + + d[0] = *ymd++; d[1] = *ymd++; d[2] = 0; + if (!convert.atoi2(d, &temp.day, false)) return false; + if (*ymd++ != '/') return false; + + y[0] = *ymd++; y[1] = *ymd++; y[2] = 0; + if (strlen(ymd) == 10) { y[2] = *ymd++; y[3] = *ymd++; y[4] = 0; } + if (!convert.atoi2(y, &temp.year, false)) return false; + if (temp.year < 100) { if (temp.year > 20) temp.year += 2000; else temp.year += 2100; } + + if (temp.month < 1 || temp.month > 12 || temp.day < 1 || temp.day > 31 || temp.year < 2000 || temp.year > 2300) return false; + + date->year = temp.year; + date->month = temp.month; + date->day = temp.day; + return true; +} + +Site site; + +#endif diff --git a/src/telescope/mount/site/Site.h b/src/telescope/mount/site/Site.h new file mode 100755 index 0000000..d7caec6 --- /dev/null +++ b/src/telescope/mount/site/Site.h @@ -0,0 +1,158 @@ +//-------------------------------------------------------------------------------------------------- +// observatory time +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/convert/Convert.h" +#include "../../../libApp/commands/ProcessCmds.h" +#include "../../../lib/calendars/Calendars.h" +#include "../../../lib/tls/Tls.h" + +#ifndef SITE_MIN_PPS_SUB_MICRO + #define SITE_MIN_PPS_SUB_MICRO 4 +#endif + +extern volatile unsigned long fracLAST; + +typedef struct LatitudeExtras { + double sine; + double cosine; + double absval; + double sign; +} Latitude; + +typedef struct LocationExtras { + LatitudeExtras latitude; + bool ready; +} LocationExtras; + +#pragma pack(1) +#define LocationSize 40 +typedef struct Location { + double latitude; // in radians + double longitude; // in radians + float elevation; // in meters + float timezone; + char name[16]; +} Location; +#pragma pack() + +class Site { + public: + void init(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // update/apply the site latitude and longitude, necessary for LAST calculations etc. + void updateLocation(); + + // update the initError status and restore the park position if necessary + void updateTLS(); + + // gets the UT1 Julian date/time + JulianDate getDateTime(); + + // sets the UT1 Julian date/time and updates sidereal time + void setDateTime(JulianDate julianDate); + + // gets the time in sidereal hours + double getSiderealTime(); + + // sets the sidereal time from the UT date/time + void setSiderealTime(JulianDate julianDate); + + // checks if the date and time were set + bool isDateTimeReady(); + + // gets sidereal period, in sub-micro counts per second + unsigned long getSiderealPeriod(); + + // sets sidereal period, in sub-micro counts per second + void setSiderealPeriod(unsigned long period); + + // gets sidereal ratio + // slower rates are < 1.0, faster rates are > 1.0 + inline float getSiderealRatio() { return (float)SIDEREAL_PERIOD/siderealPeriod; } + + Location location; + LocationExtras locationEx; + + // for internal use, when to timeout date/time updates + unsigned long updateTimeoutTime = 0; + + bool dateIsReady = false; + bool timeIsReady = false; + + #if TIME_LOCATION_SOURCE != OFF + TimeLocationSource *tls; + #endif + + #if TIME_LOCATION_SOURCE_FALLBACK != OFF + TimeLocationSource *tlsFallback; + #endif + + private: + // gets the time in hours that have passed since Julian Day was set (UT1) + double getTime(); + + // sets the time in sidereal hours + void setLAST(JulianDate julianDate, double time); + + // convert julian date/time to local apparent sidereal time + double julianDateToLAST(JulianDate julianDate); + + // convert julian date/time to greenwich apparent sidereal time + double julianDateToGAST(JulianDate julianDate); + + // reads the julian date information from NV + void readJD(); + + // reads the location information from NV + // locationNumber can be 0..3 + void readLocation(uint8_t number); + + // sets the site altitude in meters + bool setElevation(float e); + + // convert UT1 to local standard date/time + JulianDate UT1ToLocal(JulianDate ut1); + + // convert local standard to UT1 date/time + JulianDate localToUT1(JulianDate local); + + // adjust into the 0 to 24 range + double rangeHours(double time); + + // adjust into the 0 to 12 range + double rangeAmPm(double time); + + // convert string in format MM/DD/YY or MM/DD/YYYY to Date (changes only date) + bool strToDate(char *ymd, GregorianDate *date); + + // the current UT1 date and time + JulianDate ut1; + double fracHOUR = 0; + unsigned long fracSTART = 0; + + bool writeDate = true; + bool writeTime = true; + + // sidereal period in sub-microsecond counts + unsigned long siderealPeriod = 0; + + // handle to sidereal timer LAST task + uint8_t taskHandle = 0; + + // site number 0..3 + uint8_t locationNumber = 0; + + // site nv keys + uint16_t nvKey[4]; +}; + +extern Site site; + +#endif \ No newline at end of file diff --git a/src/telescope/mount/st4/St4.cpp b/src/telescope/mount/st4/St4.cpp new file mode 100755 index 0000000..c1caaec --- /dev/null +++ b/src/telescope/mount/st4/St4.cpp @@ -0,0 +1,301 @@ +// --------------------------------------------------------------------------------------------------- +// Guide, commands to move the mount in any direction at a series of fixed rates + +#include "St4.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" + +#include "../../Telescope.h" +#include "../Mount.h" +#include "../guide/Guide.h" +#include "../goto/Goto.h" +#include "../status/Status.h" + +#if ST4_INTERFACE == ON + #include "../../../lib/pushButton/PushButton.h" + + #if ST4_HAND_CONTROL == ON + #include "../../../lib/serial/Serial_ST4_Master.h" + #define debounceMs 100L + #else + #define debounceMs 5L + #endif + + #include "../../../lib/serial/Serial_Local.h" + + Button st4Axis1Rev(ST4_RA_E_PIN, ST4_INTERFACE_INIT, LOW | HYST(debounceMs)); + Button st4Axis1Fwd(ST4_RA_W_PIN, ST4_INTERFACE_INIT, LOW | HYST(debounceMs)); + Button st4Axis2Fwd(ST4_DEC_N_PIN, ST4_INTERFACE_INIT, LOW | HYST(debounceMs)); + Button st4Axis2Rev(ST4_DEC_S_PIN, ST4_INTERFACE_INIT, LOW | HYST(debounceMs)); + + // Single byte guide commands + #define ccMe 14 + #define ccMw 15 + #define ccMn 16 + #define ccMs 17 + #define ccQe 18 + #define ccQw 19 + #define ccQn 20 + #define ccQs 21 + + void st4Wrapper() { + st4.poll(); + } + + void serialSt4Wrapper() { + st4.pollSerial(); + } + + void St4::init() { + long rate = round(1000.0F/HAL_FRACTIONAL_SEC); + VF("MSG: Mount, ST4 start monitor task (rate "); V(rate); VF("ms priority 2)... "); + if (tasks.add(rate, 0, true, 2, st4Wrapper, "St4Mntr")) { VLF("success"); } else { VLF("FAILED!"); } + } + + void St4::poll() { + static bool shcActive = false; + + st4Axis1Rev.poll(); + if (!shcActive) { + st4Axis1Fwd.poll(); + st4Axis2Fwd.poll(); + st4Axis2Rev.poll(); + } + + #if ST4_HAND_CONTROL == ON + + if (shcActive) { + // Smart Hand Controller deactivate + if (!st4Axis1Rev.hasTone()) { + pinMode(ST4_DEC_S_PIN, ST4_INTERFACE_INIT); + pinMode(ST4_DEC_N_PIN, ST4_INTERFACE_INIT); + + serialST4.end(); + + // stop the SHC task + VLF("MSG: Mount, stop SerialST4 comms task"); + tasks.remove(handleSerialST4); + handleSerialST4 = 0; + + VLF("MSG: SerialST4, deactivated"); + shcActive = false; + } + return; + } + + // Smart Hand Controller activate + if (!shcActive && st4Axis1Rev.hasTone() && st4Axis1Fwd.hasTone()) { + pinMode(ST4_DEC_S_PIN, OUTPUT); // SerialST4 clock + pinMode(ST4_DEC_N_PIN, OUTPUT); // SerialST4 data out + + serialST4.begin(); + + // start the SHC comms task + VF("MSG: Mount, start SerialST4 comms task (rate 100us priority 1)... "); + handleSerialST4 = tasks.add(0, 0, true, 1, serialSt4Wrapper, "St4Comm"); + if (handleSerialST4) { VLF("success"); } else { VLF("FAILED!"); } + tasks.setPeriodMicros(handleSerialST4, 100); + tasks.setTimingMode(handleSerialST4, TM_MINIMUM); + + VLF("MSG: SerialST4, activated"); + shcActive = true; + return; + } + + // standard hand control + const long Shed_ms = 4000; + + // stop any guide that might be triggered by combination button presses + if (st4Axis1Rev.isDown() && st4Axis1Fwd.isDown()) guide.stopAxis1(); + if (st4Axis2Fwd.isDown() && st4Axis2Rev.isDown()) guide.stopAxis2(); + + // see if a combination was down for long enough for an alternate mode + static bool altModeA = false; + static bool altModeB = false; + + #if GOTO_FEATURE == ON + const long AltMode_ms = 2000; + if (goTo.state == GS_NONE && !goTo.isHomePaused()) { + if (st4Axis1Rev.timeDown() > AltMode_ms && st4Axis1Fwd.timeDown() > AltMode_ms && !altModeB) { + if (!altModeA) { altModeA = true; mountStatus.soundBeep(); } + } + if (st4Axis2Fwd.timeDown() > AltMode_ms && st4Axis2Rev.timeDown() > AltMode_ms && !altModeA) { + if (!altModeB) { altModeB = true; mountStatus.soundBeep(); } + } + } + #endif + + // if the alternate mode is allowed & selected & hasn't timed out, handle it + if ( (altModeA || altModeB) && (st4Axis2Fwd.timeUp() < Shed_ms || st4Axis2Rev.timeUp() < Shed_ms || st4Axis1Rev.timeUp() < Shed_ms || st4Axis1Fwd.timeUp() < Shed_ms) ) { + + if (altModeA) { + uint8_t currentRateSelect = ((uint8_t)guide.settings.axis1RateSelect) & 0b11111110; + + // adjust guide rate faster + if (st4Axis1Fwd.wasPressed() && !st4Axis1Rev.wasPressed()) { + if (!mount.isTracking()) { SERIAL_LOCAL.transmit(":B+#"); mountStatus.soundClick(); } else { + if (currentRateSelect <= (GOTO_FEATURE == ON ? 6 : 4)) { + guide.settings.axis1RateSelect = (GuideRateSelect)(currentRateSelect + 2); + guide.settings.axis2RateSelect = (GuideRateSelect)(currentRateSelect + 2); + if (GUIDE_SEPARATE_PULSE_RATE == ON && guide.settings.axis1RateSelect <= GR_1X) guide.settings.pulseRateSelect = guide.settings.axis1RateSelect; + mountStatus.soundClick(); + } + } + } + + // adjust guide rate slower + if (st4Axis1Rev.wasPressed() && !st4Axis1Fwd.wasPressed()) { + if (!mount.isTracking()) { SERIAL_LOCAL.transmit(":B-#"); mountStatus.soundClick(); } else { + if (currentRateSelect >= 2) { + guide.settings.axis1RateSelect = (GuideRateSelect)(currentRateSelect - 2); + guide.settings.axis2RateSelect = (GuideRateSelect)(currentRateSelect - 2); + if (GUIDE_SEPARATE_PULSE_RATE == ON && guide.settings.axis1RateSelect <= GR_1X) guide.settings.pulseRateSelect = guide.settings.axis1RateSelect; + mountStatus.soundClick(); } + } + } + + // tracking on/off + if (st4Axis2Fwd.wasPressed() && !st4Axis2Rev.wasPressed()) { + mount.tracking(!mount.isTracking()); + mountStatus.soundClick(); + } + + // sync or accept align + if (st4Axis2Rev.wasPressed() && !st4Axis2Fwd.wasPressed()) { + #if GOTO_FEATURE == ON + if (goTo.alignDone()) SERIAL_LOCAL.transmit(":CS#"); else goTo.alignAddStar(); + #else + SERIAL_LOCAL.transmit(":CS#"); + #endif + mountStatus.soundClick(); + } + } + + if (altModeB) { + #if ST4_HAND_CONTROL_FOCUSER == ON + const char moveSlow[2][8] = {":F12#", ":F22#"}; + const char moveFast[2][8] = {":F14#", ":F24#"}; + const char moveIn[2][8] = {":F1-#", ":F2-#"}; + const char moveOut[2][8] = {":F1+#", ":F2+#"}; + static int fs = 0; + static int fn = 0; + static int focuserNumber = 0; + + // select focuser 1 or 2 + if (!fn && !fs) { + if (st4Axis1Fwd.wasPressed() && !st4Axis1Rev.wasPressed()) { focuserNumber = 1; mountStatus.soundClick(); } + if (st4Axis1Rev.wasPressed() && !st4Axis1Fwd.wasPressed()) { focuserNumber = 0; mountStatus.soundClick(); } + } + + // move selected focuser out + if (!fn) { + if (st4Axis2Rev.isDown() && st4Axis2Fwd.isUp()) { + if (fs == 0) { SERIAL_LOCAL.transmit(moveSlow[focuserNumber]); fs++; } else + if (fs == 1) { SERIAL_LOCAL.transmit(moveIn[focuserNumber]); fs++; } else + if (fs == 2 && st4Axis2Rev.timeDown() > 4000) { SERIAL_LOCAL.transmit(moveFast[focuserNumber]); fs++; } else + if (fs == 3) { SERIAL_LOCAL.transmit(moveIn[focuserNumber]); fs++; } + } + if (st4Axis2Rev.isUp()) { if (fs > 0) { SERIAL_LOCAL.transmit(":F1Q#:F2Q#"); fs = 0; } } + } + + // move selected focuser in + if (!fs) { + if (st4Axis2Fwd.isDown() && st4Axis2Rev.isUp()) { + if (fn == 0) { SERIAL_LOCAL.transmit(moveSlow[focuserNumber]); fn++; } else + if (fn == 1) { SERIAL_LOCAL.transmit(moveOut[focuserNumber]); fn++; } else + if (fn == 2 && st4Axis2Fwd.timeDown() > 4000) { SERIAL_LOCAL.transmit(moveFast[focuserNumber]); fn++; } else + if (fn == 3) { SERIAL_LOCAL.transmit(moveOut[focuserNumber]); fn++; } + } + if (st4Axis2Fwd.isUp()) { if (fn > 0) { SERIAL_LOCAL.transmit(":F1Q#:F2Q#"); fn = 0; } } + } + #else + // select next user catalog item + if (st4Axis1Fwd.wasPressed() && !st4Axis1Rev.wasPressed()) { SERIAL_LOCAL.transmit(":LN#"); mountStatus.soundClick(); } + + // select previous user catalog item + if (st4Axis1Rev.wasPressed() && !st4Axis1Fwd.wasPressed()) { SERIAL_LOCAL.transmit(":LB#"); mountStatus.soundClick(); } + + // goto user catalog item + if (st4Axis2Fwd.wasPressed() && !st4Axis2Rev.wasPressed()) { SERIAL_LOCAL.transmit(":LIG#"); mountStatus.soundClick(); } + + // turn sound on/off + if (st4Axis2Rev.wasPressed() && !st4Axis2Fwd.wasPressed()) { mountStatus.soundClick(); mountStatus.soundToggleEnable(); mountStatus.soundClick(); } + #endif + } + } else { + if (altModeA || altModeB) { + #if ST4_HAND_CONTROL_FOCUSER == ON + SERIAL_LOCAL.transmit(":F1Q#:F2Q#"); + #endif + altModeA = false; + altModeB = false; + mountStatus.soundBeep(); + } + #endif + + if (mount.isEnabled()) { + // guide E/W + bool pulseGuiding = GUIDE_SEPARATE_PULSE_RATE == ON && ST4_HAND_CONTROL != ON; + GuideAction st4GuideActionAxis1 = GA_BREAK; + static GuideAction lastSt4GuideActionAxis1 = GA_BREAK; + if (st4Axis1Fwd.isDown() && st4Axis1Rev.isUp()) st4GuideActionAxis1 = GA_FORWARD; + if (st4Axis1Rev.isDown() && st4Axis1Fwd.isUp()) st4GuideActionAxis1 = GA_REVERSE; + + if (st4GuideActionAxis1 != lastSt4GuideActionAxis1) { + lastSt4GuideActionAxis1 = st4GuideActionAxis1; + if (st4GuideActionAxis1 != GA_BREAK) { + #if ST4_HAND_CONTROL == ON && GOTO_FEATURE == ON + if (goTo.isHomePaused()) goTo.homeContinue(); else + if (goTo.state == GS_GOTO) goTo.abort(); else + #endif + guide.startAxis1(st4GuideActionAxis1, pulseGuiding ? guide.settings.pulseRateSelect : guide.settings.axis1RateSelect, GUIDE_TIME_LIMIT*1000); + } else guide.stopAxis1(); + } + + // guide N/S + GuideAction st4GuideActionAxis2 = GA_BREAK; + static GuideAction lastSt4GuideActionAxis2 = GA_BREAK; + if (st4Axis2Fwd.isDown() && st4Axis2Rev.isUp()) st4GuideActionAxis2 = GA_FORWARD; + if (st4Axis2Rev.isDown() && st4Axis2Fwd.isUp()) st4GuideActionAxis2 = GA_REVERSE; + + if (st4GuideActionAxis2 != lastSt4GuideActionAxis2) { + lastSt4GuideActionAxis2 = st4GuideActionAxis2; + if (st4GuideActionAxis2 != GA_BREAK) { + #if ST4_HAND_CONTROL == ON && GOTO_FEATURE == ON + if (goTo.isHomePaused()) goTo.homeContinue(); else + if (goTo.state == GS_GOTO) goTo.abort(); else + #endif + guide.startAxis2(st4GuideActionAxis2, pulseGuiding ? guide.settings.pulseRateSelect : guide.settings.axis2RateSelect, GUIDE_TIME_LIMIT*1000); + } else guide.stopAxis2(); + } + + } + + #if ST4_HAND_CONTROL == ON + } + #endif + } + + #if ST4_HAND_CONTROL == ON + void St4::pollSerial() { + char c = serialST4.poll(); + + // process any single byte guide commands + switch (c) { + case ccMe: guide.startAxis1(GA_REVERSE, guide.settings.axis1RateSelect, GUIDE_TIME_LIMIT*1000); break; + case ccMw: guide.startAxis1(GA_FORWARD, guide.settings.axis1RateSelect, GUIDE_TIME_LIMIT*1000); break; + case ccMn: guide.startAxis2(GA_FORWARD, guide.settings.axis2RateSelect, GUIDE_TIME_LIMIT*1000); break; + case ccMs: guide.startAxis2(GA_REVERSE, guide.settings.axis2RateSelect, GUIDE_TIME_LIMIT*1000); break; + case ccQe: case ccQw: guide.stopAxis1(); break; + case ccQn: case ccQs: guide.stopAxis2(); break; + } + } + #endif + +#endif + +St4 st4; + +#endif diff --git a/src/telescope/mount/st4/St4.h b/src/telescope/mount/st4/St4.h new file mode 100755 index 0000000..9404823 --- /dev/null +++ b/src/telescope/mount/st4/St4.h @@ -0,0 +1,27 @@ +//-------------------------------------------------------------------------------------------------- +// telescope mount control +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +class St4 { + public: + void init(); + + // monitor ST4 port for guiding, basic hand controller, and smart hand controller activation + void poll(); + + #if ST4_HAND_CONTROL == ON + // transmit and recieve the smart hand controller data + void pollSerial(); + #endif + + private: + uint8_t handleSerialST4 = 0; +}; + +extern St4 st4; + +#endif diff --git a/src/telescope/mount/status/Status.command.cpp b/src/telescope/mount/status/Status.command.cpp new file mode 100755 index 0000000..ac0e4f5 --- /dev/null +++ b/src/telescope/mount/status/Status.command.cpp @@ -0,0 +1,202 @@ +// --------------------------------------------------------------------------------------------------- +// Mount status LED and buzzer + +#include "Status.h" + +#if defined(MOUNT_PRESENT) + +#include "../../../lib/nv/Nv.h" + +#include "../../../lib/tls/PPS.h" + +#include "../Mount.h" +#include "../coordinates/Transform.h" +#include "../goto/Goto.h" +#include "../guide/Guide.h" +#include "../home/Home.h" +#include "../park/Park.h" +#include "../pec/Pec.h" +#include "../limits/Limits.h" +#include "../status/Status.h" + +bool Status::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + UNUSED(suppressFrame); + + if (command[0] == 'G') { + // :Gm# Gets the meridian pier-side + // Returns: E#, W#, N# (none/parked) + if (command[1] == 'm' && parameter[0] == 0) { + strcpy(reply, "?"); + Coordinate current = mount.getMountPosition(CR_MOUNT); + if (guide.state == GU_HOME_GUIDE || guide.state == GU_HOME_GUIDE_ABORT) current.pierSide = PIER_SIDE_NONE; + if (current.pierSide == PIER_SIDE_NONE) reply[0]='N'; + if (current.pierSide == PIER_SIDE_EAST) reply[0]='E'; + if (current.pierSide == PIER_SIDE_WEST) reply[0]='W'; + *numericReply = false; + } else + + // :GU# Get telescope Status + // Returns: s# + if (command[1] == 'U' && parameter[0] == 0) { + int i = 0; + if (!mount.isTracking()) reply[i++]='n'; // [n]ot tracking + if (goTo.state == GS_NONE) reply[i++]='N'; // [N]o goto + if (park.state == PS_UNPARKED) reply[i++]='p'; else // Not [p]arked + if (park.state == PS_PARKING) reply[i++]='I'; else // Parking [I]n-progress + if (park.state == PS_PARKED) reply[i++]='P'; else // [P]arked + if (park.state == PS_PARK_FAILED) reply[i++]='F'; // Park [F]ailed + if (mount.syncFromOnStepToEncoders) reply[i++]='e'; // Sync to [e]ncoders only + if (mount.isHome()) reply[i++]='H'; // At [H]ome + if (home.state == HS_HOMING) reply[i++]='h'; // Slewing [h]ome + if (home.settings.automaticAtBoot) reply[i++]='B'; // Auto home at [B]oot + #if (TIME_LOCATION_PPS_SENSE) != OFF + if (pps.synced) reply[i++]='S'; // PPS [S]ync + #endif + if (guide.activePulseGuide()) reply[i++]='G'; // Pulse [G]uide active + if (guide.active()) reply[i++]='g'; // [g]uide active + + if (mount.settings.rc == RC_REFRACTION) { reply[i++]='r'; reply[i++]='s'; } // [r]efr enabled [s]ingle axis + if (mount.settings.rc == RC_REFRACTION_DUAL) { reply[i++]='r'; } // [r]efr enabled + if (mount.settings.rc == RC_MODEL) { reply[i++]='t'; reply[i++]='s'; } // On[t]rack enabled [s]ingle axis + if (mount.settings.rc == RC_MODEL_DUAL) { reply[i++]='t'; } // On[t]rack enabled + if (mount.settings.rc == RC_NONE) { + float r = siderealToHz(mount.trackingRate); + if (fequal(r, 57.900F)) reply[i++]='('; else // Lunar rate selected + if (fequal(r, 60.000F)) reply[i++]='O'; else // SOlar rate selected + if (fequal(r, 60.136F)) reply[i++]='k'; // King rate selected + } + + if (goTo.isHomePaused()) reply[i++]='w'; // [w]aiting at home + if (goTo.isHomePauseEnabled()) reply[i++]='u'; // Pa[u]se at home enabled? + if (sound.enabled) reply[i++]='z'; // Bu[z]zer enabled? + if (goTo.isAutoFlipEnabled()) reply[i++]='a'; // [a]uto meridian flip + #if AXIS1_PEC == ON + if (pec.settings.recorded) reply[i++]='R'; // PEC data has been [R]ecorded + if (transform.isEquatorial()) + reply[i++]="/,~;^"[(int)pec.settings.state]; // PEC State (/)gnore, ready (,)lay, (~)laying, ready (;)ecord, (^)ecording + #endif + if (transform.mountType == GEM) reply[i++]='E'; else // GEM + if (transform.mountType == FORK) reply[i++]='K'; else // FORK + if (transform.mountType == ALTAZM) reply[i++]='A'; else // ALTAZM + if (transform.mountType == ALTALT) reply[i++]='L'; // ALTALT + + Coordinate current = mount.getMountPosition(CR_MOUNT); + if (guide.state == GU_HOME_GUIDE || guide.state == GU_HOME_GUIDE_ABORT) current.pierSide = PIER_SIDE_NONE; + if (current.pierSide == PIER_SIDE_NONE) reply[i++]='o'; else // Pier side n[o]ne + if (current.pierSide == PIER_SIDE_EAST) reply[i++]='T'; else // Pier side eas[T] + if (current.pierSide == PIER_SIDE_WEST) reply[i++]='W'; // Pier side [W]est + + reply[i++]='0' + guide.settings.pulseRateSelect; // Provide pulse-guide rate + reply[i++]='0' + guide.settings.axis1RateSelect; // Provide guide rate + + reply[i++]='0' + mountStatus.errorCode(); // Provide general error code + reply[i++]=0; + + *numericReply = false; + } else + + // :Gu# Get bit packed telescope status + // Returns: s# + if (command[1] == 'u' && parameter[0] == 0) { + memset(reply, (char)0b10000000, 9); + if (!mount.isTracking()) reply[0]|=0b10000001; // Not tracking + if (goTo.state == GS_NONE) reply[0]|=0b10000010; // No goto + #if (TIME_LOCATION_PPS_SENSE) != OFF + if (pps.synced) reply[0]|=0b10000100; // PPS sync + #endif + if (guide.activePulseGuide()) reply[0]|=0b10001000; // Pulse guide active + + if (mount.settings.rc == RC_REFRACTION) reply[0]|=0b11010000; // Refr enabled Single axis + if (mount.settings.rc == RC_REFRACTION_DUAL) reply[0]|=0b10010000; // Refr enabled + if (mount.settings.rc == RC_MODEL) reply[0]|=0b11100000; // OnTrack enabled Single axis + if (mount.settings.rc == RC_MODEL_DUAL) reply[0]|=0b10100000; // OnTrack enabled + if (mount.settings.rc == RC_NONE) { + float r = siderealToHz(mount.trackingRate); + if (fequal(r, 57.900F)) reply[1]|=0b10000001; else // Lunar rate selected + if (fequal(r, 60.000F)) reply[1]|=0b10000010; else // Solar rate selected + if (fequal(r, 60.136F)) reply[1]|=0b10000011; // King rate selected + } + + if (mount.syncFromOnStepToEncoders) reply[1]|=0b10000100; // Sync to encoders only + if (guide.active()) reply[1]|=0b10001000; // Guide active + if (mountStatus.startupAuthorityTrusted()) reply[1]|=0b10010000; // Startup authority trusted + if (mount.isHome()) reply[2]|=0b10000001; // At home + if (home.state == HS_HOMING) reply[2]|=0b10100000; // Slewing home + if (home.settings.automaticAtBoot) reply[2]|=0b11000000; // Auto home at boot + if (goTo.isHomePaused()) reply[2]|=0b10000010; // Waiting at home + if (goTo.isHomePauseEnabled()) reply[2]|=0b10000100; // Pause at home enabled? + if (sound.enabled) reply[2]|=0b10001000; // Buzzer enabled? + if (goTo.isAutoFlipEnabled()) reply[2]|=0b10010000; // Auto meridian flip + + if (transform.mountType == GEM) reply[3]|=0b10000001; else // GEM + if (transform.mountType == FORK) reply[3]|=0b10000010; else // FORK + if (transform.mountType == ALTAZM) reply[3]|=0b10001000; else // ALTAZM + if (transform.mountType == ALTALT) reply[3]|=0b10000100; // ALTALT + + Coordinate current = mount.getMountPosition(CR_MOUNT); + if (guide.state == GU_HOME_GUIDE || guide.state == GU_HOME_GUIDE_ABORT) current.pierSide = PIER_SIDE_NONE; + if (current.pierSide == PIER_SIDE_NONE) reply[3]|=0b10010000; else // Pier side none + if (current.pierSide == PIER_SIDE_EAST) reply[3]|=0b10100000; else // Pier side east + if (current.pierSide == PIER_SIDE_WEST) reply[3]|=0b11000000; // Pier side west + + #if AXIS1_PEC == ON + if (transform.isEquatorial()) + reply[4] = (int)pec.settings.state|0b10000000; // PEC state: 0 ignore, 1 ready play, 2 playing, 3 ready record, 4 recording + if (pec.settings.recorded) reply[4]|=0b11000000; // PEC state: data has been recorded + #endif + reply[5] = (int)park.state|0b10000000; // Park state: 0 not parked, 1 parking in-progress, 2 parked, 3 park failed + reply[6] = (int)guide.settings.pulseRateSelect|0b10000000; // Pulse-guide selection + reply[7] = (int)guide.settings.axis1RateSelect|0b10000000; // Guide selection + reply[8] = mountStatus.errorCode()|0b10000000; // General error + reply[9] = 0; + *numericReply = false; + } else + + // :GW# Get tracking and basic mount state + // Returns: s# + if (command[1] == 'W' && parameter[0] == 0) { + int i = 0; + if (transform.mountType == GEM) reply[i++] = 'G'; else + if (transform.mountType == FORK) reply[i++] = 'P'; else + if (transform.mountType == ALTAZM) reply[i++] = 'A'; else + if (transform.mountType == ALTALT) reply[i++] = 'L'; + if (mount.isTracking()) reply[i++] = 'T'; else reply[i++] = 'N'; + if (park.state == PS_PARKED) reply[i++] = 'P'; else + if (mount.isHome()) reply[i++] = 'H'; else + if (goTo.alignDone()) reply[i++] = '1'; else reply[i++] = '0'; + reply[i++] = 0; + *numericReply = false; + } else return false; + + } else + + // :SX97,[n]# Set buzzer state + // Return: see below + if (command[0] == 'S' && command[1] == 'X' && parameter[0] == '9' && parameter[1] == '7' && parameter[2] == ',' && parameter[4] == 0) { + switch (parameter[3]) { + case '0': case '1': + sound.enabled = parameter[3] - '0'; + #if STATUS_BUZZER_MEMORY == ON + settings.soundEnabled = sound.enabled; + nv().kv().put("STATUS_SETTINGS", settings); + #endif + break; + case '2': + soundBeep(); + break; + case '3': + soundAlert(); + break; + case '4': + soundClick(); + break; + default: + *commandError = CE_PARAM_RANGE; + break; + } + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/mount/status/Status.cpp b/src/telescope/mount/status/Status.cpp new file mode 100755 index 0000000..0fe01c5 --- /dev/null +++ b/src/telescope/mount/status/Status.cpp @@ -0,0 +1,105 @@ +// --------------------------------------------------------------------------------------------------- +// Mount status LED and buzzer + +#include "Status.h" + +#ifdef MOUNT_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/gpioEx/GpioEx.h" +#include "../../../lib/nv/Nv.h" + +#include "../Mount.h" +#include "../limits/Limits.h" +#include "../park/Park.h" + +#if STATUS_MOUNT_LED != OFF && MOUNT_LED_PIN != OFF + bool ledOn = false; + bool ledOff = false; + void flash() { + if (ledOff) { digitalWriteEx(MOUNT_LED_PIN, !MOUNT_LED_ON_STATE); return; } + if (ledOn) { digitalWriteEx(MOUNT_LED_PIN, MOUNT_LED_ON_STATE); return; } + static uint8_t cycle = 0; + if ((cycle++)%2 == 0) { + digitalWriteEx(MOUNT_LED_PIN, !MOUNT_LED_ON_STATE); + } else { + digitalWriteEx(MOUNT_LED_PIN, MOUNT_LED_ON_STATE); + } + } +#endif + +void generalWrapper() { mountStatus.general(); } + +// get mount status ready +void Status::init() { + #if STATUS_BUZZER_MEMORY == ON + if (!nv().kv().getOrInit("STATUS_SETTINGS", settings)) { DLF("WRN: Nv, init failed for STATUS_SETTINGS"); } + sound.enabled = settings.soundEnabled; + #endif + + #if PARK_STATUS != OFF && PARK_STATUS_PIN != OFF + pinModeEx(PARK_STATUS_PIN, OUTPUT); + #endif + + VF("MSG: Mount, status start general status task (1s rate priority 4)... "); + if (tasks.add(1000, 0, true, 4, generalWrapper, "MtStat")) { VLF("success"); } else { VLF("FAILED!"); } +} + +// mount status wake on demand +void Status::wake() { + static bool ready = false; + + if (!ready) { + #if STATUS_LED == ON + // if Mount buzzer or status LED are using the Telescope status LED pin, disable it and use here now + if (STATUS_MOUNT_LED != OFF && MOUNT_LED_PIN == STATUS_LED_PIN) tasks.remove(tasks.getHandleByName("StaLed")); + if (STATUS_BUZZER != OFF && STATUS_BUZZER_PIN == STATUS_LED_PIN) tasks.remove(tasks.getHandleByName("StaLed")); + #endif + + #if STATUS_MOUNT_LED != OFF && MOUNT_LED_PIN != OFF + if (!tasks.getHandleByName("mntLed")) { + pinModeEx(MOUNT_LED_PIN, OUTPUT); + VF("MSG: Mount, status start LED task (variable rate priority 4)... "); + statusTaskHandle = tasks.add(0, 0, true, 4, flash, "mntLed"); + if (statusTaskHandle) { VLF("success"); } else { VLF("FAILED!"); } + } + #endif + + #if STATUS_BUZZER != OFF + VLF("MSG: Mount, status start buzzer"); + sound.init(); + #endif + + ready = true; + } +} + +// mount status LED flash rate (in ms) +void Status::flashRate(int period) { + #if STATUS_MOUNT_LED != OFF && MOUNT_LED_PIN != OFF + if (period == 0) { period = 500; ledOff = true; } else ledOff = false; + if (period == 1) { period = 500; ledOn = true; } else ledOn = false; + tasks.setPeriod(statusTaskHandle, period/2UL); + #else + period = period; + #endif +} + +uint8_t Status::errorCode() { + return limits.errorCode(); +} + +bool Status::startupAuthorityTrusted() { + return mount.startupAuthorityTrusted(); +} + +// mount misc. general status indicators +void Status::general() { + #if PARK_STATUS != OFF && PARK_STATUS_PIN != OFF + digitalWriteEx(PARK_STATUS_PIN, (park.state == PS_PARKED) ? PARK_STATUS : !PARK_STATUS) + #endif +} + +Status mountStatus; + +#endif diff --git a/src/telescope/mount/status/Status.h b/src/telescope/mount/status/Status.h new file mode 100755 index 0000000..adfa91f --- /dev/null +++ b/src/telescope/mount/status/Status.h @@ -0,0 +1,52 @@ +//-------------------------------------------------------------------------------------------------- +// Mount status LED and buzzer +#pragma once + +#include "../../../Common.h" + +#ifdef MOUNT_PRESENT + +#include "../../../libApp/commands/ProcessCmds.h" +#include "../../../lib/sound/Sound.h" + +#define SF_STOPPED 0 +#define SF_SLEWING 1 + +typedef struct StatusSettings { + bool soundEnabled; +} StatusSettings; + +class Status { + public: + // get mount status ready + void init(); + + // mount status wake on demand + void wake(); + + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // current mount fault/status code for external reporting + uint8_t errorCode(); + bool startupAuthorityTrusted(); + + // mount status LED flash rate (in ms) + void flashRate(int period); + + // mount misc. general status indicators + void general(); + + inline void soundAlert() { if (sound.enabled) wake(); sound.alert(); } + inline void soundBeep() { if (sound.enabled) wake(); sound.beep(); } + inline void soundClick() { if (sound.enabled) wake(); sound.click(); } + inline void soundToggleEnable() { sound.enabled = !sound.enabled; } + + private: + uint8_t statusTaskHandle = 0; + Sound sound; + StatusSettings settings = {false}; +}; + +extern Status mountStatus; + +#endif diff --git a/src/telescope/rotator/Rotator.h b/src/telescope/rotator/Rotator.h new file mode 100755 index 0000000..630396f --- /dev/null +++ b/src/telescope/rotator/Rotator.h @@ -0,0 +1,19 @@ +//-------------------------------------------------------------------------------------------------- +// local and remote telescope rotator control switchboard + +#pragma once + +#include "RotatorBase.h" + +// ----------------------------------------------------------------------------- +// Include the chosen implementation +// - CLIENT: Rotator is a proxy class +// - otherwise (local present): Rotator is the local hardware class +// and REMOTE (server) support is compiled in as additional code paths. +// ----------------------------------------------------------------------------- +#if defined(ROTATOR_CAN_CLIENT_PRESENT) + #include "remote/Rotator.h" // defines class Rotator (proxy) +#elif defined(ROTATOR_PRESENT) + #include "local/Rotator.h" // defines class Rotator (local hardware) + // conditionally includes/declares server hooks when ROTATOR_CAN_SERVER_PRESENT is defined +#endif diff --git a/src/telescope/rotator/RotatorBase.h b/src/telescope/rotator/RotatorBase.h new file mode 100755 index 0000000..f57c932 --- /dev/null +++ b/src/telescope/rotator/RotatorBase.h @@ -0,0 +1,110 @@ + +// ----------------------------------------------------------------------------- +// Where the base CAN ID's start for this CANbus transport +// ----------------------------------------------------------------------------- +#pragma once + +#include "../../Common.h" + +#ifndef CAN_ROTATOR_REQ_ID + #define CAN_ROTATOR_REQ_ID (0x00 + 3) +#endif + +#ifndef CAN_ROTATOR_RSP_ID + #define CAN_ROTATOR_RSP_ID (CAN_ROTATOR_REQ_ID + 0x10) +#endif + +#ifndef CAN_ROTATOR_HB_ID_BASE + #define CAN_ROTATOR_HB_ID_BASE (CAN_ROTATOR_REQ_ID + 0x20) +#endif + +// ----------------------------------------------------------------------------- +// CAN_ROTATOR role selection +// OFF : no CAN rotator support +// ON : telescope/mount forwards rotator commands over CAN (client/proxy) +// REMOTE : remote node answers rotator commands over CAN (server/local axis3) +// ----------------------------------------------------------------------------- + +#ifndef CAN_ROTATOR + #define CAN_ROTATOR OFF +#endif + +// ----------------------------------------------------------------------------- +// Validate CAN requirements +// ----------------------------------------------------------------------------- +#if (CAN_ROTATOR != OFF) + #if !defined(CAN_PLUS) || (CAN_PLUS == OFF) + #error "Rotator: CAN_ROTATOR requires CAN_PLUS to be enabled." + #endif +#endif + +// ----------------------------------------------------------------------------- +// Derive presence macros used by Telescope.cpp command router +// ----------------------------------------------------------------------------- +#if (CAN_ROTATOR == ON) + // ROTATOR_PRESENT must NOT exist + #define ROTATOR_CLIENT_PRESENT // a remote rotator may be present + #define ROTATOR_CAN_CLIENT_PRESENT // client implementation using CAN +#elif (CAN_ROTATOR == REMOTE) + // ROTATOR_PRESENT must exist since we need Axis3 + #define ROTATOR_CAN_SERVER_PRESENT // server implementation (compiled into local rotator) +#endif + +// ----------------------------------------------------------------------------- +// Enforce “only one rotator” contract +// ----------------------------------------------------------------------------- + +// CLIENT build must NOT also have a local Axis3 rotator. +#if defined(ROTATOR_PRESENT) && defined(ROTATOR_CLIENT_PRESENT) + #error "Config: both local AXIS3 rotator (ROTATOR_PRESENT) and CAN_ROTATOR ON are enabled. Choose one." +#endif + +// REMOTE build MUST have local Axis3 rotator (otherwise it can't serve anything). +#if !defined(ROTATOR_PRESENT) && defined(ROTATOR_CAN_SERVER_PRESENT) + #error "Config: CAN_ROTATOR REMOTE requires a local AXIS3 rotator (ROTATOR_PRESENT)." +#endif + +// ----------------------------------------------------------------------------- +// CANbus command opcode set (5-bit). Per-ID namespace, so rotator "owns" these. +// ----------------------------------------------------------------------------- + +#define ROT_OP_PARK_HP 0x08 // :hP# | TX: none -> RX: none (numericReply=1; CE_1 on success) +#define ROT_OP_UNPARK_HR 0x09 // :hR# | TX: none -> RX: none (numericReply=1; CE_1 on success) + +#define ROT_OP_ACTIVE_RA 0x0A // :rA# | TX: none -> RX: none (numericReply=1; CE_1 on success) + +#define ROT_OP_GET_STATUS_T 0x01 // :rT# | TX: none -> RX: ASCII bytes "M"/"S"["D"]["R"] + rateDigit ('1'..'5') +#define ROT_OP_GET_MIN_I 0x0B // :rI# | TX: none -> RX: i32LE deg (rounded) +#define ROT_OP_GET_MAX_M 0x0C // :rM# | TX: none -> RX: i32LE deg (rounded) +#define ROT_OP_GET_DEG_PERSTEP_D 0x0D // :rD# | TX: none -> RX: f32LE deg_per_step + +#define ROT_OP_BACKLASH_b 0x0E // :rb# | TX: (none) -> RX: i16LE steps (numericReply=0) + // :rb[n]# | TX: i16LE steps -> RX: none (numericReply=1; CE_1 on success) + +#define ROT_OP_STOP_Q 0x02 // :rQ# | TX: none -> RX: none (numericReply=0) +#define ROT_OP_SET_RATE_1_9 0x0F // :r[1..9]# | TX: u8 rateDigit (1..9) -> RX: none (numericReply=0) + +#define ROT_OP_GET_WORKRATE_W 0x10 // :rW# | TX: none -> RX: f32LE deg_per_sec (settings.gotoRate) +#define ROT_OP_CONTINUOUS_c 0x11 // :rc# | TX: none -> RX: none (numericReply=0) // ignored/no-op + +#define ROT_OP_MOVE_CW_GT 0x03 // :r># | TX: none -> RX: none (numericReply=0) +#define ROT_OP_MOVE_CCW_LT 0x04 // :r<# | TX: none -> RX: none (numericReply=0) + +#define ROT_OP_GET_ANGLE_G 0x06 // :rG# | TX: none -> RX: f32LE deg (axis3.getInstrumentCoordinate()) +#define ROT_OP_GOTO_REL_rr 0x12 // :rr[deg]# | TX: f32LE deg_relative -> RX: none (numericReply=0) + +#define ROT_OP_GOTO_S 0x05 // :rS[deg]# | TX: f32LE deg_absolute -> RX: none (numericReply=1; CE_1 on success else error) + +#define ROT_OP_ZERO_Z 0x07 // :rZ# | TX: none -> RX: none (numericReply=0) +#define ROT_OP_HALFTRAVEL_F 0x13 // :rF# | TX: none -> RX: none (numericReply=0) +#define ROT_OP_HALFTARGET_C 0x14 // :rC# | TX: none -> RX: none (numericReply=0) + +#define ROT_OP_DEROT_EN_PLUS 0x15 // :r+# | TX: none -> RX: none (numericReply=0) +#define ROT_OP_DEROT_DIS_MINUS 0x16 // :r-# | TX: none -> RX: none (numericReply=0) + +#define ROT_OP_GOTO_PAR_P 0x17 // :rP# | TX: none -> RX: none (numericReply=0) +#define ROT_OP_DEROT_REV_R 0x18 // :rR# | TX: none -> RX: none (numericReply=0) + +#define ROT_OP_AVAIL_GX98 0x19 // :GX98# | TX: none -> RX: u8 ASCII 'D'/'R'/'N' (numericReply=0) + +#define ROT_OP_DRIVER_STATUS 0x20 // :GXU3# | TX: none -> RX: u8 packedStatus diff --git a/src/telescope/rotator/local/Rotator.axis.cpp b/src/telescope/rotator/local/Rotator.axis.cpp new file mode 100755 index 0000000..1552953 --- /dev/null +++ b/src/telescope/rotator/local/Rotator.axis.cpp @@ -0,0 +1,108 @@ +//-------------------------------------------------------------------------------------------------- +// local telescope rotator control, axis3 + +#include "Rotator.h" + +#ifdef ROTATOR_PRESENT + +namespace { + +#if defined(AXIS3_KTECH_PRESENT) + const KTechDriverSettings DriverSettingsAxis3 = {AXIS3_DRIVER_MODEL, AXIS3_DRIVER_STATUS}; + KTechMotor motor_3(3, AXIS3_REVERSE, &DriverSettingsAxis3); + +#elif defined(AXIS3_MKS42D_PRESENT) + const MksDriverSettings DriverSettingsAxis3 = {AXIS3_DRIVER_MODEL, AXIS3_DRIVER_STATUS}; + Mks42DMotor motor_3(3, AXIS3_REVERSE, &DriverSettingsAxis3, AXIS3_STEPS_PER_DEGREE); + +#elif defined(AXIS3_SERVO_PRESENT) + ServoControl servoControlAxis3; + + #if AXIS3_ENCODER == AB + Quadrature encAxis3(3, AXIS3_ENCODER_A_PIN, AXIS3_ENCODER_B_PIN); + #elif AXIS3_ENCODER == AB_ESP32 + QuadratureEsp32 encAxis3(3, AXIS3_ENCODER_A_PIN, AXIS3_ENCODER_B_PIN); + #elif AXIS3_ENCODER == CW_CCW + CwCcw encAxis3(3, AXIS3_ENCODER_A_PIN, AXIS3_ENCODER_B_PIN); + #elif AXIS3_ENCODER == PULSE_DIR + PulseDir encAxis3(3, AXIS3_ENCODER_A_PIN, AXIS3_ENCODER_B_PIN); + #elif AXIS3_ENCODER == PULSE_ONLY + PulseOnly encAxis3(3, AXIS3_ENCODER_A_PIN, &servoControlAxis3.directionHint); + #elif AXIS3_ENCODER == VIRTUAL + VirtualEnc encAxis3(3); + #elif AXIS3_ENCODER == SERIAL_BRIDGE + SerialBridge encAxis3(3); + #endif + + #if AXIS3_SERVO_FEEDBACK == PID + Pid feedbackAxis3(AXIS3_PID_P, AXIS3_PID_I, AXIS3_PID_D); + #elif AXIS3_SERVO_FEEDBACK == DUAL_PID + DualPid feedbackAxis3(AXIS3_PID_P, AXIS3_PID_I, AXIS3_PID_D, AXIS3_PID_P_GOTO, AXIS3_PID_I_GOTO, AXIS3_PID_D_GOTO, AXIS3_PID_SENSITIVITY); + #endif + + #if AXIS3_SERVO_FLTR == KALMAN + KalmanFilter filterAxis3(AXIS3_SERVO_FLTR_MEAS_U, AXIS3_SERVO_FLTR_VARIANCE); + #elif AXIS3_SERVO_FLTR == ROLLING + RollingFilter filterAxis3(AXIS3_SERVO_FLTR_WSIZE); + #elif AXIS3_SERVO_FLTR == OFF + Filter filterAxis3; + #endif + + const ServoPins DriverPinsAxis3 = {AXIS3_SERVO_PH1_PIN, AXIS3_SERVO_PH1_STATE, AXIS3_SERVO_PH2_PIN, AXIS3_SERVO_PH2_STATE, AXIS3_ENABLE_PIN, AXIS3_ENABLE_STATE, AXIS3_M0_PIN, AXIS3_M1_PIN, AXIS3_M2_PIN, AXIS3_M3_PIN, AXIS3_FAULT_PIN}; + const ServoSettings DriverSettingsAxis3 = {AXIS3_DRIVER_MODEL, AXIS3_DRIVER_STATUS, AXIS3_SERVO_ACCELERATION}; + + #if AXIS3_DRIVER_MODEL == SERVO_EE + ServoEE driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3); + #elif AXIS3_DRIVER_MODEL == SERVO_PE + ServoPE driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3); + #elif AXIS3_DRIVER_MODEL == SERVO_TMC2130_DC + ServoTmc2130DC driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3); + #elif AXIS3_DRIVER_MODEL == SERVO_TMC5160_DC + ServoTmc5160DC driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3); + #elif AXIS3_DRIVER_MODEL == SERVO_TMC2209 + ServoTmc2209 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_MOTOR_STEPS_PER_DEGREE/AXIS3_STEPS_PER_DEGREE, AXIS3_DRIVER_MICROSTEPS, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_DECAY, AXIS3_DRIVER_DECAY_GOTO); + #elif AXIS3_DRIVER_MODEL == SERVO_TMC5160 + ServoTmc5160 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_MOTOR_STEPS_PER_DEGREE/AXIS3_STEPS_PER_DEGREE, AXIS3_DRIVER_MICROSTEPS, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_DECAY, AXIS3_DRIVER_DECAY_GOTO); + #elif AXIS3_DRIVER_MODEL == SERVO_KTECH + ServoKTech driver3(3, &DriverSettingsAxis3, AXIS3_MOTOR_STEPS_PER_DEGREE/AXIS3_STEPS_PER_DEGREE); + #endif + + ServoMotor motor_3(3, AXIS3_REVERSE, ((ServoDriver*)&driver3), &filterAxis3, &encAxis3, AXIS3_ENCODER_ORIGIN, AXIS3_ENCODER_REVERSE == ON, &feedbackAxis3, &servoControlAxis3); + +#elif defined(AXIS3_STEP_DIR_PRESENT) + const StepDirDriverPins DriverPinsAxis3 = {AXIS3_M0_PIN, AXIS3_M1_PIN, AXIS3_M2_PIN, AXIS3_M2_ON_STATE, AXIS3_M3_PIN, AXIS3_DECAY_PIN, AXIS3_FAULT_PIN}; + const StepDirDriverSettings DriverSettingsAxis3 = {AXIS3_DRIVER_MODEL, AXIS3_DRIVER_STATUS, AXIS3_DRIVER_MICROSTEPS, AXIS3_DRIVER_MICROSTEPS_GOTO, AXIS3_DRIVER_DECAY, AXIS3_DRIVER_DECAY_GOTO}; + #if AXIS3_DRIVER_MODEL >= STEP_DIR_DRIVER_FIRST && AXIS3_DRIVER_MODEL < TMC_DRIVER_FIRST + StepDirGeneric driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3); + #elif AXIS3_DRIVER_MODEL == TMC2130 + StepDirTmc2130 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_DRIVER_IHOLD, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_IGOTO, AXIS3_DRIVER_INTPOL); + #elif AXIS3_DRIVER_MODEL == TMC2160 + StepDirTmc2160 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_DRIVER_IHOLD, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_IGOTO, AXIS3_DRIVER_INTPOL); + #elif AXIS3_DRIVER_MODEL == TMC2208 + StepDirTmc2208 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_DRIVER_IHOLD, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_IGOTO, AXIS3_DRIVER_INTPOL); + #elif AXIS3_DRIVER_MODEL == TMC2209 || AXIS3_DRIVER_MODEL == TMC2226 + StepDirTmc2209 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_DRIVER_IHOLD, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_IGOTO, AXIS3_DRIVER_INTPOL); + #elif AXIS3_DRIVER_MODEL == TMC2660 + StepDirTmc2660 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_DRIVER_IHOLD, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_IGOTO, AXIS3_DRIVER_INTPOL); + #elif AXIS3_DRIVER_MODEL == TMC5160 + StepDirTmc5160 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_DRIVER_IHOLD, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_IGOTO, AXIS3_DRIVER_INTPOL); + #elif AXIS3_DRIVER_MODEL == TMC5161 + StepDirTmc5161 driver3(3, &DriverPinsAxis3, &DriverSettingsAxis3, AXIS3_DRIVER_IHOLD, AXIS3_DRIVER_IRUN, AXIS3_DRIVER_IGOTO, AXIS3_DRIVER_INTPOL); + #endif + + const StepDirPins StepDirPinsAxis3 = {AXIS3_STEP_PIN, AXIS3_STEP_STATE, AXIS3_DIR_PIN, AXIS3_ENABLE_PIN, AXIS3_ENABLE_STATE}; + StepDirMotor motor_3(3, AXIS3_REVERSE, &StepDirPinsAxis3, ((StepDirDriver*)&driver3)); + +#else + #error "Configuration (Config.h): ROTATOR_PRESENT without AXIS3_DRIVER_MODEL should never happen!" +#endif + +const AxisPins PinsAxis3 = {AXIS3_SENSE_LIMIT_MIN_PIN, AXIS3_SENSE_HOME_PIN, AXIS3_SENSE_LIMIT_MAX_PIN, {AXIS3_SENSE_HOME, AXIS3_SENSE_HOME_INIT, AXIS3_SENSE_HOME_DIST_LIMIT, AXIS3_SENSE_LIMIT_MIN, AXIS3_SENSE_LIMIT_MAX, AXIS3_SENSE_LIMIT_INIT}}; +const AxisSettings SettingsAxis3 = {AXIS3_STEPS_PER_DEGREE, {AXIS3_LIMIT_MIN, AXIS3_LIMIT_MAX}, AXIS3_BACKLASH_RATE}; +} + +Motor& motor3 = motor_3; + +Axis axis3(3, &PinsAxis3, &SettingsAxis3, AXIS_MEASURE_DEGREES); + +#endif diff --git a/src/telescope/rotator/local/Rotator.can.command.cpp b/src/telescope/rotator/local/Rotator.can.command.cpp new file mode 100755 index 0000000..d1aa410 --- /dev/null +++ b/src/telescope/rotator/local/Rotator.can.command.cpp @@ -0,0 +1,264 @@ +//-------------------------------------------------------------------------------------------------- +// local telescope rotator control, using the CANbus interface + +#include "Rotator.h" + +#ifdef ROTATOR_CAN_SERVER_PRESENT + +#include "../../../lib/convert/Convert.h" +#include "../../../lib/axis/Axis.h" + +#include "../../mount/Mount.h" + +extern Axis axis3; + +// default command semantics (mirrors the common command contract): +// handled=true, numericReply=true, suppressFrame=false, commandError=CE_NONE +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +void Rotator::processCommand() { + if (!ready) return; + + bool handled = true; + bool numericReply = true; + bool suppressFrame = false; + CommandError commandError = CE_NONE; + + switch (opCode()) { + + // :hP# + case ROT_OP_PARK_HP: { + CommandError e = park(); + commandError = (e == CE_NONE) ? CE_1 : e; + } break; + + // :hR# + case ROT_OP_UNPARK_HR: { + CommandError e = unpark(); + commandError = (e == CE_NONE) ? CE_1 : e; + } break; + + // :rA# + case ROT_OP_ACTIVE_RA: { + commandError = CE_1; + } break; + + // :rT# + case ROT_OP_GET_STATUS_T: { + // "M" or "S" + optional "D" + optional "R" + rateDigit + writeU8(axis3.isSlewing() ? (uint8_t)'M' : (uint8_t)'S'); + if (!axis3.isSlewing()) { + if (derotatorEnabled) writeU8((uint8_t)'D'); + if (derotatorReverse) writeU8((uint8_t)'R'); + } + writeU8((uint8_t)('0' + getGotoRate())); + numericReply = false; + } break; + + // :rI# + case ROT_OP_GET_MIN_I: { + const int32_t deg = (int32_t)lround(axis3.getLimitMin()); + writeI32(deg); + numericReply = false; + } break; + + // :rM# + case ROT_OP_GET_MAX_M: { + const int32_t deg = (int32_t)lround(axis3.getLimitMax()); + writeI32(deg); + numericReply = false; + } break; + + // :rD# + case ROT_OP_GET_DEG_PERSTEP_D: { + const float degPerStep = (float)(1.0 / axis3.getStepsPerMeasure()); + writeF32(degPerStep); + numericReply = false; + } break; + + // :rb# or :rb[n]# + case ROT_OP_BACKLASH_b: { + // If args present, this is a set; otherwise it's a get. + if (remaining() == 0) { + const int16_t steps = (int16_t)lround(getBacklash()); + writeI16(steps); + numericReply = false; + } else { + int16_t steps = 0; + if (!readI16(steps)) { commandError = CE_PARAM_FORM; break; } + CommandError e = setBacklash((long)steps); + commandError = (e == CE_NONE) ? CE_1 : e; + } + } break; + + // :rQ# + case ROT_OP_STOP_Q: + if (axis3.isHoming()) { axis3.autoSlewAbort(); homing = false; } + else axis3.autoSlewStop(); + numericReply = false; + break; + + // :r[1..9]# + case ROT_OP_SET_RATE_1_9: { + uint8_t digit = 0; + if (!readU8(digit)) { commandError = CE_PARAM_FORM; break; } + if (digit < 1 || digit > 9) { commandError = CE_PARAM_RANGE; break; } + if (digit < 5) setMoveRate((int)digit); + else setGotoRate((int)digit - 4); + numericReply = false; + } break; + + // :rW# + case ROT_OP_GET_WORKRATE_W: { + const float dps = (float)settings.gotoRate; + writeF32(dps); + numericReply = false; + } break; + + // :rc# (ignored/no-op) + case ROT_OP_CONTINUOUS_c: + numericReply = false; + break; + + // :r># + case ROT_OP_MOVE_CW_GT: + commandError = move(DIR_FORWARD); + numericReply = false; + break; + + // :r<# + case ROT_OP_MOVE_CCW_LT: + commandError = move(DIR_REVERSE); + numericReply = false; + break; + + // :rG# + case ROT_OP_GET_ANGLE_G: { + const float deg = (float)axis3.getInstrumentCoordinate(); + writeF32(deg); + numericReply = false; + } break; + + // :rr[deg]# + case ROT_OP_GOTO_REL_rr: { + float rel = 0.0F; + if (!readF32(rel)) { commandError = CE_PARAM_FORM; break; } + const double t = axis3.getTargetCoordinate(); + commandError = gotoTarget((double)t + (double)rel); + numericReply = false; + } break; + + // :rS[deg]# + case ROT_OP_GOTO_S: { + float deg = 0.0F; + if (!readF32(deg)) { commandError = CE_PARAM_FORM; break; } + commandError = gotoTarget(deg); + // numericReply left as default (true) + } break; + + // :rZ# + case ROT_OP_ZERO_Z: + settings.parkState = PS_UNPARKED; + commandError = axis3.resetPosition(0.0); + axis3.setBacklashSteps(getBacklash()); + numericReply = false; + break; + + // :rF# + case ROT_OP_HALFTRAVEL_F: + settings.parkState = PS_UNPARKED; + commandError = axis3.resetPosition((axis3.getLimitMax() + axis3.getLimitMin())/2.0F); + axis3.setBacklashSteps(getBacklash()); + numericReply = false; + break; + + // :rC# + case ROT_OP_HALFTARGET_C: { + if (AXIS3_SENSE_HOME != OFF) { + if (settings.parkState == PS_UNPARKED) { + axis3.enable(true); + axis3.setFrequencySlew(settings.gotoRate); + commandError = axis3.autoSlewHome(); + if (commandError == CE_NONE) homing = true; + } else commandError = CE_PARKED; + } else { + commandError = gotoTarget((axis3.getLimitMax() + axis3.getLimitMin())/2.0F); + } + numericReply = false; + } break; + + // :r+# + case ROT_OP_DEROT_EN_PLUS: + #ifdef MOUNT_PRESENT + if (transform.mountType == ALTAZM) { + if (settings.parkState == PS_UNPARKED) derotatorEnabled = true; + else commandError = CE_PARKED; + } + #endif + numericReply = false; + break; + + // :r-# + case ROT_OP_DEROT_DIS_MINUS: + derotatorEnabled = false; + axis3.setSynchronizedFrequency(0.0F); + numericReply = false; + break; + + // :rP# + case ROT_OP_GOTO_PAR_P: + #ifdef MOUNT_PRESENT + { + Coordinate current = mount.getPosition(); + commandError = gotoTarget(parallacticAngle(¤t)); + } + #endif + numericReply = false; + break; + + // :rR# + case ROT_OP_DEROT_REV_R: + derotatorReverse = !derotatorReverse; + numericReply = false; + break; + + // :GX98# + case ROT_OP_AVAIL_GX98: { + uint8_t c = (uint8_t)'N'; + #if defined(MOUNT_PRESENT) + if (transform.mountType == ALTAZM) c = (uint8_t)'D'; + else c = (uint8_t)'R'; + #else + c = (uint8_t)'R'; + #endif + writeU8(c); + numericReply = false; + } break; + + // :GXU3# + case ROT_OP_DRIVER_STATUS: { + DriverStatus status = axis3.motor->getDriverStatus(); + if (status.active) { + uint8_t packedStatus = 0; + packedStatus |= (status.standstill ? 1: 0); + packedStatus |= (status.outputA.openLoad ? 1 : 0) << 1; + packedStatus |= (status.outputB.openLoad ? 1 : 0) << 2; + packedStatus |= (status.outputA.shortToGround ? 1 : 0) << 3; + packedStatus |= (status.outputB.shortToGround ? 1 : 0) << 4; + packedStatus |= (status.overTemperature ? 1 : 0) << 5; + packedStatus |= (status.overTemperatureWarning ? 1 : 0) << 6; + packedStatus |= (status.fault ? 1 : 0) << 7; + writeU8(packedStatus); + numericReply = false; + } else commandError = CE_0; + } break; + + default: + handled = false; + commandError = CE_CMD_UNKNOWN; + break; + } + + sendResponse(handled, suppressFrame, numericReply, commandError); +} + +#endif diff --git a/src/telescope/rotator/local/Rotator.command.cpp b/src/telescope/rotator/local/Rotator.command.cpp new file mode 100755 index 0000000..e5c0ce4 --- /dev/null +++ b/src/telescope/rotator/local/Rotator.command.cpp @@ -0,0 +1,270 @@ +//-------------------------------------------------------------------------------------------------- +// local telescope rotator control, commands + +#include "Rotator.h" + +#ifdef ROTATOR_PRESENT + +#include "../../../lib/convert/Convert.h" +#include "../../../lib/axis/Axis.h" +#include "../../mount/Mount.h" + +extern Axis axis3; + +// by default reply[80] == "", suppressFrame == false, numericReply == true, and commandError == CE_NONE +// return true if the command has been completely handled and no further command() will be called, or false if not +// for commands that are handled repeatedly commandError might contain CE_NONE or CE_1 to indicate success +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +bool Rotator::command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError) { + if (!ready) return false; + + // process any rotator axis commands + if (axis3.command(reply, command, parameter, suppressFrame, numericReply, commandError)) return true; + + if (command[0] == 'h') { + // :hP# Moves rotator to the park position + // Return: 0 on failure + // 1 on success + if (command[1] == 'P' && parameter[0] == 0) { + CommandError e = park(); + if (e == CE_NONE) *commandError = CE_1; else { VF("MSG: Rotator, park error "); VL(e); *commandError = e; } + return false; + } else + + // :hR# Restore parked rotator to operation + // Return: 0 on failure + // 1 on success + if (command[1] == 'R' && parameter[0] == 0) { + CommandError e = unpark(); + if (e == CE_NONE) *commandError = CE_1; else { VF("MSG: Rotator, unpark error "); VL(e); *commandError = e; } + return false; + } else return false; + } else + + // r - rotator Commands + if (command[0] == 'r') { + + // :rA# rotator Active? + // Return: 0 on failure (no rotator) + // 1 on success + if (command[1] == 'A' && parameter[0] == 0) { + // empty command for success response 1 + } else + + // :rT# Get rotator sTatus + // Returns: s# + if (command[1] == 'T') { + if (axis3.isSlewing()) strcat(reply, "M"); else { // [M]oving + strcat(reply, "S"); // [S]topped) + if (derotatorEnabled) strcat(reply, "D"); // [D]e-Rotate enabled + if (derotatorReverse) strcat(reply, "R"); // De-Rotate [R]everse + } + char temp[2] = "0"; temp[0] = '0' + getGotoRate(); strcat(reply, temp); // [1] to [5] for 0.5x to 2x goto rate + *numericReply = false; + } else + + // :rI# Get rotator mInimum position (in degrees) + // Returns: n# + if (command[1] == 'I') { + sprintf(reply, "%ld",(long)round(axis3.getLimitMin())); + *numericReply = false; + } else + + // :rM# Get rotator Max position (in degrees) + // Returns: n# + if (command[1] == 'M') { + sprintf(reply, "%ld",(long)round(axis3.getLimitMax())); + *numericReply = false; + } else + + // :rD# Get rotator degrees per step + // Returns: n.n# + if (command[1] == 'D') { + sprintF(reply, "%7.5f", 1.0/axis3.getStepsPerMeasure()); + *numericReply = false; + } else + + // :rb# Get rotator Backlash amount (in steps) + // Return: n# + if (command[1] == 'b' && parameter[0] == 0) { + sprintf(reply, "%ld",(long)round(getBacklash())); + *numericReply = false; + } else + + // :rb[n]# Set rotator Backlash amount (in steps) + // Return: 0 on failure + // 1 on success + if (command[1] == 'b') { + *commandError = setBacklash(atol(parameter)); + } else + + // :rQ# Stop (Quit) rotator movement + // Returns: Nothing + if (command[1] == 'Q') { + if (axis3.isHoming()) { + axis3.autoSlewAbort(); + homing = false; + } else axis3.autoSlewStop(); + *numericReply = false; + } else + + // :r[n]# Set rotator move or goto rate + // move where n = 1 for 0.01 deg/s, 2 for 0.1 deg/s, 3 for 1.0 deg/s, 4 for 0.5x goto rate + // goto rate where n = 5 for 0.5x, 6 for 0.66x, 7 for 1x, 8 for 1.5x, 9 for 2x AXISn_SLEW_RATE_BASE_DESIRED + // Returns: Nothing + if (command[1] >= '1' && command[1] <= '9') { + if (strlen(parameter) == 0) { + int v = command[1] - '0'; + if (v < 5) setMoveRate(v); else setGotoRate(v - 4); + } else *commandError = CE_PARAM_FORM; + *numericReply = false; + } else + + // :rW# Get working slew rate in deg/s + // Returns: d.d# + if (command[1] == 'W') { + sprintF(reply, "%0.1f", settings.gotoRate); + *numericReply = false; + } else + + // :rc# Set continious move + // Returns: Nothing + if (command[1] == 'c') { + // this command is ignored, all rotator movement in OnStepX is continious + *numericReply = false; + } else + + // :r># Move rotator CW + // Returns: Nothing + if (command[1] == '>') { + *commandError = move(DIR_FORWARD); + *numericReply = false; + } else + + // :r<# Move rotator CCW + // Returns: Nothing + if (command[1] == '<') { + *commandError = move(DIR_REVERSE); + *numericReply = false; + } else + + // :rG# Get rotator current angle + // Returns: sDDD*MM# + if (command[1] == 'G') { + convert.doubleToDms(reply, axis3.getInstrumentCoordinate(), true, true, PM_LOW); + *numericReply = false; + } else + + // :rr[sDDD*MM, etc.]# + // Set rotator target angle relative (in degrees) + // Returns: Nothing + if (command[1] == 'r') { + double r, t; + convert.dmsToDouble(&r, parameter, true); + t = axis3.getTargetCoordinate(); + *commandError = gotoTarget(t + r); + *numericReply = false; + } else + + // :rS[sDDD*MM, etc.]# + // Move rotator to Set target angle (in degrees) + // Return: 0 on failure + // 1 on success + if (command[1] == 'S') { + int i = 0; + double t, s = 1.0; + if (parameter[0] == '-') s = -1.0; + if (parameter[0] == '+' || parameter[0] == '-') i = 1; + if (convert.dmsToDouble(&t, ¶meter[i], false)) *commandError = gotoTarget(s*t); else *commandError = CE_PARAM_FORM; + } else + + // :rZ# Set rotator position to Zero degrees + // Returns: Nothing + if (command[1] == 'Z') { + settings.parkState = PS_UNPARKED; + *commandError = axis3.resetPosition(0.0); + axis3.setBacklashSteps(getBacklash()); + *numericReply = false; + } else + + // :rF# Set rotator position as Half-travel + // Returns: Nothing + if (command[1] == 'F') { + settings.parkState = PS_UNPARKED; + *commandError = axis3.resetPosition((axis3.getLimitMax() + axis3.getLimitMin())/2.0F); + axis3.setBacklashSteps(getBacklash()); + *numericReply = false; + } else + + // :rC# Move rotator to half-travel target position + // Returns: Nothing + if (command[1] == 'C') { + if (AXIS3_SENSE_HOME != OFF) { + if (settings.parkState == PS_UNPARKED) { + axis3.enable(true); + axis3.setFrequencySlew(settings.gotoRate); + *commandError = axis3.autoSlewHome(); + if (*commandError == CE_NONE) { + homing = true; + } + } else *commandError = CE_PARKED; + } else { + *commandError = gotoTarget((axis3.getLimitMax() + axis3.getLimitMin())/2.0F); + } + *numericReply = false; + } else + + // :r+# Derotator Enable + // Returns: Nothing + if (command[1] == '+') { + #ifdef MOUNT_PRESENT + if (transform.mountType == ALTAZM) { + if (settings.parkState == PS_UNPARKED) derotatorEnabled = true; else *commandError = CE_PARKED; + } + #endif + *numericReply = false; + } else + + // :r-# Derotator Disable + // Returns: Nothing + if (command[1] == '-') { + derotatorEnabled = false; + axis3.setSynchronizedFrequency(0.0F); + *numericReply = false; + } else + + // :rP# Rotator move to parallactic angle + // Returns: Nothing + if (command[1] == 'P') { + #ifdef MOUNT_PRESENT + Coordinate current = mount.getPosition(); + *commandError = gotoTarget(parallacticAngle(¤t)); + #endif + *numericReply = false; + } else + + // :rR# Derotator Reverse direction + // Returns: Nothing + if (command[1] == 'R') { + derotatorReverse = !derotatorReverse; + *numericReply = false; + } else *commandError = CE_CMD_UNKNOWN; + + } else + + // :GX98# Get rotator availablity + // Returns: D# for rotate/derotate + // R# for rotate only + // N# for none + if (command[0] == 'G' && command[1] == 'X' && parameter[0] == '9' && parameter[1] == '8' && parameter[2] == 0) { + *numericReply = false; + #if defined(MOUNT_PRESENT) + if (transform.mountType == ALTAZM) strcpy(reply, "D"); else + #endif + strcpy(reply, "R"); + } else return false; + + return true; +} + +#endif diff --git a/src/telescope/rotator/local/Rotator.cpp b/src/telescope/rotator/local/Rotator.cpp new file mode 100755 index 0000000..005071c --- /dev/null +++ b/src/telescope/rotator/local/Rotator.cpp @@ -0,0 +1,320 @@ +//-------------------------------------------------------------------------------------------------- +// local telescope rotator control + +#include "Rotator.h" + +#ifdef ROTATOR_PRESENT + +#include "../../../lib/tasks/OnTask.h" +#include "../../../lib/nv/Nv.h" + +#include "../../Telescope.h" +#include "../../mount/Mount.h" +#include "../../mount/site/Site.h" + +void rotWrapper() { rotator.monitor(); } + +#if defined(ROTATOR_CAN_SERVER_PRESENT) +// 1 Hz heartbeat TX for this remote rotator node +static void rotHeartbeatWrapper() { + if (!canPlus.ready) return; + const uint16_t hbId = (uint16_t)(CAN_ROTATOR_HB_ID_BASE); + const uint8_t b = 0; // payload reserved; currently unused + canPlus.writePacket((int)hbId, &b, 1); +} +#endif + +// initialize rotator +bool Rotator::init() { + if (ready) return true; + + // time to wait before writing position to nv after last movement of rotator + if (nv().device().endurance() == NvDevice::Endurance::Mid) writeDelay = 60; else + if (nv().device().endurance() == NvDevice::Endurance::High) writeDelay = 5; + + #if defined(ROTATOR_CAN_SERVER_PRESENT) + if (!CanTransportServer::init(true, 2)) return false; + #endif + + nvKey = nv().kv().computeKey("ROTATOR_SETTINGS"); + if (!nv().kv().getOrInit(nvKey, settings)) { DLF("WRN: Nv, init failed for ROTATOR_SETTINGS"); } + + settings.backlash = constrain(settings.backlash, 0, 10000); + settings.gotoRate = constrain(settings.gotoRate, 0.125, 20); + settings.parkState = constrain(settings.parkState, PS_PARK_FAILED, PS_UNPARKING); + settings.position = constrain(settings.position, AXIS3_LIMIT_MIN, AXIS3_LIMIT_MAX); + + VLF("MSG: Rotator, init (Axis3)"); + if (!axis3.init(&motor3)) { + initError.driver = true; + DLF("ERR: Rotator::init(), no motion controller for Axis3!"); + return false; + } + axis3.resetPositionSteps(0); + axis3.setBacklashSteps(settings.backlash); + axis3.setFrequencyMax(AXIS3_SLEW_RATE_BASE_DESIRED*2.0F); + axis3.setFrequencyMin(0.01F); + axis3.setFrequencySlew(AXIS3_SLEW_RATE_BASE_DESIRED); + axis3.setSlewAccelerationTime(AXIS3_ACCELERATION_TIME); + axis3.setSlewAccelerationTimeAbort(AXIS3_RAPID_STOP_TIME); + if (AXIS3_POWER_DOWN == ON) axis3.setPowerDownTime(AXIS3_POWER_DOWN_TIME); + + ready = true; + + return true; +} + +void Rotator::begin() { + if (!ready) return; + + axis3.calibrateDriver(); + + // start monitor task + VF("MSG: Rotator, start derotation task (rate 1s priority 6)... "); + if (tasks.add(1000, 0, true, 6, rotWrapper, "RotMon")) { VLF("success"); } else { VLF("FAILED!"); } + + // start heartbeat task + #if defined(ROTATOR_CAN_SERVER_PRESENT) + VF("MSG: Rotator, starting CAN heartbeat task (rate 1s priority 6)... "); + if (tasks.add(1000, 0, true, 6, rotHeartbeatWrapper, "RotHB")) { VLF("success"); } else { VLF("FAILED!"); } + #endif + + unpark(); +} + +// get backlash in steps +int Rotator::getBacklash() { + return settings.backlash; +} + +// set backlash in steps +CommandError Rotator::setBacklash(int value) { + if (!ready) return CE_CMD_UNKNOWN; + + if (value < 0 || value > 10000) return CE_PARAM_RANGE; + if (settings.parkState >= PS_PARKED) return CE_PARKED; + + settings.backlash = value; + nv().kv().put(nvKey, settings); + axis3.setBacklashSteps(settings.backlash); + + return CE_NONE; +} + +#ifdef MOUNT_PRESENT + // returns parallactic angle in degrees + double Rotator::parallacticAngle(Coordinate *coord) { + return radToDeg(atan2(sin(coord->h), cos(coord->d)*tan(site.location.latitude) - sin(coord->d)*cos(coord->h))); + } + + // returns parallactic rate in degrees per second + double Rotator::parallacticRate(Coordinate *coord) { + // one minute of hour angle in degrees = 15/60 = 0.25 + Coordinate ahead = *coord; + ahead.h += degToRad(0.125); + Coordinate behind = *coord; + behind.h -= degToRad(0.125); + double b = parallacticAngle(&behind); + double a = parallacticAngle(&ahead); + if (b > 90.0 && a < -90.0) a += 360.0; + if (b < -90.0 && a > 90.0) b += 360.0; + return (a - b)/60.0; + } +#endif + +// set move rate, 1 for 0.01 deg/sec slew, 2 for 0.1 deg/sec, 3 for 1 deg/sec, 4 for 0.5x goto rate +void Rotator::setMoveRate(int value) { + if (!ready) return; + + switch (value) { + case 1: moveRate = 0.01F; break; + case 2: moveRate = 0.1F; break; + case 3: moveRate = 1.0F; break; + case 4: moveRate = settings.gotoRate/2.0F; break; + default: moveRate = 0.1F; break; + } + if (moveRate > settings.gotoRate/2.0F) moveRate = settings.gotoRate/2.0F; +} + +// start slew in the specified direction +CommandError Rotator::move(Direction dir) { + if (!ready) return CE_CMD_UNKNOWN; + + if (settings.parkState >= PS_PARKED) return CE_PARKED; + + axis3.enable(true); + axis3.setSynchronizedFrequency(0.0F); + + return axis3.autoSlew(dir, moveRate); +} + +// get goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base +int Rotator::getGotoRate() { + if (settings.gotoRate < AXIS3_SLEW_RATE_BASE_DESIRED/1.75) return 1; + if (settings.gotoRate < AXIS3_SLEW_RATE_BASE_DESIRED/1.25) return 2; + if (settings.gotoRate < AXIS3_SLEW_RATE_BASE_DESIRED*1.25) return 3; + if (settings.gotoRate < AXIS3_SLEW_RATE_BASE_DESIRED*1.75) return 4; else return 5; +} + +// set goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base +void Rotator::setGotoRate(int value) { + switch (value) { + case 1: settings.gotoRate = AXIS3_SLEW_RATE_BASE_DESIRED/2.0; break; + case 2: settings.gotoRate = AXIS3_SLEW_RATE_BASE_DESIRED/1.5; break; + case 3: settings.gotoRate = AXIS3_SLEW_RATE_BASE_DESIRED*1.0; break; + case 4: settings.gotoRate = AXIS3_SLEW_RATE_BASE_DESIRED*1.5; break; + case 5: settings.gotoRate = AXIS3_SLEW_RATE_BASE_DESIRED*2.0; break; + default: settings.gotoRate = AXIS3_SLEW_RATE_BASE_DESIRED; break; + } + nv().kv().put(nvKey, settings); +} + +// move rotator to a specific location +CommandError Rotator::gotoTarget(float target) { + if (!ready) return CE_CMD_UNKNOWN; + + if (settings.parkState >= PS_PARKED) return CE_PARKED; + + VF("MSG: Rotator, goto target coordinate set ("); V(target); VL(" deg)"); + VLF("MSG: Rotator, attempting goto"); + + axis3.enable(true); + axis3.setSynchronizedFrequency(0.0F); + axis3.setTargetCoordinate(target); + + CommandError e = axis3.autoGoto(settings.gotoRate); + if (e != CE_NONE) { VLF("MSG: Rotator, goto failed"); } + + return e; +} + +// parks rotator at current position +CommandError Rotator::park() { + if (!ready) return CE_CMD_UNKNOWN; + + if (settings.parkState == PS_PARKED) return CE_NONE; + if (settings.parkState == PS_PARKING) return CE_PARK_FAILED; + if (settings.parkState == PS_UNPARKING) return CE_PARK_FAILED; + if (settings.parkState == PS_PARK_FAILED) return CE_PARK_FAILED; + + derotatorEnabled = false; + + VLF("MSG: Rotator, parking"); + + axis3.enable(true); + axis3.setBacklash(0.0F); + settings.position = axis3.getInstrumentCoordinate(); + axis3.setTargetCoordinatePark(settings.position); + + CommandError e = axis3.autoGoto(settings.gotoRate); + + if (e == CE_NONE) { + settings.parkState = PS_PARKING; + nv().kv().put(nvKey, settings); + } + + return e; +} + +// unparks rotator +CommandError Rotator::unpark() { + if (!ready) return CE_CMD_UNKNOWN; + + if (settings.parkState == PS_PARKING) return CE_PARK_FAILED; + if (settings.parkState == PS_UNPARKING) return CE_PARK_FAILED; + if (settings.parkState == PS_PARK_FAILED) return CE_PARK_FAILED; + + // setting write delay to 0 disables on-the-fly position writes and forces strict parking + if (writeDelay == 0) { + if (settings.parkState != PS_PARKED) return CE_NOT_PARKED; + } + + axis3.enable(true); + VF("MSG: Rotator, unpark position "); V(settings.position); VL(" deg"); + + // simple unpark if we didn't actually park + if (settings.parkState == PS_UNPARKED) { + axis3.setInstrumentCoordinate(settings.position); + nv().kv().put(nvKey, settings); + return CE_NONE; + } + + axis3.setBacklash(0.0F); + axis3.setInstrumentCoordinatePark(settings.position); + + axis3.setBacklash(settings.backlash); + axis3.setTargetCoordinate(settings.position); + + CommandError e = axis3.autoGoto(settings.gotoRate); + + if (e == CE_NONE) { + settings.parkState = PS_UNPARKING; + nv().kv().put(nvKey, settings); + } + + return e; +} + +// poll rotator to handle parking and derotation +void Rotator::monitor() { + secs++; + + if (axis3.isSlewing() || settings.position == axis3.getInstrumentCoordinate()) writeTime = secs + writeDelay; + + if (!axis3.isSlewing()) { + + if (settings.parkState == PS_PARKING) { + if (axis3.atTarget()) { + axis3.enable(false); + settings.parkState = PS_PARKED; + nv().kv().put(nvKey, settings); + #if DEBUG == VERBOSE + long offset = axis3.getInstrumentCoordinateSteps() - axis3.getMotorPositionSteps(); + VF("MSG: Rotator, park motor target "); VL(axis3.getTargetCoordinateSteps() - offset); + VF("MSG: Rotator, park motor position "); VL(axis3.getMotorPositionSteps()); + #endif + } + } else + + if (settings.parkState == PS_UNPARKING) { + if (axis3.atTarget()) { + settings.parkState = PS_UNPARKED; + nv().kv().put(nvKey, settings); + } + } else + + if (settings.parkState == PS_UNPARKED) { + #ifdef MOUNT_PRESENT + if (derotatorEnabled && transform.mountType == ALTAZM) { + float pr = 0.0F; + if (!axis3.getSynchronized()) axis3.setSynchronized(true); + Coordinate current = mount.getPosition(); + pr = parallacticRate(¤t); + if (derotatorReverse) pr = -pr; + axis3.setSynchronized(true); + axis3.setSynchronizedFrequency(pr); + } + #endif + + if (homing) { + axis3.resetPosition((axis3.getLimitMax() + axis3.getLimitMin())/2.0F); + axis3.setBacklashSteps(getBacklash()); + homing = false; + } + + // delayed write of rotator position + if (writeDelay != 0) { + if (secs > writeTime) { + settings.position = axis3.getInstrumentCoordinate(); + nv().kv().put(nvKey, settings); + VF("MSG: Rotator, writing position ("); V(settings.position); VL(" deg) to NV"); + } + } + } + + } +} + +Rotator rotator; + +#endif diff --git a/src/telescope/rotator/local/Rotator.h b/src/telescope/rotator/local/Rotator.h new file mode 100755 index 0000000..c89f5a0 --- /dev/null +++ b/src/telescope/rotator/local/Rotator.h @@ -0,0 +1,109 @@ +//-------------------------------------------------------------------------------------------------- +// local telescope rotator control +#pragma once + +#include "../../../Common.h" + +#include "../RotatorBase.h" + +#ifdef ROTATOR_PRESENT + +#include "../../../lib/nv/Nv.h" +#include "../../../lib/axis/Axis.h" +#include "../../../libApp/commands/ProcessCmds.h" +#include "../../Telescope.h" +#include "../../mount/coordinates/Transform.h" + +#pragma pack(1) +#define RotatorSettingsSize 11 +typedef struct RotatorSettings { + ParkState parkState; + int16_t backlash; // in steps + float position; // in degrees + float gotoRate; // in degrees/s +} RotatorSettings; +#pragma pack() + +#ifdef ROTATOR_CAN_SERVER_PRESENT +#include "../../../lib/canTransport/CanTransportServer.h" +class Rotator : public CanTransportServer { +private: + void processCommand() override; +public: + Rotator() : CanTransportServer((uint16_t)(CAN_ROTATOR_REQ_ID), (uint16_t)(CAN_ROTATOR_RSP_ID)) {} +#else +class Rotator { +public: +#endif + + // initialize rotator + bool init(); + void begin(); + + // process rotator commands + bool command(char *reply, char *command, char *parameter, bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // poll rotator to handle parking and derotation + void monitor(); + +private: + // get backlash in steps + int getBacklash(); + + // set backlash in steps + CommandError setBacklash(int value); + + #ifdef MOUNT_PRESENT + // returns parallactic angle in degrees + double parallacticAngle(Coordinate *coord); + + // returns parallactic rate in degrees per second + double parallacticRate(Coordinate *coord); + #endif + + // set move rate + void setMoveRate(int value); + + // start slew in the specified direction + CommandError move(Direction dir); + + // get goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base + int getGotoRate(); + + // set goto rate, 1 for 0.5x base, 2 for 0.66x base, 3 for base, 4 for 1.5x base, 5 for 2x base + void setGotoRate(int value); + + // move rotator to a specific location + CommandError gotoTarget(float target); + + // parks rotator at current position + CommandError park(); + + // unparks rotator + CommandError unpark(); + + float moveRate = 0.1; // in degs/sec + + RotatorSettings settings = {PS_UNPARKED, 0, 0.0F, AXIS3_SLEW_RATE_BASE_DESIRED}; + + bool ready = false; + + bool derotatorEnabled = false; + bool derotatorReverse = false; + bool homing = false; + + unsigned long writeTime = 0; + unsigned long secs = 0; + + uint8_t parkHandle = 0; + + uint16_t writeDelay = 600; // time in seconds + uint32_t nvKey; +}; + +extern Motor& motor3; +extern Axis axis3; + +extern Rotator rotator; + +#endif diff --git a/src/telescope/rotator/remote/Rotator.can.decode.cpp b/src/telescope/rotator/remote/Rotator.can.decode.cpp new file mode 100755 index 0000000..332aae8 --- /dev/null +++ b/src/telescope/rotator/remote/Rotator.can.decode.cpp @@ -0,0 +1,95 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope rotator control, using the CANbus interface (CAN client decode) + +#include "Rotator.h" + +#if defined(ROTATOR_CAN_CLIENT_PRESENT) + +#include "../../../lib/convert/Convert.h" + +bool Rotator::decodeResponse(char *reply) { + if (!reply) return false; + reply[0] = 0; + + switch (opCode()) { + + // :rT# -> RX: ASCII bytes "M"/"S"["D"]["R"] + rateDigit + // Accept variable length and copy as-is. + case ROT_OP_GET_STATUS_T: { + int16_t dataLen = readRemainingBytes((uint8_t*)reply); + reply[dataLen] = 0; + } break; + + // :rG# -> RX: f32 degrees; convert to DMS string for LX200 output + case ROT_OP_GET_ANGLE_G: { + float deg = 0.0f; + if (!readF32(deg)) return false; + convert.doubleToDms(reply, (double)deg, true, true, PM_LOW); + } break; + + // :rI# -> RX: i32 degrees (rounded) + case ROT_OP_GET_MIN_I: { + int32_t deg = 0; + if (!readI32(deg)) return false; + sprintf(reply, "%ld", (long)deg); + } break; + + // :rM# -> RX: i32 degrees (rounded) + case ROT_OP_GET_MAX_M: { + int32_t deg = 0; + if (!readI32(deg)) return false; + sprintf(reply, "%ld", (long)deg); + } break; + + // :rD# -> RX: f32 deg_per_step + case ROT_OP_GET_DEG_PERSTEP_D: { + float dps = 0.0f; + if (!readF32(dps)) return false; + sprintF(reply, "%7.5f", dps); + } break; + + // :rb# -> RX: i16 steps (read mode) OR no payload (write mode) + case ROT_OP_BACKLASH_b: { + int16_t steps = 0; + if (!readI16(steps)) return false; + sprintf(reply, "%ld", (long)steps); + } break; + + // :rW# -> RX: f32 deg_per_sec (settings.gotoRate) + case ROT_OP_GET_WORKRATE_W: { + float dps = 0.0f; + if (!readF32(dps)) return false; + sprintF(reply, "%0.1f", dps); + } break; + + // :GX98# -> RX: u8 ASCII 'D'/'R'/'N' + case ROT_OP_AVAIL_GX98: { + uint8_t c = 0; + if (!readU8(c)) return false; + reply[0] = (char)c; + reply[1] = 0; + } break; + + // :GXU3# -> RX u8 packed status + case ROT_OP_DRIVER_STATUS: { + uint8_t packedStatus; + readU8(packedStatus); + strcat(reply, ( packedStatus & 1) ? "ST," : ","); + strcat(reply, ((packedStatus << 1) & 1) ? "OA," : ","); + strcat(reply, ((packedStatus << 2) & 1) ? "OB," : ","); + strcat(reply, ((packedStatus << 3) & 1) ? "GA," : ","); + strcat(reply, ((packedStatus << 4) & 1) ? "GB," : ","); + strcat(reply, ((packedStatus << 5) & 1) ? "OT," : ","); // > 150C + strcat(reply, ((packedStatus << 6) & 1) ? "PW," : ","); // > 120C + strcat(reply, ((packedStatus << 7) & 1) ? "GF" : ""); + } break; + + default: + // Most ops are "no reply payload"; empty reply is fine + reply[0] = 0; + } + + return true; +} + +#endif diff --git a/src/telescope/rotator/remote/Rotator.can.encode.cpp b/src/telescope/rotator/remote/Rotator.can.encode.cpp new file mode 100755 index 0000000..f04a8e2 --- /dev/null +++ b/src/telescope/rotator/remote/Rotator.can.encode.cpp @@ -0,0 +1,187 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope rotator control, using the CANbus interface + +#include "Rotator.h" + +#if defined(ROTATOR_CAN_CLIENT_PRESENT) + +#include "../../../lib/convert/Convert.h" + +bool Rotator::encodeRequest(char *command, char *parameter) { + // Contract: both pointers are always provided by the command router. + if (!command || !parameter) return false; + + // -------------------------------------------------------------------------- + // h - park/unpark + // -------------------------------------------------------------------------- + if (command[0] == 'h') { + + // :hP# + if (command[1] == 'P' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_PARK_HP)) return false; + } else + + // :hR# + if (command[1] == 'R' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_UNPARK_HR)) return false; + } else + return false; + + } else + + // -------------------------------------------------------------------------- + // r - rotator commands (keep same order as Rotator.command.cpp) + // -------------------------------------------------------------------------- + if (command[0] == 'r') { + + // :rA# + if (command[1] == 'A' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_ACTIVE_RA)) return false; + } else + + // :rT# + if (command[1] == 'T' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_GET_STATUS_T)) return false; + } else + + // :rI# + if (command[1] == 'I' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_GET_MIN_I)) return false; + } else + + // :rM# + if (command[1] == 'M' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_GET_MAX_M)) return false; + } else + + // :rD# + if (command[1] == 'D' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_GET_DEG_PERSTEP_D)) return false; + } else + + // :rb# (get) + if (command[1] == 'b' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_BACKLASH_b)) return false; + } else + + // :rb[n]# (set) + if (command[1] == 'b' && parameter[0] != 0) { + if (!beginNewRequest(ROT_OP_BACKLASH_b)) return false; + if (!writeI16((int16_t)atol(parameter))) return false; + } else + + // :rQ# + if (command[1] == 'Q' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_STOP_Q)) return false; + } else + + // :r[1..9]# (no parameter allowed) + if (command[1] >= '1' && command[1] <= '9') { + if (parameter[0] != 0) return false; + if (!beginNewRequest(ROT_OP_SET_RATE_1_9)) return false; + if (!writeU8((uint8_t)(command[1] - '0'))) return false; + } else + + // :rW# + if (command[1] == 'W' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_GET_WORKRATE_W)) return false; + } else + + // :rc# + if (command[1] == 'c' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_CONTINUOUS_c)) return false; + } else + + // :r># + if (command[1] == '>' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_MOVE_CW_GT)) return false; + } else + + // :r<# + if (command[1] == '<' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_MOVE_CCW_LT)) return false; + } else + + // :rG# + if (command[1] == 'G' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_GET_ANGLE_G)) return false; + } else + + // :rr[sDDD*MM, etc.]# + if (command[1] == 'r' && parameter[0] != 0) { + double r = 0.0; + if (!convert.dmsToDouble(&r, parameter, true)) return false; + if (!beginNewRequest(ROT_OP_GOTO_REL_rr)) return false; + if (!writeF32((float)r)) return false; + } else + + // :rS[sDDD*MM, etc.]# + if (command[1] == 'S' && parameter[0] != 0) { + int i = 0; + double t = 0.0, s = 1.0; + if (parameter[0] == '-') s = -1.0; + if (parameter[0] == '+' || parameter[0] == '-') i = 1; + if (!convert.dmsToDouble(&t, ¶meter[i], false)) return false; + + if (!beginNewRequest(ROT_OP_GOTO_S)) return false; + if (!writeF32((float)(s * t))) return false; + } else + + // :rZ# + if (command[1] == 'Z' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_ZERO_Z)) return false; + } else + + // :rF# + if (command[1] == 'F' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_HALFTRAVEL_F)) return false; + } else + + // :rC# + if (command[1] == 'C' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_HALFTARGET_C)) return false; + } else + + // :r+# + if (command[1] == '+' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_DEROT_EN_PLUS)) return false; + } else + + // :r-# + if (command[1] == '-' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_DEROT_DIS_MINUS)) return false; + } else + + // :rP# + if (command[1] == 'P' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_GOTO_PAR_P)) return false; + } else + + // :rR# + if (command[1] == 'R' && parameter[0] == 0) { + if (!beginNewRequest(ROT_OP_DEROT_REV_R)) return false; + } else + return false; + + return true; + } else + + if (command[0] == 'G' && command[1] == 'X') { + // :GX98# Get rotator availability + if (parameter[0] == '9' && parameter[1] == '8' && parameter[2] == 0) { + if (!beginNewRequest(ROT_OP_AVAIL_GX98)) return false; + } else + + // :GXU3# Get rotator driver status + if (parameter[0] == 'U' && parameter[1] == '3' && parameter[2] == 0) { + if (!beginNewRequest(ROT_OP_DRIVER_STATUS)) return false; + } else + return false; + + return true; + } + + return false; +} + +#endif diff --git a/src/telescope/rotator/remote/Rotator.cpp b/src/telescope/rotator/remote/Rotator.cpp new file mode 100755 index 0000000..4b43923 --- /dev/null +++ b/src/telescope/rotator/remote/Rotator.cpp @@ -0,0 +1,66 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope rotator control, using the CANbus interface + +#include "Rotator.h" + +#ifdef ROTATOR_CAN_CLIENT_PRESENT + +// -------------------------------------------------------------------------- +// Heartbeat RX (local node): update last-heard timestamp for rotator +// -------------------------------------------------------------------------- +static void rotHb(uint8_t data[8], uint8_t len) { (void)data; (void)len; rotator.heartbeat(); } + +void Rotator::begin() { + canPlus.callbackRegisterId((int)(CAN_ROTATOR_HB_ID_BASE), rotHb); +} + +// by default reply[80] == "", suppressFrame == false, numericReply == true, and commandError == CE_NONE +// return true if the command has been completely handled and no further command() will be called, or false if not +// for commands that are handled repeatedly commandError might contain CE_NONE or CE_1 to indicate success +// numericReply=true means boolean/numeric-style responses (e.g., CE_1/CE_0/errors) rather than a payload +bool Rotator::command(char *reply, char *command, char *parameter, + bool *suppressFrame, bool *numericReply, CommandError *commandError) { + if (!canPlus.ready) return false; + + // r?, hP, hR, GX98 - rotator commands only + if (!(command[0] == 'r') && + !(command[0] == 'h' && command[1] == 'P') && + !(command[0] == 'h' && command[1] == 'R') && + !(command[0] == 'G' && command[1] == 'X' && (parameter[0] == '9' || parameter[0] == 'U'))) return false; + + // Presence gate + if (!heartbeatFresh()) return false; + + // Build CAN request + if (!encodeRequest(command, parameter)) return false; + + // Transport failure => reply unknown + bool handled = false; + if (!transactRequest(handled, *suppressFrame, *numericReply, *commandError)) { *commandError = CE_REPLY_UNKNOWN; return true; } + + // Remote explicitly did not recognize/handle the command + if (!handled) { + *commandError = CE_CMD_UNKNOWN; + *numericReply = true; + *suppressFrame = false; + reply[0] = 0; + return true; + } + + // Numeric reply: nothing to decode; caller will render numeric based on commandError + if (*numericReply) return true; + + // Payload reply: decode into reply buffer; decode failure => reply unknown + if (!decodeResponse(reply)) { + *commandError = CE_REPLY_UNKNOWN; + *numericReply = true; + *suppressFrame = false; + reply[0] = 0; + } + + return true; +} + +Rotator rotator; + +#endif diff --git a/src/telescope/rotator/remote/Rotator.h b/src/telescope/rotator/remote/Rotator.h new file mode 100755 index 0000000..b933b58 --- /dev/null +++ b/src/telescope/rotator/remote/Rotator.h @@ -0,0 +1,48 @@ +//-------------------------------------------------------------------------------------------------- +// remote telescope rotator control, using the CANbus interface +#pragma once + +#include "../../../Common.h" + +#include "../RotatorBase.h" + +#ifdef ROTATOR_CAN_CLIENT_PRESENT + +#include "../../../lib/canTransport/CanTransportClient.h" +#include "../../../libApp/commands/ProcessCmds.h" + +class Rotator : public CanTransportClient { + public: + Rotator() : CanTransportClient((uint16_t)(CAN_ROTATOR_REQ_ID), (uint16_t)(CAN_ROTATOR_RSP_ID)) {} + + void begin(); + + bool command(char *reply, char *command, char *parameter, + bool *suppressFrame, bool *numericReply, CommandError *commandError); + + // Heartbeat event + inline void heartbeat() { lastHeartbeatMs = hb_stamp_2ms_odd(); } + + private: + bool encodeRequest(char *command, char *parameter); + bool decodeResponse(char *reply); + + static inline uint32_t hb_stamp_2ms_odd() { + return millis() | 1UL; + } + + // Heartbeat presence + inline bool heartbeatFresh() const { + const uint32_t last = ATOMIC_LOAD(lastHeartbeatMs); + if (last == 0U) return false; + const uint32_t now = hb_stamp_2ms_odd(); + return (uint32_t)(now - last) <= 2000U; + } + + // Heartbeat timestamps (ms) + volatile uint32_t lastHeartbeatMs = 0; +}; + +extern Rotator rotator; + +#endif