diff --git a/.github/scripts/check-pg-versions.sh b/.github/scripts/check-pg-versions.sh index e07f7538fda..33ba4054088 100755 --- a/.github/scripts/check-pg-versions.sh +++ b/.github/scripts/check-pg-versions.sh @@ -187,7 +187,9 @@ while IFS= read -r file; do fi # Determine companion file (.c <-> .h) - local companion="" + # NOTE: this loop runs in the main script body, not inside a function, so + # `local` is invalid here and aborts the script under `set -e`. + companion="" if [[ "$file" == *.c ]]; then companion="${file%.c}.h" elif [[ "$file" == *.h ]]; then diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 00000000000..59832a74691 --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,112 @@ +name: Build tagged release + +# Manually-triggered release build: builds all targets via ci.yml and +# publishes a (pre)release on this repository with the firmware hexes +# and SITL resources attached. +on: + push: + tags: + - 'v*-srxl2-*' + branches: + - 'release/srxl2-*' + workflow_dispatch: + inputs: + tag: + description: 'Release tag to create (e.g. v9.1.0-srxl2-beta1)' + required: true + default: 'v9.1.0-srxl2-beta1' + prerelease: + description: 'Mark as pre-release' + required: false + default: true + type: boolean + +permissions: + contents: write + +jobs: + build: + name: build + uses: ./.github/workflows/ci.yml + + release: + name: Release + needs: [build] + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Get version + id: version + run: | + VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]/, "", $2); print $2 }') + echo "version=${VERSION}" >> $GITHUB_OUTPUT + - name: Compute tag + id: tagname + run: | + if [ "${GITHUB_REF_TYPE}" = "tag" ]; then + echo "tag=${GITHUB_REF_NAME}" >> $GITHUB_OUTPUT + else + echo "tag=v${{ steps.version.outputs.version }}-$(basename "${GITHUB_REF_NAME}")" >> $GITHUB_OUTPUT + fi + - name: download artifacts + uses: actions/download-artifact@v4 + with: + path: hexes + pattern: matrix-inav-* + merge-multiple: true + - name: download sitl linux + uses: actions/download-artifact@v4 + with: + path: resources/sitl/linux + pattern: inav-*SITL-Linux + merge-multiple: true + - name: download sitl linux aarch64 + uses: actions/download-artifact@v4 + with: + path: resources/sitl/linux/arm64 + pattern: inav-*SITL-Linux-aarch64 + merge-multiple: true + - name: download sitl windows + uses: actions/download-artifact@v4 + with: + path: resources/sitl/windows + pattern: inav-*SITL-WIN + merge-multiple: true + - name: download sitl mac + uses: actions/download-artifact@v4 + with: + path: resources/sitl/macos + pattern: inav-*SITL-MacOS + merge-multiple: true + - name: Consolidate sitl files + run: | + zip -r -9 sitl-resources.zip resources/ + - name: Upload release artifacts + uses: softprops/action-gh-release@v2 + with: + name: inav-${{ steps.version.outputs.version }} ${{ github.event.inputs.tag || steps.tagname.outputs.tag }} + tag_name: ${{ github.event.inputs.tag || steps.tagname.outputs.tag }} + prerelease: true + draft: false + make_latest: false + files: | + hexes/*.hex + sitl-resources.zip + body: | + ### Spektrum SRXL2 ESC beta + + Firmware build with Spektrum SRXL2 Smart ESC support: direct + single-wire control of a Smart ESC from a UART TX pad, with ESC + telemetry. See `docs/SRXL2_ESC.md` for wiring, configuration and + debugging. + + Flash with **Full chip erase**. + + ### Repository: + ${{ github.repository }} + + ### Branch: + ${{ github.ref_name }} + + ### Commit: + ${{ github.sha }} diff --git a/PR.md b/PR.md new file mode 100644 index 00000000000..3f782efa00d --- /dev/null +++ b/PR.md @@ -0,0 +1,59 @@ +# Add Spektrum SRXL2 Smart ESC support + +This adds support for driving a Spektrum Smart ESC directly from a UART using +the SRXL2 protocol, instead of PWM. The ESC signal wire goes to the TX pad of a +free hardware UART and the same wire carries telemetry back (voltage, current, +RPM, FET temperature), so you get Smart ESC telemetry without needing a +Spektrum receiver in between. + +The flight controller takes the bus master role, following the state machine of +the official SpektrumRC/SRXL2 reference code: addressed handshake polls, a +reply to the ESC's unprompted handshake at power-up, the final broadcast +handshake, then channel data at 100Hz with a telemetry poll every 10th frame. +Channel values use the normal Spektrum ±100% range (0x2AA0..0xD554) — sending +raw 0 as throttle makes the ESC reject it as invalid and keep beeping, that one +took a while to figure out on the bench. + +What's included: + +- `SRXL2` motor protocol (`motor_pwm_protocol = SRXL2`), no hardware timer used +- "Spektrum SRXL2 ESC" serial port function for the Ports tab +- ESC telemetry wired into the esc_sensor backend, so `vbat_meter_type = ESC`, + `current_meter_type = ESC`, OSD ESC RPM/temperature etc. all work as usual. + RPM is converted from electrical RPM using `motor_poles`. +- A MOTOR REVERSE mode for reversible ESCs. It drives the SRXL2 reverse + channel; when the direction change is actually applied is up to the ESC + firmware (it only reverses through zero throttle), so the switch is safe to + flip in advance, e.g. for reverse-thrust landings. +- `debug_mode = SRXL2` with counters (frames, CRC errors, handshake state) + that make wiring problems easy to spot from the Sensors tab +- `docs/SRXL2_ESC.md` with wiring, configuration and troubleshooting notes + +Bench tested on a MATEKF405SE with an Avian Lite 85A (SPMXAE85A) and a 14 pole +motor, fixed wing configuration with a single motor: handshake, throttle, +voltage/current/RPM telemetry and the reverse switch all working. Not flight +tested yet. + +Current limitations: + +- single ESC on motor 1 only (see below) +- needs a hardware UART (no SoftSerial), and targets with >512KB flash + +About the single motor limitation: it's not as arbitrary as it looks. Two ESCs +can't share the wire — Spektrum ESCs all ship with the same fixed device ID +(0x40) and there's no way to change it, so on a shared bus they'd both answer +handshakes and telemetry polls at the same time. And even if the IDs were +different, SRXL2 carries one set of channel data for the whole bus, so every +ESC on it would read the same throttle channel — no differential thrust, which +kills the main reason to have per-motor control in the first place. The real +path to twins is one UART per ESC, each with its own bus and its own throttle +from the mixer. That needs the driver state to become per-instance instead of +the current single global one; it's a reasonable follow-up, but I'd rather land +the single-motor case first since that's what I can actually test on my bench. + +A note for review: I picked serial function bit 28 and permanentId 70 for the +new mode box, which were the next free ones at the time of writing. Happy to +renumber if they're already spoken for by another PR. + +Companion configurator PR adds the SRXL2 protocol entry, the port function and +the mode name. diff --git a/docs/SRXL2_ESC.md b/docs/SRXL2_ESC.md new file mode 100644 index 00000000000..0e21ce67d97 --- /dev/null +++ b/docs/SRXL2_ESC.md @@ -0,0 +1,127 @@ +# Spektrum SRXL2 ESC support + +INAV can drive a Spektrum Smart ESC (Avian/Firma "Smart" line) directly over its +SRXL2 throttle wire, replacing the PWM signal with a digital half-duplex link +that also carries ESC telemetry (RPM, voltage, current, FET temperature) back +to the flight controller. The flight controller acts as the SRXL2 bus master +(device ID `0x21`, the role a Spektrum receiver normally plays) and the ESC is +the polled device (ID `0x40`). + +## Wiring + +- Connect the ESC signal wire to the **TX pad** of a free **hardware UART** + (not SoftSerial). SRXL2 is a single-wire half-duplex bus: the UART runs in + bidirectional mode on the TX pin and the RX pad of that UART stays unused. +- Common ground between FC and ESC as usual. +- On F4 targets the pin runs open-drain with the internal pull-up + (the same proven configuration Betaflight uses for SRXL2), so the line + idles high with no external components. +- Mind target pin sharing: e.g. on MATEKF405SE the TX2 pad (PA2) doubles as + SoftSerial1 — do not enable SoftSerial on the pad used for the ESC. + +## Configuration + +Ports tab: set the chosen UART's peripheral function to **Spektrum SRXL2 ESC** +(115200 baud). Outputs/Mixer: set the ESC protocol to **SRXL2**. + +CLI equivalent: + +``` +serial 268435456 115200 115200 0 115200 # function bit 28 = SRXL2_ESC +set motor_pwm_protocol = SRXL2 +save +``` + +Telemetry consumers work as with other ESC telemetry sources, e.g. +`set vbat_meter_type = ESC` to read pack voltage from the ESC. + +Current limitations: + +- Single ESC (motor index 0). Multi-ESC SRXL2 is not supported. +- RPM is converted from the ESC's electrical RPM using `motor_poles` + (default 14) — set it to match the attached motor. +- Available on targets with more than 512KB flash (`USE_SRXL2_ESC`). + +### Reversible ESCs + +When the motor protocol is SRXL2 a **MOTOR REVERSE** mode appears in the +Modes tab. Assign it to a switch to drive the ESC's reverse channel +(channel 6): switch active = reverse requested. The ESC's own firmware +decides when the direction change is applied (typically only through zero +throttle), so the switch can be armed safely in advance — e.g. for +reverse-thrust landings. INAV's throttle handling is unchanged; this is a +plain pass-through of the direction request. + +## Protocol details + +The driver (`src/main/drivers/srxl2_esc.c`) implements the master side of the +official Spektrum SRXL2 state machine (reference: SpektrumRC/SRXL2 +`spm_srxl.c`): + +- **Handshake**: an addressed handshake (`src 0x21 → dst 0x40`) is sent every + 50ms. The ESC's own unprompted handshake (`dst 0x00`, sent after its 50ms + power-up listen window) is answered immediately. When the ESC replies with a + handshake addressed back to us, the driver sends the **final broadcast + handshake (`dst 0xFF`)** — required by the official device state machine to + enter its Running state — and switches to normal operation. +- **Baud**: the handshake advertises `baudSupported = 0` (115200 only). The + driver never switches baud, so it must never advertise 400k capability: + the ESC would switch on the broadcast handshake and drop off the bus. +- **Control frames** (type `0xCD`) are sent at 100Hz with channel mask `0x41`: + channel 0 = throttle, channel 6 = reverse. Every 10th frame polls the ESC + for telemetry (`replyID = 0x40`); the ESC answers with a telemetry frame + (type `0x80`) carrying the standard XBUS ESC telemetry block. +- **Channel values** use the Spektrum convention: the full 16-bit range spans + ±150% servo travel with 32768 = center. The driver maps + `mincommand..max_throttle` onto the standard ±100% band + `0x2AA0..0xD554`. Raw 0 must never be sent as throttle — it decodes as + −150% (a sub-900µs pulse equivalent) and Smart ESCs reject it as an + invalid throttle, keeping their no-signal beep even with the bus link up. +- **Half-duplex turnaround**: the driver never starts transmitting within + 300µs of the last received byte, so ESC replies are not stomped. All frames + use CRC16-CCITT (seed 0) transmitted big-endian. + +## Debugging + +Set `debug_mode = SRXL2` and watch the debug values (Configurator Sensors tab +or blackbox): + +| debug | Meaning | +|-------|---------| +| 0 | Link state: 0 = handshaking, 1 = running | +| 1 | Handshake frames transmitted | +| 2 | Bytes received (includes our own half-duplex echo) | +| 3 | Valid SRXL2 frames received | +| 4 | CRC failures (non-zero = electrical/wiring problem) | +| 5 | Handshake replies received from the ESC | +| 6 | Telemetry frames received from the ESC | +| 7 | Current throttle channel value (~10912 at idle) | + +Interpretation guide: debug 1 growing with debug 2 stuck at 0 means nothing is +coming back (wiring); debug 4 growing means a noisy line; debug 5 > 0 with +debug 0 = 0 means a handshake logic problem; debug 6 growing proves the ESC is +in its running state and answering polls. + +## Development notes + +Issues found and fixed while bringing the driver up on real hardware +(MATEKF405SE + Spektrum Smart ESC), kept here so future work doesn't +rediscover them: + +1. **CRC byte order**: the CRC is big-endian on the wire; the RX validator + must compare without byte-swapping the computed value. +2. **Current units**: the XBUS telemetry block reports current in 10mA units, + which is already the centiampere unit `escSensorData_t` expects — no + scaling. +3. **Handshake state machine**: the ESC's unprompted handshake (`dst 0`) is a + request, not a confirmation — it must be answered, and only the addressed + reply plus our final `0xFF` broadcast completes the sequence. +4. **Line drive**: plain `SERIAL_BIDIR` (open-drain + pull-up). `SERIAL_BIDIR_PP` + must not be used: the push-pull pin has no pull-up, so the line floats + whenever the half-duplex UART releases the driver between frames. +5. **Throttle encoding**: see Channel values above — the ±100% band, never + raw 0. The symptom of a wrong band is subtle: handshake and telemetry work + but the ESC keeps its no-throttle beep. +6. **Build system**: `PWM_TYPE_SRXL2` uses no hardware timer + (`usesHwTimer = false`) and `pwmCompleteMotorUpdate()` must exist on + non-DSHOT targets too (`USE_DSHOT || USE_SRXL2_ESC` guard). diff --git a/docs/Settings.md b/docs/Settings.md index 243314fc6d8..eb103b0824e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -794,6 +794,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | LULU | | | SBUS2 | | | OSD_REFRESH | | +| SRXL2 | | --- @@ -2905,9 +2906,9 @@ Autopilot type to advertise for MAVLink telemetry --- -### mavlink_ext_status_rate +### mavlink_port1_ext_status_rate -Rate of the extended status message for MAVLink telemetry +Rate of the extended status message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2915,9 +2916,9 @@ Rate of the extended status message for MAVLink telemetry --- -### mavlink_extra1_rate +### mavlink_port1_extra1_rate -Rate of the extra1 message for MAVLink telemetry +Rate of the extra1 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2925,9 +2926,9 @@ Rate of the extra1 message for MAVLink telemetry --- -### mavlink_extra2_rate +### mavlink_port1_extra2_rate -Rate of the extra2 message for MAVLink telemetry +Rate of the extra2 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2935,9 +2936,9 @@ Rate of the extra2 message for MAVLink telemetry --- -### mavlink_extra3_rate +### mavlink_port1_extra3_rate -Rate of the extra3 message for MAVLink telemetry +Rate of the extra3 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2945,9 +2946,19 @@ Rate of the extra3 message for MAVLink telemetry --- -### mavlink_min_txbuffer +### mavlink_port1_high_latency -Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. +Enable MAVLink high-latency mode on port 1 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port1_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages. | Default | Min | Max | | --- | --- | --- | @@ -2955,9 +2966,9 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry --- -### mavlink_pos_rate +### mavlink_port1_pos_rate -Rate of the position message for MAVLink telemetry +Rate of the position message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2965,21 +2976,22 @@ Rate of the position message for MAVLink telemetry --- -### mavlink_radio_type +### mavlink_port1_radio_type -Mavlink radio type. Affects how RSSI and LQ are reported on OSD. +MAVLink radio type for port 1. Affects RSSI and LQ reporting on OSD. | Allowed Values | | | --- | --- | | GENERIC | Default | | ELRS | | | SIK | | +| MLRS | | --- -### mavlink_rc_chan_rate +### mavlink_port1_rc_chan_rate -Rate of the RC channels message for MAVLink telemetry +Rate of the RC channels message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2987,6 +2999,105 @@ Rate of the RC channels message for MAVLink telemetry --- +### mavlink_port2_high_latency + +Enable MAVLink high-latency mode on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port2_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port2_radio_type + +MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + +### mavlink_port3_high_latency + +Enable MAVLink high-latency mode on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port3_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port3_radio_type + +MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + +### mavlink_port4_high_latency + +Enable MAVLink high-latency mode on port 4 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port4_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port4_radio_type + +MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + ### mavlink_sysid MAVLink System ID @@ -3323,6 +3434,7 @@ Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, | DSHOT150 | | | DSHOT300 | | | DSHOT600 | | +| SRXL2 | | --- diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f4ab6479a5f..359e24bce94 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -267,6 +267,8 @@ main_sources(COMMON_SRC drivers/rcc.h drivers/serial.c drivers/serial.h + drivers/srxl2_esc.c + drivers/srxl2_esc.h drivers/sound_beeper.c drivers/sound_beeper.h drivers/stack_check.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b33868af8b2..82708d535da 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,6 +80,7 @@ typedef enum { DEBUG_LULU, DEBUG_SBUS2, DEBUG_OSD_REFRESH, + DEBUG_SRXL2, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 44a33220c20..7813338dcfb 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -70,6 +70,7 @@ static const motorProtocolProperties_t motorProtocolProperties[] = { [PWM_TYPE_DSHOT150] = { .usesHwTimer = true, .isDSHOT = true }, [PWM_TYPE_DSHOT300] = { .usesHwTimer = true, .isDSHOT = true }, [PWM_TYPE_DSHOT600] = { .usesHwTimer = true, .isDSHOT = true }, + [PWM_TYPE_SRXL2] = { .usesHwTimer = false, .isDSHOT = false }, }; pwmInitError_e getPwmInitError(void) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index afe9301f7e7..a87774cfffd 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -47,6 +47,7 @@ typedef enum { PWM_TYPE_DSHOT150, PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT600, + PWM_TYPE_SRXL2, } motorPwmProtocolTypes_e; typedef enum { diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index a0e983fcf19..0cae6c79ced 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -34,6 +34,7 @@ #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" +#include "drivers/srxl2_esc.h" #include "io/servo_sbus.h" #include "sensors/esc_sensor.h" @@ -211,6 +212,11 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) } } +static void pwmWriteSrxl2(uint8_t index, uint16_t value) +{ + srxl2EscWriteMotor(index, value); +} + void pwmWriteMotor(uint8_t index, uint16_t value) { if (motorWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) { @@ -518,7 +524,27 @@ static bool executeDShotCommands(void){ } #endif +#else // digital motor protocol + +// This stub is needed to avoid ESC_SENSOR dependency on DSHOT +void pwmRequestMotorTelemetry(int motorIndex) +{ + UNUSED(motorIndex); +} + +#endif + +// pwmCompleteMotorUpdate must also exist for SRXL2-only targets (no DSHOT), +// where it just drives the SRXL2 transmit. The digital/DSHOT body below is +// kept behind USE_DSHOT so it is omitted on those targets. +#if defined(USE_DSHOT) || defined(USE_SRXL2_ESC) void pwmCompleteMotorUpdate(void) { + if (initMotorProtocol == PWM_TYPE_SRXL2) { + srxl2EscUpdate(micros()); + return; + } + +#ifdef USE_DSHOT // This only makes sense for digital motor protocols if (!isMotorProtocolDigital()) { return; @@ -574,16 +600,8 @@ void pwmCompleteMotorUpdate(void) { #endif } #endif +#endif } - -#else // digital motor protocol - -// This stub is needed to avoid ESC_SENSOR dependency on DSHOT -void pwmRequestMotorTelemetry(int motorIndex) -{ - UNUSED(motorIndex); -} - #endif void pwmMotorPreconfigure(void) @@ -608,6 +626,18 @@ void pwmMotorPreconfigure(void) motorWritePtr = pwmWriteStandard; break; + case PWM_TYPE_SRXL2: + // SRXL2 paces its own transmissions inside srxl2EscUpdate() and + // pwmCompleteMotorUpdate() returns early for it, so the digital update + // interval is unused here. Avoid calling motorConfigDigitalUpdateInterval() + // which is only defined when USE_DSHOT is enabled. + if (srxl2EscInit()) { + motorWritePtr = pwmWriteSrxl2; + } else { + motorWritePtr = pwmWriteNull; + } + break; + #ifdef USE_DSHOT case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: @@ -642,6 +672,9 @@ uint32_t getEscUpdateFrequency(void) { case PWM_TYPE_DSHOT600: return 16000; + case PWM_TYPE_SRXL2: + return SRXL2_ESC_UPDATE_HZ; + case PWM_TYPE_ONESHOT125: default: return 1000; @@ -676,6 +709,10 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bo motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, getEscUpdateFrequency(), enableOutput); break; + case PWM_TYPE_SRXL2: + motors[motorIndex].pwmPort = NULL; + break; + default: motors[motorIndex].pwmPort = NULL; break; diff --git a/src/main/drivers/srxl2_esc.c b/src/main/drivers/srxl2_esc.c new file mode 100644 index 00000000000..b2ab9f01b8e --- /dev/null +++ b/src/main/drivers/srxl2_esc.c @@ -0,0 +1,461 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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. + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "common/crc.h" +#include "common/maths.h" + +#include "config/feature.h" + +#include "drivers/serial.h" +#include "drivers/time.h" +#include "drivers/srxl2_esc.h" + +#include "fc/config.h" + +#include "flight/mixer.h" + +#include "io/serial.h" + +#if defined(USE_SRXL2_ESC) + +#define SRXL2_ESC_BAUDRATE 115200 +#define SRXL2_ESC_FRAME_TIMEOUT_US 500 +#define SRXL2_ESC_INTERVAL_US 10000 +#define SRXL2_ESC_HANDSHAKE_INTERVAL_US 50000 +// Minimum bus-idle time after the last received byte before we may transmit, +// so we never stomp on an ESC reply in progress (~2 byte times at 115200, +// same value Betaflight uses for its SRXL2 reply quiescence). +#define SRXL2_ESC_QUIESCENCE_US 300 +#define SRXL2_ESC_REQUEST_EVERY_N_FRAMES 10 + +#define SRXL2_HEADER 0xA6 +#define SRXL2_PACKET_TYPE_HANDSHAKE 0x21 +#define SRXL2_PACKET_TYPE_CONTROL 0xCD +#define SRXL2_PACKET_TYPE_TELEMETRY 0x80 +#define SRXL2_CONTROL_CMD_CHANNEL 0x00 + +#define SRXL2_RECEIVER_ID 0x21 +#define SRXL2_ESC_ID 0x40 +#define SRXL2_BROADCAST_ID 0xFF +#define SRXL2_RECEIVER_PRIORITY 0x0A +// Per the official SRXL2 spec, baudSupported 0 = 115200 only, 1 = 400k +// capable. We never switch the UART off 115200, so advertising 400k would +// make the ESC change baud on the final broadcast handshake and lose us. +#define SRXL2_RECEIVER_BAUDRATE 0 +#define SRXL2_RECEIVER_INFO 0x07 +#define SRXL2_RECEIVER_UID 0x27A2C29C + +#define XBUS_ESC_ID 0x20 + +typedef struct srxl2Handshake_s { + uint8_t header; + uint8_t type; + uint8_t len; + uint8_t sourceId; + uint8_t destId; + uint8_t priority; + uint8_t baudrate; + uint8_t info; + uint32_t uid; + uint16_t crc; +} __attribute__((packed)) srxl2Handshake_t; + +typedef struct srxl2ChannelData_s { + int8_t rssi; + uint16_t frameLosses; + uint32_t channelMask; + uint16_t throttle; + uint16_t reverse; +} __attribute__((packed)) srxl2ChannelData_t; + +typedef struct srxl2ControlPacket_s { + uint8_t header; + uint8_t type; + uint8_t len; + uint8_t command; + uint8_t replyId; + srxl2ChannelData_t channelData; + uint16_t crc; +} __attribute__((packed)) srxl2ControlPacket_t; + +typedef struct srxl2TelemetryPacket_s { + uint8_t header; + uint8_t type; + uint8_t len; + uint8_t destId; + uint8_t payload[16]; + uint16_t crc; +} __attribute__((packed)) srxl2TelemetryPacket_t; + +typedef struct xbusEsc_s { + uint8_t identifier; + uint8_t sid; + uint16_t rpm; + uint16_t voltsInput; + uint16_t tempFet; + uint16_t currentMotor; + uint16_t tempBec; + uint8_t currentBec; + uint8_t voltageBec; + uint8_t throttle; + uint8_t powerOut; +} __attribute__((packed)) xbusEsc_t; + +static serialPort_t *srxl2EscPort; +static bool srxl2ReverseActive; +static uint16_t debugTxFrames; +static uint16_t debugRxValidFrames; +static uint16_t debugRxCrcFails; +static uint16_t debugRxBytes; +static uint16_t debugHandshakeReplies; +static uint16_t debugTelemetryFrames; +static timeUs_t lastRxActivityUs; +static bool srxl2EscInitialized; +static uint16_t srxl2MotorValues[MAX_SUPPORTED_MOTORS]; +static escSensorData_t srxl2Telemetry[MAX_SUPPORTED_MOTORS]; +static uint8_t rxBuffer[sizeof(srxl2TelemetryPacket_t)]; +static uint8_t rxBufferPosition; +static timeUs_t lastTxTimeUs; +static uint8_t frameCounter; +static uint8_t escId; +static bool handshakeConfirmed; + +static uint16_t swap16(uint16_t value) +{ + return (uint16_t)((value << 8) | (value >> 8)); +} + +// SRXL2 channel values span the full 16-bit range over +/-150% servo travel +// with 32768 = center (per the official spec comment on SrxlChannelData). A +// Spektrum receiver at -100%/+100% stick sends ~0x2AA0/0xD554, so that band +// is what the ESC's throttle logic expects. Raw 0 is -150% (a sub-900us +// pulse equivalent): Smart ESCs treat it as an invalid throttle and keep +// emitting the no-signal beep even with the bus link up. +#define SRXL2_CHANNEL_LOW 0x2AA0 +#define SRXL2_CHANNEL_HIGH 0xD554 + +static uint16_t scaleThrottleToSrxl2(uint16_t value) +{ + if (value <= motorConfig()->mincommand) { + return SRXL2_CHANNEL_LOW; + } + + // Lowest 2 bits of a channel value are reserved (RFU) - keep them clear. + return (uint16_t)scaleRange(value, motorConfig()->mincommand, getMaxThrottle(), SRXL2_CHANNEL_LOW, SRXL2_CHANNEL_HIGH) & 0xFFFC; +} + +static void srxl2EscSendHandshake(uint8_t destId) +{ + srxl2Handshake_t packet; + + packet.header = SRXL2_HEADER; + packet.type = SRXL2_PACKET_TYPE_HANDSHAKE; + packet.len = sizeof(packet); + packet.sourceId = SRXL2_RECEIVER_ID; + packet.destId = destId; + packet.priority = SRXL2_RECEIVER_PRIORITY; + packet.baudrate = SRXL2_RECEIVER_BAUDRATE; + packet.info = SRXL2_RECEIVER_INFO; + packet.uid = SRXL2_RECEIVER_UID; + packet.crc = swap16(crc16_ccitt_update(0, &packet, sizeof(packet) - sizeof(packet.crc))); + + serialWriteBuf(srxl2EscPort, (const uint8_t *)&packet, sizeof(packet)); + debugTxFrames++; +} + +static void srxl2EscSendControlPacket(void) +{ + srxl2ControlPacket_t packet; + + packet.header = SRXL2_HEADER; + packet.type = SRXL2_PACKET_TYPE_CONTROL; + packet.len = sizeof(packet); + packet.command = SRXL2_CONTROL_CMD_CHANNEL; + packet.replyId = ((frameCounter % SRXL2_ESC_REQUEST_EVERY_N_FRAMES) == 0) ? escId : 0; + packet.channelData.rssi = 100; + packet.channelData.frameLosses = 0; + packet.channelData.channelMask = 0x41; + packet.channelData.throttle = srxl2MotorValues[0]; + packet.channelData.reverse = srxl2ReverseActive ? SRXL2_CHANNEL_HIGH : SRXL2_CHANNEL_LOW; + packet.crc = swap16(crc16_ccitt_update(0, &packet, sizeof(packet) - sizeof(packet.crc))); + + serialWriteBuf(srxl2EscPort, (const uint8_t *)&packet, sizeof(packet)); + frameCounter++; +} + +static bool srxl2EscValidateFrame(const uint8_t *buffer, uint8_t length) +{ + if (length < 6) { + return false; + } + + // CRC is transmitted big-endian (MSB first) on the wire, so read it back the + // same way and compare against the locally computed value without byte-swapping. + const uint16_t expected = crc16_ccitt_update(0, buffer, length - 2); + const uint16_t received = ((uint16_t)buffer[length - 2] << 8) | buffer[length - 1]; + + return expected == received; +} + +static void srxl2EscParseEscTelemetry(const uint8_t *payload) +{ + xbusEsc_t esc; + + memcpy(&esc, payload, sizeof(esc)); + + srxl2Telemetry[0].dataAge = 0; + srxl2Telemetry[0].temperature = (swap16(esc.tempFet) == 0xFFFF) ? 0 : (int16_t)(swap16(esc.tempFet) / 10); + srxl2Telemetry[0].voltage = (swap16(esc.voltsInput) == 0xFFFF) ? 0 : (int16_t)swap16(esc.voltsInput); + // current_motor is reported in 10 mA units, which is exactly the centiampere + // (0.01 A) unit expected by escSensorData_t, so store it without scaling. + srxl2Telemetry[0].current = (swap16(esc.currentMotor) == 0xFFFF) ? 0 : (int32_t)swap16(esc.currentMotor); + // The ESC reports electrical RPM (in 10 RPM units): convert to shaft RPM + // using the pole count of the attached motor, same as DSHOT telemetry. + srxl2Telemetry[0].rpm = (swap16(esc.rpm) == 0xFFFF) ? 0 : (uint32_t)swap16(esc.rpm) * 10 * 2 / MAX(2, motorConfig()->motorPoleCount); +} + +static void srxl2EscHandleIncomingByte(uint8_t value) +{ + if (rxBufferPosition == 0 && value != SRXL2_HEADER) { + return; + } + + if (rxBufferPosition < sizeof(rxBuffer)) { + rxBuffer[rxBufferPosition++] = value; + } else { + rxBufferPosition = 0; + return; + } + + if (rxBufferPosition < 3) { + return; + } + + const uint8_t expectedLength = rxBuffer[2]; + if (expectedLength == 0 || expectedLength > sizeof(rxBuffer)) { + rxBufferPosition = 0; + return; + } + + if (rxBufferPosition < expectedLength) { + return; + } + + if (srxl2EscValidateFrame(rxBuffer, expectedLength)) { + debugRxValidFrames++; + if (rxBuffer[1] == SRXL2_PACKET_TYPE_HANDSHAKE && rxBuffer[3] == SRXL2_ESC_ID) { + if (rxBuffer[4] == 0) { + // At power-up (or after 50ms of bus silence) the ESC sends an + // unprompted handshake (sourceId = ESC, destId = 0). Per the + // official SRXL2 master behaviour, reply immediately with a + // handshake addressed to it; its addressed reply follows. + srxl2EscSendHandshake(SRXL2_ESC_ID); + } else { + // Handshake addressed back to us: the ESC answered our poll. + // Per the official SRXL2 device state machine the ESC only + // enters its running state (and starts accepting throttle) on + // the final broadcast handshake, so send it now. + escId = rxBuffer[3]; + debugHandshakeReplies++; + srxl2EscSendHandshake(SRXL2_BROADCAST_ID); + handshakeConfirmed = true; + } + } else if (rxBuffer[1] == SRXL2_PACKET_TYPE_TELEMETRY && rxBuffer[3] == SRXL2_RECEIVER_ID && rxBuffer[4] == XBUS_ESC_ID) { + handshakeConfirmed = true; + debugTelemetryFrames++; + srxl2EscParseEscTelemetry(&rxBuffer[4]); + } else if (rxBuffer[1] == SRXL2_PACKET_TYPE_TELEMETRY && rxBuffer[3] == SRXL2_BROADCAST_ID) { + // Telemetry addressed to 0xFF is the SRXL2 re-handshake request: + // the ESC lost the link, so restart the discovery sequence. + handshakeConfirmed = false; + } + } else { + debugRxCrcFails++; + } + + rxBufferPosition = 0; +} + +bool srxl2EscInit(void) +{ + srxl2EscInitialized = false; + srxl2EscPort = NULL; + + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_SRXL2_ESC); + if (!portConfig) { + return false; + } + + // SRXL2 is a single-wire half-duplex bus: throttle command and telemetry share + // one line on the UART TX pin, so the port must be opened in bidirectional mode. + // Plain SERIAL_BIDIR (open-drain with internal pull-up on F4) is the proven + // configuration for SRXL2 on STM32 — it is what Betaflight uses for its + // field-tested SRXL2 driver — and it keeps the line at a defined high level + // between frames. SERIAL_BIDIR_PP must NOT be used here: the push-pull pin + // has no pull-up, so the line floats whenever the half-duplex UART releases + // the driver, feeding noise to the ESC. + srxl2EscPort = openSerialPort(portConfig->identifier, FUNCTION_SRXL2_ESC, NULL, NULL, SRXL2_ESC_BAUDRATE, MODE_RXTX, SERIAL_NOT_INVERTED | SERIAL_BIDIR); + if (!srxl2EscPort) { + return false; + } + + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + srxl2MotorValues[i] = SRXL2_CHANNEL_LOW; + } + memset(srxl2Telemetry, 0xFF, sizeof(srxl2Telemetry)); + rxBufferPosition = 0; + lastTxTimeUs = 0; + lastRxActivityUs = 0; + frameCounter = 0; + escId = SRXL2_ESC_ID; + handshakeConfirmed = false; + srxl2ReverseActive = false; + debugTxFrames = 0; + debugRxValidFrames = 0; + debugRxCrcFails = 0; + debugRxBytes = 0; + debugHandshakeReplies = 0; + debugTelemetryFrames = 0; + srxl2EscInitialized = true; + + return true; +} + +void srxl2EscWriteMotor(uint8_t index, uint16_t value) +{ + if (index < MAX_SUPPORTED_MOTORS) { + srxl2MotorValues[index] = scaleThrottleToSrxl2(value); + } +} + +void srxl2EscSetReverse(bool reverse) +{ + // Transmitted on the SRXL2 reverse channel; when and how the direction + // change is applied (e.g. only through zero throttle) is governed by the + // ESC's own firmware. + srxl2ReverseActive = reverse; +} + +void srxl2EscUpdate(timeUs_t currentTimeUs) +{ + if (!srxl2EscInitialized || !srxl2EscPort) { + return; + } + + if (serialRxBytesWaiting(srxl2EscPort) > 0) { + lastRxActivityUs = currentTimeUs; + while (serialRxBytesWaiting(srxl2EscPort) > 0) { + debugRxBytes++; + srxl2EscHandleIncomingByte(serialRead(srxl2EscPort)); + } + } + + DEBUG_SET(DEBUG_SRXL2, 0, handshakeConfirmed ? 1 : 0); + DEBUG_SET(DEBUG_SRXL2, 1, debugTxFrames); + DEBUG_SET(DEBUG_SRXL2, 2, debugRxBytes); + DEBUG_SET(DEBUG_SRXL2, 3, debugRxValidFrames); + DEBUG_SET(DEBUG_SRXL2, 4, debugRxCrcFails); + DEBUG_SET(DEBUG_SRXL2, 5, debugHandshakeReplies); + DEBUG_SET(DEBUG_SRXL2, 6, debugTelemetryFrames); + DEBUG_SET(DEBUG_SRXL2, 7, srxl2MotorValues[0]); + + // Half-duplex turnaround: never start transmitting while the ESC may + // still be replying (this function runs from the scheduler busy-loop, so + // it re-checks within microseconds). + if ((currentTimeUs - lastRxActivityUs) < SRXL2_ESC_QUIESCENCE_US) { + return; + } + + const timeUs_t txIntervalUs = handshakeConfirmed ? SRXL2_ESC_INTERVAL_US : SRXL2_ESC_HANDSHAKE_INTERVAL_US; + if ((currentTimeUs - lastTxTimeUs) < txIntervalUs) { + return; + } + + lastTxTimeUs = currentTimeUs; + + if (!handshakeConfirmed) { + srxl2EscSendHandshake(SRXL2_ESC_ID); + } else { + srxl2EscSendControlPacket(); + if (frameCounter == 0xFF) { + frameCounter = 1; + } + } +} + +bool srxl2EscIsInitialized(void) +{ + return srxl2EscInitialized; +} + +bool srxl2EscGetTelemetry(uint8_t index, escSensorData_t *data) +{ + if (!srxl2EscInitialized || !data || index >= MAX_SUPPORTED_MOTORS) { + return false; + } + + if (srxl2Telemetry[index].dataAge == ESC_DATA_INVALID) { + return false; + } + + *data = srxl2Telemetry[index]; + return true; +} + +#else + +bool srxl2EscInit(void) +{ + return false; +} + +void srxl2EscWriteMotor(uint8_t index, uint16_t value) +{ + UNUSED(index); + UNUSED(value); +} + +void srxl2EscSetReverse(bool reverse) +{ + UNUSED(reverse); +} + +void srxl2EscUpdate(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); +} + +bool srxl2EscIsInitialized(void) +{ + return false; +} + +bool srxl2EscGetTelemetry(uint8_t index, escSensorData_t *data) +{ + UNUSED(index); + UNUSED(data); + return false; +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/srxl2_esc.h b/src/main/drivers/srxl2_esc.h new file mode 100644 index 00000000000..5d4a2958b63 --- /dev/null +++ b/src/main/drivers/srxl2_esc.h @@ -0,0 +1,32 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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. + */ + +#pragma once + +#include +#include + +#include "drivers/time.h" +#include "sensors/esc_sensor.h" + +#define SRXL2_ESC_UPDATE_HZ 100 + +bool srxl2EscInit(void); +void srxl2EscWriteMotor(uint8_t index, uint16_t value); +void srxl2EscSetReverse(bool reverse); +void srxl2EscUpdate(timeUs_t currentTimeUs); +bool srxl2EscIsInitialized(void); +bool srxl2EscGetTelemetry(uint8_t index, escSensorData_t *data); \ No newline at end of file diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 8ed295c2d3f..6dcb9b39090 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -228,7 +228,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "GPS", "LULU", "SBUS2", - "OSD_REFRESH" + "OSD_REFRESH", + "SRXL2" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..752efdfff69 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -1042,7 +1042,7 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs) afatfs_poll(); #endif -#ifdef USE_DSHOT +#if defined(USE_DSHOT) || defined(USE_SRXL2_ESC) pwmCompleteMotorUpdate(); #endif diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 842b56563a0..51f7848ec2f 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -35,6 +35,7 @@ #include "io/osd.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "sensors/diagnostics.h" #include "sensors/sensors.h" @@ -110,6 +111,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXGIMBALCENTER, .boxName = "GIMBAL CENTER", .permanentId = 67 }, { .boxId = BOXGIMBALHTRK, .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 }, { .boxId = BOXAUTOSPEED, .boxName = "AUTO SPEED", .permanentId = 69 }, + { .boxId = BOXMOTORREVERSE, .boxName = "MOTOR REVERSE", .permanentId = 70 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -361,6 +363,14 @@ void initActiveBoxIds(void) } #endif +#ifdef USE_SRXL2_ESC + // Reversible Spektrum Smart ESC: expose the reverse switch only when the + // motor protocol actually drives the SRXL2 reverse channel. + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + ADD_ACTIVE_BOX(BOXMOTORREVERSE); + } +#endif + #if (MAX_MIXER_PROFILE_COUNT > 1) ADD_ACTIVE_BOX(BOXMIXERPROFILE); ADD_ACTIVE_BOX(BOXMIXERTRANSITION); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 48e4357a1a5..a79a4649226 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -86,6 +86,7 @@ typedef enum { BOXGIMBALCENTER = 58, BOXGIMBALHTRK = 59, BOXAUTOSPEED = 60, + BOXMOTORREVERSE = 61, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7d2e9c01385..5b88e34e196 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -27,7 +27,7 @@ tables: - name: blackbox_device values: ["SERIAL", "SPIFLASH", "SDCARD", "FILE"] - name: motor_pwm_protocol - values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600"] + values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "SRXL2"] - name: servo_protocol values: ["PWM", "SBUS", "SBUS_PWM"] - name: failsafe_procedure @@ -84,7 +84,8 @@ tables: "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", - "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH"] + "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", + "SRXL2"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 27f1ec7ffe2..228df617801 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -35,6 +35,7 @@ #include "config/config_reset.h" #include "drivers/pwm_output.h" +#include "drivers/srxl2_esc.h" #include "drivers/pwm_mapping.h" #include "drivers/time.h" @@ -369,6 +370,12 @@ static void applyTurtleModeToMotors(void) { void FAST_CODE writeMotors(void) { #if !defined(SITL_BUILD) +#ifdef USE_SRXL2_ESC + // Reversible Spektrum Smart ESC: forward the MOTOR REVERSE mode state to + // the SRXL2 reverse channel. The ESC firmware governs when the direction + // change is actually applied. + srxl2EscSetReverse(IS_RC_MODE_ACTIVE(BOXMOTORREVERSE)); +#endif for (int i = 0; i < motorCount; i++) { uint16_t motorValue; #ifdef USE_DSHOT diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 36f2e02328a..9f58b92a5f3 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -59,6 +59,7 @@ typedef enum { FUNCTION_MSP_OSD = (1 << 25), // 33554432 FUNCTION_GIMBAL = (1 << 26), // 67108864 FUNCTION_GIMBAL_HEADTRACKER = (1 << 27), // 134217728 + FUNCTION_SRXL2_ESC = (1 << 28), // 268435456 } serialPortFunction_e; #define FUNCTION_VTX_MSP FUNCTION_MSP_OSD diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index bc77c281c79..ff5f8978514 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -42,6 +42,8 @@ #include "flight/mixer.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" +#include "drivers/srxl2_esc.h" #include "sensors/esc_sensor.h" #include "io/serial.h" #include "fc/config.h" @@ -78,6 +80,27 @@ static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS]; static escSensorData_t escSensorDataCombined; static bool escSensorDataNeedsUpdate; +static bool escSensorUseSrxl2(void) +{ + return motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2 && srxl2EscIsInitialized(); +} + +static void escSensorRefreshFromSrxl2(void) +{ + if (!escSensorUseSrxl2()) { + return; + } + + for (int i = 0; i < getMotorCount(); i++) { + escSensorData_t telemetry; + if (srxl2EscGetTelemetry(i, &telemetry)) { + escSensorData[i] = telemetry; + } + } + + escSensorDataNeedsUpdate = true; +} + PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1); PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, .currentOffset = 0, // UNUSED @@ -151,12 +174,15 @@ uint32_t computeRpm(int16_t erpm) { escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc) { + escSensorRefreshFromSrxl2(); return &escSensorData[esc]; } escSensorData_t * escSensorGetData(void) { - if (!escSensorPort) { + escSensorRefreshFromSrxl2(); + + if (!escSensorPort && !escSensorUseSrxl2()) { return NULL; } @@ -207,6 +233,19 @@ bool escSensorInitialize(void) escSensorDataNeedsUpdate = true; escSensorPort = NULL; + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + escSensorData[i].dataAge = ESC_DATA_INVALID; + } + + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + if (srxl2EscIsInitialized()) { + ENABLE_STATE(ESC_SENSOR_ENABLED); + return true; + } + + return false; + } + // Fail immediately if motor output are disabled or motor outputs are not configured if (!feature(FEATURE_PWM_OUTPUT_ENABLE) || getMotorCount() == 0) { return false; @@ -224,10 +263,6 @@ bool escSensorInitialize(void) return false; } - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - escSensorData[i].dataAge = ESC_DATA_INVALID; - } - ENABLE_STATE(ESC_SENSOR_ENABLED); return true; @@ -235,6 +270,12 @@ bool escSensorInitialize(void) void escSensorUpdate(timeUs_t currentTimeUs) { + if (escSensorUseSrxl2()) { + UNUSED(currentTimeUs); + escSensorRefreshFromSrxl2(); + return; + } + if (!escSensorPort) { return; } diff --git a/src/main/target/common.h b/src/main/target/common.h index e4b7b4013fd..d12b2a46023 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -438,6 +438,7 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#define USE_SRXL2_ESC #ifdef USE_GPS #define USE_GEOZONE #define MAX_GEOZONES_IN_CONFIG 63