diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md new file mode 100644 index 00000000000..7369d9e4f71 --- /dev/null +++ b/.github/copilot-instructions.md @@ -0,0 +1,426 @@ +# INAV Copilot Instructions + +## Project Overview + +INAV is a C (C99/C11) navigation flight controller firmware for STM32 F4/F7/H7 and AT32 MCUs. It evolved from Cleanflight/Baseflight. The codebase targets resource-constrained embedded systems — every decision about memory, abstractions, and code structure reflects that. + +## Architecture + +Source lives under `src/main/` with these key subsystems: + +| Directory | Role | +|---|---| +| `fc/` | Core flight controller: init (`fc_init.c`), main loop (`fc_core.c`), task scheduling (`fc_tasks.c`), MSP handling (`fc_msp.c`) | +| `flight/` | PID controller (`pid.c`), motor mixer (`mixer.c`), altitude/position hold | +| `navigation/` | Waypoint missions, RTH, position estimation | +| `sensors/` | Gyro, accel, baro, GPS, rangefinder — detection, calibration, filtering | +| `drivers/` | Hardware abstraction: SPI, I2C, UART, timers, DMA | +| `config/` | Parameter Group (PG) system for persistent settings | +| `io/` | OSD, LED strips, serial | +| `rx/` | Receiver protocols (CRSF, SBUS, IBUS) | +| `telemetry/` | SmartPort, MAVLink, LTM, CRSF telemetry | +| `target/` | Per-board hardware definitions (`target.h`, `target.c`) | +| `programming/` | Logic conditions and global functions (GUI-programmable) | + +**Control flow**: `main.c` → `fc_init.c:init()` → `fc_tasks.c:tasksInit()` → cooperative scheduler. Critical path: Gyro → PID → Mixer → Motors (highest priority). + +## Build System + +CMake 3.13+ with out-of-source builds. **Ruby is required** for settings generation. + +```bash +# Build firmware for a specific target +mkdir build && cd build +cmake .. +make MATEKF722SE + +# Build SITL (host simulation) +cmake -DSITL=ON .. +make + +# Build and run unit tests (native, no cross-compiler) +mkdir testing && cd testing +cmake -DTOOLCHAIN= .. +make check +``` + +Each board target lives in `src/main/target/TARGETNAME/` with `target.h` (pin definitions, `#define USE_*` feature flags) and optional `CMakeLists.txt`. Targets register via MCU-specific functions like `target_stm32f405xg()`. + +## Settings System (Critical Path) + +CLI settings follow this pipeline: **`fc/settings.yaml`** → Ruby script (`src/utils/settings.rb`) → generated `settings_generated.{h,c}`. + +In `settings.yaml`: +- **`tables`**: string↔enum mappings (e.g., `off_on` → `{ "OFF", "ON" }`) +- **`constants`**: numeric bounds (e.g., `RPYL_PID_MAX: 255`) +- **`groups`**: map to Parameter Groups — each member defines `field`, `min`, `max`, `default_value`, `type`, `table`, `condition` + +After changing settings: run `python src/utils/update_cli_docs.py` to regenerate CLI docs. + +## Parameter Groups (PG) + +All persistent configuration uses the PG system. Pattern: + +```c +// 1. Define struct in header (types use _t suffix) +typedef struct { uint8_t gyro_lpf_hz; uint16_t gyro_kalman_q; } gyroConfig_t; + +// 2. Register with defaults (PG ID from config/parameter_group_ids.h) +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12); + +// 3. Access read-only +gyroConfig()->gyro_lpf_hz + +// 4. Access mutable (for CLI/MSP writes) +gyroConfigMutable()->gyro_lpf_hz = 80; +``` + +**When changing a PG struct, you MUST bump the version number** in the registration macro. CI enforces this via `pg-version-check.yml`. + +## Motor Output Protocols + +### Architecture Overview + +Motor output uses a **function-pointer abstraction** in `drivers/pwm_output.c`. A single `pwmWriteFuncPtr` is assigned at init based on the selected protocol, so `pwmWriteMotor(index, value)` dispatches through it to the correct implementation. Key files: + +| File | Role | +|---|---| +| `drivers/pwm_mapping.h` | Protocol enum (`motorPwmProtocolTypes_e`), protocol properties | +| `drivers/pwm_mapping.c` | Timer→motor/servo allocation, init, conflict checking | +| `drivers/pwm_output.h` | Public API: `pwmWriteMotor()`, `pwmCompleteMotorUpdate()`, DShot commands | +| `drivers/pwm_output.c` | All protocol implementations: PWM, OneShot, Multishot, Brushed, DShot | +| `flight/mixer.c` | PID→motor mixing, scaling, calls `pwmWriteMotor()` per motor | +| `drivers/timer.h` | Timer infrastructure: `TCH_t`, `timerHardware_t`, DMA API | + +### Available Protocols + +```c +typedef enum { + PWM_TYPE_STANDARD = 0, // 1-2ms pulse @ 400Hz + PWM_TYPE_ONESHOT125, // 125-250µs pulse @ 1kHz + PWM_TYPE_MULTISHOT, // 5-25µs pulse @ 2kHz + PWM_TYPE_BRUSHED, // 0-100% duty cycle, configurable Hz + PWM_TYPE_DSHOT150, // Digital 150kbit/s (3MHz timer, 4kHz update) + PWM_TYPE_DSHOT300, // Digital 300kbit/s (6MHz timer, 8kHz update) + PWM_TYPE_DSHOT600, // Digital 600kbit/s (12MHz timer, 16kHz update) +} motorPwmProtocolTypes_e; +``` + +### Analog Protocols (PWM / OneShot125 / Multishot / Brushed) + +All analog protocols share the same write function (`pwmWriteStandard()`), which writes directly to the timer CCR register: `*ccr = value * pulseScale + pulseOffset`. They differ only in timing parameters passed to `motorConfigPwm()`: + +- **Standard PWM**: `sMin=1ms, sLen=1ms` at 400Hz on a 1MHz timer +- **OneShot125**: `sMin=125µs, sLen=125µs` at 1kHz +- **Multishot**: `sMin=5µs, sLen=20µs` at 2kHz +- **Brushed**: `sMin=0, sLen=0` (full period = 100% scale), user-configurable rate + +### DShot (Digital Protocol) + +DShot encodes throttle as a **16-bit digital frame**: 11-bit throttle (48–2047) + 1-bit telemetry request + 4-bit CRC. Each bit is a fixed-width pulse where the high-time encodes 0 or 1 (bit period = 20 timer ticks; 0 = 7 ticks high, 1 = 14 ticks high). + +**Implementation is always DMA-based** (never bit-banging). Two DMA modes exist: + +- **Channel DMA** (default): each timer channel has its own DMA stream. Uses `timerPWMConfigChannelDMA()`. +- **Burst DMA** (`USE_DSHOT_DMAR`): a single DMA stream per timer handles up to 4 channels. Enabled per-target in `target.h` for boards with shared DMA stream constraints. + +DMA buffer: 18 elements per motor (16 data bits + 2 reset periods). + +**DShot commands** (direction, beeper, etc.) use a circular queue. `sendDShotCommand(cmd)` enqueues; during `pwmCompleteMotorUpdate()`, `executeDShotCommands()` dequeues and sends values 0–47 (the special command range) on all motors with telemetry bit forced high. + +### ESC Telemetry + +ESC telemetry provides per-motor RPM, voltage, current, and temperature via a **dedicated UART** (not the DShot signal line — INAV does not implement bidirectional DShot). Key files: `sensors/esc_sensor.{h,c}`, consumer in `flight/rpm_filter.c`, `sensors/battery.c`, `io/osd.c`. Feature guard: `USE_ESC_SENSOR`. + +**Protocol**: BLHeli_32 / KISS compatible — 10-byte frames at 115200 baud: + +| Byte | Content | +|---|---| +| 0 | Temperature (°C, uint8) | +| 1–2 | Voltage (big-endian, centivolt) | +| 3–4 | Current (big-endian, centiamp) | +| 5–6 | Consumption (unused) | +| 7–8 | eRPM (big-endian, uint16) | +| 9 | CRC8 checksum | + +**Data flow**: + +``` +ESC hardware → UART (115200 baud) → escSensorUpdate() [called from fc_core.c] + │ State machine round-robin: requests telemetry bit on one motor at a time + │ via pwmRequestMotorTelemetry(motor) → sets DShot packet bit 0 + ▼ +escSensorData[MAX_SUPPORTED_MOTORS] // per-motor: rpm, voltage, current, temp + │ + ├─► rpm_filter.c: rpmFilterUpdateTask() → per-motor notch filters on gyro + ├─► battery.c: VOLTAGE_SENSOR_ESC / CURRENT_SENSOR_ESC → vbat, amperage + ├─► osd.c: OSD_ESC_RPM, OSD_ESC_TEMPERATURE elements + └─► fc_msp.c: MSP2_INAV_ESC_RPM, MSP2_INAV_ESC_TELEM +``` + +**DShot telemetry bit integration**: In `prepareDshotPacket()`, bit 0 of the 16-bit DShot frame is the telemetry request flag: `(value << 1) | (requestTelemetry ? 1 : 0)`. `escSensorUpdate()` calls `pwmRequestMotorTelemetry(i)` which sets `motors[i].requestTelemetry = true` for the current motor in round-robin. In **listenOnly mode** (`esc_sensor_listen_only = ON`), BLHeli32 Auto Telemetry sends data without DShot bit requests. + +**RPM → eRPM conversion**: `rpm = eRPM * 100 / (motorPoleCount / 2)`. The `motor_poles` setting (default 14) is critical for correct RPM calculation and RPM-based gyro filtering. + +**Key data structures**: + +```c +escSensorData_t { + uint8_t dataAge; // incremented on timeout, 255 = invalid + int16_t temperature; // °C + int16_t voltage; // centivolt + int32_t current; // mA + uint32_t rpm; // mechanical RPM +} + +escSensorConfig_t { // PG-managed + uint16_t currentOffset; // offset for FC/VTX/cam current draw (mA) + uint8_t listenOnly; // 1 = BLHeli32 Auto Telemetry (no DShot request) +} +``` + +### Mixer → Motor Output Flow + +``` +PID loop → motor[i] array (FASTRAM int16_t) → writeMotors() → + ├─ DShot: scale 1000-2000 → 48-2047 via handleOutputScaling() + └─ Analog: use value directly (or 3D scaling for reversible) + → pwmWriteMotor(i, value) per motor + → pwmCompleteMotorUpdate() [DShot only]: + prepareDshotPacket() → load DMA buffers → start DMA transfer +``` + +### Init Flow + +``` +main() → init() → pwmMotorAndServoInit() → + ├─ pwmBuildTimerOutputList() // classify timers as motors/servos/LED + ├─ pwmInitMotors() → + │ ├─ pwmMotorPreconfigure() // assign motorWritePtr based on protocol + │ └─ per motor: pwmMotorConfig() → + │ ├─ analog: motorConfigPwm() → pwmOutConfig() → timerConfigBase() + │ └─ DShot: motorConfigDshot() → pwmOutConfig() + timerPWMConfigChannelDMA() + └─ pwmInitServos() +``` + +### Platform-Specific Timer Implementations + +| Platform | Timer implementation file | DMA API | Burst DMA | +|---|---|---|---| +| STM32F4 | `drivers/timer_impl_stdperiph.c` | StdPeriph | Supported (per-target) | +| STM32F7/H7 | `drivers/timer_impl_hal.c` | HAL | Supported (per-target) | +| AT32 | `drivers/timer_impl_stdperiph_at32.c` | AT32 DMAMUX | Not used | + +`USE_DSHOT` and `USE_DSHOT_DMAR` are per-target `#define`s in `target.h`. + +### Key Data Structures + +```c +pwmOutputPort_t { + TCH_t *tch; // timer channel handle + volatile timCCR_t *ccr; // CCR register for analog write + float pulseScale, pulseOffset; // value→µs conversion + timerDMASafeType_t dmaBuffer[18]; // DMA buffer for DShot +} + +pwmOutputMotor_t { + pwmOutputPort_t *pwmPort; // physical port (NULL if not timer-based) + uint16_t value; // last written value + bool requestTelemetry; // DShot telemetry flag +} + +motorConfig_t { // PG-managed persistent config + uint16_t mincommand; // disarmed value (typically 1000) + uint16_t motorPwmRate; // update frequency (brushed only) + uint8_t motorPwmProtocol; // index in motorPwmProtocolTypes_e + uint16_t digitalIdleOffsetValue; + uint8_t motorPoleCount; // for RPM calculation from eRPM +} +``` + +## Sensors + +### Architecture: `drivers/` vs `sensors/` Split + +- **`drivers//`** — Hardware abstraction: bus communication (SPI/I2C), chip-specific detection, raw data reads. Contains the device struct (`xxxDev_t`) with function pointers. +- **`sensors/`** — High-level logic: detection orchestration, calibration, filtering, conversion, alignment, health monitoring. Contains the state struct (e.g. `baro_t`) that wraps the dev struct. +- **`sensors/initialisation.c`** — Master init: calls `gyroInit()`, `accInit()`, `baroInit()`, `compassInit()`, `pitotInit()`, `rangefinderInit()`, `opflowInit()`, `temperatureInit()` in sequence. Persists detected sensors to EEPROM. + +GPS is a special case: lives in `io/gps.h` (serial protocol, not a bus device). + +### Driver Abstraction: Function Pointer Tables + +Each sensor type has a `xxxDev_t` struct with function pointers populated during detection: + +| Sensor | Dev Struct | Key Function Pointers | +|---|---|---| +| Gyro | `gyroDev_t` | `initFn`, `readFn`, `temperatureFn`, `intStatusFn`, `scale` | +| Accel | `accDev_t` | `initFn`, `readFn`, `acc_1G` | +| Baro | `baroDev_t` | `start_ut`, `get_ut`, `start_up`, `get_up`, `calculate` | +| Compass | `magDev_t` | `init`, `read`, `magAlign` | +| Rangefinder | `rangefinderDev_t` | `init`, `update`, `read`, `maxRangeCm` | +| Pitot | `pitotDev_t` | `start`, `get`, `calculate` | + +### Detection Pattern (switch-case with fall-through) + +All sensors use the same pattern. Example from baro: + +```c +bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { + switch (baroHardwareToUse) { + case BARO_AUTODETECT: + case BARO_BMP085: +#ifdef USE_BARO_BMP085 + if (bmp085Detect(dev)) { baroHardware = BARO_BMP085; break; } +#endif + if (baroHardwareToUse != BARO_AUTODETECT) break; + FALLTHROUGH; + case BARO_MS5611: + // ...fall through all supported sensors + } +} +``` + +Identical pattern for `gyroDetect()`, `compassDetect()`, `pitotDetect()`, etc. + +### Sensor Data Flow (Gyro — critical path) + +``` +Hardware (SPI/I2C) → gyroDev->readFn() → gyroADCRaw[3] + → subtract gyroZero (calibration) + → applySensorAlignment() (chip orientation: CW0/90/180/270 + flip) + → applyBoardAlignment() (rotation matrix from user config) + → × gyroDev->scale → gyro.gyroADCf[3] (°/s) + → filter chain: RPM filter → LPF → Dynamic Notch → Kalman + → IMU fusion → PID controller → Mixer → Motors +``` + +### Board Alignment + +Defined in `sensors/boardalignment.{h,c}`: +- **PG**: `boardAlignment_t` — `rollDeciDegrees`, `pitchDeciDegrees`, `yawDeciDegrees` +- Two levels applied in sequence: per-chip `applySensorAlignment()` (8 orientations) → board-level `applyBoardAlignment()` (rotation matrix) +- Compass has an additional independent alignment layer for external magnetometers + +### Supported Hardware + +- **Gyro/Accel**: MPU6000, MPU6500, MPU9250, BMI160, BMI088, ICM20689, ICM42605, BMI270, LSM6DXX +- **Baro**: BMP085, MS5611, MS5607, BMP280, BMP388, LPS25H, SPL06, DPS310, 2SMPB-02B +- **Compass**: HMC5883L, AK8975, AK8963, QMC5883L, QMC5883P, LIS3MDL, RM3100, VCM5883, MLX90393, IST8310, IST8308 +- **Rangefinder**: VL53L0X, VL53L1X, US42, TOF10102, TeraRanger EVO, USD1, NanoRadar +- **Pitot**: MS4525, ADC, DLVR L10D, Virtual +- **GPS**: u-blox M8+ (auto-config, protocol ≥15.0 required), MSP + +### GPS + +Lives in `io/gps.{h,c}`. Protocols: `GPS_UBLOX` (primary, auto-config), `GPS_MSP` (remote), `GPS_FAKE` (test). State machine: `GPS_UNKNOWN` → `GPS_INITIALIZING` → `GPS_RUNNING`. Key struct `gpsSolutionData_t`: fixType, numSat, lat/lon (1e-7°), alt (cm), velNED, groundSpeed, groundCourse, eph/epv, hdop. + +## Telemetry + +### Architecture + +Each protocol implements 3 functions: `initTelemetry()`, `checkTelemetryState()`, `handleTelemetry()`. All dispatched from `telemetry.c` under `#ifdef USE_TELEMETRY_*` guards. Task: `TASK_TELEMETRY` in `fc_tasks.c`. + +### Supported Protocols (11 total) + +| Protocol | Files | Key Characteristics | +|---|---|---| +| **SmartPort** | `telemetry/smartport.*` | FrSky S.Port, polled, half-duplex | +| **CRSF** | `telemetry/crsf.*` | Crossfire/ELRS, shares RX port, GPS+battery+attitude+flight mode | +| **MAVLink** | `telemetry/mavlink.*` | v1/v2, bidirectional, waypoint upload/download, configurable rates per stream | +| **LTM** | `telemetry/ltm.*` | Lightweight, G/A/S/O/X/N frames at different rates, shareable port | +| **HoTT** | `telemetry/hott.*` | Graupner, EAM+GPS modules | +| **iBUS** | `telemetry/ibus.*` | FlySky, shareable port with RX | +| **SBUS2** | `telemetry/sbus2.*` | Futaba, 32 slots in SBUS frame gaps | +| **SRXL** | `telemetry/srxl.*` | Spektrum, text generation OSD | +| **GHST** | `telemetry/ghst.*` | ImmersionRC Ghost, shares RX port | +| **Jeti EX Bus** | `telemetry/jetiexbus.*` | Jeti, 128-byte EX frames | +| **SIM** | `telemetry/sim.*` | GSM modem, GPS/failsafe/events via SMS | + +### MAVLink Details + +- Versions v1 and v2 (configurable). Autopilot type: `GENERIC` or `ARDUPILOT` (ArduCopter/ArduPlane mode mapping). +- **TX streams** with configurable rates: heartbeat, sys_status, attitude, vfr_hud, gps_raw_int, global_position_int, rc_channels, battery_status. +- **RX handlers**: `MISSION_COUNT/ITEM/REQUEST` (full waypoint protocol), `RC_CHANNELS_OVERRIDE`, `COMMAND_INT`, `ADSB_VEHICLE`. + +### Port Sharing + +CRSF, GHST, SRXL, SBUS2 telemetry shares the RX serial port (bidirectional). LTM and iBUS can share via `TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK`. Other protocols need a dedicated serial port with `FUNCTION_TELEMETRY_*` flag. + +## Receivers (RX) + +### Abstraction + +Each RX protocol populates an `rxRuntimeConfig_t` with function pointers during init: + +```c +typedef struct rxRuntimeConfig_s { + uint8_t channelCount; + rcReadRawDataFnPtr rcReadRawFn; // read raw channel value + rcFrameStatusFnPtr rcFrameStatusFn; // RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE + rcProcessFrameFnPtr rcProcessFrameFn; // optional post-processing + uint16_t *channelData; // channel data buffer +} rxRuntimeConfig_t; +``` + +### Supported Protocols (15 serial + MSP + SIM) + +| Protocol | File | Baud | Notes | +|---|---|---|---| +| **CRSF** | `rx/crsf.c` | 420000 | Crossfire/ELRS, bidirectional | +| **GHST** | `rx/ghst.c` | 420000 | ImmersionRC Ghost | +| **SBUS** | `rx/sbus.c` | 100000 | FrSky, inverted | +| **SBUS Fast** | `rx/sbus.c` | 200000 | Fast variant | +| **SBUS2** | `rx/sbus.c` | 100000 | Futaba, with telemetry slots | +| **FBUS** | `rx/fport2.c` | — | FrSky Bus | +| **F.Port** | `rx/fport.c` | — | FrSky, RC+telemetry single wire | +| **F.Port 2** | `rx/fport2.c` | — | FrSky v2 | +| **iBUS** | `rx/ibus.c` | 115200 | FlySky | +| **SUMD** | `rx/sumd.c` | 115200 | Graupner | +| **Spektrum** | `rx/spektrum.c` | — | DSM 1024/2048 + SRXL | +| **SRXL2** | `rx/srxl2.c` | — | Spektrum v2 | +| **Jeti EX Bus** | `rx/jetiexbus.c` | — | Jeti | +| **MAVLink** | `rx/mavlink.c` | — | RC via `RC_CHANNELS_OVERRIDE` | +| **MSP** | `rx/msp.c` | — | RC via MSP protocol | + +### Channel Data Flow + +``` +RX hardware → UART → protocol driver (decode frame) + → rcFrameStatusFn() → RX_FRAME_COMPLETE + → rcReadRawFn() per channel → remap via rcmap[] (AETR default) + → validate (rx_min_usec..rx_max_usec), hold last valid on invalid + → rcChannels[].data (1000-2000 PWM range) + → failsafe check → rc_controls.c → flight modes, PID inputs +``` + +### RSSI / Link Quality + +Sources (auto-detect priority): ADC pin → RX channel → protocol-native (CRSF/GHST) → MSP. Digital protocols provide `rxLinkStatistics_t`: uplinkRSSI (dBm), uplinkLQ (0-100), uplinkSNR (dB), rfMode, txPower (mW). + +## Coding Conventions + +- **Style**: 4-space indent, K&R braces, `#pragma once` for headers +- **Naming**: types `_t`, enums `_e`, functions `camelCase`, constants `UPPER_SNAKE`, booleans `is*/can*/has*` +- **Memory attributes**: `FASTRAM`, `DMA_RAM`, `STATIC_FASTRAM` — respect these for real-time performance +- **Feature guards**: wrap optional code in `#ifdef USE_FEATURE` / `#endif` +- **Test visibility**: use `STATIC_UNIT_TESTED` instead of `static` when a function needs test access +- **MISRA C**: follow MISRA C guidelines + +## Unit Tests + +Google Test framework, files in `src/test/unit/*_unittest.cc` (C++). Tests link against the C firmware sources. Disabled tests use `.cc.txt` extension. + +## Cross-Platform Considerations + +Code must work across STM32F4/F7/H7 and AT32F43x. Use `#if defined(STM32F4)` guards for MCU-specific code. Key differences: F7/H7 have cache management, H7 has complex memory regions (DTCM, SRAM), AT32 has different peripheral APIs. Timer/DMA implementations are split per platform (see Motor Output Protocols section). + +## Common Pitfalls + +- Breaking PG structure without version bump → CI failure and config corruption +- Breaking MSP protocol → configurator incompatibility +- Ignoring `FASTRAM` placement → real-time performance degradation +- Hardcoding values instead of using PG system +- Forgetting `#ifdef USE_*` guards → build failures on targets without that feature +- Mixing DMA streams across motors/servos without checking `pwm_mapping.c` conflict logic +- PRs must target `maintenance-X.x` branch (current version), never `master` 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/docs/Settings.md b/docs/Settings.md index cbc526adb33..75df230eff6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3273,6 +3273,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 e060e558f89..5d9407979e7 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/common/log.c b/src/main/common/log.c index c2cc30a3a48..ce6562d5657 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -205,8 +205,10 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t { // Print lines of up to maxBytes bytes. We need 5 characters per byte // 0xAB[space|\n] - const size_t charsPerByte = 5; - const size_t maxBytes = 8; + // Use enum constants (not const size_t locals) so the buffer size is a true + // integer constant expression. Newer Apple clang otherwise folds the const + // locals and reports the array as a VLA (-Werror=gnu-folding-constant). + enum { charsPerByte = 5, maxBytes = 8 }; char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator size_t bufPos = LOG_PREFIX_FORMATTED_SIZE; const uint8_t *inputPtr = buffer; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index a0239642abb..e987d6c33c5 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 4a6722046c5..f916a6c57d5 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 a4efb9e3008..ba1167c5741 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..d6851bc2bbd --- /dev/null +++ b/src/main/drivers/srxl2_esc.c @@ -0,0 +1,359 @@ +/* + * 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 "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_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_RECEIVER_PRIORITY 0x0A +#define SRXL2_RECEIVER_BAUDRATE 1 +#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 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)); +} + +static uint16_t scaleThrottleToSrxl2(uint16_t value) +{ + if (value <= motorConfig()->mincommand) { + return 0; + } + + return (uint16_t)scaleRange(value, motorConfig()->mincommand, getMaxThrottle(), 0, 65532); +} + +static void srxl2EscSendHandshake(void) +{ + srxl2Handshake_t packet; + + packet.header = SRXL2_HEADER; + packet.type = SRXL2_PACKET_TYPE_HANDSHAKE; + packet.len = sizeof(packet); + packet.sourceId = SRXL2_RECEIVER_ID; + packet.destId = SRXL2_ESC_ID; + 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)); +} + +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 = 0; + 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); + srxl2Telemetry[0].rpm = (swap16(esc.rpm) == 0xFFFF) ? 0 : (uint32_t)swap16(esc.rpm) * 10; +} + +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)) { + if (rxBuffer[1] == SRXL2_PACKET_TYPE_HANDSHAKE && rxBuffer[3] == SRXL2_ESC_ID) { + escId = rxBuffer[3]; + handshakeConfirmed = true; + } else if (rxBuffer[1] == SRXL2_PACKET_TYPE_TELEMETRY && rxBuffer[3] == SRXL2_RECEIVER_ID && rxBuffer[4] == XBUS_ESC_ID) { + handshakeConfirmed = true; + srxl2EscParseEscTelemetry(&rxBuffer[4]); + } else if (rxBuffer[1] == SRXL2_PACKET_TYPE_TELEMETRY && rxBuffer[3] == 0xFF) { + handshakeConfirmed = false; + } + } + + 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. + srxl2EscPort = openSerialPort(portConfig->identifier, FUNCTION_SRXL2_ESC, NULL, NULL, SRXL2_ESC_BAUDRATE, MODE_RXTX, SERIAL_NOT_INVERTED | SERIAL_BIDIR); + if (!srxl2EscPort) { + return false; + } + + memset(srxl2MotorValues, 0, sizeof(srxl2MotorValues)); + memset(srxl2Telemetry, 0xFF, sizeof(srxl2Telemetry)); + rxBufferPosition = 0; + lastTxTimeUs = 0; + frameCounter = 0; + escId = SRXL2_ESC_ID; + handshakeConfirmed = false; + srxl2EscInitialized = true; + + return true; +} + +void srxl2EscWriteMotor(uint8_t index, uint16_t value) +{ + if (index < MAX_SUPPORTED_MOTORS) { + srxl2MotorValues[index] = scaleThrottleToSrxl2(value); + } +} + +void srxl2EscUpdate(timeUs_t currentTimeUs) +{ + if (!srxl2EscInitialized || !srxl2EscPort) { + return; + } + + while (serialRxBytesWaiting(srxl2EscPort) > 0) { + srxl2EscHandleIncomingByte(serialRead(srxl2EscPort)); + } + + if ((currentTimeUs - lastTxTimeUs) < SRXL2_ESC_INTERVAL_US) { + return; + } + + lastTxTimeUs = currentTimeUs; + + if (!handshakeConfirmed) { + srxl2EscSendHandshake(); + } 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 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..98b308353e3 --- /dev/null +++ b/src/main/drivers/srxl2_esc.h @@ -0,0 +1,31 @@ +/* + * 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 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/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/settings.yaml b/src/main/fc/settings.yaml index 15a0c0fff4f..537a888a2fb 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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 67416a16dff..ce04e6713f5 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -770,7 +770,7 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val) int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart); // We can show up to 7 digits in decimalPart. int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER); - STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); + STATIC_ASSERT(GPS_DEGREES_DIVIDER == 10000000, adjust_max_decimal_digits); int decimalDigits; bool djiCompat = false; // Assume DJICOMPAT mode is no enabled 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 bd7730226f6..6a7f1bf1b07 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -214,6 +214,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